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