class HatchSubsystem(Subsystem):
def __init__(self):
# Anything the subsystem has goes here
super().__init__("HatchSubsystem")
self.angleMotor = CANSparkMax(robotmap.CAN_REV_HATCH_ANGLE, MotorType.kBrushed)
self.hatchLauncher = Solenoid(robotmap.HATCH_LAUNCHER_PCM_PORT)
self.hatchLauncher.set(False)
def initDefaultCommand(self):
# This is where the command to do if nothing else happens goes
self.setDefaultCommand(TiltHatchCommand())
def tilt(self, speed):
# This function lets the mechanism tilt
self.angleMotor.set(robotmap.HATCH_ARM_SPEED_REDUCTION * speed)
def releaseHatch(self):
# This function extends the poker
self.hatchLauncher.set(True)
def retract(self):
# This function retracts the poker
self.hatchLauncher.set(False)
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 ArmSubsystem(Subsystem):
def __init__(self):
self.encoderUnit = 4096
self.gearRatio = 93.33
self.armSpeedMultiplier = 1
self.armBottomLimit = 5
self.armUpperLimit = 200
self.resetValue = 0
self.bottomLimitSwitch = DigitalInput(ArmLimitSwitch)
self.armMotor1 = CANSparkMax(armBaseMotor1,MotorType.kBrushless)
self.armMotor2 = CANSparkMax(armBaseMotor2,MotorType.kBrushless)
self.armEncoder1 = self.armMotor1.getEncoder()
self.armEncoder2 = self.armMotor2.getEncoder()
self.currentArmPower = 0
self.isOverride = False
#def initDefaultCommand(self):
# self.setDefaultCommand(AnalogArmCommand())
def getDistanceTicks(self):
return ((self.armEncoder1.getPosition() + self.armEncoder2.getPosition()) / 2) - resetValue
def getArmMotor1Pos(self):
return self.armEncoder1.getPosition
def getArmMotor2Pos(self):
return self.armEncoder2.getPosition
def getRotationAngle(self):
return (getDistanceTicks()/108) * -360
def updateBottomLimit(self):
if not self.bottomLimitSwitch.get():
self.armBottomLimit = getRotationAngle()
def isArmAtBottom(self):
updateBottomLimit()
if (getRotationAngle() >= (armBottomLimit - 2)) and (getRotationAngle() <= (armBottomLimit + 2)):
return True
else:
return False
#def isArmAtTop(self):
#def getLimitSwitch(self):
def setArmPower(self,power):
if isArmAtBottom() and power > 0:
self.currentArmPower = 0
else:
self.currentArmPower = power
def setArmPowerOverride(self,power):
if isOverride:
self.currentArmPower = power
def updateOutputs(self):
self.armMotor1.set(currentArmPower * armSpeedMultiplier)
self.armMotor2.set(currentArmPower * armSpeedMultiplier)
def getGravityCompensation(self):
if getRotationAngle() <= 3: return 0
else: return math.sin(getRotationAngle() + 25)
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 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 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())