Exemplo n.º 1
0
    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)
Exemplo n.º 2
0
    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)
Exemplo n.º 3
0
    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)
Exemplo n.º 4
0
 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)
Exemplo n.º 5
0
 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'))
Exemplo n.º 6
0
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")
Exemplo n.º 7
0
    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())))
Exemplo n.º 8
0
    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
Exemplo n.º 9
0
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)
Exemplo n.º 10
0
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()
Exemplo n.º 11
0
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)
Exemplo n.º 12
0
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