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