def test_absolute_tolerance(pid_controller: PIDController, pid_runner: ControllerRunner): reference = 50 tolerance = 10 inp = pid_controller.measurement_source pid_controller.setAbsoluteTolerance(tolerance) pid_controller.setReference(reference) pid_runner.enable() time.sleep(1) assert not pid_controller.atReference(), ( "Error was in tolerance when it should not have been. Error was %f" % pid_controller.getError()) inp.return_value = reference + tolerance / 2 time.sleep(1) assert pid_controller.atReference(), ( "Error was not in tolerance when it should have been. Error was %f" % pid_controller.getError()) inp.return_value = reference + 10 * tolerance time.sleep(1) assert not pid_controller.atReference(), ( "Error was in tolerance when it should not have been. Error was %f" % pid_controller.getError())
def test_percent_tolerance(pid_controller: PIDController, pid_runner: ControllerRunner): reference = 50 tolerance = 10 inp = pid_controller.measurement_source pid_controller.setPercentTolerance(tolerance) pid_controller.setReference(reference) pid_runner.enable() assert not pid_controller.atReference(), ( "Error was in tolerance when it should not have been. Error was %f" % pid_controller.getError()) # half of percent tolerance away from reference inp.return_value = reference + tolerance / 200 * input_range time.sleep(1) assert pid_controller.atReference(), ( "Error was not in tolerance when it should have been. Error was %f" % pid_controller.getError()) # double percent tolerance away from reference inp.return_value = reference + tolerance / 50 * input_range time.sleep(1) assert not pid_controller.atReference(), ( "Error was in tolerance when it should not have been. Error was %f" % pid_controller.getError())
def robotInit(self): self.driveMotor = rev.CANSparkMax(1, rev.MotorType.kBrushless) self.turnMotor = rev.CANSparkMax(4, rev.MotorType.kBrushless) self.turnEncoder = self.turnMotor.getEncoder() self.turnEncoder.setPositionConversionFactor(20) # PID coefficients self.kP = 5e-5 self.kI = 1e-6 self.kD = 0 self.kIz = 0 self.PIDTolerance = 0 #PID controllers for the turn motors self.turnController = PIDController(self.kP, self.kI, self.kD) self.turnController.setTolerance(self.PIDTolerance) self.turnController.enableContinuousInput(0, 360) self.joystick = wpilib.Joystick(0) self.joystickDeadband = .05 self.timer = wpilib.Timer() #used to use it while testing stuff, don't need it now, but oh well # Push PID Coefficients to SmartDashboard wpilib.SmartDashboard.putNumber("P Gain", self.kP) wpilib.SmartDashboard.putNumber("I Gain", self.kI) wpilib.SmartDashboard.putNumber("D Gain", self.kD) wpilib.SmartDashboard.putNumber("I Zone", self.kIz) wpilib.SmartDashboard.putNumber("Set Rotations", 0) wpilib.SmartDashboard.putNumber("Tolerance", self.PIDTolerance) wpilib.SmartDashboard.putBoolean("Manual Control", False) wpilib.SmartDashboard.putBoolean("Manual Speed", 0)
def test_pidcontroller_calculate_rate7(input, setpoint, expected_error, expected_output): pid = PIDController(0.1, 0, 0.075) pid.enableContinuousInput(-180.0, 180.0) pid.setIntegratorRange(-1, 1) pid.setSetpoint(setpoint) out = pid.calculate(input) assert pid.getPositionError() == pytest.approx(expected_error, 0.01) # assert out == pytest.approx(expected_output, 0.01) assert out != 0
def setup(self): # Heading PID controller self.heading_pid = PIDController( Kp=1.5, Ki=0.01, Kd=0.03, measurement_source=self.imu.getAngle, period=1 / 50, ) # this gain is being changed depending on speed self.heading_pid.setInputRange(-math.pi, math.pi) self.heading_pid.setOutputRange(-3, 3) self.heading_pid.setContinuous() self.modules = [ self.module_a, self.module_b, self.module_c, self.module_d ] self.odometry_x = 0 self.odometry_y = 0 self.odometry_x_vel = 0 self.odometry_y_vel = 0 self.odometry_z_vel = 0 self.A = np.array( [ [1, 0, 1], [0, 1, 1], [1, 0, 1], [0, 1, 1], [1, 0, 1], [0, 1, 1], [1, 0, 1], [0, 1, 1], ], dtype=float, ) self.last_odometry_time = 0 # wpilib.SmartDashboard.putData("heading_pid", self.heading_pid) # figure out the contribution of the robot's overall rotation about the # z axis to each module's movement, and encode that information in a for i, module in enumerate(self.modules): module_dist = math.hypot(module.x_pos, module.y_pos) module_angle = math.atan2(module.y_pos, module.x_pos) # self.z_axis_adjustment[i*2, 0] = -module_dist*math.sin(module_angle) # self.z_axis_adjustment[i*2+1, 0] = module_dist*math.cos(module_angle) self.A[i * 2, 2] = -module_dist * math.sin(module_angle) self.A[i * 2 + 1, 2] = module_dist * math.cos(module_angle)
def test_pidcontroller_init_args0(): pid = PIDController(period=5.0, Ki=2.0, Kp=1.0, Kd=3.0) assert pid.Kp == pytest.approx(1.0, 0.01) assert pid.Ki == pytest.approx(2.0, 0.01) assert pid.Kd == pytest.approx(3.0, 0.01) assert pid.period == pytest.approx(5.0, 0.01)
def robotInit(self): self.kP = 0.03 self.kI = 0.00 self.kD = 0.00 self.kF = 0.00 self.exampleButton = 7 #Sets up your PID controller with set constants self.controller = PIDController(self.kP, self.kI, self.kD) #set the range of values the Input will give #spark max are 42counts per revolution self.controller.enableContinuousInput(0, 42) #you can find information about the motor stuff in the documentation self.speed = self.controller.calculate(rev.CANSparkMax(4,rev.MotorType.kBrushless).getEncoder())
def test_pidcontroller_init_args8(): with pytest.raises(TypeError) as exinfo: PIDController() assert ( exinfo.value.args[0] == "__init__() missing 3 required positional arguments: 'Kp', 'Ki', and 'Kd'" )
def pid_controller(): inp = Mock() inp.return_value = 0 controller = PIDController(0.05, 0.0, 0.0, measurement_source=inp) controller.setInputRange(-input_range / 2, input_range / 2) yield controller controller.close()
class MyRobot(wpilib.TimedRobot): def robotInit(self): self.kP = 0.03 self.kI = 0.00 self.kD = 0.00 self.kF = 0.00 self.exampleButton = 7 #Sets up your PID controller with set constants self.controller = PIDController(self.kP, self.kI, self.kD) #set the range of values the Input will give #spark max are 42counts per revolution self.controller.enableContinuousInput(0, 42) #you can find information about the motor stuff in the documentation self.speed = self.controller.calculate(rev.CANSparkMax(4,rev.MotorType.kBrushless).getEncoder()) def autonomousInit(self): pass def autonomousPeriodic(self): pass def teleopInit(self): pass def teleopPeriodic(self): #use this to control the motor: rev.CANSparkMax(4,rev.MotorType.kBrushless).set(0.5) if self.auxiliary1.getRawButton(self.exampleButton): self.controller.setpoint(84) else: rev.CANSparkMax(4,rev.MotorType.kBrushless).set(self.speed)
def robotInit(self): self.driveMotor = rev.CANSparkMax(1, rev.MotorType.kBrushless) self.turnMotor = rev.CANSparkMax(4, rev.MotorType.kBrushless) self.turnEncoder = self.turnMotor.getEncoder() self.turnEncoder.setPositionConversionFactor(20) # PID coefficients self.kP = .0039 self.kI = 2e-6 self.kD = 0 #PID controllers for the turn motors self.turnController = PIDController(self.kP, self.kI, self.kD) self.PIDTolerance = 1.0 self.turnController.setTolerance(self.PIDTolerance) self.turnController.enableContinuousInput(0, 360) self.joystick = wpilib.Joystick(0) self.joystickDeadband = .1 self.timer = wpilib.Timer() #used to use it while testing stuff, don't need it now, but oh well self.robotLength = 10.0 self.robotWidth = 10.0 wpilib.CameraServer.launch()
class MyRobot(wpilib.TimedRobot): def robotInit(self): self.driveMotor = rev.CANSparkMax(1, rev.MotorType.kBrushless) self.turnMotor = rev.CANSparkMax(4, rev.MotorType.kBrushless) self.turnEncoder = self.turnMotor.getEncoder() self.turnEncoder.setPositionConversionFactor(20) # PID coefficients self.kP = .0039 self.kI = 2e-6 self.kD = 0 #PID controllers for the turn motors self.turnController = PIDController(self.kP, self.kI, self.kD) self.PIDTolerance = 1.0 self.turnController.setTolerance(self.PIDTolerance) self.turnController.enableContinuousInput(0, 360) self.joystick = wpilib.Joystick(0) self.joystickDeadband = .1 self.timer = wpilib.Timer() #used to use it while testing stuff, don't need it now, but oh well self.robotLength = 10.0 self.robotWidth = 10.0 wpilib.CameraServer.launch() def encoderBoundedPosition(self, encoder): #I don't know if there's a set continuous for encoders, but it's easy enough to write position = encoder.getPosition() position %= 360 if position < 0: position += 360 return position def turnSpeedCalculator(self): speed = self.turnController.calculate(self.encoderBoundedPosition(self.turnEncoder)) if abs(speed) > 1: speed /= abs(speed) return speed def swerveMath(self, x, y): driveSpeed = math.hypot(x, y) driveSpeed = min(driveSpeed, 1) angle = math.degrees(math.atan2(x, y)) #works for all 4 quadrants return (driveSpeed, angle) def swerveDrive(self, x, y): speeds = self.swerveMath(x, y) if max(abs(x), abs(y)) == 0: self.driveMotor.set(0) self.turnMotor.set(0) else: #checking whether to go to angle and drive forward or go to other side and drive backward position = self.encoderBoundedPosition(self.turnEncoder) goal = speeds[1] difference = abs(position - goal) if difference < 90 or difference > 270: self.turnController.setSetpoint(goal) self.driveMotor.set(speeds[0]) else: if goal < 180: self.turnController.setSetpoint(goal + 180) self.driveMotor.set(-speeds[0]) else: self.turnController.setSetpoint(goal - 180) self.driveMotor.set(-speeds[0]) self.turnMotor.set(self.turnSpeedCalculator()) '''I've been debating adding some checks to try and make this more efficient, but efficiency isn't super important right now and, more importantly, I don't really know python so anything I can think of would save minimal time (or possibly even make it take longer).''' def brakeMode(self): self.driveMotor.setIdleMode(rev.IdleMode.kBrake) self.turnMotor.setIdleMode(rev.IdleMode.kBrake) def coastMode(self): self.driveMotor.setIdleMode(rev.IdleMode.kCoast) self.turnMotor.setIdleMode(rev.IdleMode.kCoast) def checkDeadband(self, axis): if axis < self.joystickDeadband and axis > -self.joystickDeadband: axis = 0 return axis def autonomousInit(self): self.brakeMode() '''We want the motors in brake mode while we are actually using them, which would be anytime during auto or teleop. However, we want them in coast mode while disabled so that people can easily spin/adjust the wheels as needed. Not super important, but also not hard to add''' def autonomousPeriodic(self): pass def teleopInit(self): self.brakeMode() def teleopPeriodic(self): if self.joystick.getRawButton(1): x = -.4*self.checkDeadband(self.joystick.getX()) y = -.4*self.checkDeadband(self.joystick.getY()) else: x = -1*self.checkDeadband(self.joystick.getX()) y = -1*self.checkDeadband(self.joystick.getY()) self.swerveDrive(x, y) def disabledInit(self): self.coastMode()
def test_pidcontroller_init_args7(): with pytest.raises(TypeError) as exinfo: PIDController(Kp=2.0, Ki=3.0) assert (exinfo.value.args[0] == "__init__() missing 1 required positional argument: 'Kd'")
def _get_pid(): _pid = PIDController(Kp=1.0, Ki=0.25, Kd=0.75) return _pid
def test_pidcontroller_calculate_displacement3(d, source1, source2, output1, output2, output3): pid = PIDController(0, 0, d) # D is change in error coeff for kDisplacement pid.enableContinuousInput(0, 100.0) pid.setIntegratorRange(-1, 1) pid.setSetpoint(50.0) out = pid.calculate(source1) # assert out == pytest.approx(output1, 0.01) assert out > 0 out = pid.calculate(source1) assert out == pytest.approx(output2, 0.01) out = pid.calculate(source2) # assert out == pytest.approx(output3, 0.01) assert out < 0
def test_pidcontroller_calculate_displacement1(p, source1, source2, output1, output2): pid = PIDController(p, 0, 0) # P is proportional error coeff for kDisplacement pid.enableContinuousInput(0, 100.0) pid.setIntegratorRange(-1, 1) pid.setSetpoint(50.0) out = pid.calculate(source1) # assert out == pytest.approx(output1, 0.01) assert out > 0 out = pid.calculate(source1) # assert out == pytest.approx(output1, 0.01) assert out > 0 out = pid.calculate(source2) # assert out == pytest.approx(output2, 0.01) assert out > 0
def test_absolute_tolerance(pid_controller: PIDController): setpoint = 50 tolerance = 10 pid_controller.setTolerance(tolerance) pid_controller.setSetpoint(setpoint) pid_controller.calculate(0) assert not pid_controller.atSetpoint(), ( "Error was in tolerance when it should not have been. Error was %f" % pid_controller.getPositionError()) measurement = setpoint + tolerance / 2 pid_controller.calculate(measurement) assert pid_controller.atSetpoint(), ( "Error was not in tolerance when it should have been. Error was %f" % pid_controller.getPositionError()) measurement = setpoint + 10 * tolerance pid_controller.calculate(measurement) assert not pid_controller.atSetpoint(), ( "Error was in tolerance when it should not have been. Error was %f" % pid_controller.getPositionError())
def robotInit(self): #I got fed up with how long 'frontLeft, frontRight, etc.' looked #Drive motors #turret caster thing self.vP = 0 self.vI = 0 self.vD = 0 self.turretTurnController = PIDController(self.vP, self.vI, self.vD) turretTurnController.enableContinuousInput(0, 270) #controlling the angle of the hood self.hoodServo = #find something to address the servo #spinner velocity control self.sP = 0 self.sI = 0 self.sD = 0 self.turretHoodController = PIDController(self.sp,self.sI,self.sD) self.turretVelocityController.enableContinuousInput(200,6000) self.turretMotor = rev.CANSparkMax(0, rev.MotorType.kBrushless) self.idealAngle= 30 #metric #Confirmation on velocity bounds: False self.maxVelocity = 5000*0.0762 self.minVelocity = 2000*0.0762 self.flDriveMotor = rev.CANSparkMax(2, rev.MotorType.kBrushless) self.frDriveMotor = rev.CANSparkMax(1, rev.MotorType.kBrushless) self.rlDriveMotor = rev.CANSparkMax(7, rev.MotorType.kBrushless) self.rrDriveMotor = rev.CANSparkMax(5, rev.MotorType.kBrushless) self.driveMotors = (self.flDriveMotor, self.frDriveMotor, self.rlDriveMotor, self.rrDriveMotor) #Turn motors self.flTurnMotor = rev.CANSparkMax(3, rev.MotorType.kBrushless) self.frTurnMotor = rev.CANSparkMax(4, rev.MotorType.kBrushless) self.rlTurnMotor = rev.CANSparkMax(8, rev.MotorType.kBrushless) self.rrTurnMotor = rev.CANSparkMax(6, rev.MotorType.kBrushless) self.turnMotors = (self.flTurnMotor, self.frTurnMotor, self.rlTurnMotor, self.rrTurnMotor) #Turn encoders self.flTurnEncoder = self.flTurnMotor.getEncoder() self.frTurnEncoder = self.frTurnMotor.getEncoder() self.rlTurnEncoder = self.rlTurnMotor.getEncoder() self.rrTurnEncoder = self.rrTurnMotor.getEncoder() self.turnEncoders = (self.flTurnEncoder, self.frTurnEncoder, self.rlTurnEncoder, self.rrTurnEncoder) for encoder in self.turnEncoders: encoder.setPositionConversionFactor(20) #makes the encoder output in degrees '''We should use whichever encoder (built-in or absolute) gives the greatest results. If we use the absolute, easy. If we use the build-in, the following will be an issue and a potential solution. Every time we turn off/on the robot, these encoders will read 0 and the absolute encoder will read the real value. I didn't do it here, but we will need a function that moves all the turn motors so that their positions are set to what the absolute encoder says it is. Something like for index, encoder in enumerate(self.turnEncoders): encoder.setPosition(self.absoluteEncoders[index]%360) because I've been using an unhealthy amount of for loops this reason. This would allow the auto to go from there, although the motors should have been zeroed beforehand. There would be a similar function that can be ran in teleop when a sufficiently out-of-the-way button is pressed so that we can automatically reset the motors to 0 when in the pits, something like for index, encoder in enumerate(self.turnEncoders): encoder.setPosition(self.absoluteEncoders[index]%360) self.turnControllers[index].setReference(0, rev.controlType.kPosition) that of course then removes itself from the queue once all four motor encoders are at 0 and all the absolute encoders are at 0. Despite a reset being available in the pits, we would still need to set the motor encoders to the absolute encoder positions to account for possible bumping, absent-minded turning, or in case we didn't have time to return to the pits and so were only able to reset the wheel positions by hand''' # PID coefficients self.kP = .0039 self.kI = 0 self.kD = 2.0e-6 self.PIDTolerance = 1.0 #PID controllers for the turn motors self.flTurnController = PIDController(self.kP, self.kI, self.kD) self.frTurnController = PIDController(self.kP, self.kI, self.kD) self.rlTurnController = PIDController(self.kP, self.kI, self.kD) self.rrTurnController = PIDController(self.kP, self.kI, self.kD) self.turnControllers = (self.flTurnController, self.frTurnController, self.rlTurnController, self.rrTurnController) for controller in self.turnControllers: controller.setTolerance(self.PIDTolerance) controller.enableContinuousInput(0, 360) self.joystick = wpilib.Joystick(0) self.joystickDeadband = .05 self.timer = wpilib.Timer() #used to use it while testing stuff, don't need it now, but oh well self.robotLength = 10.0 self.robotWidth = 10.0
class MyRobot(wpilib.TimedRobot): def robotInit(self): #I got fed up with how long 'frontLeft, frontRight, etc.' looked #Drive motors #turret caster thing self.vP = 0 self.vI = 0 self.vD = 0 self.turretTurnController = PIDController(self.vP, self.vI, self.vD) turretTurnController.enableContinuousInput(0, 270) #controlling the angle of the hood self.hoodServo = #find something to address the servo #spinner velocity control self.sP = 0 self.sI = 0 self.sD = 0 self.turretHoodController = PIDController(self.sp,self.sI,self.sD) self.turretVelocityController.enableContinuousInput(200,6000) self.turretMotor = rev.CANSparkMax(0, rev.MotorType.kBrushless) self.idealAngle= 30 #metric #Confirmation on velocity bounds: False self.maxVelocity = 5000*0.0762 self.minVelocity = 2000*0.0762 self.flDriveMotor = rev.CANSparkMax(2, rev.MotorType.kBrushless) self.frDriveMotor = rev.CANSparkMax(1, rev.MotorType.kBrushless) self.rlDriveMotor = rev.CANSparkMax(7, rev.MotorType.kBrushless) self.rrDriveMotor = rev.CANSparkMax(5, rev.MotorType.kBrushless) self.driveMotors = (self.flDriveMotor, self.frDriveMotor, self.rlDriveMotor, self.rrDriveMotor) #Turn motors self.flTurnMotor = rev.CANSparkMax(3, rev.MotorType.kBrushless) self.frTurnMotor = rev.CANSparkMax(4, rev.MotorType.kBrushless) self.rlTurnMotor = rev.CANSparkMax(8, rev.MotorType.kBrushless) self.rrTurnMotor = rev.CANSparkMax(6, rev.MotorType.kBrushless) self.turnMotors = (self.flTurnMotor, self.frTurnMotor, self.rlTurnMotor, self.rrTurnMotor) #Turn encoders self.flTurnEncoder = self.flTurnMotor.getEncoder() self.frTurnEncoder = self.frTurnMotor.getEncoder() self.rlTurnEncoder = self.rlTurnMotor.getEncoder() self.rrTurnEncoder = self.rrTurnMotor.getEncoder() self.turnEncoders = (self.flTurnEncoder, self.frTurnEncoder, self.rlTurnEncoder, self.rrTurnEncoder) for encoder in self.turnEncoders: encoder.setPositionConversionFactor(20) #makes the encoder output in degrees '''We should use whichever encoder (built-in or absolute) gives the greatest results. If we use the absolute, easy. If we use the build-in, the following will be an issue and a potential solution. Every time we turn off/on the robot, these encoders will read 0 and the absolute encoder will read the real value. I didn't do it here, but we will need a function that moves all the turn motors so that their positions are set to what the absolute encoder says it is. Something like for index, encoder in enumerate(self.turnEncoders): encoder.setPosition(self.absoluteEncoders[index]%360) because I've been using an unhealthy amount of for loops this reason. This would allow the auto to go from there, although the motors should have been zeroed beforehand. There would be a similar function that can be ran in teleop when a sufficiently out-of-the-way button is pressed so that we can automatically reset the motors to 0 when in the pits, something like for index, encoder in enumerate(self.turnEncoders): encoder.setPosition(self.absoluteEncoders[index]%360) self.turnControllers[index].setReference(0, rev.controlType.kPosition) that of course then removes itself from the queue once all four motor encoders are at 0 and all the absolute encoders are at 0. Despite a reset being available in the pits, we would still need to set the motor encoders to the absolute encoder positions to account for possible bumping, absent-minded turning, or in case we didn't have time to return to the pits and so were only able to reset the wheel positions by hand''' # PID coefficients self.kP = .0039 self.kI = 0 self.kD = 2.0e-6 self.PIDTolerance = 1.0 #PID controllers for the turn motors self.flTurnController = PIDController(self.kP, self.kI, self.kD) self.frTurnController = PIDController(self.kP, self.kI, self.kD) self.rlTurnController = PIDController(self.kP, self.kI, self.kD) self.rrTurnController = PIDController(self.kP, self.kI, self.kD) self.turnControllers = (self.flTurnController, self.frTurnController, self.rlTurnController, self.rrTurnController) for controller in self.turnControllers: controller.setTolerance(self.PIDTolerance) controller.enableContinuousInput(0, 360) self.joystick = wpilib.Joystick(0) self.joystickDeadband = .05 self.timer = wpilib.Timer() #used to use it while testing stuff, don't need it now, but oh well self.robotLength = 10.0 self.robotWidth = 10.0 def encoderBoundedPosition(self, encoder): #I don't know if there's a set continuous for encoders, but it's easy enough to write position = encoder.getPosition() position %= 360 if position < 0: position += 360 return position def turnSpeedCalculator(self, i): speed = self.turnControllers[i].calculate(self.encoderBoundedPosition(self.turnEncoders[i])) if abs(speed) > 1: speed /= abs(speed) return speed def stopDriveMotors(self): for motor in self.driveMotors: motor.set(0) for motor in self.turnMotors: motor.set(0) def swerveMath(self, x, y, z): r = math.hypot(self.robotLength, self.robotWidth) a = x - z*(self.robotLength/r) b = x + z*(self.robotLength/r) c = y - z*(self.robotWidth/r) d = y + z*(self.robotWidth/r) flSpeed = math.hypot(b, c) frSpeed = math.hypot(b, d) rlSpeed = math.hypot(a, c) rrSpeed = math.hypot(a, d) maxSpeed = max(flSpeed, frSpeed, rlSpeed, rrSpeed) if maxSpeed > 1: #this way speed proportions are kept the same flSpeed /= maxSpeed frSpeed /= maxSpeed rlSpeed /= maxSpeed rrSpeed /= maxSpeed flAngle = math.degrees(math.atan2(b, c)) #works for all 4 quadrants frAngle = math.degrees(math.atan2(b, d)) rlAngle = math.degrees(math.atan2(a, c)) rrAngle = math.degrees(math.atan2(a, d)) return (flSpeed, flAngle, frSpeed, frAngle, rlSpeed, rlAngle, rrSpeed, rrAngle) def swerveDrive(self, x, y, z): speeds = self.swerveMath(x, y, z) if max(abs(x), abs(y), abs(z)) == 0: self.stopDriveMotors() else: for i in range(4): #checking whether to go to angle and drive forward or go to other side and drive backward position = self.encoderBoundedPosition(self.turnEncoders[i]) goal = speeds[2*i+1] difference = abs(position - goal) if difference < 90 or difference > 270: self.turnControllers[i].setSetpoint(goal) self.driveMotors[i].set(speeds[2*i]) else: if goal < 180: self.turnControllers[i].setSetpoint(goal + 180) self.driveMotors[i].set(-speeds[2*i]) else: self.turnControllers[i].setSetpoint(goal - 180) self.driveMotors[i].set(-speeds[2*i]) self.turnMotors[i].set(self.turnSpeedCalculator(i)) '''I've been debating adding some checks to try and make this more efficient, but efficiency isn't super important right now and, more importantly, I don't really know python so anything I can think of would save minimal time (or possibly even make it take longer).''' def brakeMode(self): for motor in self.driveMotors: motor.setIdleMode(rev.IdleMode.kBrake) for motor in self.turnMotors: motor.setIdleMode(rev.IdleMode.kBrake) def coastMode(self): for motor in self.driveMotors: motor.setIdleMode(rev.IdleMode.kCoast) for motor in self.turnMotors: motor.setIdleMode(rev.IdleMode.kCoast) def checkDeadband(self, axis): if abs(axis) < self.joystickDeadband: axis = 0 return axis def turretShoot(self,x,y,z): #the turret must first align with the plane of the center of the target turretTurnController.setSetpoint(0) turretOutput = self.turretTurnController.calculate(Yaw) self.turretMotor.set(turretOutput) height= self.dx*tan(self.yaw) #equation to find the necessary velocity pulled from wikipedia vDesired= #(insert Liam math here if (vDesired > self.maxVelocity) or (vDesired < self.minVelocity): desiredAngle = #insert more Liam math here else: #velocity control turretHoodController.setSetpoint(self.idealAngle) hoodOutput = self.turretHoodController.calculate(self.hoodEncoder) self.hoodServo.set(hoodOutput) def autonomousInit(self): self.brakeMode() '''We want the motors in brake mode while we are actually using them, which would be anytime during auto or teleop. However, we want them in coast mode while disabled so that people can easily spin/adjust the wheels as needed. Not super important, but also not hard to add''' def autonomousPeriodic(self): pass def teleopInit(self): self.brakeMode() def teleopPeriodic(self): x = self.checkDeadband(self.joystick.getX()) y = self.checkDeadband(self.joystick.getY()) z = self.checkDeadband(self.joystick.getZ()) self.swerveDrive(x, y, z) def disabledInit(self): self.coastMode()
def pid_controller(): controller = PIDController(0.05, 0.0, 0.0) controller.enableContinuousInput(-input_range / 2, input_range / 2) yield controller controller.close()
class SwerveChassis: WIDTH = 0.75 LENGTH = 0.75 imu: NavX module_a: SwerveModule module_b: SwerveModule module_c: SwerveModule module_d: SwerveModule # tunables here purely for debugging odometry_x = tunable(0) odometry_y = tunable(0) # odometry_x_vel = tunable(0) # odometry_y_vel = tunable(0) # odometry_z_vel = tunable(0) hold_heading = tunable(True) def __init__(self): self.vx = 0 self.vy = 0 self.vz = 0 self.field_oriented = False self.momentum = False self.automation_running = False def setup(self): # Heading PID controller self.heading_pid = PIDController( Kp=6.0, Ki=0.0, Kd=0.05, measurement_source=self.imu.getAngle, period=1 / 50) # this gain is being changed depending on speed self.heading_pid.setInputRange(-math.pi, math.pi) self.heading_pid.setOutputRange(-3, 3) self.heading_pid.setContinuous() self.modules = [ self.module_a, self.module_b, self.module_c, self.module_d ] self.odometry_x = 0 self.odometry_y = 0 self.odometry_x_vel = 0 self.odometry_y_vel = 0 self.odometry_z_vel = 0 self.A = np.array( [ [1, 0, 1], [0, 1, 1], [1, 0, 1], [0, 1, 1], [1, 0, 1], [0, 1, 1], [1, 0, 1], [0, 1, 1], ], dtype=float, ) self.last_odometry_time = 0 # wpilib.SmartDashboard.putData("heading_pid", self.heading_pid) # figure out the contribution of the robot's overall rotation about the # z axis to each module's movement, and encode that information in a for i, module in enumerate(self.modules): module_dist = math.hypot(module.x_pos, module.y_pos) module_angle = math.atan2(module.y_pos, module.x_pos) # self.z_axis_adjustment[i*2, 0] = -module_dist*math.sin(module_angle) # self.z_axis_adjustment[i*2+1, 0] = module_dist*math.cos(module_angle) self.A[i * 2, 2] = -module_dist * math.sin(module_angle) self.A[i * 2 + 1, 2] = module_dist * math.cos(module_angle) def set_heading_sp_current(self): self.set_heading_sp(self.imu.getAngle()) def set_heading_sp(self, setpoint): self.heading_pid.setReference(setpoint) def heading_hold_on(self): self.set_heading_sp_current() self.heading_pid.reset() self.hold_heading = True def heading_hold_off(self): self.hold_heading = False def on_enable(self): self.heading_hold_on() self.last_heading = self.imu.getAngle() self.odometry_updated = False for module in self.modules: module.reset_encoder_delta() self.last_odometry_time = time.monotonic() def execute(self): pid_z = 0 if self.hold_heading: pid_z = self.heading_pid.update() if self.momentum and abs(self.imu.getHeadingRate()) < 0.005: self.momentum = False if self.vz not in [0.0, None]: self.momentum = True if self.momentum: self.set_heading_sp_current() pid_z = 0 input_vz = 0 if self.vz is not None: input_vz = self.vz if abs(pid_z) < 0.1 and math.hypot(self.vx, self.vy) < 0.01: pid_z = 0 vz = input_vz + pid_z angle = self.imu.getAngle() # TODO: re-enable if we end up not using callback method self.update_odometry() # self.odometry_updated = False # reset for next timestep for module in self.modules: # Calculate the additional vx and vy components for this module # required to achieve our desired angular velocity vz_x = -module.dist * vz * math.sin(module.angle) vz_y = module.dist * vz * math.cos(module.angle) if self.field_oriented: vx, vy = self.robot_orient(self.vx, self.vy, angle) else: vx, vy = self.vx, self.vy module.set_velocity(vx + vz_x, vy + vz_y, absolute_rotation=False) if abs(math.hypot(self.vx, self.vy)) > 0.5: self.heading_pid.setP(2.0) else: self.heading_pid.setP(6.0) def update_odometry(self, *args): # TODO: re-enable if we end up not using callback method # if self.odometry_updated: # return heading = self.imu.getAngle() odometry_outputs = np.zeros((8, 1)) velocity_outputs = np.zeros((8, 1)) # betas = [] # phi_dots = [] for i, module in enumerate(self.modules): module.update_odometry() odometry_x, odometry_y = module.get_cartesian_delta() velocity_x, velocity_y = module.get_cartesian_vel() odometry_outputs[i * 2, 0] = odometry_x odometry_outputs[i * 2 + 1, 0] = odometry_y velocity_outputs[i * 2, 0] = velocity_x velocity_outputs[i * 2 + 1, 0] = velocity_y module.reset_encoder_delta() # betas.append(module.measured_azimuth) # phi_dots.append(module.wheel_angular_vel) # q = np.array(betas) # lambda_e = self.icre.estimate_lmda(q) # print(lambda_e) now = time.monotonic() vx, vy, vz = self.robot_movement_from_odometry(velocity_outputs, heading) # delta_x, delta_y, delta_z = self.robot_movement_from_odometry( # odometry_outputs, heading, z_vel=self.imu.getHeadingRate() # ) delta_t = now - self.last_odometry_time delta_x = vx * delta_t delta_y = vy * delta_t self.odometry_x += delta_x self.odometry_y += delta_y self.odometry_x_vel = vx self.odometry_y_vel = vy self.odometry_z_vel = vz self.last_heading = heading self.odometry_updated = True self.set_modules_drive_brake() self.last_odometry_time = now def robot_movement_from_odometry(self, odometry_outputs, angle, z_vel=0): lstsq_ret = np.linalg.lstsq(self.A, odometry_outputs, rcond=None) x, y, theta = lstsq_ret[0].reshape(3) # TODO: re-enable if we move back to running in the same thread x_field, y_field = self.field_orient(x, y, angle + z_vel * (1 / 200)) # x_field, y_field = self.field_orient(x, y, angle) return x_field, y_field, theta def set_velocity_heading(self, vx, vy, heading): """Set a translational velocity and a rotational orientation to achieve. Args: vx: (forward) component of the robot's desired velocity. In m/s. vy: (leftward) component of the robot's desired velocity. In m/s. heading: the heading the robot is to face. """ self.vx = vx self.vy = vy self.vz = None self.set_heading_sp(heading) def set_inputs(self, vx: float, vy: float, vz: float, *, field_oriented: bool = True): """Set chassis vx, vy, and vz components of inputs. Args: vx: (forward) component of the robot's desired velocity. In m/s. vy: (leftward) component of the robot's desired velocity. In m/s. vz: The vz (counter-clockwise rotation) component of the robot's desired [angular] velocity. In radians/s. field_oriented: Whether the inputs are field or robot oriented. """ self.vx = vx self.vy = vy self.vz = vz self.field_oriented = field_oriented @staticmethod def robot_orient(vx: float, vy: float, heading: float) -> Tuple[float, float]: """Turn a vx and vy relative to the field into a vx and vy based on the robot. Args: vx: vx to robot orient vy: vy to robot orient heading: current heading of the robot in radians CCW from +x axis. Returns: tuple of robot oriented vx and vy """ c_h = math.cos(heading) s_h = math.sin(heading) oriented_vx = vx * c_h + vy * s_h oriented_vy = -vx * s_h + vy * c_h return oriented_vx, oriented_vy @staticmethod def field_orient(vx: float, vy: float, heading: float) -> Tuple[float, float]: """Turn a vx and vy relative to the robot into a vx and vy based on the field. Args: vx: vx to field orient vy: vy to field orient heading: current heading of the robot in radians CCW from +x axis. Returns: tuple of field oriented vx and vy """ c_h = math.cos(heading) s_h = math.sin(heading) oriented_vx = vx * c_h - vy * s_h oriented_vy = vx * s_h + vy * c_h return oriented_vx, oriented_vy @property def position(self): return self.odometry_x, self.odometry_y @property def speed(self): return math.hypot(self.odometry_x_vel, self.odometry_y_vel) @property def all_aligned(self): return all(module.aligned for module in self.modules) def set_modules_drive_coast(self): for module in self.modules: module.set_drive_coast() def set_modules_drive_brake(self): for module in self.modules: module.set_drive_brake()
class MyRobot(wpilib.TimedRobot): def robotInit(self): self.driveMotor = rev.CANSparkMax(1, rev.MotorType.kBrushless) self.turnMotor = rev.CANSparkMax(4, rev.MotorType.kBrushless) self.turnEncoder = self.turnMotor.getEncoder() self.turnEncoder.setPositionConversionFactor(20) # PID coefficients self.kP = 5e-5 self.kI = 1e-6 self.kD = 0 self.kIz = 0 self.PIDTolerance = 0 #PID controllers for the turn motors self.turnController = PIDController(self.kP, self.kI, self.kD) self.turnController.setTolerance(self.PIDTolerance) self.turnController.enableContinuousInput(0, 360) self.joystick = wpilib.Joystick(0) self.joystickDeadband = .05 self.timer = wpilib.Timer() #used to use it while testing stuff, don't need it now, but oh well # Push PID Coefficients to SmartDashboard wpilib.SmartDashboard.putNumber("P Gain", self.kP) wpilib.SmartDashboard.putNumber("I Gain", self.kI) wpilib.SmartDashboard.putNumber("D Gain", self.kD) wpilib.SmartDashboard.putNumber("I Zone", self.kIz) wpilib.SmartDashboard.putNumber("Set Rotations", 0) wpilib.SmartDashboard.putNumber("Tolerance", self.PIDTolerance) wpilib.SmartDashboard.putBoolean("Manual Control", False) wpilib.SmartDashboard.putBoolean("Manual Speed", 0) def encoderBoundedPosition(self, encoder): #I don't know if there's a set continuous for encoders, but it's easy enough to write position = encoder.getPosition() position %= 360 if position < 0: position += 360 return position def brakeMode(self): self.driveMotor.setIdleMode(rev.IdleMode.kBrake) self.turnMotor.setIdleMode(rev.IdleMode.kBrake) def coastMode(self): self.driveMotor.setIdleMode(rev.IdleMode.kCoast) self.turnMotor.setIdleMode(rev.IdleMode.kCoast) def turnSpeedCalculator(self): speed = self.turnController.calculate(self.encoderBoundedPosition(self.turnEncoder)) if abs(speed) > 1: speed /= abs(speed) return speed def autonomousInit(self): self.brakeMode() def autonomousPeriodic(self): pass def teleopInit(self): self.brakeMode() def teleopPeriodic(self): # Read data from SmartDashboard p = wpilib.SmartDashboard.getNumber("P Gain", self.kP) i = wpilib.SmartDashboard.getNumber("I Gain", self.kI) d = wpilib.SmartDashboard.getNumber("D Gain", self.kD) iz = wpilib.SmartDashboard.getNumber("I Zone", self.kIz) tolerance = wpilib.SmartDashboard.getNumber("Tolerance", self.PIDTolerance) test = wpilib.SmartDashboard.getBoolean("Manual Control", False) controlSpeed = wpilib.SmartDashboard.getNumber("Manual Speed", 0) # Update PIDController datapoints with the latest from SmartDashboard if p != self.kP: self.turnController.setP(p) self.kP = p if i != self.kI: self.turnController.setI(i) self.kI = i if d != self.kD: self.turnController.setD(d) self.kD = d if tolerance != self.PIDTolerance: self.turnController.setTolerance(tolerance) self.PIDTolerance = tolerance speed = self.turnSpeedCalculator() wpilib.SmartDashboard.putNumber("Process Variable", self.encoderBoundedPosition(self.turnEncoder)) setpoint = wpilib.SmartDashboard.getNumber("Set Rotations", 0) self.turnController.setSetpoint(setpoint) wpilib.SmartDashboard.putNumber("Motor Input", speed) if test: self.turnMotor.set(controlSpeed) else: self.turnMotor.set(speed) def disabledInit(self): self.coastMode()