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 _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 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 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
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())))
