class Flipper(Subsystem):
FLIPPER_HIGH = -95.0
FLIPPER_LOW = 0.0
def __init__(self):
super().__init__()
self.flippy_motor = CANSparkMax(RM.FLIPPY_MOTOR, MotorType.kBrushless)
self.flippy_motor.restoreFactoryDefaults()
self.encoder = self.flippy_motor.getEncoder()
def initDefaultCommand(self):
self.setDefaultCommand(self.Stopification(self))
class Stopification(Command):
def __init__(self, flipper):
super().__init__(subsystem=flipper)
self._flipper = flipper
def initialize(self):
self._flipper.flippy_motor.set(0.0)
def isFinished(self):
return False
class Smash(Command):
def __init__(self, flipper, flip_direction):
super().__init__(subsystem=flipper)
self._flipper = flipper
self._flip_direction = flip_direction
self._cycle_counter = 0
def isFinished(self):
return False
def execute(self):
position = self._flipper.encoder.getPosition()
if self._flip_direction == Flipper.FlipDirection.UP:
if position > Flipper.FLIPPER_HIGH:
self._flipper.flippy_motor.set(-1.0)
else:
self._flipper.flippy_motor.set(0.0)
elif self._flip_direction == Flipper.FlipDirection.DOWN:
if position < Flipper.FLIPPER_LOW:
self._flipper.flippy_motor.set(0.5)
else:
self._flipper.flippy_motor.set(0.0)
def interrupted(self):
self._flipper.flippy_motor.set(0.0)
self._cycle_counter = 0
def end(self):
self._flipper.flippy_motor.set(0.0)
self._cycle_counter = 0
class FlipDirection(enum.IntEnum):
UP = 0
DOWN = 1
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 Robot(MagicRobot):
def createObjects(self):
self.joystick = wpilib.Joystick(2)
self.winch_button = JoystickButton(self.joystick, 6)
self.cp_extend_button = JoystickButton(self.joystick, 5)
self.cp_solenoid = wpilib.DoubleSolenoid(4, 5)
self.cp_motor = CANSparkMax(10, MotorType.kBrushed)
self.solenoid = wpilib.Solenoid(0)
self.btn_solenoid = JoystickButton(self.joystick, 1)
self.intake = WPI_VictorSPX(1)
self.cp_motor_button = JoystickButton(self.joystick, 7)
self.winch_motor = CANSparkMax(8, MotorType.kBrushless)
self.six = JoystickButton(self.joystick, 12)
self.shooter_motor = CANSparkMax(7, MotorType.kBrushed)
self.shooter_motor.restoreFactoryDefaults()
self.shooter_motor.setOpenLoopRampRate(0)
self.intake_in = JoystickButton(self.joystick, 3)
self.intake_out = JoystickButton(self.joystick, 4)
def teleopPeriodic(self):
if self.winch_button.get():
self.winch_motor.set(1)
else:
self.winch_motor.set(0)
if self.six.get():
# 75% is good for initiation line
self.shooter_motor.set(0.75)
else:
self.shooter_motor.set(0)
self.solenoid.set(self.btn_solenoid.get())
if self.intake_in.get():
self.intake.set(-1)
elif self.intake_out.get():
self.intake.set(1)
else:
self.intake.set(0)
if self.cp_motor_button.get():
self.cp_motor.set(0.7)
else:
self.cp_motor.set(0)
if self.cp_extend_button.get(
) and self.cp_solenoid.get() != wpilib.DoubleSolenoid.Value.kReverse:
self.cp_solenoid.set(wpilib.DoubleSolenoid.Value.kReverse)
if not self.cp_extend_button.get(
) and self.cp_solenoid.get() != wpilib.DoubleSolenoid.Value.kForward:
self.cp_solenoid.set(wpilib.DoubleSolenoid.Value.kForward)
class TestRobot(magicbot.MagicRobot):
shooter_speed = tunable(0, writeDefault=False)
time = tunable(0)
voltage = tunable(0)
rotation = tunable(0)
def createObjects(self):
self.launcher_motor = CANSparkMax(7, MotorType.kBrushed)
self.launcher_motor.restoreFactoryDefaults()
self.launcher_motor.setOpenLoopRampRate(0)
self.intake = WPI_VictorSPX(1)
self.solenoid = wpilib.Solenoid(0)
self.encoder = self.launcher_motor.getEncoder()
# self.shooter_running = False
self.logger.addFilter(PeriodicFilter(1, bypass_level=logging.WARN))
# 2000rpm is a good setting
# set up joystick buttons
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
# self.left_button = Toggle(self.joystick_alt, 10)
# self.middle_button = Toggle(self.joystick_alt, 11)
# self.right_button = Toggle(self.joystick_alt, 12)
self.one = JoystickButton(self.joystick_alt, 7)
self.two = JoystickButton(self.joystick_alt, 8)
self.three = JoystickButton(self.joystick_alt, 9)
self.four = JoystickButton(self.joystick_alt, 10)
self.five = JoystickButton(self.joystick_alt, 11)
self.six = JoystickButton(self.joystick_alt, 12)
self.intake_in = JoystickButton(self.joystick_alt, 3)
self.intake_out = JoystickButton(self.joystick_alt, 4)
self.btn_solenoid = JoystickButton(self.joystick_alt, 1)
# Set up Speed Controller Groups
self.left_motors = wpilib.SpeedControllerGroup(CANSparkMax(1, MotorType.kBrushless), CANSparkMax(2, MotorType.kBrushless), CANSparkMax(3, MotorType.kBrushless))
self.right_motors = wpilib.SpeedControllerGroup(CANSparkMax(4, MotorType.kBrushless), CANSparkMax(5, MotorType.kBrushless), CANSparkMax(6, MotorType.kBrushless))
# Drivetrain
self.train = wpilib.drive.DifferentialDrive(self.left_motors, self.right_motors)
# self.compressor = wpilib.Compressor()
def disabledInit(self):
self.launcher_motor.set(0)
self.intake.set(0)
self.solenoid.set(False)
def teleopInit(self):
self.train.setDeadband(0.1)
# self.compressor.start()
def teleopPeriodic(self):
self.logger.info(self.encoder.getVelocity())
self.shooter_speed = self.encoder.getVelocity()
# if self.left_button.get():
# self.launcher_motor.set(-0.2)
# elif self.middle_button.get():
# self.launcher_motor.set(-0.5)
# elif self.right_button.get():
# self.launcher_motor.set(-0.7)
# else:
# self.launcher_motor.set(0)
if self.intake_in.get():
self.intake.set(-1)
elif self.intake_out.get():
self.intake.set(1)
else:
self.intake.set(0)
# BUTTON SETTINGS
if self.one.get():
self.launcher_motor.set(0)
elif self.two.get():
self.launcher_motor.set(-0.2)
elif self.three.get():
self.launcher_motor.set(-0.4)
elif self.four.get():
self.launcher_motor.set(-0.6)
elif self.five.get():
self.launcher_motor.set(-0.8)
elif self.six.get():
self.launcher_motor.set(-1)
else:
# self.launcher_motor.set(-self.joystick_alt.getAxis(3))
self.launcher_motor.set(0)
# SLIDER SETTINGS
# self.launcher_motor.set(-self.joystick_alt.getAxis(3))
self.solenoid.set(self.btn_solenoid.get())
self.train.arcadeDrive(-self.joystick_left.getY(),
self.joystick_right.getX())
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()