class MyRobot(MagicRobot): solenoids = Solenoids def createObjects(self): self.shiftBaseSolenoid = wpilib.DoubleSolenoid( rMap.conf_shifterSolenoid1, rMap.conf_shifterSolenoid2) self.rightFrontBaseMotor = CANTalon(rMap.conf_rightFrontBaseTalon) self.rightRearBaseMotor = CANTalon(rMap.conf_rightRearBaseTalon) self.leftFrontBaseMotor = CANTalon(rMap.conf_leftFrontBaseTalon) self.leftRearBaseMotor = CANTalon(rMap.conf_leftRearBaseTalon) self.rightFrontBaseMotor.enableControl() self.rightRearBaseMotor.enableControl() self.rightRearBaseMotor.setControlMode(CANTalon.ControlMode.Follower) self.rightRearBaseMotor.set(rMap.conf_rightFrontBaseTalon) self.leftFrontBaseMotor.enableControl() self.leftRearBaseMotor.enableControl() self.leftFrontBaseMotor.setInverted(True) self.leftRearBaseMotor.setControlMode(CANTalon.ControlMode.Follower) self.leftRearBaseMotor.set(rMap.conf_leftFrontBaseTalon) self.leftJoy = Joystick(rMap.conf_left_joy) self.rightJoy = Joystick(rMap.conf_right_joy) self.xbox = Joystick(rMap.conf_xbox) self.robotDrive = wpilib.RobotDrive(self.leftFrontBaseMotor, self.rightFrontBaseMotor) def autonomous(self): """Drive left and right motors for two seconds, then stop.""" MagicRobot.autonomous(self) def teleopInit(self): MagicRobot.teleopInit(self) def teleopPeriodic(self): if self.isSimulation(): # easier to control in simulation self.robotDrive.arcadeDrive(self.leftJoy) else: self.robotDrive.tankDrive(self.leftJoy.getY(), self.rightJoy.getY()) try: if self.leftJoy.getRawButton(7): self.solenoids.setShift() print("RED " + str(self.solenoids.setShift())) except: self.onException() def test(self): '''Runs during test mode''' wpilib.LiveWindow.run()
def upadte_operator(self, left_stick: Joystick, right_stick: Joystick = None): if right_stick is None: self.arcade_mode = True self.y_speed = -left_stick.getY() self.z_speed = -left_stick.getZ() else: self.arcade_mode = True self.left_speed = -left_stick.getY() self.right_speed = -right_stick.getY()
class Attack3Joystick(Sensor): """ Sensor wrapper for the Attack 3 Joystick. Has boolean attributes for buttons: trigger, button2-9 and double x_axis, y_axis for joystick position """ x_axis = y_axis = 0 trigger = button2 = button3 = \ button4 = button5 = button6 = \ button7 = button8 = button9 = \ button10 = button11 = False def __init__(self, port): """ Initializes the joystick with some USB port. """ super().__init__() self.j = Joystick(port) def poll(self): for i, state_id in enumerate(BUTTON_TABLE, 1): self.update_state(state_id, self.j.getRawButton(i)) self.x_axis = self.j.getX() self.y_axis = self.j.getY()
class MyRobot(TimedRobot): # Defines the channels that are used on the inputs. In the simulator, they have to match up with the physics.py file # This is really useful when you have a variable used hundreds of times and you want to have it set so you can # change it all in one go. RLMotorChannel = 2 RRMotorChannel = 0 FLMotorChannel = 3 FRMotorChannel = 4 DriveStickChannel = 0 # RobotInit is where all of the things we are using is initialized. def robotInit(self): # Note the lack of the camera server initialization here. # Initializing drive motors RLMotor = Spark(self.RLMotorChannel) RRMotor = Spark(self.RRMotorChannel) FLMotor = Spark(self.FLMotorChannel) FRMotor = Spark(self.FRMotorChannel) # Grouping drive motors into left and right. self.Left = SpeedControllerGroup(RLMotor, FLMotor) self.Right = SpeedControllerGroup(RRMotor, FRMotor) # Initializing drive Joystick. self.DriveStick = Joystick(self.DriveStickChannel) # Initializing drive function. self.drive = DifferentialDrive(self.Left, self.Right) # Sets the right side motors to be inverted. self.drive.setRightSideInverted(rightSideInverted=True) # Turns the drive off after .1 seconds of inactivity. self.drive.setExpiration(0.1) # Components is a dictionary that contains any variables you want to put into it. All of the variables put into # components dictionary is given to the autonomous modes. self.components = {"drive": self.drive} # Sets up the autonomous mode selector by telling it where the autonomous modes are at and what the autonomous # modes should inherit. self.automodes = autonomous.AutonomousModeSelector( "Sim_Autonomous", self.components) # Autonomous and teleop periodic both run the code on a fixed loop time. A part of TimedRobot. def autonomousPeriodic(self): # runs the autonomous modes when Autonomous is activated. self.automodes.run() def teleopPeriodic(self): # Turns on drive safety and checks to se if operator control and the robot is enabled. self.drive.setSafetyEnabled(True) if self.isOperatorControl() and self.isEnabled(): # Drives the robot with controller inputs. You can use buttons with self.DriveStick.getRawButton(ButtonNum). self.drive.arcadeDrive(self.DriveStick.getY(), -self.DriveStick.getX(), squareInputs=False)
class XboxJoystick(Sensor): """ Sensor wrapper for the Xbox Controller. Has boolean attributes for buttons: a/b/x/y/back/start_button, l/r_shoulder Attributes l/r_x/y_axis for thumbstick positions trigger_pos and keypad_pos for trigger and keypad position """ l_x_axis = l_y_axis = r_x_axis = r_y_axis = 0 trigger_pos = keypad_pos = 0 a_button = b_button = x_button = y_button = False l_shoulder = r_shoulder = back_button = start_button = False def __init__(self, port): """ Initializes the joystick with some USB port. """ super().__init__() self.j = Joystick(port) def poll(self): for i, state_id in enumerate(BUTTON_TABLE, 1): self.update_state(state_id, self.j.getRawButton(i)) # button index is offset by 1 due to wpilib 1-indexing self.l_x_axis = self.j.getX() self.l_y_axis = self.j.getY() self.r_x_axis = self.j.getRawAxis(4) self.r_y_axis = self.j.getRawAxis(5) self.trigger_pos = self.j.getZ() self.keypad_pos = self.j.getRawAxis(6)
class Joker(magicbot.MagicRobot): drivePID: DriveController chassis: Chassis def createObjects(self): """ Create motors, sensors and all your components here. """ self.chassis_left_master = WPI_TalonSRX(1) self.chassis_left_slave = WPI_TalonSRX(2) self.chassis_right_master = WPI_TalonSRX(3) self.chassis_right_slave = WPI_TalonSRX(4) self.chassis_gyro = AHRS.create_spi() self.joystick = Joystick(0) def disabledInit(self): if self.drivePID.enabled: self.drivePID.close() def robotPeriodic(self): if self.isAutonomousEnabled(): self.chassis.set_auto() elif self.isOperatorControlEnabled(): self.chassis.set_teleop() def teleopInit(self): """ Called when teleop starts. """ NetworkTables.initialize(server="10.43.20.149") self.sd = NetworkTables.getTable("SmartDashboard") self.chassis.reset_encoders() def teleopPeriodic(self): """ Called on each iteration of the control loop. """ self.chassis.set_speed(-self.joystick.getY(), -self.joystick.getZ())
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