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)))
