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 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")
class OI: """ Operator Input - This class ties together controls and commands """ 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 _update_cameras_exposure(self, exposure): # Do nothing, because RPi reads the value right from the SD trigger pass #self.robot.gear_camera.update_exposure( exposure ) #self.robot.shooter_camera.update_exposure( exposure ) def _get_axis(self, joystick, axis, inverted=False): """ Handles inverted joy axes and dead band """ def axis_func(): val = joystick.getAxis(axis) if abs(val) < JOY_DEAD_BAND: val = 0 if inverted: val *= -1 return val return axis_func
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)