def config_spark(spark: CANSparkMax, motor_config: SparkMotorConfig):
spark.restoreFactoryDefaults()
spark.enableVoltageCompensation(motor_config.voltage_compensation)
spark.setSmartCurrentLimit(motor_config.stall_limit)
spark.setIdleMode(motor_config.default_mode)
spark.setOpenLoopRampRate(motor_config.ramp_rate)
spark.setClosedLoopRampRate(motor_config.ramp_rate)
spark.setInverted(motor_config.reversed)
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 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()