def __init__(
self,
x: float,
y: float,
steer: CANSparkMax,
drive: ctre.WPI_TalonFX,
steer_reversed=False,
drive_reversed=False,
):
self.translation = Translation2d(x, y)
self.steer = steer
self.steer.setInverted(steer_reversed)
self.steer.setIdleMode(CANSparkMax.IdleMode.kBrake)
self.steer_reversed = steer_reversed
self.encoder = self.steer.getAnalog()
self.hall_effect = self.steer.getEncoder()
# make the sensor's return value between 0 and 1
self.encoder.setPositionConversionFactor(math.tau / 3.3)
self.encoder.setInverted(steer_reversed)
self.hall_effect.setPositionConversionFactor(self.STEER_GEAR_RATIO *
math.tau)
self.rezero_hall_effect()
self.steer_pid = steer.getPIDController()
self.steer_pid.setFeedbackDevice(self.hall_effect)
self.steer_pid.setSmartMotionAllowedClosedLoopError(math.pi / 180)
self.steer_pid.setP(1.85e-6)
self.steer_pid.setI(0)
self.steer_pid.setD(0)
self.steer_pid.setFF(0.583 / 12 / math.tau * 60 *
self.STEER_GEAR_RATIO)
self.steer_pid.setSmartMotionMaxVelocity(400) # RPM
self.steer_pid.setSmartMotionMaxAccel(200) # RPM/s
self.drive = drive
self.drive.setNeutralMode(ctre.NeutralMode.Brake)
self.drive.setInverted(drive_reversed)
self.drive_ff = SimpleMotorFeedforwardMeters(kS=0.757,
kV=1.3,
kA=0.0672)
drive.config_kP(slotIdx=0, value=2.21e-31, timeoutMs=20)
def __init__(self,
motor: rev.CANSparkMax,
x,
y,
angle,
circumference,
maxVoltageVelocity,
reverse=False):
"""
:param motors: a SparkMax
:param x: X position in feet
:param y: Y position in feet
:param angle: radians, direction of force. 0 is right, positive
counter-clockwise
:param circumference: wheel circumference in feet
:param maxVoltageVelocity: velocity at 100% in voltage mode, in feet
per second
:param reverse: boolean, optional
"""
super().__init__(x, y)
self.motors = [motor]
self.motorControllers = [motor.getPIDController()]
self.angle = angle
self.circumference = circumference
self.gearRatio = 1
self.encoderCountsPerFoot = 1
self.maxVoltageVelocity = maxVoltageVelocity
self.reverse = reverse
self.wheelPosition = 0
self.wheelVelocity = 0
self.driveMode = rev.ControlType.kVoltage
self.realTime = False
self._motorState = None
self._positionTarget = 0
self._oldPosition = 0
self._positionOccurence = 0
self._prevTime = time.time()
def addMotor(self, motor: rev.CANSparkMax):
self.motors.append(motor)
self.motorControllers.append(motor.getPIDController())
class LiftElevator(Subsystem):
def __init__(self):
super().__init__()
self.liftDrive = WPI_TalonSRX(RobotMap.BACK_DRIVE_TALON)
self.liftSpark = CANSparkMax(RobotMap.LIFT_SPARK, MotorType.kBrushless)
self.liftSpark.restoreFactoryDefaults()
self.liftSpark.setIdleMode(IdleMode.kCoast)
self.liftSpark.setSmartCurrentLimit(40)
self.liftEncoder = self.liftSpark.getEncoder()
self.liftPID = self.liftSpark.getPIDController()
self.setLiftPIDConstants()
self.elevatorSpark = CANSparkMax(RobotMap.ELEVATOR_SPARK,
MotorType.kBrushless)
self.elevatorSpark.restoreFactoryDefaults()
self.elevatorSpark.setIdleMode(IdleMode.kCoast)
self.elevatorSpark.setInverted(True)
self.liftSpark.setSmartCurrentLimit(60)
self.elevatorEncoder = self.elevatorSpark.getEncoder()
self.elevatorPID = self.elevatorSpark.getPIDController()
self.setElevatorPIDConstants()
# self.liftEncoder.setPosition(0)
def setLiftPIDConstants(self):
self.liftPID.setFF(Constants.LIFT_ELEVATOR_FF)
self.liftPID.setP(Constants.LIFT_ELEVATOR_P)
self.liftPID.setI(Constants.LIFT_ELEVATOR_I)
self.liftPID.setD(Constants.LIFT_ELEVATOR_D)
self.liftPID.setSmartMotionAllowedClosedLoopError(0, 0)
self.liftPID.setSmartMotionMaxVelocity(Constants.LIFT_MAX_VELOCITY, 0)
self.liftPID.setSmartMotionMaxAccel(Constants.LIFT_MAX_ACCEL, 0)
self.liftPID.setSmartMotionMinOutputVelocity(0, 0)
def setElevatorPIDConstants(self):
self.elevatorPID.setFF(Constants.LIFT_ELEVATOR_FF)
self.elevatorPID.setP(Constants.LIFT_ELEVATOR_P)
self.elevatorPID.setI(Constants.LIFT_ELEVATOR_I)
self.elevatorPID.setD(Constants.LIFT_ELEVATOR_D)
self.elevatorPID.setSmartMotionMaxVelocity(
Constants.ELEVATOR_MAX_VELOCITY, 0)
self.elevatorPID.setSmartMotionMaxAccel(Constants.ELEVATOR_MAX_ACCEL,
0)
self.elevatorPID.setSmartMotionAllowedClosedLoopError(0, 0)
self.elevatorPID.setSmartMotionMinOutputVelocity(0, 0)
def initDefaultCommand(self):
pass
def setLiftReference(self, targetPos):
self.liftPID.setReference(targetPos, ControlType.kSmartMotion)
def setElevatorReference(self, targetPos):
self.elevatorPID.setReference(targetPos, ControlType.kSmartMotion)
def setDrive(self, magnitude):
self.liftDrive.set(magnitude)
def setLift(self, targetSpeed):
self.liftDrive.set(targetSpeed)
def setElevator(self, targetSpeed):
self.elevatorSpark.set(targetSpeed)
def setLiftElevator(self, targetLiftSpeed, targetElevatorSpeed):
self.setLift(targetLiftSpeed)
self.setElevator(targetElevatorSpeed)
def stopLiftElevator(self):
self.elevatorSpark.set(0)
self.liftSpark.set(0)
def resetEncoders(self):
self.liftSpark.setEncPosition(0)
self.elevatorSpark.setEncPosition(0)
def getLiftElevatorAmps(self):
return self.liftSpark.getOutputCurrent(
), self.elevatorSpark.getOutputCurrent()
def getLiftPos(self):
return self.liftEncoder.getPosition()
def getElevatorPos(self):
return self.elevatorEncoder.getPosition()
def getLiftElevatorPos(self):
return self.getLiftPos(), self.getElevatorPos()
def getLiftElevatorTemps(self):
return self.liftSpark.getMotorTemperature(
), self.elevatorSpark.getMotorTemperature()
class Drivetrain(Subsystem):
"""
This example subsystem controls a single Talon in PercentVBus mode.
"""
def __init__(self):
"""Instantiates the motor object."""
super().__init__("SingleMotor")
self.m_lfront = CANSparkMax(4, MotorType.kBrushless)
self.m_lrear = CANSparkMax(1, MotorType.kBrushless)
self.m_rfront = CANSparkMax(2, MotorType.kBrushless)
self.m_rrear = CANSparkMax(3, MotorType.kBrushless)
# self.m_lrear.follow(self.m_lfront, invert=True)
# self.m_rrear.follow(self.m_rfront, invert=True)
self.l_pid = self.m_lfront.getPIDController()
self.r_pid = self.m_rfront.getPIDController()
self.l_enc = self.m_lfront.getEncoder()
self.r_enc = self.m_rfront.getEncoder()
self.kP = 0.1
self.kI = 1e-4
self.kD = 0
self.kIz = 0
self.kFF = 0
self.kMinOutput = -1
self.kMaxOutput = 1
self.l_pid.setP(self.kP)
self.l_pid.setI(self.kI)
self.l_pid.setD(self.kD)
self.l_pid.setIZone(self.kIz)
self.l_pid.setFF(self.kFF)
self.l_pid.setOutputRange(self.kMinOutput, self.kMaxOutput)
# self.r_pid.setP(self.kP)
# self.r_pid.setI(self.kI)
# self.r_pid.setD(self.kD)
# self.r_pid.setIZone(self.kIz)
# self.r_pid.setFF(self.kFF)
# self.r_pid.setOutputRange(self.kMinOutput, self.kMaxOutput)
# 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("Feed Forward", self.kFF)
wpilib.SmartDashboard.putNumber("Min Output", self.kMinOutput)
wpilib.SmartDashboard.putNumber("Max Output", self.kMaxOutput)
wpilib.SmartDashboard.putNumber("Set Rotations", 0)
def setSpeed(self, l_speed, r_speed):
self.m_lfront.set(l_speed)
self.m_rfront.set(r_speed)
def setPID(self, setpoint):
self.pidController.setReference(setpoint, rev.ControlType.kPosition)
def initDefaultCommand(self):
self.setDefaultCommand(FollowJoystick())
class PIDSparkMax:
"""
Wrapper for a Rev Spark MAX that exposes all the
PID setup and makes it easy to set a PID setpoint.
"""
kP = 0.035 # 6e-2
kI = 0.01 # 1e-3
kD = 0.001 # 0.2
kIz = 0.001
kFF = 0.000015
kMinOutput = -1
kMaxOutput = 1
control_mode = ControlType.kVelocity
def __init__(self, canid):
self.motor = CANSparkMax(canid, MotorType.kBrushless)
self.motor.restoreFactoryDefaults()
self.motor.setClosedLoopRampRate(1)
self._motor_pid = self.motor.getPIDController()
self._motor_pid.setP(self.kP)
self._motor_pid.setI(self.kI)
self._motor_pid.setD(self.kD)
self._motor_pid.setIZone(self.kIz)
self._motor_pid.setFF(self.kFF)
self._motor_pid.setOutputRange(self.kMinOutput, self.kMaxOutput)
self.motor.setSmartCurrentLimit(10)
def setP(self, value):
self.kP = value
self._motor_pid.setP(value)
def setI(self, value):
self.kI = value
self._motor_pid.setI(value)
def setD(self, value):
self.kD = value
self._motor_pid.setD(value)
def setIz(self, value):
self.kIz = value
self._motor_pid.setIZone(value)
def setFF(self, value):
self.kFF = value
self._motor_pid.setFF(value)
def setMOutputRange(self, min, max):
self.kMinOutput = min
self.kMaxOutput = max
self._motor_pid.setOutputRange(value)
def getEncoder(self):
return self.motor.getEncoder()
def stop(self):
self.motor.stopMotor()
def set(self, setpoint):
self._motor_pid.setReference(setpoint, self.control_mode)
def setPID(self, kP, kI, kD):
self.setP(kP)
self.setI(kI)
self.setD(kD)
def fromKu(self, Ku: float, Tu: float) -> None:
"""
Use the Zeigler-Nichols method to tune the PID based off the oscillations.
Uses a P value for oscillation along with the period of the oscullations to find the PID values.
Args:
Ku: This is the value of P that you obtain by increasing P slowly until the system starts to oscillate
Tu: This is the period of the oscillation, with one full stroke
"""
self.setP(0.6 * Ku)
self.setI(1.2 * Ku / Tu)
self.setD(3 * Ku * Tu / 40)
@property
def rpm(self):
return self.motor.getEncoder().getVelocity()