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.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 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()
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 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 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_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 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 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 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 pid_controller():
controller = PIDController(0.05, 0.0, 0.0)
controller.enableContinuousInput(-input_range / 2, input_range / 2)
yield controller
controller.close()