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
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)
def robotInit(self):
"""Robot initialization function"""
self.gyro = wpilib.ADXRS450_Gyro()
self.gyro.reset()
self.gyro.calibrate()
self.modules = [
SwerveModule(
0.8 / 2 - 0.125,
0.75 / 2 - 0.1,
CANSparkMax(9, MotorType.kBrushless),
ctre.WPI_TalonFX(3),
steer_reversed=False,
drive_reversed=True,
),
SwerveModule(
-0.8 / 2 + 0.125,
-0.75 / 2 + 0.1,
CANSparkMax(7, MotorType.kBrushless),
ctre.WPI_TalonFX(5),
steer_reversed=False,
drive_reversed=True,
),
]
self.kinematics = SwerveDrive2Kinematics(self.modules[0].translation,
self.modules[1].translation)
self.joystick = wpilib.Joystick(0)
self.spin_rate = 1.5
def __init__(self):
super().__init__("Chassis")
self.l_controller = CANSparkMax(0, MotorType.kBrushless)
self.r_controller = CANSparkMax(1, MotorType.kBrushless)
self.drive = DifferentialDrive(self.l_controller, self.r_controller)
def __init__(self, motor: rev.CANSparkMax) -> None:
self.motor = motor
self.encoder = motor.getEncoder()
self.forward_limit_switch = motor.getForwardLimitSwitch(
rev.LimitSwitchPolarity.kNormallyOpen)
self.motor.setIdleMode(rev.IdleMode.kBrake)
self.forward_limit_switch.enableLimitSwitch(True)
self.encoder.setPositionConversionFactor(self.HEIGHT_PER_REV)
self.encoder.setVelocityConversionFactor(self.HEIGHT_PER_REV / 60)
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 robotInit(self):
self.neo0: CANSparkMax = CANSparkMax(3, rev.MotorType.kBrushless)
self.neo1: CANSparkMax = CANSparkMax(11, rev.MotorType.kBrushless)
neos = wpilib.SpeedControllerGroup(self.neo0, self.neo1)
self.neo0.clearFaults()
self.neo1.clearFaults()
sd.putNumber("Neo Power", 0)
self.neo0.setOpenLoopRampRate(5)
self.neo1.setOpenLoopRampRate(5)
def createObjects(self):
self.intake_motor = ctre.TalonSRX(10)
self.intake_arm_motor = PIDSparkMax(7)
self.shooter_motor = PIDSparkMax(16)
self.shooter_feeder_motor = ctre.TalonSRX(19)
self.drivetrain_motorr1 = CANSparkMax(5, MotorType.kBrushless)
self.drivetrain_motorr2 = CANSparkMax(8, MotorType.kBrushless)
self.drivetrain_motorl1 = CANSparkMax(17, MotorType.kBrushless)
self.drivetrain_motorl2 = CANSparkMax(13, MotorType.kBrushless)
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.controls = Controls(self.joystick_left, self.joystick_right)
def __init__(self, motor: rev.CANSparkMax, reversed=False):
self.encoder = motor.getEncoder()
if reversed:
self.mod = -1
else:
self.mod = 1
self.initialValue = self.mod * self.encoder.getPosition()
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 __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()
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)
def createObjects(self):
if subsystems_enabled['limelight']:
self.limelight_table = NetworkTables.getTable("limelight")
if subsystems_enabled['navx']:
self.navx = navx.AHRS.create_spi()
if subsystems_enabled['shooter']:
self.shooter_srx_a = WPI_TalonSRX(1)
self.shooter_srx_b = WPI_TalonSRX(2)
if subsystems_enabled['neo']:
self.neo_motor = CANSparkMax(3, MotorType.kBrushless)
if subsystems_enabled['camera']:
wpilib.CameraServer.launch()
self.driver = DriverController(wpilib.XboxController(0))
self.sysop = SysopController(wpilib.XboxController(1))
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):
"""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 robotInit(self):
self.prior_autospeed = 0
self.joystick = wpilib.Joystick(0)
self.left_motor_master = CANSparkMax(1, MotorType.kBrushless)
self.right_motor_master = CANSparkMax(4, MotorType.kBrushless)
# Set up Speed Controller Groups
self.left_motors = wpilib.SpeedControllerGroup(
self.left_motor_master,
CANSparkMax(3, MotorType.kBrushless)
)
self.right_motors = wpilib.SpeedControllerGroup(
self.right_motor_master,
CANSparkMax(6, MotorType.kBrushless)
)
# Configure Gyro
# Note that the angle from the NavX and all implementors of wpilib Gyro
# must be negated because getAngle returns a clockwise positive angle
self.gyro = navx.AHRS.create_spi()
# Configure drivetrain movement
self.drive = DifferentialDrive(self.left_motors, self.right_motors)
self.drive.setDeadband(0)
# Configure encoder related functions -- getDistance and getrate should return
# units and units/s
self.encoder_constant = (1 / self.GEARING) * self.WHEEL_DIAMETER * math.pi
self.left_encoder = self.left_motor_master.getEncoder()
self.left_encoder.setPositionConversionFactor(self.encoder_constant)
self.left_encoder.setVelocityConversionFactor(self.encoder_constant / 60)
self.right_encoder = self.right_motor_master.getEncoder()
self.right_encoder.setPositionConversionFactor(self.encoder_constant)
self.right_encoder.setVelocityConversionFactor(self.encoder_constant / 60)
self.left_encoder.setPosition(0)
self.right_encoder.setPosition(0)
# Set the update rate instead of using flush because of a ntcore bug
# -> probably don't want to do this on a robot in competition
NetworkTables.getDefault().setUpdateRate(0.010)
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 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 createObjects(self):
# Joysticks
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
# 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)
# NavX (purple board on top of the RoboRIO)
self.navx = navx.AHRS.create_spi()
self.navx.reset()
def createObjects(self):
# initialize physical objects
#limelight
self.limelight_table = NetworkTables.getTable("limelight")
# drivetrain
self.limelight_table = NetworkTables.getTable('limelight')
motor_objects_left = [
CANSparkMax(port, MotorType.kBrushless)
for port in RobotMap.Drivetrain.motors_left
]
self.left_motors = wpilib.SpeedControllerGroup(motor_objects_left[0],
*motor_objects_left[1:])
motor_objects_right = [
CANSparkMax(port, MotorType.kBrushless)
for port in RobotMap.Drivetrain.motors_right
]
self.right_motors = wpilib.SpeedControllerGroup(
motor_objects_right[0], *motor_objects_right[1:])
for motor in motor_objects_left + motor_objects_right:
config_spark(motor, RobotMap.Drivetrain.motor_config)
self.differential_drive = wpilib.drive.DifferentialDrive(
self.left_motors, self.right_motors)
self.differential_drive.setMaxOutput(
RobotMap.Drivetrain.max_motor_power)
self.left_encoder = wpilib.Encoder(RobotMap.Encoders.left_encoder_b,
RobotMap.Encoders.left_encoder_a)
self.right_encoder = wpilib.Encoder(RobotMap.Encoders.right_encoder_b,
RobotMap.Encoders.right_encoder_a)
self.right_encoder.setReverseDirection(False)
self.left_encoder.setDistancePerPulse(
RobotMap.Encoders.distance_per_pulse)
self.right_encoder.setDistancePerPulse(
RobotMap.Encoders.distance_per_pulse)
self.navx_ahrs = navx.AHRS.create_spi()
self.driver = Driver(wpilib.XboxController(0))
self.sysop = Sysop(wpilib.XboxController(1))
# intake
self.intake_lift_motor = WPI_TalonSRX(RobotMap.IntakeLift.motor_port)
self.intake_lift_motor.configPeakOutputForward(
RobotMap.IntakeLift.max_power)
self.intake_lift_motor.configPeakOutputReverse(
-RobotMap.IntakeLift.max_power)
config_talon(self.intake_lift_motor, RobotMap.IntakeLift.motor_config)
self.intake_lift_motor.setSelectedSensorPosition(0)
self.intake_roller_motor = WPI_TalonSRX(
RobotMap.IntakeRoller.motor_port)
self.intake_roller_motor.configPeakOutputForward(
RobotMap.IntakeRoller.max_power)
self.intake_roller_motor.configPeakOutputReverse(
-RobotMap.IntakeRoller.max_power)
config_talon(self.intake_roller_motor,
RobotMap.IntakeRoller.motor_config)
#self.intake_roller_motor.setSelectedSensorPosition(0)
# climber
self.climber_motor = WPI_TalonSRX(RobotMap.Climber.motor_port)
config_talon(self.climber_motor, RobotMap.Climber.motor_config)
# color wheel
self.color_wheel_motor = WPI_TalonSRX(RobotMap.ColorWheel.motor_port)
config_talon(self.color_wheel_motor, RobotMap.ColorWheel.motor_config)
# shooter
self.shooter_motor = WPI_TalonSRX(RobotMap.Shooter.motor_port)
config_talon(self.shooter_motor, RobotMap.Shooter.motor_config)
# serializer
self.serializer_motor = WPI_TalonSRX(RobotMap.Serializer.motor_port)
config_talon(self.serializer_motor, RobotMap.Serializer.motor_config)
self.i2c_color_sensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
# controllers and electrical stuff
self.driver = Driver(wpilib.XboxController(0))
self.sysop = Sysop(wpilib.XboxController(1))
NetworkTables.initialize(server="roborio")
# camera server
wpilib.CameraServer.launch()
class Robot(wpilib.IterativeRobot):
WHEEL_DIAMETER = 0.1524 # Units: Meters
# Currently unused
# ENCODER_PULSE_PER_REV = 42
GEARING = 7.56 # 7.56:1 gear ratio
auto_speed_entry = ntproperty('/robot/autospeed', 0.0)
telemetry_entry = ntproperty('/robot/telemetry', [0.0], writeDefault=False)
rotate_entry = ntproperty('/robot/rotate', False)
l_encoder_pos = ntproperty('/l_encoder_pos', 0)
l_encoder_rate = ntproperty('/l_encoder_rate', 0)
r_encoder_pos = ntproperty('/r_encoder_pos', 0)
r_encoder_rate = ntproperty('/r_encoder_rate', 0)
def robotInit(self):
self.prior_autospeed = 0
self.joystick = wpilib.Joystick(0)
self.left_motor_master = CANSparkMax(1, MotorType.kBrushless)
self.right_motor_master = CANSparkMax(4, MotorType.kBrushless)
# Set up Speed Controller Groups
self.left_motors = wpilib.SpeedControllerGroup(
self.left_motor_master,
CANSparkMax(3, MotorType.kBrushless)
)
self.right_motors = wpilib.SpeedControllerGroup(
self.right_motor_master,
CANSparkMax(6, MotorType.kBrushless)
)
# Configure Gyro
# Note that the angle from the NavX and all implementors of wpilib Gyro
# must be negated because getAngle returns a clockwise positive angle
self.gyro = navx.AHRS.create_spi()
# Configure drivetrain movement
self.drive = DifferentialDrive(self.left_motors, self.right_motors)
self.drive.setDeadband(0)
# Configure encoder related functions -- getDistance and getrate should return
# units and units/s
self.encoder_constant = (1 / self.GEARING) * self.WHEEL_DIAMETER * math.pi
self.left_encoder = self.left_motor_master.getEncoder()
self.left_encoder.setPositionConversionFactor(self.encoder_constant)
self.left_encoder.setVelocityConversionFactor(self.encoder_constant / 60)
self.right_encoder = self.right_motor_master.getEncoder()
self.right_encoder.setPositionConversionFactor(self.encoder_constant)
self.right_encoder.setVelocityConversionFactor(self.encoder_constant / 60)
self.left_encoder.setPosition(0)
self.right_encoder.setPosition(0)
# Set the update rate instead of using flush because of a ntcore bug
# -> probably don't want to do this on a robot in competition
NetworkTables.getDefault().setUpdateRate(0.010)
def disabledInit(self):
print('Robot disabled')
self.drive.tankDrive(0, 0)
def robotPeriodic(self):
# feedback for users, but not used by the control program
self.l_encoder_pos = self.left_encoder.getPosition()
self.l_encoder_rate = self.left_encoder.getVelocity()
self.r_encoder_pos = self.right_encoder.getPosition()
self.r_encoder_rate = self.right_encoder.getVelocity()
def teleopInit(self):
print('Robot in operator control mode')
def teleopPeriodic(self):
self.drive.arcadeDrive(-self.joystick.getY(), self.joystick.getX())
print(f'Left Distance: {self.left_encoder.getPosition()}')
def autonomousInit(self):
print('Robot in autonomous mode')
# If you wish to just use your own robot program to use with the data logging
# program, you only need to copy/paste the logic below into your code and
# ensure it gets called periodically in autonomous mode
# Additionally, you need to set NetworkTables update rate to 10ms using the
# setUpdateRate call.
def autonomousPeriodic(self):
# Retrieve values to send back before telling the motors to do somethin
now = wpilib.Timer.getFPGATimestamp()
leftPosition = self.left_encoder.getPosition()
leftRate = self.left_encoder.getVelocity()
rightPosition = self.right_encoder.getPosition()
rightRate = self.right_encoder.getVelocity()
battery = wpilib.RobotController.getInputVoltage()
motorVolts = battery * abs(self.prior_autospeed)
leftMotorVolts = motorVolts
rightMotorVolts = motorVolts
# Retrieve the commanded speed from NetworkTables
autospeed = self.auto_speed_entry
self.prior_autospeed = autospeed
# command motors to do things
self.drive.tankDrive((-1 if self.rotate_entry else 1) * autospeed, autospeed, False)
# send telemetry data array back to NT
number_array = [
now, battery, autospeed, leftMotorVolts, rightMotorVolts,
leftPosition, rightPosition, leftRate, rightRate,
math.radians(-self.gyro.getAngle())
]
self.telemetry_entry = number_array
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())
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 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()
def addMotor(self, motor: rev.CANSparkMax):
self.motors.append(motor)
self.motorControllers.append(motor.getPIDController())
class MastBoi(Subsystem):
"""
This subsystem controls the Mast with a Neo motor.
"""
BOTTOM_ENCODER_VALUE = 0.0
MIDDLE_ENCODER_VALUE = 68.0
TOP_ENCODER_VALUE = 136.0
def __init__(self):
super().__init__("MastBoi")
self.logger = logging.getLogger("MastBoi")
self.mastMotor = CANSparkMax(RM.MAST, MotorType.kBrushless)
self.encoder = self.mastMotor.getEncoder()
self.encoder.setPosition(0.0)
def initDefaultCommand(self):
self.logger.warning("No default command for MastBoi")
# Define commands
class Stop(Command):
def __init__(self, mastboi):
super().__init__("Stop")
self.logger = logging.getLogger("Stop")
self.requires(mastboi)
self._mastboi = mastboi
def isFinished(self):
return False
def initialize(self):
self.logger.info("initialize")
self._mastboi.mastMotor.set(0.0)
class HoistTheColorsToPosition(Command):
def __init__(self, mastboi, mastPosition):
super().__init__("Hoist the Colors To Position")
self.logger = logging.getLogger("Hoist the Colors To Position")
self.requires(mastboi)
self._mastboi = mastboi
self.mastPosition = mastPosition
def isFinished(self):
return False
def initialize(self):
self.logger.info("initialize")
setPoint = 0
if self.mastPosition == self._mastboi.MastPosition.BOTTOM:
setPoint = MastBoi.BOTTOM_ENCODER_VALUE
elif self.mastPosition == self._mastboi.MastPosition.MIDDLE:
setPoint = MastBoi.MIDDLE_ENCODER_VALUE
elif self.mastPosition == self._mastboi.MastPosition.TOP:
setPoint = MastBoi.TOP_ENCODER_VALUE
else:
self.logger.warning("Unknown mast position")
self.logger.warning(
"kSmartMotion not enabled: setPoint: {}".format(setPoint))
# self._mastboi.mastMotor.set(ControlType.kSmartMotion, setPoint)
def end(self):
self.logger.info("end")
self._mastboi.mastMotor.set(0.0)
class HoistTheColors(Command):
def __init__(self, mastboi):
super().__init__("Hoist the Colors")
self.logger = logging.getLogger("Hoist the Colors")
self.requires(mastboi)
self._mastboi = mastboi
def isFinished(self):
return False
def initialize(self):
self.logger.info("initialize")
def execute(self):
power = 0.6
position = self._mastboi.encoder.getPosition()
self.logger.info("position: {}".format(position))
if position < MastBoi.TOP_ENCODER_VALUE:
self._mastboi.mastMotor.set(power)
else:
self._mastboi.mastMotor.set(0.0)
def end(self):
self.logger.info("end")
self._mastboi.mastMotor.set(0.0)
class RetrieveTheColors(Command):
def __init__(self, mastboi):
super().__init__("Retrieve the Colors")
self.logger = logging.getLogger("Retrieve the Colors")
self.requires(mastboi)
self._mastboi = mastboi
def isFinished(self):
return False
def initialize(self):
self.logger.info("initialized")
def execute(self):
power = 0.1
position = self._mastboi.encoder.getPosition()
if position > MastBoi.BOTTOM_ENCODER_VALUE:
self._mastboi.mastMotor.set(power)
else:
self._mastboi.mastMotor.set(0.0)
def end(self):
self.logger.info("end")
self._mastboi.mastMotor.set(0.0)
# Define Enums
class MastPosition(enum.IntEnum):
BOTTOM = 0
MIDDLE = 1
TOP = 2
class MastDirection(enum.IntEnum):
DOWN = 0
UP = 1
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)
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 __init__(self):
super().__init__("MastBoi")
self.logger = logging.getLogger("MastBoi")
self.mastMotor = CANSparkMax(RM.MAST, MotorType.kBrushless)
self.encoder = self.mastMotor.getEncoder()
self.encoder.setPosition(0.0)
