class Bot(magicbot.MagicRobot):
drive: drive.Drive
intake: intake.Intake
def createObjects(self):
# Joysticks
self.joystick = wpilib.Joystick(0)
# Drive motor controllers
# Dig | 0/1
# 2^1 | Left/Right
# 2^0 | Front/Rear
self.lf_motor = wpilib.Victor(0b00) # =>0
self.lr_motor = wpilib.Victor(0b01) # =>1
self.rf_motor = wpilib.Victor(0b10) # =>2
self.rr_motor = wpilib.Victor(0b11) # =>3
self.drivetrain = wpilib.drive.DifferentialDrive(wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor),
wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor))
self.btn_sarah = ButtonDebouncer(self.joystick, 2)
self.sarah = False
# Intake
self.intake_wheel_left = wpilib.Spark(5)
self.intake_wheel_right = wpilib.Spark(4)
self.intake_wheels = wpilib.SpeedControllerGroup(self.intake_wheel_left,
self.intake_wheel_right)
self.intake_wheels.setInverted(True)
self.btn_pull = JoystickButton(self.joystick, 3)
self.btn_push = JoystickButton(self.joystick, 1)
def autonomous(self):
super().autonomous()
def disabledPeriodic(self): pass
def disabledInit(self): pass
def teleopInit(self): pass
def teleopPeriodic(self):
# Normal joysticks
self.drive.move(-self.joystick.getY(),self.joystick.getX())
# Corrections for aviator joystick
#self.drive.move(-2*(self.joystick.getY()+.5),
# 2*(self.joystick.getX()+.5)+ROT_COR,
# sarah=self.sarah)
if self.btn_sarah:
self.sarah = not self.sarah
if self.btn_pull.get():
self.intake.pull()
elif self.btn_push.get():
self.intake.push()
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 Robot(magicbot.MagicRobot):
drive: Drive
intake: Intake
def createObjects(self):
# Joysticks
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
# Buttons
self.btn_intake_in = JoystickButton(self.joystick_alt, 2)
self.btn_intake_out = JoystickButton(self.joystick_alt, 1)
# Set up Speed Controller Groups
self.left_motors = wpilib.SpeedControllerGroup(
VictorSP(0),
VictorSP(1),
)
self.right_motors = wpilib.SpeedControllerGroup(
VictorSP(2),
VictorSP(3),
)
# Drivetrain
self.train = wpilib.drive.DifferentialDrive(self.left_motors,
self.right_motors)
# Intake
self.intake_motor = VictorSP(4)
def teleopPeriodic(self):
self.drive.move(-self.joystick_left.getY(), self.joystick_right.getX())
# Intake
if self.btn_intake_out.get():
if self.joystick_right.getX() >= -0.1 and self.joystick_right.getX(
) <= 0.1:
self.intake.spin(1)
elif self.btn_intake_in.get():
self.intake.spin(-1)
class Robot(magicbot.MagicRobot):
# Order of components determines order that execute methods are run
# State Machines
panel_spinner: PanelSpinner
auto_launcher: AutoShoot
# Other components
align: Align
odometry: Odometry
follower: Follower
intake: Intake
control_panel: ControlPanel
launcher: Launcher
drive: Drive
flipped = tunable(False)
ntSolenoid_state = ntproperty("/robot/ntSolenoid_state", 0)
ds_velocity_setpoint = ntproperty('/components/launcher/DS_VEL_SETPOINT',
2100)
limelight_stream_mode = ntproperty('/limelight/stream', 2)
WHEEL_DIAMETER = 0.1524 # Units: Meters
ENCODER_PULSES_PER_REV = 2048 # Ignore quadrature decoding (4x, 8192)
LAUNCHER_GEARING = 1 # No gearing since encoder is on shaft
GEARING = 7.56 # 7.56:1 gear ratio
def createObjects(self):
# Joysticks
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
# Buttons
self.btn_launcher_solenoid = JoystickButton(self.joystick_alt, 1)
self.btn_intake_ = JoystickButton(self.joystick_alt, 5)
self.btn_align = JoystickButton(self.joystick_left, 1)
self.btn_intake_in = JoystickButton(self.joystick_alt, 3)
self.btn_intake_out = JoystickButton(self.joystick_alt, 4)
self.btn_cp_extend = Toggle(self.joystick_left, 4)
self.btn_winch = JoystickButton(self.joystick_alt, 8)
self.btn_cp_motor = JoystickButton(self.joystick_left, 3)
self.btn_launcher_motor = JoystickButton(self.joystick_alt, 12)
self.btn_launcher_idle = Toggle(self.joystick_alt, 10)
self.btn_launcher_motor_close = JoystickButton(self.joystick_alt,
11) # Initiation Line
self.btn_launcher_motor_dynamic = JoystickButton(self.joystick_alt, 9)
self.btn_slow_movement = JoystickButton(self.joystick_right, 1)
self.btn_intake_solenoid = Toggle(self.joystick_alt, 2)
self.btn_scissor_extend = Toggle(self.joystick_alt, 7)
self.btn_color_sensor = JoystickButton(self.joystick_left, 5)
self.btn_cp_stop = JoystickButton(self.joystick_left, 2)
self.btn_invert_y_axis = JoystickButton(self.joystick_left, 6)
self.btn_rotation_sensitivity = JoystickButton(self.joystick_right, 1)
self.btn_intake_bottom_out = JoystickButton(self.joystick_alt, 6)
# Set up motors for encoders
self.master_left = CANSparkMax(1, MotorType.kBrushless)
self.master_right = CANSparkMax(4, MotorType.kBrushless)
self.encoder_constant = (1 /
self.GEARING) * self.WHEEL_DIAMETER * math.pi
self.left_encoder = self.master_left.getEncoder()
self.left_encoder.setPositionConversionFactor(self.encoder_constant)
self.left_encoder.setVelocityConversionFactor(self.encoder_constant /
60)
self.right_encoder = self.master_right.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 up Speed Controller Groups
self.left_motors = wpilib.SpeedControllerGroup(
self.master_left, CANSparkMax(3, MotorType.kBrushless))
self.right_motors = wpilib.SpeedControllerGroup(
self.master_right, CANSparkMax(6, MotorType.kBrushless))
# Drivetrain
self.train = wpilib.drive.DifferentialDrive(self.left_motors,
self.right_motors)
# Winch
self.winch_motors = wpilib.SpeedControllerGroup(
CANSparkMax(8, MotorType.kBrushless),
CANSparkMax(9, MotorType.kBrushless))
self.scissor_solenoid = wpilib.DoubleSolenoid(6, 7)
self.hook_motor = WPI_VictorSPX(3)
# Control Panel Spinner
self.colorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
self.cp_solenoid = wpilib.DoubleSolenoid(
5, 4) # Reversed numbers so kForward is extended
self.cp_motor = WPI_VictorSPX(2)
self.ultrasonic = Ultrasonic(3, 4)
self.ultrasonic.setAutomaticMode(True)
self.ultrasonic.setEnabled(True)
# Intake
self.intake_motor = WPI_VictorSPX(1)
self.intake_motor_lower = CANSparkMax(40, MotorType.kBrushed)
self.intake_solenoid = wpilib.DoubleSolenoid(2, 1)
self.intake_switch = wpilib.DigitalInput(2)
# Launcher
# self.launcher_spark = CANSparkMax(40, MotorType.kBrushed)
# self.launcher_spark.setInverted(True)
self.launcher_motor = CANSparkMax(10, MotorType.kBrushed)
self.launcher_motor.setInverted(True)
self.launcher_motor_follower = CANSparkMax(12, MotorType.kBrushed)
self.launcher_motor_follower.follow(self.launcher_motor)
self.launcher_solenoid = wpilib.Solenoid(0)
# Don't use index pin and change to k1X encoding to decrease rate measurement jitter
self.launcher_encoder = self.launcher_motor.getEncoder(
CANEncoder.EncoderType.kQuadrature, 8192)
self.rpm_controller = self.launcher_motor.getPIDController()
self.launcher_sensor = wpilib.Ultrasonic(0, 1)
self.launcher_sensor.setAutomaticMode(True)
self.launcher_sensor.setEnabled(True)
self.launcher_encoder.setPosition(0)
# NavX (purple board on top of the RoboRIO)
self.navx = navx.AHRS.create_spi()
self.navx.reset()
# Limelight
self.limelight = Limelight()
# Camera Stream
CameraServer.launch('camera/camera.py:main')
# Robot motion control
self.kinematics = KINEMATICS # Use kinematics from inner trajectory generator code
self.drive_feedforward = DRIVE_FEEDFORWARD
self.trajectories = load_trajectories()
# LED strip
self.led_driver = BlinkinLED(0)
def autonomous(self):
self.right_motors.setInverted(True)
super().autonomous()
self.limelight.changePipeline(0)
def disabledInit(self):
self.navx.reset()
self.limelight_stream_mode = 0
def disabledPeriodic(self):
self.limelight.averagePose()
def teleopInit(self):
self.right_motors.setInverted(False)
self.drive.squared_inputs = True
self.drive.speed_constant = 1.05
self.limelight.changePipeline(0)
self.drive.rotational_constant = 0.5
self.inverse = 1
def teleopPeriodic(self):
# if self.btn_invert_y_axis.get():
# self.flipped = True
# self.inverse *= -1
# else:
# self.flipped = False
# self.logger.info(self.ultrasonic.getRangeInches())
if self.btn_invert_y_axis.get():
self.inverse = 1
else:
self.inverse = -1
if self.btn_rotation_sensitivity.get():
self.drive.rotational_constant = 0.1
self.drive.move(self.inverse * self.joystick_left.getY(),
self.joystick_right.getX())
# Align (Overrides self.drive.move() because it's placed after)
if self.btn_align.get() and self.limelight.targetExists():
self.drive.set_target(self.limelight.getYaw(), relative=True)
if self.btn_align.get():
self.limelight.TurnLightOn(True)
self.drive.align()
else:
self.limelight.TurnLightOn(False)
self.drive.set_target(None)
if self.btn_slow_movement.get():
# 10% of original values
self.drive.rotational_constant = 0.54
self.drive.speed_constant = 0.5
# Control Panel Spinner
self.control_panel.set_solenoid(self.btn_cp_extend.get())
if self.btn_cp_extend.get():
self.ntSolenoid_state = True
else:
self.ntSolenoid_state = False
if self.btn_scissor_extend.get():
self.scissor_solenoid.set(wpilib.DoubleSolenoid.Value.kForward)
else:
self.scissor_solenoid.set(wpilib.DoubleSolenoid.Value.kReverse)
# Color Sensor
if self.btn_color_sensor.get():
self.panel_spinner.spin_to()
if self.btn_cp_motor.get():
self.control_panel.spin(0.5)
# Launcher
if self.btn_launcher_motor.get():
self.launcher.setVelocity(4630)
elif self.btn_launcher_motor_close.get():
self.launcher.setVelocity(3900)
elif self.btn_launcher_motor_dynamic.get():
# self.limelight.TurnLightOn(True)
self.launcher.setVelocity(self.ds_velocity_setpoint)
# if self.limelight.targetExists():
# self.launcher.setVelocityFromDistance(self.limelight.pitch_angle, 4670)
elif self.btn_launcher_idle.get():
self.launcher.setPercentOutput(1)
if self.btn_launcher_solenoid.get():
self.auto_launcher.fire_when_ready()
if self.btn_cp_stop.get():
self.panel_spinner.done()
# Intake
if self.btn_intake_out.get():
self.intake.spin_inside(1)
elif self.btn_intake_in.get():
self.intake.spin(-1, 0.4)
elif self.btn_intake_bottom_out.get():
self.intake.spin_lower(-0.4)
if self.btn_intake_solenoid.get():
self.intake_solenoid.set(wpilib.DoubleSolenoid.Value.kReverse)
else:
self.intake_solenoid.set(wpilib.DoubleSolenoid.Value.kForward)
# Winch
if self.btn_winch.get():
self.winch_motors.set(0.65)
else:
self.winch_motors.set(
0
) # Must use set(0) when not pressed because there is no component
# Hook
if self.joystick_alt.getPOV(0) == 90:
self.hook_motor.set(0.5)
elif self.joystick_alt.getPOV(0) == 270:
self.hook_motor.set(-0.5)
else:
self.hook_motor.set(0)
if self.joystick_alt.getPOV(0) == 0:
self.launcher.fire()
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 DeepSpaceRobot(magicbot.MagicRobot):
# state machines
climb3: Climb3
# components
cargo: Cargo
climber: Climber
drivetrain: Drivetrain
elevator: Elevator
hatch: Hatch
drive_joystick: wpilib.Joystick
def createObjects(self):
# Drivetrain
self.motor_fl0 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["fl"][0],
rev.MotorType.kBrushless) # 11
self.motor_fr0 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["fr"][0],
rev.MotorType.kBrushless) # 15
self.motor_rl0 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["rl"][0],
rev.MotorType.kBrushless) # 13
self.motor_rr0 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["rr"][0],
rev.MotorType.kBrushless) # 17
self.motor_rr0.setInverted(True)
self.motor_fr0.setInverted(True)
# Set Motors to follow each other
# Ignore secondary motors for simulation, not fully supported yet
if not self.isSimulation():
self.motor_fr1 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["fr"][1],
rev.MotorType.kBrushless)
self.motor_fl1 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["fl"][1],
rev.MotorType.kBrushless)
self.motor_rl1 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["rl"][1],
rev.MotorType.kBrushless)
self.motor_rr1 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["rr"][1],
rev.MotorType.kBrushless)
self.motor_fl1.follow(self.motor_fl0)
self.motor_fr1.follow(self.motor_fr0)
self.motor_rl1.follow(self.motor_rl0)
self.motor_rr1.follow(self.motor_rr0)
self.drivetrain_train = wpilib.drive.MecanumDrive(
self.motor_fl0, self.motor_rl0, self.motor_fr0, self.motor_rr0)
self.drivetrain_gyro = wpilib.ADXRS450_Gyro()
# Elevator
self.elevator_motor1 = ctre.TalonSRX(robotmap.ELEVATOR["motors"][0])
self.elevator_motor2 = ctre.TalonSRX(robotmap.ELEVATOR["motors"][1])
self.elevator_motor2.follow(self.elevator_motor1)
self.elevator_limit_switch_bottom = wpilib.DigitalInput(
robotmap.ELEVATOR["lower"])
self.elevator_encoder = TalonEncoder(self.elevator_motor1)
# Hatch
self.hatch_hold_piston = wpilib.Solenoid(
robotmap.INTAKE["hatch"]["actuator"])
self.hatch_lift_piston = wpilib.Solenoid(
robotmap.INTAKE["hatch"]["lift"])
# Cargo
self.cargo_motor = ctre.TalonSRX(robotmap.INTAKE["cargo"]["actuator"])
self.cargo_lift_piston = wpilib.Solenoid(
robotmap.INTAKE["cargo"]["lift"])
# Climber
self.climber_front_left_motor = ctre.VictorSPX(
robotmap.CLIMBER_FL["motor"])
self.climber_front_right_motor = ctre.VictorSPX(
robotmap.CLIMBER_FR["motor"])
self.climber_back_motor = ctre.VictorSPX(
robotmap.CLIMBER_BACK["motor"])
self.climber_wheel_motor = ctre.VictorSPX(robotmap.CLIMBER_WHEELS)
# Climber Limit Switches
self.climber_front_left_upper_switch = wpilib.DigitalInput(
robotmap.CLIMBER_FL["switch"]["top"])
self.climber_front_right_upper_switch = wpilib.DigitalInput(
robotmap.CLIMBER_FR["switch"]["top"])
self.climber_front_left_lower_switch = wpilib.DigitalInput(
robotmap.CLIMBER_FL["switch"]["bottom"])
self.climber_front_right_lower_switch = wpilib.DigitalInput(
robotmap.CLIMBER_FR["switch"]["bottom"])
self.climber_back_upper_switch = wpilib.DigitalInput(
robotmap.CLIMBER_BACK["switch"]["top"])
self.climber_back_lower_switch = wpilib.DigitalInput(
robotmap.CLIMBER_BACK["switch"]["bottom"])
# Joystick 1
self.drive_joystick = wpilib.Joystick(0)
self.slow_button = JoystickButton(self.drive_joystick, 1)
self.bottom_tower_front_button = JoystickButton(self.drive_joystick, 9)
self.bottom_tower_back_button = JoystickButton(self.drive_joystick, 10)
self.perp_button = JoystickButton(self.drive_joystick, 6)
self.top_tower_front_button = JoystickButton(self.drive_joystick, 7)
self.top_tower_back_button = JoystickButton(self.drive_joystick, 8)
self.climb_button = JoystickButton(self.drive_joystick, 3)
self.climb_cancel_button = JoystickButton(self.drive_joystick, 4)
# Joystick 2
self.op_joystick = wpilib.Joystick(1)
self.tower_l1_button = JoystickButton(self.op_joystick, 1)
self.tower_l2_button = JoystickButton(self.op_joystick, 2)
self.tower_l3_button = JoystickButton(self.op_joystick, 3)
self.elevator_load_button = JoystickButton(self.op_joystick, 4)
self.hatch_panel_button = JoystickButton(self.op_joystick, 9)
self.cargo_ball_button = JoystickButton(self.op_joystick, 10)
self.defense_mode_button = JoystickButton(self.op_joystick, 5)
self.intake_button = JoystickButton(self.op_joystick, 5)
self.release_button = JoystickButton(self.op_joystick, 6)
# Climb Joystick
self.climb_joystick = wpilib.Joystick(2)
self.climber_front_lift_button = JoystickButton(self.climb_joystick, 1)
self.climber_back_lift_button = JoystickButton(self.climb_joystick, 2)
self.climber_front_lower_button = JoystickButton(
self.climb_joystick, 3)
self.climber_back_lower_button = JoystickButton(self.climb_joystick, 4)
self.climber_drive_button = JoystickButton(self.climb_joystick, 5)
self.climber_reverse_button = JoystickButton(self.climb_joystick, 6)
def teleopInit(self):
self.elevator.recalculate_ratio()
self.elevator.cargo_mode()
def teleopPeriodic(self):
# Standard Mecanum Driving
forward = -self.drive_joystick.getY()
strafe = self.drive_joystick.getX()
turn = self.drive_joystick.getRawAxis(3)
slow = self.slow_button.get()
gyro_manually_set = False
# Auto Rotation
if self.top_tower_front_button.get():
self.drivetrain.rotate_to_angle(
self.drivetrain.find_nearest_angle(-30))
gyro_manually_set = True
elif self.bottom_tower_front_button.get():
self.drivetrain.rotate_to_angle(
self.drivetrain.find_nearest_angle(30))
gyro_manually_set = True
elif self.bottom_tower_back_button.get():
self.drivetrain.rotate_to_angle(
self.drivetrain.find_nearest_angle(150))
gyro_manually_set = True
elif self.top_tower_back_button.get():
self.drivetrain.rotate_to_angle(
self.drivetrain.find_nearest_angle(-150))
gyro_manually_set = True
elif self.perp_button.get():
self.drivetrain.rotate_to_angle(self.drivetrain.nearest_90)
gyro_manually_set = True
HEIGHTS = [0, 0, 0]
if self.hatch_panel_button.get():
self.hatch.lower()
self.cargo.lift()
self.elevator.hatch_mode()
elif self.cargo_ball_button.get():
self.cargo.lower()
self.hatch.lift()
self.elevator.cargo_mode()
elif self.defense_mode_button.get():
self.cargo.lift()
self.hatch.lift()
self.elevator.defense_mode()
if self.elevator.height_for_hatch:
HEIGHTS = ROCKET_HATCH_HEIGHTS
elif self.elevator.height_for_cargo:
HEIGHTS = ROCKET_CARGO_HEIGHTS
if self.tower_l1_button.get():
self.elevator.move_to_setpoint(HEIGHTS[0])
elif self.tower_l2_button.get():
self.elevator.move_to_setpoint(HEIGHTS[1])
elif self.tower_l3_button.get():
self.elevator.move_to_setpoint(HEIGHTS[2])
elif self.elevator_load_button.get():
self.elevator.move_to_setpoint(ROCKET_HATCH_HEIGHTS[0] if self.
elevator.height_for_hatch else 0)
if self.climber_front_lift_button.get():
self.climber.front_up()
if self.climber_back_lift_button.get():
self.climber.rear_up()
if self.climber_front_lower_button.get():
self.climber.front_down()
if self.climber_back_lower_button.get():
self.climber.rear_down()
if self.climber_drive_button.get():
self.climber.drive_forward()
if self.climber_reverse_button.get():
self.climber.drive_backwards()
if self.climb_button.get():
self.climb3.climb()
if self.climb_cancel_button.get():
self.climb3.stop()
if self.elevator.height_for_hatch:
if self.intake_button.get():
self.hatch.hold()
if self.release_button.get():
self.hatch.release()
if self.elevator.height_for_cargo:
if self.intake_button.get():
self.cargo.intake()
if self.release_button.get():
self.cargo.outtake()
self.drivetrain.move(
forward,
strafe,
turn,
gyro=True,
driver=True,
slow=slow,
gyro_manually_set=gyro_manually_set,
)
def disabledInit(self):
# Set everything to the state it should be in for being disabled
self.elevator.recalculate_ratio()
self.drivetrain.move(0, 0, 0)
self.elevator.stop()
class MyRobot(wpilib.TimedRobot):
# Called once on robot startup.
# Declare and initialize the joystick and all of the subsystems.
# More documentation on each of these constructors is present in their file.
def robotInit(self):
try:
wpilib.CameraServer.launch()
except:
pass
self.stick = wpilib.Joystick(0)
self.base = Base(rev.CANSparkMax(4, rev.MotorType.kBrushless),
rev.CANSparkMax(2, rev.MotorType.kBrushless),
rev.CANSparkMax(3, rev.MotorType.kBrushless),
rev.CANSparkMax(1, rev.MotorType.kBrushless),
AHRS.create_spi(wpilib.SPI.Port.kMXP), self.stick)
self.lift = Lift(WPI_TalonSRX(3), wpilib.DigitalInput(0),
wpilib.DigitalInput(1), self.stick)
self.arm = Arm(wpilib.Solenoid(5, 2), wpilib.Solenoid(5, 4),
wpilib.Solenoid(5, 1), wpilib.Solenoid(5, 5),
self.stick)
self.testvar = 0
self.ballintake = BallIntake(WPI_TalonSRX(4))
try:
self.ledController = LEDController.getInstance()
except:
pass
# Various one-time initializations happen here.
self.lift.initSensor()
self.base.navx.reset()
try:
NetworkTables.initialize()
except:
pass
try:
self.visiontable = NetworkTables.getTable("visiontable")
except:
pass
self.lights = False
self.op = False
self.testButton = JoystickButton(self.stick, 16)
self.roller = Roller(WPI_TalonSRX(20), WPI_TalonSRX(10), self.stick,
"init")
# Called repeatedly in a loop while the robot is disabled.
def disabledPeriodic(self):
try:
if wpilib.DriverStation.getInstance().getAlliance(
) == wpilib.DriverStation.Alliance.Blue:
self.ledController.setState(LEDController.STATE_BLUE_STANDBY)
elif wpilib.DriverStation.getInstance().getAlliance(
) == wpilib.DriverStation.Alliance.Red:
self.ledController.setState(LEDController.STATE_RED_STANDBY)
else:
self.ledController.setState(LEDController.STATE_DISABLED)
except:
self.ledController.setState(LEDController.STATE_DISABLED)
self.lift.log()
self.arm.log()
self.base.log()
self.ballintake.log()
self.roller.log()
if self.testButton.get():
if self.testvar == 17:
self.testvar = 0
self.ledController.setState(self.testvar)
self.testvar += 1
self.ledController.update()
wpilib.SmartDashboard.putNumber(
"visionerror", self.visiontable.getNumber("heading_error", 0))
# Called once on teleop start.
# Rezero lift encoder and rezero navx heading.
def teleopInit(self):
self.lift.initSensor()
self.base.navx.reset()
self.roller.state = "init"
self.ledController.update()
# Called repeatedly during teleop.
# Update all of the subsystems and log new data to SmartDashboard.
def teleopPeriodic(self):
self.base.update()
self.lift.update()
self.arm.update()
self.ballintake.update()
self.roller.update()
self.ledController.setState(11)
self.ledController.update()
self.lift.log()
self.arm.log()
self.base.log()
self.ballintake.log()
self.roller.log()
self.ledController.log()
autonomousPeriodic = teleopPeriodic
autonomousInit = teleopInit
class Arm():
# Initialize a new instance of Arm.
# @param hs1, hs2 - the two solenoids for the hatch grabber.
# @param as1, as2 - the two solenoids for the arm.
# @param stick - the joystick instance.
def __init__(self, hs1, hs2, as1, as2, stick):
self.hatchsolenoid1 = hs1
self.hatchsolenoid2 = hs2
self.armsolenoid1 = as1
self.armsolenoid2 = as2
# the corresponding solenoids for each subsystem must be in opposite
# states.
self.hatchsolenoid1.set(False)
self.hatchsolenoid2.set(True)
self.armsolenoid1.set(False)
self.armsolenoid2.set(True)
# Currently trigger and front button.
# TODO separate controls out into separate config class.
self.hatchbutton = JoystickButton(stick, 2)
self.armbutton = JoystickButton(stick, 1)
self.hatchenabled = False
self.armenabled = False
# Toggle the state of the hatch by inverting both hatch solenoids.
def toggleHatch(self):
self.hatchsolenoid1.set(not self.hatchsolenoid1.get())
self.hatchsolenoid2.set(not self.hatchsolenoid2.get())
# Toggle the state of the arm by inverting both arm solenoids.
def toggleArm(self):
self.armsolenoid1.set(not self.armsolenoid1.get())
self.armsolenoid2.set(not self.armsolenoid2.get())
# Update the arm and the hatch via two simple state machines that will
# toggle the hatch and the arm solenoids depending on the state of the
# joystick buttons.
def update(self):
if self.hatchbutton.get() and not self.hatchenabled:
self.hatchenabled = True
self.toggleHatch()
LEDController.getInstance().setState(LEDController.STATE_HATCH)
elif not self.hatchbutton.get() and self.hatchenabled:
self.hatchenabled = False
if self.armbutton.get() and not self.armenabled:
self.armenabled = True
self.toggleArm()
elif not self.armbutton.get() and self.armenabled:
self.armenabled = False
# log arm subsystem-related information to SmartDashboard
def log(self):
wpilib.SmartDashboard.putBoolean("hatchsolenoid1_status",
self.hatchsolenoid1.get())
wpilib.SmartDashboard.putBoolean("hatchsolenoid2_status",
self.hatchsolenoid2.get())
wpilib.SmartDashboard.putBoolean("armsolenoid1_status",
self.armsolenoid1.get())
wpilib.SmartDashboard.putBoolean("armsolenoid2_status",
self.armsolenoid2.get())
wpilib.SmartDashboard.putBoolean("arm_enabled", self.armenabled)
wpilib.SmartDashboard.putBoolean("hatch_enabled", self.hatchenabled)
wpilib.SmartDashboard.putBoolean("hatch_button",
self.hatchbutton.get())
wpilib.SmartDashboard.putBoolean("arm_button", self.armbutton.get())
class MyRobot(MagicRobot):
#
# Define components here
#
drive = drive.Drive
arm = arm.Arm
#
# Define constants here
#
# Robot attributes
WHEEL_DIAMETER = 0.5 # 6 inches
ENCODER_COUNTS_PER_REV = 360
# Pathfinder constants
MAX_VELOCITY = 5 # ft/s
MAX_ACCELERATION = 6
def createObjects(self):
"""
Initialize all wpilib motors & sensors
"""
# Joysticks
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
self.btn_fine_movement = JoystickButton(self.joystick_right, 2)
# Drive motor controllers
# ID SCHEME:
# 10^1: 1 = left, 2 = right
# 10^0: 0 = front, 5 = rear
self.lf_motor = WPI_TalonSRX(10)
self.lr_motor = WPI_VictorSPX(15)
self.rf_motor = WPI_TalonSRX(20)
self.rr_motor = WPI_VictorSPX(25)
self.lf_motor.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Absolute
)
self.rf_motor.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Absolute
)
# Following masters
self.lr_motor.follow(self.lf_motor)
self.rr_motor.follow(self.rf_motor)
# Drive init
self.train = wpilib.drive.DifferentialDrive(
self.lf_motor, self.rf_motor
)
# Arm
self.arm_motor = WPI_TalonSRX(0)
self.arm_motor.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Absolute
)
self.arm_limit = wpilib.DigitalInput(4)
# Gyro
self.gyro = navx.AHRS.create_spi()
self.gyro.reset()
self.control_loop_wait_time = 0.02
def autonomous(self):
"""
Prepare for and start autonomous mode.
"""
self.drive.reset()
self.drive.squared_inputs = False
self.drive.rotational_constant = 0.5
# Call autonomous
super().autonomous()
def teleopPeriodic(self):
"""
Place code here that does things as a result of operator
actions
"""
self.drive.move(
-self.joystick_left.getY(),
self.joystick_right.getX(),
self.btn_fine_movement.get(),
)
class Panthera(magicbot.MagicRobot):
drive: drive.Drive
winch: winch.Winch
arm: arm.Arm
recorder: recorder.Recorder
time = tunable(0)
plates = tunable('')
voltage = tunable(0)
rotation = tunable(0)
unified_control = tunable(False)
recording = tunable(False)
stabilize = tunable(False)
stabilizer_threshold = tunable(30)
stabilizer_aggression = tunable(5)
def createObjects(self):
"""
Initialize robot components.
"""
# Joysticks
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
# Buttons
self.btn_claw = ButtonDebouncer(self.joystick_left, 1)
self.btn_forearm = ButtonDebouncer(self.joystick_right, 1)
self.btn_up = JoystickButton(self.joystick_left, 3)
self.btn_down = JoystickButton(self.joystick_left, 2)
self.btn_climb = JoystickButton(self.joystick_right, 11)
# Buttons on alternative joystick
self.btn_claw_alt = ButtonDebouncer(self.joystick_alt, 1)
self.btn_forearm_alt = ButtonDebouncer(self.joystick_alt, 2)
self.btn_climb_alt = JoystickButton(self.joystick_alt, 3)
# Buttons for toggling control options and aides
self.btn_unified_control = ButtonDebouncer(self.joystick_alt, 8)
self.btn_record = ButtonDebouncer(self.joystick_left, 6)
self.btn_stabilize = ButtonDebouncer(self.joystick_alt, 12)
self.btn_fine_movement = JoystickButton(self.joystick_right, 2)
# Drive motor controllers
# ID SCHEME:
# 10^1: 1 = left, 2 = right
# 10^0: 0 = front, 5 = rear
self.lf_motor = WPI_TalonSRX(10)
self.lr_motor = WPI_TalonSRX(15)
self.rf_motor = WPI_TalonSRX(20)
self.rr_motor = WPI_TalonSRX(25)
# Follow front wheels with rear to save CAN packets
self.lr_motor.follow(self.lf_motor)
self.rr_motor.follow(self.rf_motor)
# Drivetrain
self.train = wpilib.drive.DifferentialDrive(self.lf_motor, self.rf_motor)
# Winch
self.winch_motors = wpilib.SpeedControllerGroup(wpilib.Victor(7), wpilib.Victor(8))
# Motion Profiling
self.position_controller = motion_profile.PositionController()
# Arm
self.elevator = wpilib.Victor(5)
self.forearm = wpilib.DoubleSolenoid(2, 3)
self.claw = wpilib.DoubleSolenoid(0, 1)
# NavX (purple board on top of the RoboRIO)
self.navx = navx.AHRS.create_spi()
self.navx.reset()
# Utility
self.ds = wpilib.DriverStation.getInstance()
self.timer = wpilib.Timer()
self.pdp = wpilib.PowerDistributionPanel(0)
self.compressor = wpilib.Compressor()
# Camera server
wpilib.CameraServer.launch('camera/camera.py:main')
def robotPeriodic(self):
"""
Executed periodically regardless of mode.
"""
self.time = int(self.timer.getMatchTime())
self.voltage = self.pdp.getVoltage()
self.rotation = self.navx.getAngle() % 360
def autonomous(self):
"""
Prepare for and start autonomous mode.
"""
self.compressor.stop()
self.drive.squared_inputs = False
self.drive.rotational_constant = 0.5
self.plates = ''
wpilib.Timer.delay(1)
# Read data on plate colors from FMS.
# 3.10: "The FMS provides the ALLIANCE color assigned to each PLATE to the Driver Station software. Immediately following the assignment of PLATE color prior to the start of AUTO."
# Will fetch a string of three characters ('L' or 'R') denoting position of the current alliance's on the switches and scale, with the nearest structures first.
# More information: http://wpilib.screenstepslive.com/s/currentCS/m/getting_started/l/826278-2018-game-data-details
self.plates = self.ds.getGameSpecificMessage()
# Call autonomous
super().autonomous()
def disabledInit(self):
"""
Executed once right away when robot is disabled.
"""
# Reset Gyro to 0
self.navx.reset()
def disabledPeriodic(self):
"""
Executed periodically while robot is disabled.
Useful for testing.
"""
pass
def teleopInit(self):
"""
Executed when teleoperated mode begins.
"""
self.compressor.start()
self.drive.squared_inputs = True
self.drive.rotational_constant = 0.5
def teleopPeriodic(self):
"""
Executed periodically while robot is in teleoperated mode.
"""
# Read from joysticks and move drivetrain accordingly
self.drive.move(-self.joystick_left.getY(),
self.joystick_right.getX(),
self.btn_fine_movement.get())
if self.stabilize:
if abs(self.navx.getPitch()) > self.stabilizer_threshold:
self.drive.move(self.navx.getPitch() / 180 * self.stabilizer_aggression, 0)
if self.btn_stabilize.get():
self.stabilize = not self.stabilize
if self.btn_unified_control.get():
self.unified_control = not self.unified_control
# Winch
if (self.btn_climb.get() and self.unified_control) or self.btn_climb_alt.get():
self.winch.run()
# Arm
if (self.btn_claw.get() and self.unified_control) or self.btn_claw_alt.get():
self.arm.actuate_claw()
if (self.btn_forearm.get() and self.unified_control) or self.btn_forearm_alt.get():
self.arm.actuate_forearm()
self.arm.move(-self.joystick_alt.getY())
if self.unified_control:
if self.btn_up.get():
self.arm.up()
if self.btn_down.get():
self.arm.down()
if self.btn_record.get():
if self.recording:
self.recording = False
self.recorder.stop()
else:
self.recording = True
self.recorder.start(self.voltage)
if self.recording:
self.recorder.capture((self.joystick_left, self.joystick_right, self.joystick_alt))
class Roller():
POSITION_TOP = 3100
def __init__(self, roller_motor, pivot, stick, initial_state):
self.roller_motor = roller_motor
self.pivot = pivot
self.stick = stick
self.rollerenabler = JoystickButton(self.stick, 14)
self.pivotupbutton = JoystickButton(self.stick, 5)
self.pivotdownbutton = JoystickButton(self.stick, 10)
self.pistonoutbutton = JoystickButton(self.stick, 8)
self.pistoninbutton = JoystickButton(self.stick, 7)
#self.ratchetswitchbutton = JoystickButton(self.stick, 15)
#self.ratchetbtnpressed = False
#self.pivotstopbutton = JoystickButton(self.stick, 14)
#self.pivotinitbutton = JoystickButton(self.stick, 13)
self.rollerout = JoystickButton(self.stick, 9)
self.rollerin = JoystickButton(self.stick, 6)
self.state = initial_state
self.pivot.selectProfileSlot(0, 0)
self.switch = wpilib.DigitalInput(2)
self.count = 0
self.sol2 = wpilib.Solenoid(5, 3)
self.sol1 = wpilib.Solenoid(5, 0)
self.sol1.set(False)
self.sol2.set(True)
self.ratchet = wpilib.Solenoid(5, 6)
self.ratchetEnabled = False
self.ratchet.set(not self.ratchetEnabled)
def update(self):
if (self.state is not "init") and (not self.rollerenabler.get()):
self.pivot.set(WPI_TalonSRX.ControlMode.PercentOutput, 0)
self.roller_motor.set(WPI_TalonSRX.ControlMode.PercentOutput, 0)
self.state = "manual"
self.sol1.set(False)
self.sol2.set(True)
return
if self.state == "manual":
# if self.pivot.getSelectedSensorPosition() >= self.POSITION_TOP and self.stick.getY() >0:
# self.pivot.set(WPI_TalonSRX.ControlMode.PercentOutput, 0)
# else:
# self.pivot.set(WPI_TalonSRX.ControlMode.PercentOutput, self.stick.getY())
#pass
if self.pivotupbutton.get():
#self.pivot.setQuadraturePosition(0)
self.state = "up"
elif self.pivotdownbutton.get():
self.state = "down"
if self.pistonoutbutton.get():
self.sol1.set(True)
self.sol2.set(False)
if self.pistoninbutton.get():
self.sol1.set(False)
self.sol2.set(True)
#elif self.pivotstopbutton.get():
#self.pivot.setQuadraturePosition(0)
#self.state = "manual"
#elif self.pivotinitbutton.get():
# self.state = "init"
self.state_table[self.state](self)
if self.rollerin.get():
self.roller_motor.set(WPI_TalonSRX.ControlMode.PercentOutput, -1)
#elif self.rollerout.get():
# self.roller_motor.set(WPI_TalonSRX.ControlMode.PercentOutput, -1)
else:
self.roller_motor.set(WPI_TalonSRX.ControlMode.PercentOutput, 0)
def log(self):
wpilib.SmartDashboard.putString("roller_state", self.state)
wpilib.SmartDashboard.putNumber("roller_position",
self.pivot.getSelectedSensorPosition())
def state_init(self):
self.ratchet.set(not self.ratchetEnabled)
if self.rollerenabler.get():
self.pivot.set(WPI_TalonSRX.ControlMode.PercentOutput, 0)
self.pivot.setQuadraturePosition(0)
self.state = 'manual'
return
if self.switch.get():
self.pivot.set(WPI_TalonSRX.ControlMode.PercentOutput, -.3)
else:
self.pivot.set(WPI_TalonSRX.ControlMode.PercentOutput, 0)
self.pivot.setQuadraturePosition(0)
self.ratchet.set(self.ratchetEnabled)
self.state = 'manual'
def state_manual(self):
pass
def state_up(self):
self.pivot.set(WPI_TalonSRX.ControlMode.Position, self.POSITION_TOP)
#if self.pivot.getSelectedSensorPosition() > self.POSITION_TOP + 500:
# self.pivot.set(WPI_TalonSRX.ControlMode.PercentOutput, 0)
# self.state = 'manual'
if self.pistonoutbutton.get():
self.sol1.set(True)
self.sol2.set(False)
if self.pistoninbutton.get():
self.sol1.set(False)
self.sol2.set(True)
self.ratchet.set(self.ratchetEnabled)
def state_down(self):
self.pivot.set(WPI_TalonSRX.ControlMode.PercentOutput, .1)
self.ratchet.set(not self.ratchetEnabled)
if self.count == 0:
self.state = "post_state_down"
self.count += 1
def post_state_down(self):
self.count = 0
self.ratchet.set(not self.ratchetEnabled)
self.pivot.set(WPI_TalonSRX.ControlMode.PercentOutput, -.4)
if self.pistonoutbutton.get():
self.sol1.set(True)
self.sol2.set(False)
if self.pistoninbutton.get():
self.sol1.set(False)
self.sol2.set(True)
state_table = {
"post_state_down": post_state_down,
"manual": state_manual,
"up": state_up,
"down": state_down,
"init": state_init
}
class Robot(MagicRobot):
drivetrain = Drivetrain
climber = Climber
elevator = Elevator
intake = Intake
mode = RobotMode.switch
rumbling = False
def createObjects(self):
wpilib.CameraServer.launch()
wpilib.LiveWindow.disableAllTelemetry()
self.left_drive_motor = WPI_TalonSRX(0)
WPI_TalonSRX(1).set(WPI_TalonSRX.ControlMode.Follower,
self.left_drive_motor.getDeviceID())
self.right_drive_motor = WPI_TalonSRX(2)
WPI_TalonSRX(3).set(WPI_TalonSRX.ControlMode.Follower,
self.right_drive_motor.getDeviceID())
self.robot_drive = wpilib.drive.DifferentialDrive(
self.left_drive_motor, self.right_drive_motor)
self.r_intake_motor = WPI_VictorSPX(4)
self.l_intake_motor = WPI_VictorSPX(5)
self.elevator_winch = WPI_TalonSRX(6)
self.climber_motor = WPI_TalonSRX(7)
self.wrist_motor = WPI_TalonSRX(8)
self.intake_ir = wpilib.AnalogInput(0)
self.intake_solenoid = wpilib.DoubleSolenoid(2, 3)
self.right_drive_joystick = wpilib.Joystick(0)
self.left_drive_joystick = wpilib.Joystick(1)
self.operator_joystick = wpilib.Joystick(2)
self.compressor = wpilib.Compressor()
self.elevator_limit_switch = wpilib.DigitalInput(0)
self.climber_motor = WPI_TalonSRX(7)
self.navx = AHRS.create_spi()
self.path_tracking_table = NetworkTables.getTable("path_tracking")
self.down_button = ButtonDebouncer(self.operator_joystick, 1)
self.right_button = ButtonDebouncer(self.operator_joystick, 2)
self.left_button = ButtonDebouncer(self.operator_joystick, 3)
self.has_cube_button = ButtonDebouncer(self.operator_joystick, 5)
self.up_button = ButtonDebouncer(self.operator_joystick, 4)
self.left_bumper_button = JoystickButton(self.operator_joystick, 5)
self.right_bumper_button = JoystickButton(self.operator_joystick, 6)
def up_mode(self):
self.mode += 1
def down_mode(self):
self.mode -= 1
def autonomous(self):
self.intake.reset_wrist()
super().autonomous()
def teleopInit(self):
self.intake.reset_wrist_up()
def teleopPeriodic(self):
self.right = -self.right_drive_joystick.getRawAxis(1)
self.left = -self.left_drive_joystick.getRawAxis(1)
self.right = 0 if abs(self.right) < 0.1 else self.right
self.left = 0 if abs(self.left) < 0.1 else self.left
self.drivetrain.tank(math.copysign(self.right**2, self.right),
math.copysign(self.left**2, self.left))
# outtake
if self.operator_joystick.getRawAxis(3) > 0.01:
self.intake.set_speed(
self.operator_joystick.getRawAxis(3) * 0.8 + 0.2)
elif self.operator_joystick.getRawButton(3):
self.intake.intake()
else:
self.intake.hold()
elevator_speed = -self.operator_joystick.getY(0)
if self.down_button.get():
self.down_mode()
elif self.up_button.get():
self.up_mode()
joystick_val = -self.operator_joystick.getRawAxis(1)
if joystick_val > 0.2:
joystick_val -= 0.2
elif joystick_val < -0.2:
joystick_val += 0.2
else:
joystick_val = 0.0
self.elevator.move_setpoint(joystick_val / 50.0 * 20)
if self.right_bumper_button.get():
self.intake.wrist_down()
self.intake.intake()
self.intake.open_arm()
if self.intake.has_cube():
self.rumbling = True
else:
self.intake.close_arm()
if self.left_bumper_button.get():
self.climber.set_speed(-self.operator_joystick.getRawAxis(5))
else:
wrist_speed = self.operator_joystick.getRawAxis(5)
wrist_speed = 0 if abs(wrist_speed) < 0.2 else wrist_speed
self.intake.move_wrist_setpoint(wrist_speed)
if self.has_cube_button.get():
self.intake.toggle_has_cube()
self.operator_joystick.setRumble(
wpilib.Joystick.RumbleType.kRightRumble, 1 if self.rumbling else 0)
self.operator_joystick.setRumble(
wpilib.Joystick.RumbleType.kLeftRumble, 1 if self.rumbling else 0)
self.rumbling = False
def disabledPeriodic(self):
self.drivetrain._update_odometry()
self.elevator.reset_position()
def testPeriodic(self):
self.wrist_motor.set(self.operator_joystick.getRawAxis(5) * 0.6)
self.elevator_winch.set(self.operator_joystick.getRawAxis(1))
self.intake_solenoid.set(wpilib.DoubleSolenoid.Value.kReverse)
self.left_drive_motor.set(0)
self.right_drive_motor.set(0)
class Base():
# Initialize a new instance of Base.
# @param fl, rl, fr, rr - the front-left, rear-left, front-right, rear-right
# motors of the drive base, respectively.
# @param navx - the NavX instance.
# @param stick - the Joystick instance.
def __init__(self, fl, rl, fr, rr, navx, stick):
self.fl = fl
self.rl = rl
self.fr = fr
self.rr = rr
self.drive = wpilib.drive.MecanumDrive(fl, rl, fr, rr)
self.navx = navx
self.stick = stick
self.foctoggle = JoystickButton(self.stick, 11)
self.focenabled = False
self.foctoggledown = False
# Ignore all joystick inputs that are below a certain threshold.
def deadband(self, val):
if abs(val) < .2:
val = 0
return val
# Update the mecanum drive and FOC status.
def update(self):
if wpilib.DriverStation.getInstance().getAlliance(
) == wpilib.DriverStation.Alliance.Blue:
LEDController.getInstance().setState(
LEDController.STATE_BLUE_MOTION)
elif wpilib.DriverStation.getInstance().getAlliance(
) == wpilib.DriverStation.Alliance.Red:
LEDController.getInstance().setState(
LEDController.STATE_RED_MOTION)
else:
LEDController.getInstance().setState(LEDController.STATE_DISABLED)
# You *cannot* use a simple if statement without the helper focenabled
# variable to toggle FOC state.
if self.foctoggle.get() and not self.foctoggledown:
self.focenabled = not self.focenabled
self.foctoggledown = True
elif not self.foctoggle.get() and self.foctoggledown:
self.foctoggledown = False
scaleval = (1 - self.stick.getThrottle()) * 0.8 + 0.1
# Based on FOC state, decide whether or not to pass NavX heading.
if self.focenabled:
self.drive.driveCartesian(
self.deadband(self.stick.getX()) * scaleval * 2,
-self.deadband(self.stick.getY()) * scaleval,
self.stick.getZ() * 0.25 * scaleval, self.navx.getAngle())
else:
self.drive.driveCartesian(
self.deadband(self.stick.getX()) * scaleval * 2,
-self.deadband(self.stick.getY()) * scaleval,
self.stick.getZ() * 0.25 * scaleval)
# Log as much data as possible to SmartDashboard for debug purposes.
def log(self):
wpilib.SmartDashboard.putBoolean("foc_enabled", self.focenabled)
wpilib.SmartDashboard.putNumber("fl_velocity", self.fl.get())
wpilib.SmartDashboard.putNumber("fr_velocity", self.fr.get())
wpilib.SmartDashboard.putNumber("rl_velocity", self.rl.get())
wpilib.SmartDashboard.putNumber("rr_velocity", self.rr.get())
wpilib.SmartDashboard.putNumber("fl_temperature",
self.fl.getMotorTemperature())
wpilib.SmartDashboard.putNumber("fr_temperature",
self.fr.getMotorTemperature())
wpilib.SmartDashboard.putNumber("rl_temperature",
self.rl.getMotorTemperature())
wpilib.SmartDashboard.putNumber("rr_temperature",
self.rr.getMotorTemperature())
wpilib.SmartDashboard.putNumber("navx_heading", self.navx.getAngle())
wpilib.SmartDashboard.putNumber("navx_velocity_x",
self.navx.getVelocityX())
wpilib.SmartDashboard.putNumber("navx_velocity_y",
self.navx.getVelocityY())
wpilib.SmartDashboard.putNumber("navx_accel_x",
self.navx.getWorldLinearAccelX())
wpilib.SmartDashboard.putNumber("navx_accel_y",
self.navx.getWorldLinearAccelY())
wpilib.SmartDashboard.putNumber("navx_temp", self.navx.getTempC())
wpilib.SmartDashboard.putNumber("foc_button", self.foctoggle.get())
wpilib.SmartDashboard.putNumber("stick_x", self.stick.getX())
wpilib.SmartDashboard.putNumber("stick_y", self.stick.getY())
wpilib.SmartDashboard.putNumber("stick_z", self.stick.getZ())
class Robot(magicbot.MagicRobot):
# Automations
# TODO: bad name
seek_target: seek_target.SeekTarget
# Controllers
# recorder: recorder.Recorder
# Components
follower: trajectory_follower.TrajectoryFollower
drive: drive.Drive
lift: lift.Lift
hatch_manipulator: hatch_manipulator.HatchManipulator
cargo_manipulator: cargo_manipulator.CargoManipulator
climber: climber.Climber
ENCODER_PULSE_PER_REV = 1024
WHEEL_DIAMETER = 0.5
"""
manual_lift_control = tunable(True)
stabilize = tunable(False)
stabilizer_threshold = tunable(30)
stabilizer_aggression = tunable(5)
"""
"""
time = tunable(0)
voltage = tunable(0)
yaw = tunable(0)
"""
def createObjects(self):
"""
Initialize robot components.
"""
# For using teleop in autonomous
self.robot = self
# Joysticks
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
# Buttons
self.button_strafe_left = JoystickButton(self.joystick_left, 4)
self.button_strafe_right = JoystickButton(self.joystick_left, 5)
self.button_strafe_forward = JoystickButton(self.joystick_left, 3)
self.button_strafe_backward = JoystickButton(self.joystick_left, 2)
self.button_slow_rotation = JoystickButton(self.joystick_right, 4)
self.button_lift_actuate = ButtonDebouncer(self.joystick_alt, 2)
self.button_manual_lift_control = ButtonDebouncer(self.joystick_alt, 6)
self.button_hatch_kick = JoystickButton(self.joystick_alt, 1)
self.button_cargo_push = JoystickButton(self.joystick_alt, 5)
self.button_cargo_pull = JoystickButton(self.joystick_alt, 3)
self.button_cargo_pull_lightly = JoystickButton(self.joystick_alt, 4)
self.button_climb_front = JoystickButton(self.joystick_right, 3)
self.button_climb_back = JoystickButton(self.joystick_right, 2)
self.button_target = JoystickButton(self.joystick_right, 8)
# Drive motor controllers
# ID SCHEME:
# 10^1: 1 = left, 2 = right
# 10^0: 0 = front, 5 = rear
self.lf_motor = WPI_TalonSRX(10)
self.lr_motor = WPI_TalonSRX(15)
self.rf_motor = WPI_TalonSRX(20)
self.rr_motor = WPI_TalonSRX(25)
encoder_constant = ((1 / self.ENCODER_PULSE_PER_REV) *
self.WHEEL_DIAMETER * math.pi)
self.r_encoder = wpilib.Encoder(0, 1)
self.r_encoder.setDistancePerPulse(encoder_constant)
self.l_encoder = wpilib.Encoder(2, 3)
self.l_encoder.setDistancePerPulse(encoder_constant)
self.l_encoder.setReverseDirection(True)
# Drivetrain
self.train = wpilib.drive.MecanumDrive(self.lf_motor, self.lr_motor,
self.rf_motor, self.rr_motor)
# Functional motors
self.lift_motor = WPI_TalonSRX(40)
self.lift_motor.setSensorPhase(True)
self.lift_switch = wpilib.DigitalInput(4)
self.lift_solenoid = wpilib.DoubleSolenoid(2, 3)
self.hatch_solenoid = wpilib.DoubleSolenoid(0, 1)
self.left_cargo_intake_motor = WPI_TalonSRX(35)
# TODO: electricians soldered one motor in reverse.
# self.left_cargo_intake_motor.setInverted(True)
self.right_cargo_intake_motor = WPI_TalonSRX(30)
"""
self.cargo_intake_motors = wpilib.SpeedControllerGroup(self.left_cargo_intake_motor,
self.right_cargo_intake_motor)
"""
self.right_cargo_intake_motor.follow(self.left_cargo_intake_motor)
self.front_climb_piston = wpilib.DoubleSolenoid(4, 5)
self.back_climb_piston = wpilib.DoubleSolenoid(6, 7)
# Tank Drivetrain
"""
self.tank_train = wpilib.drive.DifferentialDrive(wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor),
wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor))
"""
# Load trajectories
self.generated_trajectories = load_trajectories()
# NavX
self.navx = navx.AHRS.create_spi()
self.navx.reset()
# Utility
# self.ds = wpilib.DriverStation.getInstance()
# self.timer = wpilib.Timer()
self.pdp = wpilib.PowerDistributionPanel(0)
self.compressor = wpilib.Compressor()
# Camera server
wpilib.CameraServer.launch('camera/camera.py:main')
wpilib.LiveWindow.disableAllTelemetry()
def robotPeriodic(self):
"""
Executed periodically regardless of mode.
"""
# self.time = int(self.timer.getMatchTime())
# self.voltage = self.pdp.getVoltage()
# self.yaw = self.navx.getAngle() % 360
pass
def autonomous(self):
"""
Prepare for and start autonomous mode.
"""
# Call autonomous
super().autonomous()
def disabledInit(self):
"""
Executed once right away when robot is disabled.
"""
# Reset Gyro to 0
self.navx.reset()
def disabledPeriodic(self):
"""
Executed periodically while robot is disabled.
Useful for testing.
"""
pass
def teleopInit(self):
"""
Executed when teleoperated mode begins.
"""
self.lift.zero = self.lift_motor.getSelectedSensorPosition()
self.lift.current_position = 5000000
self.compressor.start()
def teleopPeriodic(self):
"""
Executed periodically while robot is in teleoperated mode.
"""
# Read from joysticks and move drivetrain accordingly
self.drive.move(x=-self.joystick_left.getY(),
y=self.joystick_left.getX(),
rot=self.joystick_right.getX(),
real=True,
slow_rot=self.button_slow_rotation.get())
"""
self.drive.strafe(self.button_strafe_left.get(),
self.button_strafe_right.get(),
self.button_strafe_forward.get(),
self.button_strafe_backward.get())
"""
"""
for button in range(7, 12 + 1):
if self.joystick_alt.getRawButton(button):
self.lift.target(button)
"""
# if self.manual_lift_control:
self.lift.move(-self.joystick_alt.getY())
"""
else:
# self.lift.correct(-self.joystick_alt.getY())
# self.lift.approach()
pass
"""
"""
if self.button_manual_lift_control:
# self.manual_lift_control = not self.manual_lift_control
pass
"""
if self.button_hatch_kick.get():
self.hatch_manipulator.extend()
else:
self.hatch_manipulator.retract()
if self.button_target.get():
self.seek_target.seek()
if self.button_lift_actuate.get():
self.lift.actuate()
if self.button_cargo_push.get():
self.cargo_manipulator.push()
elif self.button_cargo_pull.get():
self.cargo_manipulator.pull()
elif self.button_cargo_pull_lightly.get():
self.cargo_manipulator.pull_lightly()
if self.button_climb_front.get():
self.climber.extend_front()
else:
self.climber.retract_front()
if self.button_climb_back.get():
self.climber.extend_back()
else:
self.climber.retract_back()
"""
class Lift():
# Encoder constants for the lift.
# TODO recalibrate.
POSITION_MAX = -6.624 * (10**4)
POSITION_HATCH_TOP = -3.265 * (10**4)
POSITION_HATCH_MIDDLE = -3.265 * (10**4)
POSITION_HATCH_BOTTOM = 0.000
POSITION_BALL_TOP = -6.500 * (10**4)
POSITION_BALL_MIDDLE = -3.184 * (10**4)
POSITION_BALL_BOTTOM = -0.000 * (10**4)
POSITION_HUMAN_PLAYER = -3.993 * (10**4)
# Initialize a new Lift instance.
# @param lift_talon - the motor for the lift.
# @param upper_switch - the upper limit switch.
# @param lower_switch - the lower limit switch.
# @param stick - the Joystick instance.
def __init__(self, lift_talon, upper_switch, lower_switch, stick):
self.lift_motor = lift_talon
self.upper_switch = upper_switch
self.lower_switch = lower_switch
# I don't think there's a better way to do this. Using an array will
# sacrifice too much readability for not much more conciseness.
self.manual_up = JoystickButton(stick, 4)
self.manual_down = JoystickButton(stick, 3)
self.ball_bottom = JoystickButton(stick, 5)
self.ball_middle = JoystickButton(stick, 6)
self.ball_top = JoystickButton(stick, 7)
self.hatch_bottom = JoystickButton(stick, 10)
self.hatch_middle = JoystickButton(stick, 9)
self.hatch_top = JoystickButton(stick, 8)
self.lift_disabler = JoystickButton(stick, 14)
self.state = "manual_stop"
def getTalon(self):
return self.lift_motor
# Rezero lift_motor encoder and tell it to use the 1st PID slot
def initSensor(self):
self.lift_motor.selectProfileSlot(0, 0)
self.lift_motor.setQuadraturePosition(0)
def state_manual_up(self):
self.lift_motor.set(WPI_TalonSRX.ControlMode.PercentOutput, -.8)
LEDController.getInstance().setState(LEDController.STATE_LIFT_UP)
def state_manual_down(self):
self.lift_motor.set(WPI_TalonSRX.ControlMode.PercentOutput, .45)
LEDController.getInstance().setState(LEDController.STATE_LIFT_DOWN)
def state_manual_stop(self):
self.lift_motor.set(WPI_TalonSRX.ControlMode.PercentOutput, -0.05)
def state_hatch_bottom(self):
self.lift_motor.set(WPI_TalonSRX.ControlMode.Position,
self.POSITION_HATCH_BOTTOM)
if abs(self.lift_motor.getSelectedSensorPosition() -
self.POSITION_HATCH_BOTTOM) > 10:
return
elif self.lift_motor.getSelectedSensorPosition(
) < self.POSITION_HATCH_BOTTOM:
LEDController.getInstance().setState(LEDController.STATE_LIFT_UP)
elif self.lift_motor.getSelectedSensorPosition(
) > self.POSITION_HATCH_BOTTOM:
LEDController.getInstance().setState(LEDController.STATE_LIFT_DOWN)
def state_hatch_middle(self):
self.lift_motor.set(WPI_TalonSRX.ControlMode.Position,
self.POSITION_HATCH_MIDDLE)
if abs(self.lift_motor.getSelectedSensorPosition() -
self.POSITION_HATCH_MIDDLE) > 10:
return
elif self.lift_motor.getSelectedSensorPosition(
) < self.POSITION_HATCH_MIDDLE:
LEDController.getInstance().setState(LEDController.STATE_LIFT_UP)
elif self.lift_motor.getSelectedSensorPosition(
) > self.POSITION_HATCH_MIDDLE:
LEDController.getInstance().setState(LEDController.STATE_LIFT_DOWN)
def state_hatch_top(self):
self.lift_motor.set(WPI_TalonSRX.ControlMode.Position,
self.POSITION_HATCH_TOP)
if abs(self.lift_motor.getSelectedSensorPosition() -
self.POSITION_HATCH_TOP) > 10:
return
elif self.lift_motor.getSelectedSensorPosition(
) < self.POSITION_HATCH_TOP:
LEDController.getInstance().setState(LEDController.STATE_LIFT_UP)
elif self.lift_motor.getSelectedSensorPosition(
) > self.POSITION_HATCH_TOP:
LEDController.getInstance().setState(LEDController.STATE_LIFT_DOWN)
def state_ball_bottom(self):
self.lift_motor.set(WPI_TalonSRX.ControlMode.Position,
self.POSITION_BALL_BOTTOM)
if abs(self.lift_motor.getSelectedSensorPosition() -
self.POSITION_BALL_BOTTOM) > 10:
return
elif self.lift_motor.getSelectedSensorPosition(
) < self.POSITION_BALL_BOTTOM:
LEDController.getInstance().setState(LEDController.STATE_LIFT_UP)
elif self.lift_motor.getSelectedSensorPosition(
) > self.POSITION_BALL_BOTTOM:
LEDController.getInstance().setState(LEDController.STATE_LIFT_DOWN)
def state_ball_middle(self):
self.lift_motor.set(WPI_TalonSRX.ControlMode.Position,
self.POSITION_BALL_MIDDLE)
if abs(self.lift_motor.getSelectedSensorPosition() -
self.POSITION_BALL_MIDDLE) > 10:
return
elif self.lift_motor.getSelectedSensorPosition(
) < self.POSITION_BALL_MIDDLE:
LEDController.getInstance().setState(LEDController.STATE_LIFT_UP)
elif self.lift_motor.getSelectedSensorPosition(
) > self.POSITION_BALL_MIDDLE:
LEDController.getInstance().setState(LEDController.STATE_LIFT_DOWN)
def state_ball_top(self):
self.lift_motor.set(WPI_TalonSRX.ControlMode.Position,
self.POSITION_BALL_TOP)
if abs(self.lift_motor.getSelectedSensorPosition() -
self.POSITION_BALL_TOP) > 10:
return
elif self.lift_motor.getSelectedSensorPosition(
) < self.POSITION_BALL_TOP:
LEDController.getInstance().setState(LEDController.STATE_LIFT_UP)
elif self.lift_motor.getSelectedSensorPosition(
) > self.POSITION_BALL_TOP:
LEDController.getInstance().setState(LEDController.STATE_LIFT_DOWN)
# Large table with all of the states. This is a dictionary that will let
# you look up functions by the name of the state.
state_table = {
"manual_up": state_manual_up,
"manual_down": state_manual_down,
"manual_stop": state_manual_stop,
"hatch_bottom": state_hatch_bottom,
"hatch_middle": state_hatch_middle,
"hatch_top": state_hatch_top,
"ball_bottom": state_ball_bottom,
"ball_middle": state_ball_middle,
"ball_top": state_ball_top
}
# update the lift.
def update(self):
if self.lift_disabler.get():
return
if self.ball_bottom.get():
self.state = "ball_bottom"
elif self.ball_middle.get():
self.state = "ball_middle"
elif self.ball_top.get():
self.state = "ball_top"
elif self.hatch_bottom.get():
self.state = "hatch_bottom"
elif self.hatch_middle.get():
self.state = "hatch_middle"
elif self.hatch_top.get():
self.state = "hatch_top"
# Manual up and down MUST OVERRIDE all of the auto lift states.
if self.manual_up.get():
self.state = "manual_up"
elif self.manual_down.get():
self.state = "manual_down"
elif self.state is "manual_up" or self.state is "manual_down":
self.state = "manual_stop"
# limit switch checks. If a limit switch is pressed, rezero the
# encoder and go into manual_stop state.
if not self.upper_switch.get():
self.lift_motor.setQuadraturePosition(int(self.POSITION_MAX))
if self.lift_motor.get() < -0.07 or self.state is "manual_up":
self.lift_motor.set(WPI_TalonSRX.ControlMode.PercentOutput, 0)
self.state = "manual_stop"
return
if not self.lower_switch.get():
self.lift_motor.setQuadraturePosition(0)
if self.lift_motor.get() > 0.07 or self.state is "manual_down":
self.lift_motor.set(WPI_TalonSRX.ControlMode.PercentOutput, 0)
self.state = "manual_stop"
return
# execute the state.
self.state_table[self.state](self)
# Log as much data as possible to SmartDashboard.
def log(self):
wpilib.SmartDashboard.putString("lift_state", self.state)
wpilib.SmartDashboard.putNumber(
"lift_position", self.lift_motor.getSelectedSensorPosition())
wpilib.SmartDashboard.putNumber("lift_velocity", self.lift_motor.get())
wpilib.SmartDashboard.putBoolean("manual_up_button",
self.manual_up.get())
wpilib.SmartDashboard.putBoolean("manual_down_button",
self.manual_down.get())
wpilib.SmartDashboard.putBoolean("ball_bottom_button",
self.ball_bottom.get())
wpilib.SmartDashboard.putBoolean("ball_middle_button",
self.ball_middle.get())
wpilib.SmartDashboard.putBoolean("ball_top_button",
self.ball_top.get())
wpilib.SmartDashboard.putBoolean("hatch_bottom_button",
self.hatch_bottom.get())
wpilib.SmartDashboard.putBoolean("hatch_middle_button",
self.hatch_middle.get())
wpilib.SmartDashboard.putBoolean("hatch_top_button",
self.hatch_top.get())
wpilib.SmartDashboard.putBoolean("upper_limit",
self.upper_switch.get())
wpilib.SmartDashboard.putBoolean("lower_limit",
self.lower_switch.get())
