def handle_chassis_inputs(self, joystick: wpilib.Joystick, gamepad: wpilib.XboxController) -> None: throttle = scale_value(joystick.getThrottle(), 1, -1, 0.1, 1) vx = 3 * throttle * rescale_js(-joystick.getY(), 0.1) vz = 3 * throttle * rescale_js(-joystick.getTwist(), 0.1) self.chassis.drive(vx, vz) if joystick.getRawButtonPressed(3): # TODO reset heading pass
def handle_chassis_inputs(self, joystick: wpilib.Joystick, gamepad: wpilib.XboxController) -> None: throttle = scale_value(joystick.getThrottle(), 1, -1, 0.1, 1) vx = 3 * throttle * rescale_js(-joystick.getY(), 0.1) vz = 3 * throttle * rescale_js(-joystick.getTwist(), 0.1) self.chassis.drive(vx, vz) if joystick.getRawButtonPressed(3): self.chassis.reset_odometry( geometry.Pose2d(-3, 0, geometry.Rotation2d( math.pi))) # the starting position on the field self.target_estimator.reset()
class Robot(wpilib.IterativeRobot): def robotInit(self): """ This function is called upon program startup and should be used for any initialization code. """ self.drive = Drive() self.stick = Joystick(0) 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. """ pass def autonomousPeriodic(self): """This function is called periodically during autonomous.""" self.drive.moveToPosition(10000, 'left') def teleopInit(self): pass def teleopPeriodic(self): """This function is called periodically during operator control.""" speed = self.stick.getY() * -1 rotation = self.stick.getTwist() # self.drive.moveSpeed(speed, speed) self.drive.arcade(speed, rotation) def testInit(self): pass def testPeriodic(self): pass
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.kSticks = robotmap.Sticks self.dStick = Joystick(self.kSticks['drive']) self.cStick = Joystick(self.kSticks['control']) self.drive = Drive(self) self.cubeGrabber = Grabber(self) # holds data from the FMS about the field elements. self.fielddata = None 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. """ # get field data -- currently removed until we add an IF statement # self.fielddata = wpilib.DriverStation.getInstance().getGameSpecificMessage() self.drive.driveLeftMaster.setQuadraturePosition(0, 0) self.drive.driveRightMaster.setQuadraturePosition(0, 0) def autonomousPeriodic(self): """This function is called periodically during autonomous.""" ## get field data #if not self.fielddata: #self.fielddata = wpilib.DriverStation.getInstance().getGameSpecificMessage() #nearswitch, scale, farswitch = list(self.fielddata) #if self.fielddata[0] == 'R': #self.drive.arcade(.5, .2) #else: #self.drive.arcade(.5, -.2) self.drive.arcade(.5, 0) def teleopInit(self): self.drive.driveLeftMaster.setQuadraturePosition(0, 0) self.drive.driveRightMaster.setQuadraturePosition(0, 0) def teleopPeriodic(self): """This function is called periodically during operator control.""" speed = self.dStick.getY() * -1 rotation = self.dStick.getTwist() # self.drive.moveSpeed(speed, speed) self.drive.arcade(speed, rotation) self.cubeGrabber.grabberFunction() def testInit(self): pass def testPeriodic(self): pass
def handle_chassis_inputs(self, joystick: wpilib.Joystick) -> None: throttle = scale_value(joystick.getThrottle(), 1, -1, 0, 1) vx = 3 * throttle * rescale_js(-joystick.getY(), 0.1) vz = 3 * throttle * rescale_js(-joystick.getTwist(), 0.1) self.chassis.drive(vx, vz)
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 OI: def __init__(self): self.reversed = True self.arcade_drive = True self.turbo = False self.controls = { "reverse": 2, # top "arcade_switch": 3, "turbo": 1, # trigger "reset": 4 } self.driver_controller = Joystick(0) self.left_power = 0 self.right_power = 0 def get_button_config_pressed(self, controller, button_name): return controller.getRawButtonPressed(self.controls[button_name]) def get_button_config_held(self, controller, button_name): return controller.getRawButton(self.controls[button_name]) def curve_input(self, i): deadzone = 0.1 if abs(i) < deadzone: return 0 return math.pow(i, 3) / 1.25 def reset(self): self.left_power = 0 self.right_power = 0 self.reversed = False self.arcade_drive = True self.turbo = False def execute(self): if self.get_button_config_pressed(self.driver_controller, "reverse"): self.reversed = not self.reversed if self.get_button_config_pressed(self.driver_controller, "arcade_switch"): self.arcade_drive = not self.arcade_drive self.turbo = self.get_button_config_held(self.driver_controller, "turbo") self.left_power = self.driver_controller.getY() self.right_power = self.driver_controller.getTwist() reversed_constant = -1 if self.reversed else 1 self.left_power = self.curve_input(self.left_power * reversed_constant) self.right_power = self.curve_input(self.right_power * reversed_constant) if self.turbo: self.left_power *= 1.25 self.right_power *= 1.25 if self.get_button_config_pressed(self.driver_controller, "reset"): self.reset()
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.kSticks = robotmap.Sticks self.dStick = Joystick(self.kSticks['drive']) self.cStick = Joystick(self.kSticks['control']) self.drive = Drive(self) self.cubeGrabber = Grabber(self) # TODO: see examples/pacgoat/robot.py # we need Classes defining autonomous commands to call for Forward # and Reverse #self.autoChooser = wpilib.SendableChooser() #self.autoChooser.addDefault("Forward", Forward(self)) #self.autoChooser.addObject("Reverse", Reverse(self)) #wpilib.SmartDashboard.putData("Auto Mode", self.autoChooser) 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. """ # get field data #self.fielddata = wpilib.DriverStation.getInstance().getGameSpecificMessage() def autonomousPeriodic(self): """This function is called periodically during autonomous.""" #nearswitch, scale, farswitch = list(self.fielddata) # # if nearswitch == 'R': # self.drive.arcade(.5, .2) # else: # self.drive.arcade(.5, -.2) pass def teleopInit(self): pass def teleopPeriodic(self): """This function is called periodically during operator control.""" speed = self.dStick.getY() * -1 rotation = self.dStick.getTwist() # self.drive.moveSpeed(speed, speed) if self.isSimulation(): self.drive.arcade(speed, rotation) else: self.drive.arcadeWithRPM(speed, rotation, 2800) self.cubeGrabber.grabberFunction() # TODO: need something like this in commands that autonomous or teleop # would run to make sure an exception doesn't crash the code during # competition. #raise ValueError("Checking to see what happens when we get an exception") def testInit(self): pass def testPeriodic(self): wpilib.LiveWindow.setEnabled(True) pass