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()