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 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 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
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.turretVelocityController = 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): #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) vDesired= #(insert Liam math here if (vDesired > self.maxVelocity) or (vDesired < self.minVelocity): desiredAngle = #insert more Liam math here self.hoodServo.set(self.idealAngle) else: #velocity control self.hoodServo.set(self.idealAngle) self.turretVelocityController.setSetpoint(vDesired) outputV = self.turretVelocityController.calculate(self.turretMotor.getVelocity) self.turretMotor.set(outputV) 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()
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()
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()