def setupDashboardCommands(self):
# Set up auton chooser
self.autonChooser = SendableChooser()
self.autonChooser.setDefaultOption(
"Do Nothing", wpilib.command.WaitCommand(3.0, "Do Nothing"))
self.autonChooser.addOption("Drive Forward", DriveTickTimed(1.0, 1.0))
self.autonChooser.addOption("Drive Backward",
DriveTickTimed(-1.0, -1.0))
self.autonChooser.addOption("Rotate Right", DriveTickTimed(1.0, -1.0))
self.autonChooser.addOption("Rotate Left", DriveTickTimed(-1.0, 1.0))
SmartDashboard.putData("Auto mode", self.autonChooser)
# Set up utility controls
SmartDashboard.putData("Measure", Measure())
# Drive controls
DriveHuman.setupDashboardControls()
SmartDashboard.putData("Brake Control",
DriveHuman.createBrakeModeToggleCommand())
SmartDashboard.putData("Flip Front",
DriveHuman.createFlipFrontCommand())
# Debug tools (if enabled)
if self.debug:
SmartDashboard.putData("CPU Load Test", LoadTest())
SmartDashboard.putData("Drive Subsystem", subsystems.drive)
dd = subsystems.drive.getDifferentialDrive()
if dd != None:
SmartDashboard.putData("DifferentialDrive", dd)
def robotInit(self):
'''
This is a good place to set up your subsystems and anything else that
you will need to access later.
'''
wpilib.CameraServer.launch()
self.lastAuto = False
Command.getRobot = lambda x=0: self
# Load system preferences prior to constructing
# subsystems
map.loadPreferences()
# Construct subsystems prior to constructing commands
#self.limelight = Limelight.Limelight(self) #not a subsystem
self.hatch = HatchMech.HatchMech(self)
self.hatch.initialize()
self.cargo = CargoMech.CargoMech() #not a subsystem
self.cargo.initialize()
self.climber = Climber.Climber() #not a subsystem
self.climber.initialize(self)
self.drive = Drive.Drive(self)
self.compressor = wpilib.Compressor(0)
self.timer = wpilib.Timer()
self.timer.start()
self.watch = wpilib.Watchdog(150, None)
'''
Since OI instantiates commands and commands need access to subsystems,
OI must be initialized after subsystems.
'''
[self.joystick0, self.joystick1, self.xbox] = oi.commands()
self.updateDashboardInit()
self.DriveStraight = DriveStraight()
self.DriveStraightSide = DriveStraightSide()
self.LeftCargo = LeftCargo()
self.RightCargo = RightCargo()
self.CenterCargo = CenterCargo()
self.SetSpeedDT = SetSpeedDT()
self.CenterCargoPart2 = CenterCargoPart2()
# Set up auton chooser
self.autonChooser = SendableChooser()
self.autonChooser.setDefaultOption("Drive Straight", "DriveStraight")
self.autonChooser.addOption("Left Cargo", "LeftCargo")
self.autonChooser.addOption("Right Cargo", "RightCargo")
self.autonChooser.addOption("Do Nothing", "DoNothing")
self.autonChooser.addOption("Level 1 Center", "Level1Center")
self.autonChooser.addOption("Drive Straight Side", "DriveStraightSide")
SmartDashboard.putData("Auto mode", self.autonChooser)
def _create_smartdashboard_buttons(self):
self._auto_program_chooser = SendableChooser()
#self._auto_program_chooser.addDefault("Cross Line", 1)
#self._auto_program_chooser.addObject("Hang Gear", 2)
self._auto_program_chooser.addObject("Do Nothing", 3)
SmartDashboard.putData("Autonomous", self._auto_program_chooser)
self._starting_chooser = SendableChooser()
self._starting_chooser.addDefault("Left", 1)
self._starting_chooser.addObject("Center", 2)
self._starting_chooser.addObject("Right", 3)
SmartDashboard.putData("Starting_Position", self._starting_chooser)
def sendableChooser(self):
self.startingChooser = SendableChooser()
self.startingChooser.addDefault('Move Forward Only', '!')
self.startingChooser.addObject('Starting Left', 'L')
self.startingChooser.addObject('Starting Middle', 'M')
self.startingChooser.addObject('Starting Right', 'R')
wpilib.SmartDashboard.putData('Starting Side', self.startingChooser)
self.startingDelayChooser = SendableChooser()
self.startingDelayChooser.addDefault('0', 0)
self.startingDelayChooser.addObject('1', 1)
self.startingDelayChooser.addObject('2', 2)
self.startingDelayChooser.addObject('3', 3)
self.startingDelayChooser.addObject('4', 4)
self.startingDelayChooser.addObject('5', 5)
self.startingDelayChooser.addObject('6', 6)
self.startingDelayChooser.addObject('7', 7)
self.startingDelayChooser.addObject('8', 8)
self.startingDelayChooser.addObject('9', 9)
self.startingDelayChooser.addObject('10', 10)
self.startingDelayChooser.addObject('11', 11)
self.startingDelayChooser.addObject('12', 12)
self.startingDelayChooser.addObject('13', 13)
self.startingDelayChooser.addObject('14', 14)
self.startingDelayChooser.addObject('15', 15)
wpilib.SmartDashboard.putData('Delay Time(sec)', self.startingDelayChooser)
self.switchOrScale = SendableChooser()
self.switchOrScale.addDefault('Switch', 'Switch')
self.switchOrScale.addObject('Scale', 'Scale')
wpilib.SmartDashboard.putData('Switch or Scale', self.switchOrScale)
def __init__(self, robot):
self.robot = robot
# update OI with logs
Event_Manager.add_listeners({
'auto.periodic' : self.log,
'teleop.periodic' : self.log
})
# notify listeners when SmartDashboard is updated
sd = NetworkTable.getTable('SmartDashboard')
sd.addTableListener(self.dashboard_listener, True)
# add autonomous mode chooser to smartdashboard
self.auto_choose = SendableChooser()
self.auto_choose.addObject('Auto Do Nothing', Autonomous.Modes.DO_NOTHING)
self.auto_choose.addObject('Auto Move Forward', Autonomous.Modes.MOVE_FORWARD)
self.auto_choose.addObject('Auto One Object', Autonomous.Modes.ONE_OBJECT)
SmartDashboard.putData('Autonomous Mode', self.auto_choose)
# joystick events
self.joystick = Joystick(0)
l_bumper = Joystick_Button(self.joystick, lc.L_BUMPER)
l_trigger = Joystick_Button(self.joystick, lc.L_TRIGGER)
r_bumper = Joystick_Button(self.joystick, lc.R_BUMPER)
r_trigger = Joystick_Button(self.joystick, lc.R_TRIGGER)
btn_one = Joystick_Button(self.joystick, 1)
btn_two = Joystick_Button(self.joystick, 2)
# update the joysick axis periodically
Event_Manager.add_listener('teleop.periodic', self.update_axis)
l_bumper.while_pressed(lambda: Event_Manager.trigger('joystick.l_bumper.while_pressed'))
l_trigger.while_pressed(lambda: Event_Manager.trigger('joystick.l_trigger.while_pressed'))
l_bumper.when_pressed(lambda: Event_Manager.trigger('joystick.l_bumper.when_pressed'))
l_trigger.when_pressed(lambda: Event_Manager.trigger('joystick.l_trigger.when_pressed'))
l_bumper.when_released(lambda: Event_Manager.trigger('joystick.l_bumper.when_released'))
l_trigger.when_released(lambda: Event_Manager.trigger('joystick.l_trigger.when_released'))
r_bumper.when_pressed(lambda: Event_Manager.trigger('joystick.r_bumper.when_pressed'))
btn_two.when_pressed(lambda: Event_Manager.trigger('joystick.btn_two.when_pressed'))
btn_one.when_pressed(lambda: Event_Manager.trigger('joystick.btn_one.when_pressed'))
class OI:
"""
This class is the glue that binds the controls on the physical operator
interface to the commands and command groups that allow control of the robot.
"""
_config = None
_command_config = None
_controllers = []
_auto_program_chooser = None
_starting_chooser = None
def __init__(self,
robot,
configfile='/home/lvuser/py/configs/joysticks.ini'):
self.robot = robot
self._config = configparser.ConfigParser()
self._config.read(configfile)
self._init_joystick_binding()
for i in range(2):
self._controllers.append(self._init_joystick(i))
self._create_smartdashboard_buttons()
def setup_button_bindings(self):
#release_gear_a_button = JoystickButton(self._controllers[UserController.SCORING], JoystickButtons.A)
#release_gear_a_button.whileHeld(ReleaseGear(self.robot))
pass
def get_axis(self, user, axis):
"""Read axis value for specified controller/axis.
Args:
user: Controller ID to read from
axis: Axis ID to read from.
Return:
Current position for the specified axis. (Range [-1.0, 1.0])
"""
controller = self._controllers[user]
value = 0.0
if axis == JoystickAxis.DPADX:
value = controller.getPOV()
if value == 90:
value = 1.0
elif value == 270:
value = -1.0
else:
value = 0.0
elif axis == JoystickAxis.DPADY:
value = controller.getPOV()
if value == 0:
value = -1.0
elif value == 180:
value = 1.0
else:
value = 0.0
else:
config_section = "JoyConfig" + str(user)
dead_zone = self._config.getfloat(config_section, "DEAD_ZONE")
value = controller.getRawAxis(axis)
if abs(value) < dead_zone:
value = 0.0
return value
def get_button_state(self, user, button):
return self._controllers[user].getRawButton(button)
def _create_smartdashboard_buttons(self):
self._auto_program_chooser = SendableChooser()
#self._auto_program_chooser.addDefault("Cross Line", 1)
#self._auto_program_chooser.addObject("Hang Gear", 2)
self._auto_program_chooser.addObject("Do Nothing", 3)
SmartDashboard.putData("Autonomous", self._auto_program_chooser)
self._starting_chooser = SendableChooser()
self._starting_chooser.addDefault("Left", 1)
self._starting_chooser.addObject("Center", 2)
self._starting_chooser.addObject("Right", 3)
SmartDashboard.putData("Starting_Position", self._starting_chooser)
def get_auto_choice(self):
return self._auto_program_chooser.getSelected()
def get_position(self):
value = self._starting_chooser.getSelected()
return value
def _init_joystick(self, driver):
config_section = "JoyConfig" + str(driver)
stick = wpilib.Joystick(self._config.getint(config_section, "PORT"),
self._config.getint(config_section, "AXES"),
self._config.getint(config_section, "BUTTONS"))
return stick
def _init_joystick_binding(self):
axis_binding_section = "AxisBindings"
JoystickAxis.LEFTX = self._config.getint(axis_binding_section, "LEFTX")
JoystickAxis.LEFTY = self._config.getint(axis_binding_section, "LEFTY")
JoystickAxis.RIGHTX = self._config.getint(axis_binding_section,
"RIGHTX")
JoystickAxis.RIGHTY = self._config.getint(axis_binding_section,
"RIGHTY")
JoystickAxis.DPADX = self._config.getint(axis_binding_section, "DPADX")
JoystickAxis.DPADY = self._config.getint(axis_binding_section, "DPADY")
button_binding_section = "ButtonBindings"
JoystickButtons.X = self._config.getint(button_binding_section, "X")
JoystickButtons.A = self._config.getint(button_binding_section, "A")
JoystickButtons.B = self._config.getint(button_binding_section, "B")
JoystickButtons.Y = self._config.getint(button_binding_section, "Y")
JoystickButtons.LEFTBUMPER = self._config.getint(
button_binding_section, "LEFTBUMPER")
JoystickButtons.RIGHTBUMPER = self._config.getint(
button_binding_section, "RIGHTBUMPER")
JoystickButtons.LEFTTRIGGER = self._config.getint(
button_binding_section, "LEFTTRIGGER")
JoystickButtons.RIGHTTRIGGER = self._config.getint(
button_binding_section, "RIGHTTRIGGER")
JoystickButtons.BACK = self._config.getint(button_binding_section,
"BACK")
JoystickButtons.START = self._config.getint(button_binding_section,
"START")
def __init__(self, robot):
self.robot = robot
# Controllers
# Xbox
self.xbox_controller_1 = controls.xbox_controller.Xbox_Controller(0)
self.xbox_controller_2 = controls.xbox_controller.Xbox_Controller(1)
## COMMANDS ##
##Controller 1 Commands
# DRIVE COMMANDS
self.drive_command = commands.drivetrain.Drive_With_Tank_Values(
self.robot,
self._get_axis(self.xbox_controller_1,
controls.xbox_controller.XBOX_AXIS_RIGHT_X,
inverted=INVERT_XBOX_RIGHT_X),
self._get_axis(self.xbox_controller_1,
controls.xbox_controller.XBOX_AXIS_LEFT_Y,
inverted=INVERT_XBOX_LEFT_Y),
self._get_axis(self.xbox_controller_1,
controls.xbox_controller.XBOX_AXIS_LEFT_X,
inverted=INVERT_XBOX_LEFT_X),
)
self.robot.drive.setDefaultCommand(self.drive_command)
self.auto_choose = SendableChooser()
self.auto_choose.addObject('Do Nothing',
commands.autonomous.Do_Nothing(self.robot))
self.auto_choose.addDefault(
'Cross Baseline', commands.autonomous.Cross_Baseline(self.robot))
self.auto_choose.addObject(
'Center Gear',
commands.autonomous.Move_To_Gear(self.robot,
const.ID_AUTO_CENTER_GEAR))
self.auto_choose.addDefault(
'Center Gear No Camera',
commands.autonomous.Center_Gear_Without_Camera(self.robot))
self.auto_choose.addObject(
'Right Gear',
commands.autonomous.Move_To_Gear(self.robot,
const.ID_AUTO_RIGHT_GEAR))
self.auto_choose.addObject(
'Right Gear And Shoot Red',
commands.autonomous.Move_To_Gear_And_Shoot(self.robot,
const.ID_AUTO_RED_SIDE))
self.auto_choose.addObject(
'Left Gear',
commands.autonomous.Move_To_Gear(self.robot,
const.ID_AUTO_LEFT_GEAR))
self.auto_choose.addObject(
'Left Gear And Shoot Blue',
commands.autonomous.Move_To_Gear_And_Shoot(
self.robot, const.ID_AUTO_BLUE_SIDE))
self.auto_choose.addObject(
'Shoot From Hopper Red',
commands.autonomous.Shoot_From_Hopper(self.robot,
const.ID_AUTO_RED_SIDE))
self.auto_choose.addObject(
'Shoot From Hopper Blue',
commands.autonomous.Shoot_From_Hopper(self.robot,
const.ID_AUTO_BLUE_SIDE))
self.auto_choose.addObject(
'Shoot From Start',
commands.autonomous.Shoot_From_Start(self.robot))
SmartDashboard.putData('Autonomous Mode', self.auto_choose)
# Toggle Drive State
self.button_drive_shift = Xbox_Trigger(self.xbox_controller_1,
XBOX_BTN_LEFT_TRIGGER)
self.button_drive_shift.whenPressed(
commands.drivetrain.Toggle_Drive_State(self.robot))
# Toggle drivetrain gear
if not const.DEMO_MODE:
self.button_gear_shift = Xbox_Button(self.xbox_controller_1,
XBOX_BTN_LEFT_BUMPER)
self.button_gear_shift.whenPressed(
commands.drivetrain.Toggle_Gear_State(self.robot))
# Slow speed for driving in Y direction so that we slow down before the gear peg
self.button_slow_driving = Xbox_Trigger(self.xbox_controller_1,
XBOX_BTN_RIGHT_TRIGGER)
self.button_slow_driving.whenPressed(
commands.drivetrain.Set_Sensitivity_Low(self.robot))
self.button_slow_driving.whenReleased(
commands.drivetrain.Set_Sensitivity_High(self.robot))
# Drives backwards small distance, to align better for gear feeding
self.button_drive_gear_feeder_reverse = Xbox_Button(
self.xbox_controller_1, XBOX_BTN_X)
self.button_drive_gear_feeder_reverse.whenPressed(
commands.drivetrain.Back_Up(self.robot))
if not const.DEMO_MODE:
# For testing
self.button_rotate_to_boiler = Xbox_Button(self.xbox_controller_1,
XBOX_BTN_Y)
self.button_rotate_to_boiler.whenPressed(
commands.drivetrain.Rotate_To_Boiler(self.robot))
# For testing
self.rotate = Xbox_Button(self.xbox_controller_1,
XBOX_BTN_RIGHT_BUMPER)
self.rotate.whenPressed(
commands.drivetrain.Rotate_To_Angle(self.robot, -52))
self.rotate_to_gear = Xbox_Button(self.xbox_controller_1,
XBOX_BTN_A)
self.rotate_to_gear.whenPressed(
commands.drivetrain.Rotate_To_Gear(self.robot))
## Controller 2 Commands
# Run Shooter
if const.DEMO_MODE:
# Lower top speed on shooter when in demo mode
self.button_run_shooter = Xbox_Trigger(self.xbox_controller_2,
XBOX_BTN_RIGHT_TRIGGER)
self.button_run_shooter.whenPressed(
commands.shooter.Run_Shooter_Wheel(self.robot))
self.button_run_shooter.whenReleased(
commands.shooter.Stop_Shooter_Wheel(self.robot))
else:
self.button_run_shooter = Xbox_Trigger(self.xbox_controller_2,
XBOX_BTN_RIGHT_TRIGGER)
self.button_run_shooter.whenPressed(
commands.shooter.Run_Shooter_Wheel_Full_Speed(self.robot))
self.button_run_shooter.whenReleased(
commands.shooter.Stop_Shooter_Wheel(self.robot))
# Run Agitator
self.button_run_agitator = Xbox_Trigger(self.xbox_controller_2,
XBOX_BTN_LEFT_TRIGGER)
self.button_run_agitator.whenPressed(
commands.agitator.Run_Agitator(self.robot, 0.8))
self.button_run_agitator.whenReleased(
commands.agitator.Stop_Agitator(self.robot))
# FEEDER COMMANDS
# Toggles feeder on/off when pressed
self.button_run_feeder_forward = Xbox_Button(self.xbox_controller_2,
XBOX_BTN_RIGHT_BUMPER)
self.button_run_feeder_forward.whenPressed(
commands.feeder.Run_Feeder(self.robot, 1.0))
self.button_run_feeder_forward.whenReleased(
commands.feeder.Stop_Feeder(self.robot))
self.button_run_feeder_backward = Xbox_Button(self.xbox_controller_2,
XBOX_BTN_LEFT_BUMPER)
self.button_run_feeder_backward.whenPressed(
commands.feeder.Run_Feeder(self.robot, -1.0))
self.button_run_feeder_backward.whenReleased(
commands.feeder.Stop_Feeder(self.robot))
if not const.DEMO_MODE:
# GILLOTUINNEE
self.button_toggle_gear_lexan = Xbox_Button(
self.xbox_controller_2, XBOX_BTN_Y)
self.button_toggle_gear_lexan.whenPressed(
commands.gear_lexan.Toggle_Gear_Lexan_State(self.robot))
#self.button_toggle_gear_lexan.whenPressed( commands.gear_lexan.Set_Gear_Lexan_State( self.robot, const.ID_GEAR_LEXAN_OPEN ) )
#self.button_toggle_gear_lexan.whenReleased( commands.gear_lexan.Set_Gear_Lexan_State( self.robot, const.ID_GEAR_LEXAN_CLOSED) )
# Release gear
self.button_toggle_gear_release = Xbox_Button(self.xbox_controller_2,
XBOX_BTN_A)
self.button_toggle_gear_release.whenPressed(
commands.gear_claw.Open_Container(self.robot))
self.button_toggle_gear_release.whenReleased(
commands.gear_claw.Close_Container(self.robot))
# Toggle funnel
self.button_toggle_gear_funnel = Xbox_Button(self.xbox_controller_2,
XBOX_BTN_B)
self.button_toggle_gear_funnel.whenPressed(
commands.gear_funnel.Toggle_Gear_Funnel_State(self.robot))
self.button_toggle_gear_funnel.whenPressed(
commands.gear_lexan.Set_Gear_Lexan_State(
self.robot, const.ID_GEAR_LEXAN_CLOSED))
self.button_toggle_gear_funnel.whenReleased(
commands.gear_funnel.Toggle_Gear_Funnel_State(self.robot))
# CLIMBER COMMANDS
self.button_run_climber_up = Joystick_POV(
self.xbox_controller_2, controls.joystick_pov.JOY_POV_UP)
self.button_run_climber_up.whenPressed(
commands.climber.Run_Climber(self.robot, 1.0))
self.button_run_climber_up.whenReleased(
commands.climber.Stop_Climber(self.robot))
#self.button_run_climber_down = Joystick_POV( self.xbox_controller_1, controls.joystick_pov.JOY_POV_DOWN )
#self.button_run_climber_down.whenPressed( commands.climber.Run_Climber( self.robot, -0.2 ) )
#self.button_run_climber_down.whenReleased( commands.climber.Stop_Climber( self.robot ) )
#self.button_change_gear_camera_type = Xbox_Button( self.xbox_controller_2, XBOX_BTN_BACK )
#self.button_change_gear_camera_type.whenPressed( commands.gear_camera.Toggle_Gear_Camera_Mode( self.robot ) )
#self.button_change_shooter_camera_type = Xbox_Button( self.xbox_controller_2, XBOX_BTN_START )
#self.button_change_shooter_camera_type.whenPressed( commands.shooter_camera.Toggle_Shooter_Camera_Mode( self.robot ) )
#smart dashboard changing PID values
dp_trigger = SmartDashboard_Update_Trigger('Drive P: ',
self.robot.drive.default_kP)
dp_trigger.whenActive(
Command_Call(lambda: self.robot.drive.update_pid(p=dp_trigger.
get_key_value())))
di_trigger = SmartDashboard_Update_Trigger('Drive I: ',
self.robot.drive.default_kI)
di_trigger.whenActive(
Command_Call(lambda: self.robot.drive.update_pid(i=di_trigger.
get_key_value())))
dd_trigger = SmartDashboard_Update_Trigger('Drive D: ',
self.robot.drive.default_kD)
dd_trigger.whenActive(
Command_Call(lambda: self.robot.drive.update_pid(d=dd_trigger.
get_key_value())))
#smart dashboard changing Shooter PID values
sp_trigger = SmartDashboard_Update_Trigger(
'Shooter P: ', self.robot.shooter.default_kP)
sp_trigger.whenActive(
Command_Call(lambda: self.robot.shooter.update_pid(
p=sp_trigger.get_key_value())))
si_trigger = SmartDashboard_Update_Trigger(
'Shooter I: ', self.robot.shooter.default_kI)
si_trigger.whenActive(
Command_Call(lambda: self.robot.shooter.update_pid(
i=si_trigger.get_key_value())))
sd_trigger = SmartDashboard_Update_Trigger(
'Shooter D: ', self.robot.shooter.default_kD)
sd_trigger.whenActive(
Command_Call(lambda: self.robot.shooter.update_pid(
d=sd_trigger.get_key_value())))
sf_trigger = SmartDashboard_Update_Trigger(
'Shooter F: ', self.robot.shooter.default_kF)
sf_trigger.whenActive(
Command_Call(lambda: self.robot.shooter.update_pid(
f=sf_trigger.get_key_value())))
enc_trigger = SmartDashboard_Update_Trigger(
'ENC Autocorrect constant ',
const.DRIVE_CORRECTION_PROPORTION_FORWARD_ENC)
enc_trigger.whenActive(
Command_Call(
lambda: const.update_enc_const(enc_trigger.get_key_value())))
drive_corr_trigger = SmartDashboard_Update_Trigger(
'Drive Correction Enbaled ', const.DRIVE_CORRECTION_ENABLED)
drive_corr_trigger.whenActive(
Command_Call(lambda: const.update_auto_correct(drive_corr_trigger.
get_key_value())))
shooter_trigger = SmartDashboard_Update_Trigger(
'Shooter Speed ', const.SHOOTER_SPEED)
shooter_trigger.whenActive(
Command_Call(lambda: const.update_shooter_speed(shooter_trigger.
get_key_value())))
# Trigger for changing cameras exposure
cam_exp_trigger = SmartDashboard_Update_Trigger(
'Cam Exposure ', const.DEFAULT_CAMERA_EXPOSURE)
cam_exp_trigger.whenActive(
Command_Call(lambda: self._update_cameras_exposure(
cam_exp_trigger.get_key_value())))
def setupDashboardControls():
# Only need to set up dashbard drive controls once
global modeChooser
if modeChooser == None:
SmartDashboard.putBoolean("Quick Turn", False)
SmartDashboard.putBoolean("Square Inputs", True)
SmartDashboard.putNumber("Fixed Left", 0.4)
SmartDashboard.putNumber("Fixed Right", 0.4)
SmartDashboard.putNumber("Rotation Gain", 0.5)
SmartDashboard.putNumber("Slow Gain", 0.5)
mc = SendableChooser()
mc.addDefault("Arcade", kModeArcade)
mc.addOption("Tank", kModeTank)
mc.addOption("Curvature", kModeCurvature)
mc.addDefault("Indexed Arcade", kModeIndexedArcade)
mc.addDefault("Indexed Tank", kModeIndexedTank)
mc.addOption("Fixed", kModeFixed)
SmartDashboard.putData("Drive Mode", mc)
modeChooser = mc
class Robot(wpilib.IterativeRobot):
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.pneumaticControlModuleCANID = robotmap.PCM
self.kDriveTrain = robotmap.DriveTrain
self.kCubeGrabber = robotmap.CubeGrabber
self.kElevator = robotmap.Elevator
self.kSticks = robotmap.Sticks
self.kClimber = robotmap.Climber
self.dStick = Joystick(self.kSticks['drive'])
self.cStick = Joystick(self.kSticks['control'])
self.drive = Drive(self)
self.cubeGrabber = cubeGrabber(self)
self.elevator = Elevator(self)
self.climber = Climber(self)
self.sendableChooser()
def robotPeriodic(self):
pass
def disabledInit(self):
pass
def disabledPeriodic(self):
self.drive.stop()
def autonomousInit(self):
"""This function is run once each time the robot enters autonomous mode."""
self.autonomous = Autonomous(self)
self.autonomous.reset()
self.drive.autoInit()
def autonomousPeriodic(self):
"""This function is called periodically during autonomous."""
#self.autonomous.testMove(self.autonomous.WALL_TO_SCALE, -1, False)
#self.autonomous.testAngle(-90, -1)
#self.elevator.setElevatorPosition(self.elevator.kScale)
#self.autonomous.start()
self.autonomous.run()
#self.elevator.setElevatorPosition(-20000)
#self.autonomous.telemetry()
def teleopInit(self):
self.drive.teleInit()
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
speed = (self.dStick.getY() * -1)**3
rotation = self.dStick.getTwist()/(1.1+self.dStick.getRawAxis(3))
# self.drive.moveSpeed(speed, speed)
self.drive.arcadeWithRPM(speed, rotation, 2800)
self.cubeGrabber.grabberFunction()
#
self.elevator.elevatorFunction()
#self.elevator.telemetry()
self.climber.climberFunction()
def testInit(self):
pass
def testPeriodic(self):
wpilib.LiveWindow.setEnabled(True)
pass
def sendableChooser(self):
self.startingChooser = SendableChooser()
self.startingChooser.addDefault('Move Forward Only', '!')
self.startingChooser.addObject('Starting Left', 'L')
self.startingChooser.addObject('Starting Middle', 'M')
self.startingChooser.addObject('Starting Right', 'R')
wpilib.SmartDashboard.putData('Starting Side', self.startingChooser)
self.startingDelayChooser = SendableChooser()
self.startingDelayChooser.addDefault('0', 0)
self.startingDelayChooser.addObject('1', 1)
self.startingDelayChooser.addObject('2', 2)
self.startingDelayChooser.addObject('3', 3)
self.startingDelayChooser.addObject('4', 4)
self.startingDelayChooser.addObject('5', 5)
self.startingDelayChooser.addObject('6', 6)
self.startingDelayChooser.addObject('7', 7)
self.startingDelayChooser.addObject('8', 8)
self.startingDelayChooser.addObject('9', 9)
self.startingDelayChooser.addObject('10', 10)
self.startingDelayChooser.addObject('11', 11)
self.startingDelayChooser.addObject('12', 12)
self.startingDelayChooser.addObject('13', 13)
self.startingDelayChooser.addObject('14', 14)
self.startingDelayChooser.addObject('15', 15)
wpilib.SmartDashboard.putData('Delay Time(sec)', self.startingDelayChooser)
self.switchOrScale = SendableChooser()
self.switchOrScale.addDefault('Switch', 'Switch')
self.switchOrScale.addObject('Scale', 'Scale')
wpilib.SmartDashboard.putData('Switch or Scale', self.switchOrScale)
class MyRobot(CommandBasedRobot):
dashboard = True
frequency = 20
period = 1 / frequency
def __init__(self):
super().__init__(self.period)
def robotInit(self):
'''
This is a good place to set up your subsystems and anything else that
you will need to access later.
'''
wpilib.CameraServer.launch()
self.lastAuto = False
self.lastAlign = False
self.lastStraightAlign = False
self.teleop = False
Command.getRobot = lambda x=0: self
# Load system preferences prior to constructing
# subsystems
map.loadPreferences()
# Construct subsystems prior to constructing commands
self.limelight = Limelight.Limelight(self) #not a subsystem
self.hatch = HatchMech.HatchMech(self)
self.hatch.initialize()
#self.cargo = CargoMech0.CargoMech()
self.cargo = CargoMech.CargoMech() #not a subsystem
self.cargo.initialize()
self.climber = Climber.Climber() #not a subsystem
self.climber.initialize(self)
self.drive = Drive.Drive(self)
self.compressor = wpilib.Compressor(0)
self.timer = wpilib.Timer()
self.timer.start()
self.watch = wpilib.Watchdog(150, None)
'''
Since OI instantiates commands and commands need access to subsystems,
OI must be initialized after subsystems.
'''
[self.joystick0, self.joystick1, self.xbox] = oi.commands()
self.updateDashboardInit()
self.DriveStraight = DriveStraight()
self.DriveStraightSide = DriveStraightSide()
self.LeftCargo = LeftCargo()
self.RightCargo = RightCargo()
self.CenterCargo = CenterCargo()
self.SetSpeedDT = SetSpeedDT()
# Set up auton chooser
self.autonChooser = SendableChooser()
#self.autonChooser.setDefaultOption("Drive Straight", "DriveStraight")
self.autonChooser.setDefaultOption("Level 1 Center", "Level1Center")
self.autonChooser.addOption("Left Cargo", "LeftCargo")
self.autonChooser.addOption("Right Cargo", "RightCargo")
self.autonChooser.addOption("Do Nothing", "DoNothing")
self.autonChooser.addOption("Level 1 Center", "Level1Center")
self.autonChooser.addOption("Drive Straight Side", "DriveStraightSide")
SmartDashboard.putData("Auto mode", self.autonChooser)
def robotPeriodic(self):
self.limelight.readLimelightData()
SmartDashboard.putBoolean("teleop", self.teleop)
if (self.dashboard): self.updateDashboardPeriodic()
def autonomousInit(self):
super().autonomousInit()
self.drive.zero()
self.timer.reset()
self.timer.start()
self.autoSelector(self.autonChooser.getSelected())
def autonomousPeriodic(self):
super().autonomousPeriodic()
#driver takes control of drivetrain
deadband = 0.1
if (abs(self.joystick0.getRawAxis(map.drive)) > abs(deadband)):
self.SetSpeedDT.start()
if (abs(self.joystick1.getRawAxis(map.drive)) > abs(deadband)):
self.SetSpeedDT.start()
self.cargo.periodic()
def teleopInit(self):
super().teleopInit()
self.disabledInit()
self.teleop = True
def teleopPeriodic(self):
self.climber.periodic()
self.cargo.periodic()
self.hatch.periodic()
super().teleopPeriodic()
def updateDashboardInit(self):
#self.drive.dashboardInit()
#self.hatch.dashboardInit()
self.cargo.dashboardInit()
#self.climber.dashboardInit()
#self.limelight.dashboardInit()
#sequences.dashboardInit()
#autonomous.dashboardInit()
#pass
def updateDashboardPeriodic(self):
#SmartDashboard.putNumber("Timer", self.timer.get())
self.drive.dashboardPeriodic()
#self.hatch.dashboardPeriodic()
self.cargo.dashboardPeriodic()
#self.climber.dashboardPeriodic()
#self.limelight.dashboardPeriodic()
#sequences.dashboardPeriodic()
#autonomous.dashboardPeriodic()
#pass
def disabledInit(self):
self.scheduler.removeAll()
self.drive.disable()
self.hatch.disable()
self.cargo.disable()
self.climber.disable()
def disabledPeriodic(self):
self.disabledInit()
def driverLeftButton(self, id):
""" Return a button off of the left driver joystick that we want to map a command to. """
return wpilib.buttons.JoystickButton(self.joystick0, id)
def driverRightButton(self, id):
""" Return a button off of the right driver joystick that we want to map a command to. """
return wpilib.buttons.JoystickButton(self.joystick1, id)
def operatorButton(self, id):
""" Return a button off of the operator gamepad that we want to map a command to. """
return wpilib.buttons.JoystickButton(self.xbox, id)
def operatorAxis(self, id):
""" Return a Joystick off of the operator gamepad that we want to map a command to. """
#id is axis channel for taking value of axis
return self.xbox.getRawAxis(id)
#wpilib.joystick.setAxisChannel(self.xbox, id)
def readOperatorButton(self, id):
""" Return button value """
return self.xbox.getRawButton(id)
def readDriverRightButton(self, id):
""" Return button value from right joystick """
return self.joystick1.getRawButton(id)
def readDriverLeftButton(self, id):
""" Return button value from left joystick """
return self.joystick0.getRawButton(id)
def autoSelector(self, auto):
if auto == "DriveStraight":
self.DriveStraight.start()
elif auto == "DoNothing":
self.disabledInit()
elif auto == "Level1Center":
self.CenterCargo.start()
elif auto == "DriverControl":
self.driverControl()
elif auto == "DriveStraightSide":
self.DriveStraightSide.start()
elif auto == "RightCargo":
self.RightCargo.start()
elif auto == "LeftCargo":
self.LeftCargo.start()
else:
self.disabledInit()
def driverControl(self):
self.SetSpeedDT.start()
class OI():
debug: bool = True
autonChooser: SendableChooser = None
driver: Joystick = None
def __init__(self):
self.driver: Joystick = Joystick(0)
self.setupDriverCommands()
self.setupDashboardCommands()
@staticmethod
def initializeNumber(label: str, defaultValue: float) -> float:
value: float = SmartDashboard.getNumber(label, defaultValue)
SmartDashboard.putNumber(label, value)
return value
@staticmethod
def initializeBoolean(label: str, defaultValue: bool) -> bool:
value: bool = SmartDashboard.getBoolean(label, defaultValue)
SmartDashboard.putBoolean(label, value)
return value
def createDriverButton(self, rawId: int) -> JoystickButton:
""" Returns a button object that you can attach event handlers (commands)
to execute when the driver presses, holds or releases the button on the gamepad. """
return JoystickButton(self.driver, rawId)
def readDriverAxis(self, rawAxisId: int) -> float:
""" Reads value of an analog axis on the driver game pad [-1.0, +1.0]. """
return self.driver.getRawAxis(rawAxisId)
def readDriverButton(self, rawButtonId: int) -> bool:
""" Returns true if driver is currently pressing the button specified. """
return self.driver.getRawButton(rawButtonId)
def setupDriverCommands(self):
flipButton = self.createDriverButton(5)
flipButton.whenPressed(DriveHuman.createFlipFrontCommand())
def getSelectedAuton(self) -> Command:
return self.autonChooser.getSelected()
def setupDashboardCommands(self):
# Set up auton chooser
self.autonChooser = SendableChooser()
self.autonChooser.setDefaultOption(
"Do Nothing", wpilib.command.WaitCommand(3.0, "Do Nothing"))
self.autonChooser.addOption("Drive Forward", DriveTickTimed(1.0, 1.0))
self.autonChooser.addOption("Drive Backward",
DriveTickTimed(-1.0, -1.0))
self.autonChooser.addOption("Rotate Right", DriveTickTimed(1.0, -1.0))
self.autonChooser.addOption("Rotate Left", DriveTickTimed(-1.0, 1.0))
SmartDashboard.putData("Auto mode", self.autonChooser)
# Drive controls
DriveHuman.setupDashboardControls()
SmartDashboard.putData("Brake Control",
DriveHuman.createBrakeModeToggleCommand())
SmartDashboard.putData("Flip Front",
DriveHuman.createFlipFrontCommand())
# Set up utility controls
SmartDashboard.putData("Measure", Measure())
# Climber settings
SmartDashboard.putData("Full Auto Climb", ClimbUp())
if self.debug:
SmartDashboard.putData("Extend Both Legs", ExtendBothLegs())
SmartDashboard.putData("Drive to Front Sensor",
DriveToFrontSensor())
SmartDashboard.putData("Retract Front Legs", RetractFrontLegs())
SmartDashboard.putData("Drive to Back Sensor", DriveToBackSensor())
SmartDashboard.putData("Retract Back Legs", RetractBackLegs())
# Debug tools (if enabled)
if self.debug:
SmartDashboard.putData("CPU Load Test", LoadTest())
SmartDashboard.putData("Drive Subsystem", subsystems.drive)
dd = subsystems.drive.getDifferentialDrive()
if dd != None:
SmartDashboard.putData("DifferentialDrive", dd)
class OI:
def __init__(self, robot):
self.robot = robot
# update OI with logs
Event_Manager.add_listeners({
'auto.periodic' : self.log,
'teleop.periodic' : self.log
})
# notify listeners when SmartDashboard is updated
sd = NetworkTable.getTable('SmartDashboard')
sd.addTableListener(self.dashboard_listener, True)
# add autonomous mode chooser to smartdashboard
self.auto_choose = SendableChooser()
self.auto_choose.addObject('Auto Do Nothing', Autonomous.Modes.DO_NOTHING)
self.auto_choose.addObject('Auto Move Forward', Autonomous.Modes.MOVE_FORWARD)
self.auto_choose.addObject('Auto One Object', Autonomous.Modes.ONE_OBJECT)
SmartDashboard.putData('Autonomous Mode', self.auto_choose)
# joystick events
self.joystick = Joystick(0)
l_bumper = Joystick_Button(self.joystick, lc.L_BUMPER)
l_trigger = Joystick_Button(self.joystick, lc.L_TRIGGER)
r_bumper = Joystick_Button(self.joystick, lc.R_BUMPER)
r_trigger = Joystick_Button(self.joystick, lc.R_TRIGGER)
btn_one = Joystick_Button(self.joystick, 1)
btn_two = Joystick_Button(self.joystick, 2)
# update the joysick axis periodically
Event_Manager.add_listener('teleop.periodic', self.update_axis)
l_bumper.while_pressed(lambda: Event_Manager.trigger('joystick.l_bumper.while_pressed'))
l_trigger.while_pressed(lambda: Event_Manager.trigger('joystick.l_trigger.while_pressed'))
l_bumper.when_pressed(lambda: Event_Manager.trigger('joystick.l_bumper.when_pressed'))
l_trigger.when_pressed(lambda: Event_Manager.trigger('joystick.l_trigger.when_pressed'))
l_bumper.when_released(lambda: Event_Manager.trigger('joystick.l_bumper.when_released'))
l_trigger.when_released(lambda: Event_Manager.trigger('joystick.l_trigger.when_released'))
r_bumper.when_pressed(lambda: Event_Manager.trigger('joystick.r_bumper.when_pressed'))
btn_two.when_pressed(lambda: Event_Manager.trigger('joystick.btn_two.when_pressed'))
btn_one.when_pressed(lambda: Event_Manager.trigger('joystick.btn_one.when_pressed'))
def log(self, data):
self.robot.drive.log()
self.robot.grabber_lift.log()
def dashboard_listener(self, source, key, value, is_new):
Event_Manager.trigger('dashboard.updated', {
'source' : source,
'key' : key,
'value' : value,
'is_new' : is_new
})
def update_axis(self, data):
Event_Manager.trigger('joystick.axis.updated', {
'x_left' : self._get_axis(self.joystick, lc.L_AXIS_X)(),
'y_left' : self._get_axis(self.joystick, lc.L_AXIS_Y)(),
'x_right' : self._get_axis(self.joystick, lc.R_AXIS_X)(),
'y_right' : self._get_axis(self.joystick, lc.R_AXIS_Y)()
})
def _get_axis(self, joystick, axis):
def axis_func():
val = joystick.getAxis(axis)
if abs(val) >= .1:
return val
else:
return 0
return axis_func