class OctoBot(wpilib.SampleRobot): #Initialise the robot. def robotInit(self): self.drivetrain = Drivetrain(self) self.oi = OI(self) def autonomous(self): while self.isAutonomous() and self.isEnabled(): Scheduler.getInstance().run() self.log() wpilib.Timer.delay(.005) def operatorControl(self): joystick = self.oi.getStick() while self.isOperatorControl() and self.isEnabled(): self.log() Scheduler.getInstance().run() wpilib.Timer.delay(.005) def disabled(self): """Stuff to do whilst disabled.""" while self.isDisabled(): self.log() wpilib.Timer.delay(.005) def test(self): """No tests""" pass def log(self): """It's big, it's heavy, it's wood.""" self.drivetrain.log()
class MyRobot(wpilib.TimedRobot): def robotInit(self): #assigns driver as controller 0 and operator as controller 1 self.driver = wpilib.XboxController(0) self.operator = wpilib.XboxController(1) #self.elevatorController = ElevatorController(self.operator, self.logger) #GYRO self.gyro = AHRS.create_spi() #DRIVETRAIN left = createTalonAndsub_units(LEFT_MASTER_ID, LEFT_sub_unit_1_ID, LEFT_sub_unit_2_ID) right = createTalonAndsub_units(RIGHT_MASTER_ID, RIGHT_sub_unit_1_ID, RIGHT_sub_unit_2_ID) self.drivetrain = Drivetrain(left, right, self.gyro) self.autoBalancing = False def robotPeriodic(self): # if self.timer % 50 == 0: # print("NavX Gyro Roll", self.gyro.getRoll()) pass def teleopInit(self): """Executed at the start of teleop mode""" self.forward = 0 def teleopPeriodic(self): deadzone_value = 0.2 max_accel = 0.15 max_forward = 1.0 max_rotate = 1.0 goal_forward = -self.driver.getRawAxis(5) rotation_value = self.driver.getX(LEFT_CONTROLLER_HAND) goal_forward = deadzone(goal_forward, deadzone_value) * max_forward delta = goal_forward - self.forward if (self.driver.getRawAxis(5) < 0): self.forward = 0 else: self.forward += delta self.drivetrain.arcade_drive(self.forward, rotation_value) def autonomousInit(self): #Because we want to drive during auton, just call the teleopInit() function to #get everything from teleop. self.teleopInit() print("auton init") def autonomousPeriodic(self): self.teleopPeriodic() print("auton periodic")
def test_drivetrain_half_speed(hal_data, robot, left_speed, right_speed, left_ex_speed, right_ex_speed): dt = Drivetrain(robot, None, '../tests/test_configs/drivetrain_half_speed.ini') assert dt is not None assert dt._left_motor is not None assert dt._right_motor is not None assert dt._robot_drive is not None assert dt._max_speed == 0.5 dt.tank_drive(left_speed, right_speed) assert abs(hal_data['pwm'][1]['value']) - abs(left_ex_speed) < 0.05 assert abs(hal_data['pwm'][2]['value']) - abs(right_ex_speed) < 0.05
def test_drivetrain_zero_speed(hal_data, robot, left_speed, right_speed, left_ex_speed, right_ex_speed): dt = Drivetrain(robot, None, '../tests/test_configs/drivetrain_zero_speed.ini') assert dt != None assert dt._left_motor != None assert dt._right_motor != None assert dt._robot_drive != None assert dt._max_speed == 0.0 dt.tankDrive(left_speed, right_speed) assert hal_data['pwm'][1]['value'] == left_ex_speed assert hal_data['pwm'][2]['value'] == right_ex_speed
def test_drivetrain_half_speed(hal_data, robot, left_speed, right_speed, left_ex_speed, right_ex_speed): dt = Drivetrain(robot, None, '../tests/test_configs/drivetrain_half_speed.ini') assert dt != None assert dt._left_motor != None assert dt._right_motor != None assert dt._robot_drive != None assert dt._max_speed == 0.5 dt.tankDrive(left_speed, right_speed) assert abs(hal_data['pwm'][1]['value']) - abs(left_ex_speed) < 0.05 assert abs(hal_data['pwm'][2]['value']) - abs(right_ex_speed) < 0.05
def test_drivetrain_zero_speed(hal_data, robot, left_speed, right_speed, left_ex_speed, right_ex_speed): dt = Drivetrain(robot, None, '../tests/test_configs/drivetrain_zero_speed.ini') assert dt is not None assert dt._left_motor is not None assert dt._right_motor is not None assert dt._robot_drive is not None assert dt._max_speed == 0.0 dt.tank_drive(left_speed, right_speed) assert hal_data['pwm'][1]['value'] == left_ex_speed assert hal_data['pwm'][2]['value'] == right_ex_speed
def test_forward(): front_left_motor = GetSet(0) rear_left_motor = GetSet(0) front_right_motor = GetSet(0) rear_right_motor = GetSet(0) drivetrain = Drivetrain(front_left_motor, rear_left_motor, front_right_motor, rear_right_motor) drivetrain.Cartesian_Drive(1.0, 0, 90, gyroAngle=0.0) assert ySpeed == 1 assert xSpeed
class DosPointOh(wpilib.SampleRobot): """Fluffy ears to scratch, lost his tail, cute little paws, likes to play fetch.""" def robotInit(self): """Initialise the robot.""" #Initialise the subsystems and the button mapping: self.drivetrain = Drivetrain(self) self.oi = OI(self) #Timeout value for the macros from the dashboard (default 15 sec). self.macroTimeout = self.oi.smart_dashboard.getInt("Macro", 15) Settings.num_macro_timeout = self.macroTimeout def autonomous(self): """Auton code.""" #Logging loop while self.isAutonomous() and self.isEnabled(): Scheduler.getInstance().run() self.log() wpilib.Timer.delay(.005) #don't burn up the cpu def operatorControl(self): """Teleop code.""" #Make the joystick work for driving: joystick = self.oi.getStick() #Logging loop while self.isOperatorControl() and self.isEnabled(): self.log() Scheduler.getInstance().run() wpilib.Timer.delay(.005) #don't burn up the cpu def disabled(self): """Code to run when disabled.""" #Stop the drivetrain for safety's sake: self.drivetrain.driveManual(0,0,0) #Logging loop while self.isDisabled(): self.log() wpilib.Timer.delay(.005) #don't burn up the cpu def test(self): """Code for testing.""" pass def log(self): """I know it doesn't log but if it does eventually it'll go here.""" #Log the things: self.drivetrain.log()
def robotInit(self): #assigns driver as controller 0 and operator as controller 1 self.driver = wpilib.XboxController(0) self.operator = wpilib.XboxController(1) #self.elevatorController = ElevatorController(self.operator, self.logger) #GYRO self.gyro = AHRS.create_spi() #DRIVETRAIN left = createTalonAndsub_units(LEFT_MASTER_ID, LEFT_sub_unit_1_ID, LEFT_sub_unit_2_ID) right = createTalonAndsub_units(RIGHT_MASTER_ID, RIGHT_sub_unit_1_ID, RIGHT_sub_unit_2_ID) self.drivetrain = Drivetrain(left, right, self.gyro) self.autoBalancing = False
def robotInit(self): """ This function is called upon program startup and should be used for any initialization code. """ self.oi = OI(self) self.drivetrain = Drivetrain(self) self.oi.setup_button_bindings() wpilib.CameraServer.launch()
def robotInit(self): """Initialises 'bot w/ all subsystems (winch needs testing) and joysticks""" #Subsystem initialisation. Woo. self.drivetrain = Drivetrain(self) self.lift = Lift(self) self.claw = Claw(self) self.mast = Mast(self) self.winch = Winch(self) self.oi = OI(self) #These are self-describing autonomouses. Waaaaaait... Autono-mouse? self.ThreeToteAutonomousCommand = ThreeToteAutonomous(self) self.CanAutonomousCommand = CanAutonomous(self) self.CanNToteAutoCommand = PlayMacro(self, "macro_1.csv") self.DriveAutonomousCommand = PlayMacro(self, "macro.csv") self.ToteAutonomousCommand = ToteAutonomous(self) self.PlayMacroCommand = PlayMacro(self, "autonomous.csv")
def robotInit(self): """Initialise the robot.""" #Initialise the subsystems and the button mapping: self.drivetrain = Drivetrain(self) self.ears = Ears(self) self.hat = Hat(self) self.tilt = Tilt(self) self.oi = OI(self) #Initialise the macro shenanigans (should probably clean this up once I get the dashboard sorted) self.macroTimeout = self.oi.smart_dashboard.getInt("Macro", 15) self.macroName = self.oi.smart_dashboard.getString("Macro Name", "macro.csv") Settings.str_macro_name = self.macroName Settings.num_macro_timeout = self.macroTimeout macro_string = str(Settings.num_macro_timeout) print("Robot initialized with a macro timeout of " + macro_string) self.simpleAutonCommand = PlayMacro(self, "macro.csv") self.shooterAutonCommand = PlayMacro(self, "macro_launch.csv")
def robotInit(self): """Initialise the robot.""" #Initialise the subsystems and the button mapping: self.drivetrain = Drivetrain(self) self.oi = OI(self) #Timeout value for the macros from the dashboard (default 15 sec). self.macroTimeout = self.oi.smart_dashboard.getInt("Macro", 15) Settings.num_macro_timeout = self.macroTimeout
def __init__(self): # Create the driver's controller. self.driverController = XboxController(constants.kDriverControllerPort) # Create an instance of the drivetrain subsystem. self.robotDrive = Drivetrain() # Configure and set the button bindings for the driver's controller. self.configureButtons() # Set the default command for the drive subsystem. It's default command will allow # the robot to drive with the controller. self.robotDrive.setDefaultCommand( RunCommand( lambda: self.robotDrive.arcadeDrive( -self.driverController.getRawAxis(1), self.driverController.getRawAxis(2) * 0.65, ), self.robotDrive, ))
def __init__(self) -> None: # The robot's subsystems and commands are defined here... self.drivetrain = Drivetrain() self.onboardIO = OnBoardIO(ChannelMode.INPUT, ChannelMode.INPUT) # Assumes a gamepad plugged into channnel 0 self.controller = wpilib.Joystick(0) # Create SmartDashboard chooser for autonomous routines self.chooser = wpilib.SendableChooser() # NOTE: The I/O pin functionality of the 5 exposed I/O pins depends on the hardware "overlay" # that is specified when launching the wpilib-ws server on the Romi raspberry pi. # By default, the following are available (listed in order from inside of the board to outside): # - DIO 8 (mapped to Arduino pin 11, closest to the inside of the board) # - Analog In 0 (mapped to Analog Channel 6 / Arduino Pin 4) # - Analog In 1 (mapped to Analog Channel 2 / Arduino Pin 20) # - PWM 2 (mapped to Arduino Pin 21) # - PWM 3 (mapped to Arduino Pin 22) # # Your subsystem configuration should take the overlays into account self._configureButtonBindings()
def test_drivetrain_channels_0_1(hal_data, robot): dt = Drivetrain(robot, None, '../tests/test_configs/drivetrain_channels_0_1.ini') assert dt is not None assert dt._left_motor is not None assert dt._right_motor is not None assert dt._robot_drive is not None assert hal_data['pwm'][0]['initialized'] is True assert hal_data['pwm'][0]['value'] == 0.0 assert hal_data['pwm'][0]['type'] == 'victorsp' assert hal_data['pwm'][0]['zero_latch'] is True assert hal_data['pwm'][1]['initialized'] is True assert hal_data['pwm'][1]['value'] == 0.0 assert hal_data['pwm'][1]['type'] == 'victorsp' assert hal_data['pwm'][1]['zero_latch'] is True assert hal_data['pwm'][2]['initialized'] is False
def robotInit(self): Command.robot = self # Init networktables NetworkTables.initialize() self.sd = NetworkTables.getTable('SmartDashboard') # Init subsystems self.drivetrain = Drivetrain() self.Arm = Arm() # Init oi self.oi = OI(self) # Init commands self.auto = Auto()
def robotInit(self): """ This is a good place to set up your subsystems and anything else that you will need to access later. """ # All commands can get access to the subsystems here subsystems.LEDS = LEDs() subsystems.DRIVETRAIN = Drivetrain() subsystems.ELEVATOR = Elevator() subsystems.PAYLOAD = Payload() subsystems.SERIAL = SerialEvent() subsystems.PIGEON = Pigeon() self.compressor = wpilib.Compressor(0) self.compressor.setClosedLoopControl(True) #self.compressor.start() """ Since OI instantiates commands and commands need access to subsystems, OI must be initialized after subsystems. """ subsystems.JOYSTICK = oi.get_joystick() wpilib.CameraServer.launch()
def robotInit(self): Command.getRobot = lambda _: self wpilib.CameraServer.launch('vision.py:main') NetworkTables.initialize(server='10.56.54.2') self.rf_motor = ctre.WPI_VictorSPX(1) self.rr_motor = ctre.WPI_VictorSPX(2) self.lf_motor = ctre.WPI_VictorSPX(5) self.lr_motor = ctre.WPI_VictorSPX(4) self.drive = wpilib.drive.MecanumDrive(self.lf_motor, self.lr_motor, self.rf_motor, self.rr_motor) self.drivetrain = Drivetrain(self.drive) self.shooter_motor = ctre.WPI_VictorSPX(3) self.shooter = Shooter(self.shooter_motor) self.intake_belt_motor = wpilib.Victor(0) self.intake_belt = ConveyorBelt(self.intake_belt_motor) self.magazine_belt_motor = wpilib.Victor(1) self.magazine_belt = ConveyorBelt(self.magazine_belt_motor, negate=True) self.shooter_belt_motor = wpilib.Victor(2) self.shooter_belt = ConveyorBelt(self.shooter_belt_motor, negate=True) self.conveyor_mode = 0 self.joystick = oi.get_joystick()
class RobotContainer: """ This class is where the bulk of the robot should be declared. Since Command-based is a "declarative" paradigm, very little robot logic should actually be handled in the :class:`.Robot` periodic methods (other than the scheduler calls). Instead, the structure of the robot (including subsystems, commands, and button mappings) should be declared here. """ def __init__(self) -> None: # The robot's subsystems and commands are defined here... self.drivetrain = Drivetrain() self.onboardIO = OnBoardIO(ChannelMode.INPUT, ChannelMode.INPUT) # Assumes a gamepad plugged into channnel 0 self.controller = wpilib.Joystick(0) # Create SmartDashboard chooser for autonomous routines self.chooser = wpilib.SendableChooser() # NOTE: The I/O pin functionality of the 5 exposed I/O pins depends on the hardware "overlay" # that is specified when launching the wpilib-ws server on the Romi raspberry pi. # By default, the following are available (listed in order from inside of the board to outside): # - DIO 8 (mapped to Arduino pin 11, closest to the inside of the board) # - Analog In 0 (mapped to Analog Channel 6 / Arduino Pin 4) # - Analog In 1 (mapped to Analog Channel 2 / Arduino Pin 20) # - PWM 2 (mapped to Arduino Pin 21) # - PWM 3 (mapped to Arduino Pin 22) # # Your subsystem configuration should take the overlays into account self._configureButtonBindings() def _configureButtonBindings(self): """Use this method to define your button->command mappings. Buttons can be created by instantiating a :class:`.GenericHID` or one of its subclasses (:class`.Joystick or :class:`.XboxController`), and then passing it to a :class:`.JoystickButton`. """ # Default command is arcade drive. This will run unless another command # is scheduler over it self.drivetrain.setDefaultCommand(self.getArcadeDriveCommand()) # Example of how to use the onboard IO onboardButtonA = commands2.button.Button(self.onboardIO.getButtonAPressed) onboardButtonA.whenActive( commands2.PrintCommand("Button A Pressed") ).whenInactive(commands2.PrintCommand("Button A Released")) # Setup SmartDashboard options self.chooser.setDefaultOption( "Auto Routine Distance", AutonomousDistance(self.drivetrain) ) self.chooser.addOption("Auto Routine Time", AutonomousTime(self.drivetrain)) wpilib.SmartDashboard.putData(self.chooser) def getAutonomousCommand(self) -> typing.Optional[commands2.CommandBase]: return self.chooser.getSelected() def getArcadeDriveCommand(self) -> ArcadeDrive: """Use this to pass the teleop command to the main robot class. :returns: the command to run in teleop """ return ArcadeDrive( self.drivetrain, lambda: self.controller.getRawAxis(1), lambda: self.controller.getRawAxis(2), )
class Lopez_Jr(wpilib.SampleRobot): def robotInit(self): """Initialises 'bot w/ all subsystems (winch needs testing) and joysticks""" #Subsystem initialisation. Woo. self.drivetrain = Drivetrain(self) self.lift = Lift(self) self.claw = Claw(self) self.mast = Mast(self) self.winch = Winch(self) self.oi = OI(self) #These are self-describing autonomouses. Waaaaaait... Autono-mouse? self.ThreeToteAutonomousCommand = ThreeToteAutonomous(self) self.CanAutonomousCommand = CanAutonomous(self) self.CanNToteAutoCommand = PlayMacro(self, "macro_1.csv") self.DriveAutonomousCommand = PlayMacro(self, "macro.csv") self.ToteAutonomousCommand = ToteAutonomous(self) self.PlayMacroCommand = PlayMacro(self, "autonomous.csv") def autonomous(self): """Woo, auton code w/ 3 modes. Needs to be tested.""" #I'll probably change these out to different macro commands. self.drivetrain.drive.setSafetyEnabled(False) try: if self.oi.smart_dashboard.getBoolean("3 Tote Auto"): self.ThreeToteAutonomousCommand.start() print("3 Tote Auto started") elif self.oi.smart_dashboard.getBoolean("Can Auto"): self.CanAutonomousCommand.start() print("Can Auto started") elif self.oi.smart_dashboard.getBoolean("Can/Tote Auto"): self.CanNToteAutoCommand.start() print("Can and Tote Auto started") elif self.oi.smart_dashboard.getBoolean("Tote Auto"): self.ToteAutonomousCommand.start() print("Tote Auto started") elif self.oi.smart_dashboard.getBoolean("Play Macro"): self.PlayMacroCommand.start() print("Macro replay started") elif self.oi.smart_dashboard.getBoolean("Drive Auto"): self.DriveAutonomousCommand.start() print("Drive Auto started") else: pass except KeyError: self.PlayMacroCommand.start() #pass while self.isAutonomous() and self.isEnabled(): Scheduler.getInstance().run() self.log() wpilib.Timer.delay(.005) # don't burn up the cpu def operatorControl(self): """Runs the 'bot with the fancy joystick and an awesome gamepad THAT IS AWESOME.""" #Cancel the autons - that could go badly otherwise. self.ThreeToteAutonomousCommand.cancel() self.CanAutonomousCommand.cancel() self.DriveAutonomousCommand.cancel() self.ToteAutonomousCommand.cancel() self.PlayMacroCommand.cancel() self.CanNToteAutoCommand.cancel() #More stuff. self.drivetrain.drive.setSafetyEnabled(True) joystick = self.oi.getJoystickLeft() while self.isOperatorControl() and self.isEnabled(): self.log() Scheduler.getInstance().run() wpilib.Timer.delay(.005) # don't burn up the cpu def disabled(self): """Stuff to do when the 'bot is disabled""" #cancel the autons IN THE NAME OF SAFETY self.ThreeToteAutonomousCommand.cancel() self.CanAutonomousCommand.cancel() self.DriveAutonomousCommand.cancel() self.ToteAutonomousCommand.cancel() self.PlayMacroCommand.cancel() self.CanNToteAutoCommand.cancel() while self.isDisabled(): self.log() wpilib.Timer.delay(.005) def test(self): """There aren't any tests.""" pass def log(self): """It's big, it's heavy, it's wood. Needs to be bigger.""" self.drivetrain.log() self.lift.log() self.claw.log() self.mast.log() self.winch.log()
def robotInit(self): self.drivetrain = Drivetrain(self) self.oi = OI(self)
class Mantis(wpilib.SampleRobot): """Fluffy ears to scratch, lost his tail, cute little paws, likes to play fetch.""" def robotInit(self): """Initialise the robot.""" #Initialise the subsystems and the button mapping: self.drivetrain = Drivetrain(self) self.ears = Ears(self) self.hat = Hat(self) self.tilt = Tilt(self) self.oi = OI(self) #Initialise the macro shenanigans (should probably clean this up once I get the dashboard sorted) self.macroTimeout = self.oi.smart_dashboard.getInt("Macro", 15) self.macroName = self.oi.smart_dashboard.getString("Macro Name", "macro.csv") Settings.str_macro_name = self.macroName Settings.num_macro_timeout = self.macroTimeout macro_string = str(Settings.num_macro_timeout) print("Robot initialized with a macro timeout of " + macro_string) self.simpleAutonCommand = PlayMacro(self, "macro.csv") self.shooterAutonCommand = PlayMacro(self, "macro_launch.csv") def autonomous(self): """Auton code.""" print("Autonomous mode activated") try: if self.oi.smart_dashboard.getBoolean("Play Macro"): self.simpleAutonCommand.start() elif self.oi.smart_dashboard.getBoolean("Shoot"): self.shooterAutonCommand.start() else: print("No macro selected, waiting for teleop...") except KeyError: pass #Logging loop while self.isAutonomous() and self.isEnabled(): Scheduler.getInstance().run() self.log() wpilib.Timer.delay(.05) #don't burn up the cpu def operatorControl(self): """Teleop code.""" print("Teleop activated") self.simpleAutonCommand.cancel() self.shooterAutonCommand.cancel() #Make the joystick work for driving: joystick = self.oi.getStick() #Logging loop while self.isOperatorControl() and self.isEnabled(): self.log() Scheduler.getInstance().run() wpilib.Timer.delay(.05) #don't burn up the cpu def disabled(self): """Code to run when disabled.""" print("Robot disabled") #Stop the drivetrain for safety's sake: self.drivetrain.driveManual(0,0) self.hat.manualSet(0) self.ears.manualSet(0) self.tilt.manualSet(0) #Logging loop while self.isDisabled(): self.log() wpilib.Timer.delay(.05) #don't burn up the cpu def test(self): """Code for testing.""" print("Test mode activated") def log(self): """Log our subsystems.""" self.drivetrain.log() self.ears.log() self.hat.log() self.tilt.log()
class Mantis(wpilib.SampleRobot): """Fluffy ears to scratch, lost his tail, cute little paws, likes to play fetch.""" def robotInit(self): """Initialise the robot.""" #Initialise the subsystems and the button mapping: self.drivetrain = Drivetrain(self) self.oi = OI(self) self.ears = Ears(self) #Timeout value for the macros from the dashboard (default 15 sec). self.macroTimeout = self.oi.smart_dashboard.getInt("Macro", 15) Settings.num_macro_timeout = self.macroTimeout #these are all actually really self explanatory. Wonder if they work. # self.camera = wpilib.USBCamera() # self.camera.setExposureManual(50) # self.camera.setBrightness(80) # self.camera.setFPS(30) # self.camera.setSize(320, 240) # self.camera.setWhiteBalanceAuto() # self.camera.updateSettings() #update with the new settings so it actually does what we want it to # server = wpilib.CameraServer.getInstance() #set up the camera server # server.startAutomaticCapture(self.camera) #start the camera server def autonomous(self): """Auton code.""" #Logging loop while self.isAutonomous() and self.isEnabled(): Scheduler.getInstance().run() self.log() wpilib.Timer.delay(.005) #don't burn up the cpu def operatorControl(self): """Teleop code.""" #Make the joystick work for driving: joystick = self.oi.getStick() #Logging loop while self.isOperatorControl() and self.isEnabled(): self.log() Scheduler.getInstance().run() wpilib.Timer.delay(.005) #don't burn up the cpu def disabled(self): """Code to run when disabled.""" #Stop the drivetrain for safety's sake: self.drivetrain.driveManual(0,0, False, False) #Logging loop while self.isDisabled(): self.log() wpilib.Timer.delay(.005) #don't burn up the cpu def test(self): """Code for testing.""" pass def log(self): """I know it doesn't log but if it does eventually it'll go here.""" #Log the things: self.drivetrain.log()
def test_drivetrain_right_disabled(hal_data, robot): dt = Drivetrain(robot, None, '../tests/test_configs/drivetrain_right_disabled.ini') assert dt is not None assert dt._left_motor is not None assert dt._right_motor is None assert dt._robot_drive is None
def drivetrain_default(robot): return Drivetrain(robot, None, '../tests/test_configs/drivetrain_default.ini')
class RobotContainer: """ This class hosts the bulk of the robot's functions. Little robot logic needs to be handled here or in the robot periodic methods, as this is a command-based system. The structure (commands, subsystems, and button mappings) should be done here. """ def __init__(self): # Create the driver's controller. self.driverController = XboxController(constants.kDriverControllerPort) # Create an instance of the drivetrain subsystem. self.robotDrive = Drivetrain() # Configure and set the button bindings for the driver's controller. self.configureButtons() # Set the default command for the drive subsystem. It's default command will allow # the robot to drive with the controller. self.robotDrive.setDefaultCommand( RunCommand( lambda: self.robotDrive.arcadeDrive( -self.driverController.getRawAxis(1), self.driverController.getRawAxis(2) * 0.65, ), self.robotDrive, )) def getAutonomousCommand(self): """Returns the command to be ran during the autonomous period.""" # Create a voltage constraint to ensure we don't accelerate too fast. autoVoltageConstraint = DifferentialDriveVoltageConstraint( SimpleMotorFeedforwardMeters( constants.ksVolts, constants.kvVoltSecondsPerMeter, constants.kaVoltSecondsSquaredPerMeter, ), constants.kDriveKinematics, maxVoltage=10, # 10 volts max. ) # Below will generate the trajectory using a set of programmed configurations # Create a configuration for the trajctory. This tells the trajectory its constraints # as well as its resources, such as the kinematics object. config = TrajectoryConfig( constants.kMaxSpeedMetersPerSecond, constants.kMaxAccelerationMetersPerSecondSquared, ) # Ensures that the max speed is actually obeyed. config.setKinematics(constants.kDriveKinematics) # Apply the previously defined voltage constraint. config.addConstraint(autoVoltageConstraint) # Start at the origin facing the +x direction. initialPosition = Pose2d(0, 0, Rotation2d(0)) # Here are the movements we also want to make during this command. # These movements should make an "S" like curve. movements = [Translation2d(1, 1), Translation2d(2, -1)] # End at this position, three meters straight ahead of us, facing forward. finalPosition = Pose2d(3, 0, Rotation2d(0)) # An example trajectory to follow. All of these units are in meters. self.exampleTrajectory = TrajectoryGenerator.generateTrajectory( initialPosition, movements, finalPosition, config, ) # Below creates the RAMSETE command ramseteCommand = RamseteCommand( # The trajectory to follow. self.exampleTrajectory, # A reference to a method that will return our position. self.robotDrive.getPose, # Our RAMSETE controller. RamseteController(constants.kRamseteB, constants.kRamseteZeta), # A feedforward object for the robot. SimpleMotorFeedforwardMeters( constants.ksVolts, constants.kvVoltSecondsPerMeter, constants.kaVoltSecondsSquaredPerMeter, ), # Our drive kinematics. constants.kDriveKinematics, # A reference to a method which will return a DifferentialDriveWheelSpeeds object. self.robotDrive.getWheelSpeeds, # The turn controller for the left side of the drivetrain. PIDController(constants.kPDriveVel, 0, 0), # The turn controller for the right side of the drivetrain. PIDController(constants.kPDriveVel, 0, 0), # A reference to a method which will set a specified # voltage to each motor. The command will pass the two parameters. self.robotDrive.tankDriveVolts, # The subsystems the command should require. [self.robotDrive], ) # Reset the robot's position to the starting position of the trajectory. self.robotDrive.resetOdometry(self.exampleTrajectory.initialPose()) # Return the command to schedule. The "andThen()" will halt the robot after # the command finishes. return ramseteCommand.andThen( lambda: self.robotDrive.tankDriveVolts(0, 0)) def configureButtons(self): """Configure the buttons for the driver's controller""" # We won't do anything with this button itself, so we don't need to # define a variable. (JoystickButton( self.driverController, XboxController.Button.kBumperRight.value).whenPressed( lambda: self.robotDrive.setSlowMaxOutput(0.5)).whenReleased( lambda: self.robotDrive.setNormalMaxOutput(1)))