def teleopInit(self):
self.gamepad.setRumble(
wpilib.interfaces.GenericHID.RumbleType.kRightRumble, 1)
def stop():
self.gamepad.setRumble(
wpilib.interfaces.GenericHID.RumbleType.kRightRumble, 0)
if self.isReal():
wpilib.Notifier(stop).startSingle(0.75)
def __init__(self, controller: Controller,
controller_output: Callable[[float], Any]) -> None:
self.controller_update = controller.update
self.controller_output = controller_output
self._this_mutex = threading.RLock()
# Ensures when disable() is called, self.controller_output() won't
# run if Controller.update() is already running at that time.
self._output_mutex = threading.RLock()
self.notifier = wpilib.Notifier(self._run)
self.notifier.startPeriodic(controller.period)
def init(left_encoder_callback,
right_encoder_callback,
gyro_callback=None,
current_pose=Pose(0, 0, 0),
wheelbase=None,
encoder_factor=1):
global _estimator, _estimator_thread
_estimator = PoseEstimator(left_encoder_callback, right_encoder_callback,
gyro_callback, current_pose, wheelbase,
encoder_factor)
if _estimator_thread is None and False:
dt = 20 / 1000
_estimator_thread = wpilib.Notifier(
run=lambda: _estimator.update(dt=dt))
_estimator_thread.startPeriodic(dt)
def createObjects(self):
"""Create magicbot components"""
# Allow player control in sandstorm (2019 specific)
self.use_teleop_in_autonomous = True
# Inputs
self.gamepad = wpilib.XboxController(0)
self.gamepad2 = wpilib.XboxController(1)
# Dashboard tabs
self.prefs = Shuffleboard.getTab("Preferences")
self.drive_tab = Shuffleboard.getTab("Drive")
self.debug_tab = Shuffleboard.getTab("Debugging")
# pi/jetson vision data
self.vision = NetworkTables.getTable("Vision")
self.cargo_yaw = self.vision.getEntry("cargoYaw")
self.tape_yaw = self.vision.getEntry("tapeYaw")
self.cargo_detected = self.vision.getEntry("cargoDetected")
self.tape_detected = self.vision.getEntry("tapeDetected")
self.tape_mode = self.vision.getEntry("tape")
# Drive motors
self.fl_drive = rev.CANSparkMax(2, rev.MotorType.kBrushless)
self.fr_drive = rev.CANSparkMax(3, rev.MotorType.kBrushless)
self.rl_drive = rev.CANSparkMax(4, rev.MotorType.kBrushless)
self.rr_drive = rev.CANSparkMax(5, rev.MotorType.kBrushless)
self.fl_drive_encoder = SparkMaxEncoder(self.fl_drive)
self.fr_drive_encoder = SparkMaxEncoder(self.fr_drive)
self.rl_drive_encoder = SparkMaxEncoder(self.rl_drive)
self.rr_drive_encoder = SparkMaxEncoder(self.rr_drive)
# Make the drive a little less jumpy
self.fl_drive.setOpenLoopRampRate(0.35)
self.fr_drive.setOpenLoopRampRate(0.35)
self.rl_drive.setOpenLoopRampRate(0.35)
self.rr_drive.setOpenLoopRampRate(0.35)
# Wheel groups for tank mode
self.left_drive = wpilib.SpeedControllerGroup(self.fl_drive,
self.rl_drive)
self.right_drive = wpilib.SpeedControllerGroup(self.fr_drive,
self.rr_drive)
# Drive trains
self.mecanum_drive = wpilib.drive.MecanumDrive(self.fl_drive,
self.rl_drive,
self.fr_drive,
self.rr_drive)
self.tank_drive = wpilib.drive.DifferentialDrive(
self.left_drive, self.right_drive)
# They can't tell the other is in control, so we just turn off the software safety
self.mecanum_drive.setSafetyEnabled(False)
self.tank_drive.setSafetyEnabled(False)
# Lift
# Comp
self.lift_motor = ctre.WPI_TalonSRX(9)
self.lift_follower = ctre.WPI_TalonSRX(8)
self.lift_follower.set(ctre.ControlMode.Follower, 9)
self.lift_motor.setInverted(True)
self.lift_follower.setInverted(True)
self.lift_encoder = ExternalEncoder(0, 1, reversed=False)
# Intake
self.wrist_motor = ctre.WPI_TalonSRX(10)
self.wrist_motor.setInverted(True)
self.intake_motor = ctre.WPI_TalonSRX(11)
# NOTE: Practice Bot (is this comment still relevant?)
self.wrist_encoder = AbsoluteMagneticEncoder(2)
# Intake grabber pistons
self.intake_grabber_piston = wpilib.DoubleSolenoid(4, 5)
# Pneumatics
self.compressor = wpilib.Compressor()
self.octacanum_shifter_front = wpilib.DoubleSolenoid(0, 1)
self.octacanum_shifter_rear = wpilib.DoubleSolenoid(2, 3)
# Default state is extended (mecanum)
self.octacanum_shifter_front.set(wpilib.DoubleSolenoid.Value.kForward)
self.octacanum_shifter_rear.set(wpilib.DoubleSolenoid.Value.kForward)
# Climbing
self.climb_piston = wpilib.DoubleSolenoid(6, 7)
self.climb_piston.set(wpilib.DoubleSolenoid.Value.kForward)
self.leg1 = rev.CANSparkMax(12, rev.MotorType.kBrushed)
# self.leg2 = rev.CANSparkMax(13, rev.MotorType.kBrushed)
# self.leg1 = ctre.WPI_TalonSRX(12)
# self.leg2 = ctre.WPI_TalonSRX(13)
# self.leg_drive = ctre.WPI_TalonSRX(17)
self.leg_drive = rev.CANSparkMax(17, rev.MotorType.kBrushed)
# Misc components
self.navx = navx.AHRS.create_spi()
# PDP for monitoring power usage
# WARN: Causes drive to stutter
# self.pdp = wpilib.PowerDistributionPanel(0)
# self.pdp.clearStickyFaults()
# self.debug_tab.add(title="PDP", value=self.pdp)
# WARN: Causes drive to stutter
# self.debug_tab.add(self.mecanum_drive)
# self.debug_tab.add(self.tank_drive)
encoders_list = self.debug_tab.getLayout("List", "Drive Encoders")
encoders_list.add(title="Front Left", value=self.fl_drive_encoder)
encoders_list.add(title="Front Right", value=self.fr_drive_encoder)
encoders_list.add(title="Rear Left", value=self.rl_drive_encoder)
encoders_list.add(title="Rear Right", value=self.rr_drive_encoder)
self.debug_tab.add(title="Lift Encoder", value=self.lift_encoder)
self.wrist_pos_dashboard = self.debug_tab.add(
value=0, title="Wrist Pos").getEntry()
# Launch camera server
# Disabled: Vision sent through Jetson/Pi
wpilib.CameraServer.launch()
# Connect to ardunio controlled leds
self.led_manager = LEDManager()
if self.isReal():
wpilib.Notifier(
lambda: self.led_manager.alliance_fader()).startSingle(2)
def teleopPeriodic(self):
# TODO: Fix for bug in wpilib
wpilib.shuffleboard.Shuffleboard.update()
self.slow = self.gamepad.getAButton()
if self.gamepad.getStickButtonPressed(GenericHID.Hand.kRight):
self.fod = not self.fod
if self.fod:
self.gamepad.setRumble(
wpilib.interfaces.GenericHID.RumbleType.kLeftRumble, 1)
def stop():
self.gamepad.setRumble(
wpilib.interfaces.GenericHID.RumbleType.kLeftRumble, 0)
if wpilib.robotbase.RobotBase.isReal():
wpilib.Notifier(stop).startSingle(0.25)
# self.led_manager.set_fast(self.fast)
if self.gamepad.getBumperPressed(
GenericHID.Hand.kRight): # TODO: Change id
self.drive_mode = self.drive_mode.toggle()
auto = self.gamepad.getBButton()
self.tape_align_ctrl.set_enabled(auto)
if not auto:
if self.drive_mode == DriveMode.MECANUM:
forward_speed = -self.gamepad.getY(GenericHID.Hand.kRight)
if self.slow:
forward_speed *= 0.75
# else:
# forward_speed *= 0.9
strafe_mult = 0.76 if self.slow else 1
# turn_mult = 0.65 if self.slow else 0.75
turn_mult = 0.65 if self.slow else 0.75
self.drive.drive_mecanum(
self.gamepad.getX(GenericHID.Hand.kRight) * strafe_mult,
forward_speed,
self.gamepad.getX(GenericHID.Hand.kLeft) * turn_mult,
fod=self.fod,
)
else:
if self.slow:
self.drive.drive_tank(
-self.gamepad.getY(GenericHID.Hand.kRight) * 0.75,
self.gamepad.getX(GenericHID.Hand.kLeft) * 0.75,
)
else:
self.drive.drive_tank(
-self.gamepad.getY(GenericHID.Hand.kRight),
self.gamepad.getX(GenericHID.Hand.kLeft),
)
pov = self.gamepad2.getPOV()
if pov == 180: # Down (Minimum)
self.lift.set_setpoint(200)
elif pov == 270: # Left
self.lift.set_setpoint(380.5)
elif pov == 0: # Up
self.lift.set_setpoint(1180.5)
elif pov == 90: # Right
self.lift.set_setpoint(2028) # max
if self.gamepad2.getYButton():
self.lift.set_setpoint(575)
elif self.gamepad2.getXButton():
self.lift.set_setpoint(420)
setpoint = self.lift.get_setpoint()
if self.gamepad2.getTriggerAxis(GenericHID.Hand.kRight) > 0.02:
self.lift.set_setpoint(setpoint + (
self.gamepad2.getTriggerAxis(GenericHID.Hand.kRight) * 85))
if self.gamepad2.getTriggerAxis(GenericHID.Hand.kLeft) > 0.02:
self.lift.set_setpoint(setpoint - (
self.gamepad2.getTriggerAxis(GenericHID.Hand.kLeft) * 85))
self.intake.set_speed(-self.gamepad2.getY(GenericHID.Hand.kLeft))
wrist_setpoint_adj = RobotDriveBase.applyDeadband(
self.gamepad2.getY(GenericHID.Hand.kRight) * 0.5, 0.15)
self.intake.set_wrist_setpoint(
self.intake.pid_controller.getSetpoint() -
(wrist_setpoint_adj * 15))
if self.gamepad.getBumperPressed(
GenericHID.Hand.kLeft) or self.gamepad2.getBumperPressed(
GenericHID.Hand.kLeft):
self.intake.toggle_grab()
if self.gamepad.getYButton():
self.drive.zero_fod()
if self.gamepad.getBackButton():
self.compressor.stop()
if self.gamepad.getStartButton():
self.compressor.start()
if self.gamepad.getXButton():
self.climb_piston.set(wpilib.DoubleSolenoid.Value.kReverse)
else:
self.climb_piston.set(wpilib.DoubleSolenoid.Value.kForward)
if self.gamepad2.getAButton():
self.intake.set_defense()
leg_speed = -marsutils.math.signed_square(
(self.gamepad.getTriggerAxis(GenericHID.Hand.kRight) +
-self.gamepad.getTriggerAxis(GenericHID.Hand.kLeft)))
self.leg1.set(leg_speed)
self.leg2.set(leg_speed)
if self.gamepad.getXButton():
# self.leg_drive.set(-self.gamepad.getY(GenericHID.Hand.kRight) * 50)
self.leg_drive.set(-1)
else:
self.leg_drive.set(0)
def teleopPeriodic(self):
# TODO: Fix for bug in wpilib (this shouldn't be needed anymore)
wpilib.shuffleboard.Shuffleboard.update()
self.slow = self.gamepad.getAButton()
# Toggle field-oriented-drive with the right stick button
if self.gamepad.getStickButtonPressed(GenericHID.Hand.kLeft):
self.fod = not self.fod
if self.fod:
self.gamepad.setRumble(
wpilib.interfaces.GenericHID.RumbleType.kLeftRumble, 1)
def stop():
self.gamepad.setRumble(
wpilib.interfaces.GenericHID.RumbleType.kLeftRumble, 0)
if wpilib.robotbase.RobotBase.isReal():
wpilib.Notifier(stop).startSingle(0.25)
# self.led_manager.set_fast(self.fast)
if self.gamepad.getBumperPressed(GenericHID.Hand.kLeft):
self.drive_mode = self.drive_mode.toggle()
# enable auto target seeking
auto = self.gamepad.getBButton()
self.tape_align_ctrl.set_enabled(auto)
if not auto:
if self.drive_mode == DriveMode.MECANUM:
forward_speed = -self.gamepad.getY(GenericHID.Hand.kLeft)
if self.slow:
forward_speed *= 0.75
# else:
# forward_speed *= 0.9
strafe_mult = 0.76 if self.slow else 1
# turn_mult = 0.65 if self.slow else 0.75
# Reduce the turn input even without slow mode because Kevin is *really fast*
turn_mult = 0.65 if self.slow else 0.75
self.drive.drive_mecanum(
self.gamepad.getX(GenericHID.Hand.kLeft) * strafe_mult,
forward_speed,
self.gamepad.getX(GenericHID.Hand.kRight) * turn_mult,
fod=self.fod,
)
else:
if self.slow:
self.drive.drive_tank(
-self.gamepad.getY(GenericHID.Hand.kLeft) * 0.75,
self.gamepad.getX(GenericHID.Hand.kRight) * 0.75,
)
else:
self.drive.drive_tank(
-self.gamepad.getY(GenericHID.Hand.kLeft),
self.gamepad.getX(GenericHID.Hand.kRight),
)
pov = self.gamepad2.getPOV()
if pov == 180: # Down (lowest rocket hatch height)
self.lift.set_setpoint(200)
elif pov == 270: # Left
self.lift.set_setpoint(380.5)
elif pov == 0: # Up
self.lift.set_setpoint(1180.5)
elif pov == 90: # Right
self.lift.set_setpoint(2028) # max
if self.gamepad2.getYButton():
self.lift.set_setpoint(575)
elif self.gamepad2.getXButton():
self.lift.set_setpoint(420)
# manual adjustment of the setpoint with analog triggers
setpoint = self.lift.get_setpoint()
if self.gamepad2.getTriggerAxis(GenericHID.Hand.kRight) > 0.02:
self.lift.set_setpoint(setpoint + (
self.gamepad2.getTriggerAxis(GenericHID.Hand.kRight) * 85))
if self.gamepad2.getTriggerAxis(GenericHID.Hand.kLeft) > 0.02:
self.lift.set_setpoint(setpoint - (
self.gamepad2.getTriggerAxis(GenericHID.Hand.kLeft) * 85))
self.intake.set_speed(-self.gamepad2.getY(GenericHID.Hand.kLeft))
wrist_setpoint_adj = RobotDriveBase.applyDeadband(
self.gamepad2.getY(GenericHID.Hand.kRight) * 0.5, 0.15)
self.intake.set_wrist_setpoint(
self.intake.pid_controller.getSetpoint() -
(wrist_setpoint_adj * 15))
if self.gamepad.getBumperPressed(
GenericHID.Hand.kLeft) or self.gamepad2.getBumperPressed(
GenericHID.Hand.kLeft):
self.intake.toggle_grab()
if self.gamepad.getYButton():
self.drive.zero_fod()
if self.gamepad.getBackButton():
self.compressor.stop()
if self.gamepad.getStartButton():
self.compressor.start()
if self.gamepad.getXButton():
self.climb_piston.set(wpilib.DoubleSolenoid.Value.kReverse)
else:
self.climb_piston.set(wpilib.DoubleSolenoid.Value.kForward)
if self.gamepad2.getAButton():
self.intake.set_defense()
leg_speed = -marsutils.math.signed_square(
(self.gamepad.getTriggerAxis(GenericHID.Hand.kRight) +
-self.gamepad.getTriggerAxis(GenericHID.Hand.kLeft)))
# The "knee", moves the legs down
self.leg1.set(leg_speed)
self.leg2.set(leg_speed)
# The leg's wheels
if self.gamepad.getXButton():
# self.leg_drive.set(-self.gamepad.getY(GenericHID.Hand.kRight) * 50)
self.leg_drive.set(-1)
else:
self.leg_drive.set(0)
def createObjects(self):
"""Create magicbot components"""
# Use robot in sandstorm
self.use_teleop_in_autonomous = True
# Inputs
self.gamepad = wpilib.XboxController(0)
self.gamepad2 = wpilib.XboxController(1)
# Dashboard items
self.prefs = Shuffleboard.getTab("Preferences")
self.drive_tab = Shuffleboard.getTab("Drive")
self.debug_tab = Shuffleboard.getTab("Debugging")
self.curiosity_compat = (self.prefs.addPersistent(
"curiosity_compat", False).withWidget("Toggle Box").getEntry())
self.vision = NetworkTables.getTable("Vision")
self.cargo_yaw = self.vision.getEntry("cargoYaw")
self.tape_yaw = self.vision.getEntry("tapeYaw")
self.cargo_detected = self.vision.getEntry("cargoDetected")
self.tape_detected = self.vision.getEntry("tapeDetected")
self.tape_mode = self.vision.getEntry("tape")
# Drive motors
# Curisoity has talons, we can use it for testing
if self.curiosity_compat.get():
self.fl_drive = ctre.WPI_TalonSRX(10)
self.fr_drive = ctre.WPI_TalonSRX(11)
self.rl_drive = ctre.WPI_TalonSRX(12)
self.rr_drive = ctre.WPI_TalonSRX(13)
# TODO: Spark max does not have sim support yet, use talons instead for now
elif self.isSimulation():
self.fl_drive = ctre.WPI_TalonSRX(2)
self.fr_drive = ctre.WPI_TalonSRX(3)
self.rl_drive = ctre.WPI_TalonSRX(4)
self.rr_drive = ctre.WPI_TalonSRX(5)
self.fl_drive_encoder = CANTalonQuadEncoder(self.fl_drive)
self.fr_drive_encoder = CANTalonQuadEncoder(self.fr_drive)
self.rl_drive_encoder = CANTalonQuadEncoder(self.rl_drive)
self.rr_drive_encoder = CANTalonQuadEncoder(self.rr_drive)
else:
self.fl_drive = rev.CANSparkMax(2, rev.MotorType.kBrushless)
self.fr_drive = rev.CANSparkMax(3, rev.MotorType.kBrushless)
self.rl_drive = rev.CANSparkMax(4, rev.MotorType.kBrushless)
self.rr_drive = rev.CANSparkMax(5, rev.MotorType.kBrushless)
self.fl_drive_encoder = SparkMaxEncoder(self.fl_drive)
self.fr_drive_encoder = SparkMaxEncoder(self.fr_drive)
self.rl_drive_encoder = SparkMaxEncoder(self.rl_drive)
self.rr_drive_encoder = SparkMaxEncoder(self.rr_drive)
self.fl_drive.setOpenLoopRampRate(0.35)
self.fr_drive.setOpenLoopRampRate(0.35)
self.rl_drive.setOpenLoopRampRate(0.35)
self.rr_drive.setOpenLoopRampRate(0.35)
# left
self.left_drive = wpilib.SpeedControllerGroup(self.fl_drive,
self.rl_drive)
# right
self.right_drive = wpilib.SpeedControllerGroup(self.fr_drive,
self.rr_drive)
# Drive trains
self.mecanum_drive = wpilib.drive.MecanumDrive(self.fl_drive,
self.rl_drive,
self.fr_drive,
self.rr_drive)
self.tank_drive = wpilib.drive.DifferentialDrive(
self.left_drive, self.right_drive)
self.mecanum_drive.setSafetyEnabled(False)
self.tank_drive.setSafetyEnabled(False)
# Lift
# TODO: IMPORTANT PRACTICE BOT vs COMP
# Practice
# self.lift_motor = ctre.WPI_VictorSPX(8)
# self.lift_follower = ctre.WPI_VictorSPX(9)
# self.lift_follower.set(ctre.ControlMode.Follower, 8)
# self.lift_encoder = ExternalEncoder(0, 1, reversed=True)
# Comp
self.lift_motor = ctre.WPI_TalonSRX(9)
self.lift_follower = ctre.WPI_TalonSRX(8)
self.lift_follower.set(ctre.ControlMode.Follower, 9)
self.lift_motor.setInverted(True)
self.lift_follower.setInverted(True)
self.lift_encoder = ExternalEncoder(0, 1, reversed=False)
# Intake
self.wrist_motor = ctre.WPI_TalonSRX(10)
self.wrist_motor.setInverted(True)
self.intake_motor = ctre.WPI_TalonSRX(11)
# NOTE: Practice Bot
self.wrist_encoder = AbsoluteMagneticEncoder(2)
# Intake pistons
# self.intake_piston = wpilib.DoubleSolenoid(4, 5)
# Intake grabber pistons
self.intake_grabber_piston = wpilib.DoubleSolenoid(4, 5)
# Pneumatics
self.compressor = wpilib.Compressor()
self.octacanum_shifter_front = wpilib.DoubleSolenoid(0, 1)
self.octacanum_shifter_rear = wpilib.DoubleSolenoid(2, 3)
# Default state is extended (mecanum)
self.octacanum_shifter_front.set(wpilib.DoubleSolenoid.Value.kForward)
self.octacanum_shifter_rear.set(wpilib.DoubleSolenoid.Value.kForward)
# Climbing
self.climb_piston = wpilib.DoubleSolenoid(6, 7)
self.climb_piston.set(wpilib.DoubleSolenoid.Value.kForward)
# self.leg1 = rev.CANSparkMax(12, rev.MotorType.kBrushed)
# self.leg2 = rev.CANSparkMax(13, rev.MotorType.kBrushed)
self.leg1 = ctre.WPI_TalonSRX(12)
self.leg2 = ctre.WPI_TalonSRX(13)
# self.leg_drive = ctre.WPI_TalonSRX(17)
self.leg_drive = rev.CANSparkMax(17, rev.MotorType.kBrushed)
# Misc components
self.navx = navx.AHRS.create_spi()
# PDP for monitoring power usage
# NOTE: Causes drive stutter
# self.pdp = wpilib.PowerDistributionPanel(0)
# self.pdp.clearStickyFaults()
# self.debug_tab.add(title="PDP", value=self.pdp)
# NOTE: Causes drive stutter
# self.debug_tab.add(self.mecanum_drive)
# self.debug_tab.add(self.tank_drive)
encoders_list = self.debug_tab.getLayout("List", "Drive Encoders")
encoders_list.add(title="Front Left", value=self.fl_drive_encoder)
encoders_list.add(title="Front Right", value=self.fr_drive_encoder)
encoders_list.add(title="Rear Left", value=self.rl_drive_encoder)
encoders_list.add(title="Rear Right", value=self.rr_drive_encoder)
self.debug_tab.add(title="Lift Encoder", value=self.lift_encoder)
self.wrist_pos_dashboard = self.debug_tab.add(
value=0, title="Wrist Pos").getEntry()
# Launch camera server
# Disabled: Vision sent through Jetson/Pi
wpilib.CameraServer.launch()
# Connect to ardunio controlled leds
self.led_manager = LEDManager()
if self.isReal():
wpilib.Notifier(
lambda: self.led_manager.alliance_fader()).startSingle(2)
def create_notifier(run):
return wpilib.Notifier(run)
