From 8300e6fb7f704caf46c698f716ac947911ad72b4 Mon Sep 17 00:00:00 2001 From: Dustin Spicuzza Date: Sun, 5 Nov 2023 01:24:19 -0500 Subject: [PATCH] Update examples for 2024 - Some of the command based examples don't work because the command framework is incomplete. See https://github.com/robotpy/robotpy-commands-v2/issues/28 --- .github/workflows/test.yml | 17 +++-- arm-simulation/physics.py | 1 + .../armbot/subsystems/drivesubsystem.py | 2 +- .../subsystems/drivesubsystem.py | 2 +- .../subsystems/drivesubsystem.py | 2 +- .../frisbee-bot/subsystems/drivesubsystem.py | 2 +- .../subsystems/drivesubsystem.py | 2 +- .../hatchbot-inlined/commands/autos.py | 64 +++++++++---------- commands-v2/hatchbot-inlined/physics.py | 3 +- .../hatchbot-inlined/robotcontainer.py | 2 +- .../subsystems/drivesubsystem.py | 2 +- .../subsystems/hatchsubsystem.py | 6 +- commands-v2/hatchbot/commands/defaultdrive.py | 4 +- .../hatchbot/commands/drivedistance.py | 2 +- commands-v2/hatchbot/commands/grabhatch.py | 2 +- .../hatchbot/commands/halvedrivespeed.py | 2 +- commands-v2/hatchbot/physics.py | 3 +- commands-v2/hatchbot/robotcontainer.py | 6 +- .../hatchbot/subsystems/drivesubsystem.py | 2 +- .../hatchbot/subsystems/hatchsubsystem.py | 2 +- commands-v2/ramsete/subsystems/drivetrain.py | 4 +- commands-v2/romi/commands/arcadedrive.py | 2 +- commands-v2/romi/commands/drivedistance.py | 2 +- commands-v2/romi/commands/drivetime.py | 2 +- commands-v2/romi/commands/turndegrees.py | 2 +- commands-v2/romi/commands/turntime.py | 2 +- commands-v2/romi/robotcontainer.py | 2 +- commands-v2/romi/subsystems/drivetrain.py | 2 +- commands-v2/scheduler-event-logging/robot.py | 2 +- commands-v2/selectcommand/robot.py | 2 +- commands-v2/selectcommand/robotcontainer.py | 21 +++--- elevator-simulation/physics.py | 1 + run_tests.sh | 16 ++--- timed/tests/basic_test.py | 4 +- 34 files changed, 96 insertions(+), 96 deletions(-) mode change 100644 => 100755 commands-v2/scheduler-event-logging/robot.py mode change 100644 => 100755 commands-v2/selectcommand/robot.py diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index bab8f5da..20a4d857 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -13,28 +13,31 @@ jobs: check: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: psf/black@stable test: runs-on: ${{ matrix.os }} strategy: matrix: - os: [windows-latest, macos-latest, ubuntu-22.04] - python_version: [3.7, 3.8, 3.9, "3.10", "3.11"] + os: ["ubuntu-22.04", "macos-12", "windows-2022"] + python_version: + - '3.8' + - '3.9' + - '3.10' + - '3.11' + - '3.12' steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: actions/setup-python@v4 with: python-version: ${{ matrix.python_version }} architecture: ${{ matrix.architecture }} - name: Install deps run: | - pip install numpy pip install -U pip - pip install 'robotpy[commands2]<2024.0.0,>=2023.1.1.0' pytest - #pip install 'robotpy[commands2,romi]<2024.0.0,>=2023.0.0b3' pytest + pip install 'robotpy[commands2]<2025.0.0,>=2024.0.0b3' numpy pytest - name: Run tests run: bash run_tests.sh shell: bash diff --git a/arm-simulation/physics.py b/arm-simulation/physics.py index c6d0f6b8..72d89db1 100644 --- a/arm-simulation/physics.py +++ b/arm-simulation/physics.py @@ -54,6 +54,7 @@ def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): math.radians(-75), math.radians(255), True, + 0, ) self.encoderSim = wpilib.simulation.EncoderSim(robot.encoder) self.motorSim = wpilib.simulation.PWMSim(robot.motor.getChannel()) diff --git a/commands-v2/armbot/subsystems/drivesubsystem.py b/commands-v2/armbot/subsystems/drivesubsystem.py index f742c999..b3fac18d 100644 --- a/commands-v2/armbot/subsystems/drivesubsystem.py +++ b/commands-v2/armbot/subsystems/drivesubsystem.py @@ -9,7 +9,7 @@ import constants -class DriveSubsystem(commands2.SubsystemBase): +class DriveSubsystem(commands2.Subsystem): # Creates a new DriveSubsystem def __init__(self) -> None: super().__init__() diff --git a/commands-v2/armbotoffboard/subsystems/drivesubsystem.py b/commands-v2/armbotoffboard/subsystems/drivesubsystem.py index 8e5f83d6..c7fc13c6 100644 --- a/commands-v2/armbotoffboard/subsystems/drivesubsystem.py +++ b/commands-v2/armbotoffboard/subsystems/drivesubsystem.py @@ -10,7 +10,7 @@ import typing -class DriveSubsystem(commands2.SubsystemBase): +class DriveSubsystem(commands2.Subsystem): def __init__(self) -> None: super().__init__() diff --git a/commands-v2/drive-distance-offboard/subsystems/drivesubsystem.py b/commands-v2/drive-distance-offboard/subsystems/drivesubsystem.py index 65423eee..4dd8888d 100644 --- a/commands-v2/drive-distance-offboard/subsystems/drivesubsystem.py +++ b/commands-v2/drive-distance-offboard/subsystems/drivesubsystem.py @@ -13,7 +13,7 @@ import examplesmartmotorcontroller -class DriveSubsystem(commands2.SubsystemBase): +class DriveSubsystem(commands2.Subsystem): def __init__(self) -> None: """Creates a new DriveSubsystem""" super().__init__() diff --git a/commands-v2/frisbee-bot/subsystems/drivesubsystem.py b/commands-v2/frisbee-bot/subsystems/drivesubsystem.py index e13991a2..331b818e 100644 --- a/commands-v2/frisbee-bot/subsystems/drivesubsystem.py +++ b/commands-v2/frisbee-bot/subsystems/drivesubsystem.py @@ -10,7 +10,7 @@ import constants -class DriveSubsystem(commands2.SubsystemBase): +class DriveSubsystem(commands2.Subsystem): def __init__(self) -> None: """Creates a new DriveSubsystem""" super().__init__() diff --git a/commands-v2/gyro-drive-commands/subsystems/drivesubsystem.py b/commands-v2/gyro-drive-commands/subsystems/drivesubsystem.py index 518479fd..0298addc 100644 --- a/commands-v2/gyro-drive-commands/subsystems/drivesubsystem.py +++ b/commands-v2/gyro-drive-commands/subsystems/drivesubsystem.py @@ -11,7 +11,7 @@ import constants -class DriveSubsystem(commands2.SubsystemBase): +class DriveSubsystem(commands2.Subsystem): def __init__(self) -> None: """Creates a new DriveSubsystem""" super().__init__() diff --git a/commands-v2/hatchbot-inlined/commands/autos.py b/commands-v2/hatchbot-inlined/commands/autos.py index c46d544d..393a2f9b 100644 --- a/commands-v2/hatchbot-inlined/commands/autos.py +++ b/commands-v2/hatchbot-inlined/commands/autos.py @@ -20,6 +20,7 @@ class Autos: def __init__(self) -> None: raise Exception("This is a utility class!") + @staticmethod def simpleAuto(drive: subsystems.drivesubsystem.DriveSubsystem): """A simple auto routine that drives forward a specified distance, and then stops.""" return commands2.FunctionalCommand( @@ -35,42 +36,41 @@ def simpleAuto(drive: subsystems.drivesubsystem.DriveSubsystem): # Require the drive subsystem ) + @staticmethod def complexAuto( driveSubsystem: subsystems.drivesubsystem.DriveSubsystem, hatchSubsystem: subsystems.hatchsubsystem.HatchSubsystem, ): """A complex auto routine that drives forward, drops a hatch, and then drives backward.""" return commands2.cmd.sequence( - [ - # Drive forward up to the front of the cargo ship - commands2.FunctionalCommand( - # Reset encoders on command start - driveSubsystem.resetEncoders, - # Drive forward while the command is executing - lambda: driveSubsystem.arcadeDrive(constants.kAutoDriveSpeed, 0), - # Stop driving at the end of the command - lambda interrupt: driveSubsystem.arcadeDrive(0, 0), - # End the command when the robot's driven distance exceeds the desired value - lambda: driveSubsystem.getAverageEncoderDistance() - >= constants.kAutoDriveDistanceInches, - # Require the drive subsystem - [driveSubsystem], - ), - # Release the hatch - hatchSubsystem.releaseHatch(), - # Drive backward the specified distance - commands2.FunctionalCommand( - # Reset encoders on command start - driveSubsystem.resetEncoders, - # Drive backwards while the command is executing - lambda: driveSubsystem.arcadeDrive(-constants.kAutoDriveSpeed, 0), - # Stop driving at the end of the command - lambda interrupt: driveSubsystem.arcadeDrive(0, 0), - # End the command when the robot's driven distance exceeds the desired value - lambda: abs(driveSubsystem.getAverageEncoderDistance()) - >= constants.kAutoBackupDistanceInches, - # Require the drive subsystem - [driveSubsystem], - ), - ] + # Drive forward up to the front of the cargo ship + commands2.FunctionalCommand( + # Reset encoders on command start + driveSubsystem.resetEncoders, + # Drive forward while the command is executing + lambda: driveSubsystem.arcadeDrive(constants.kAutoDriveSpeed, 0), + # Stop driving at the end of the command + lambda interrupt: driveSubsystem.arcadeDrive(0, 0), + # End the command when the robot's driven distance exceeds the desired value + lambda: driveSubsystem.getAverageEncoderDistance() + >= constants.kAutoDriveDistanceInches, + # Require the drive subsystem + driveSubsystem, + ), + # Release the hatch + hatchSubsystem.releaseHatch(), + # Drive backward the specified distance + commands2.FunctionalCommand( + # Reset encoders on command start + driveSubsystem.resetEncoders, + # Drive backwards while the command is executing + lambda: driveSubsystem.arcadeDrive(-constants.kAutoDriveSpeed, 0), + # Stop driving at the end of the command + lambda interrupt: driveSubsystem.arcadeDrive(0, 0), + # End the command when the robot's driven distance exceeds the desired value + lambda: abs(driveSubsystem.getAverageEncoderDistance()) + >= constants.kAutoBackupDistanceInches, + # Require the drive subsystem + driveSubsystem, + ), ) diff --git a/commands-v2/hatchbot-inlined/physics.py b/commands-v2/hatchbot-inlined/physics.py index 08832a8c..8c312ab2 100644 --- a/commands-v2/hatchbot-inlined/physics.py +++ b/commands-v2/hatchbot-inlined/physics.py @@ -11,8 +11,7 @@ import wpilib import wpilib.simulation -from wpimath.system import LinearSystemId -from wpimath.system.plant import DCMotor +from wpimath.system.plant import DCMotor, LinearSystemId import constants diff --git a/commands-v2/hatchbot-inlined/robotcontainer.py b/commands-v2/hatchbot-inlined/robotcontainer.py index eb6f8d66..e2b677bb 100644 --- a/commands-v2/hatchbot-inlined/robotcontainer.py +++ b/commands-v2/hatchbot-inlined/robotcontainer.py @@ -54,7 +54,7 @@ def __init__(self) -> None: -self.driverController.getLeftY(), -self.driverController.getRightX(), ), - [self.driveSubsystem], + self.driveSubsystem, ) ) diff --git a/commands-v2/hatchbot-inlined/subsystems/drivesubsystem.py b/commands-v2/hatchbot-inlined/subsystems/drivesubsystem.py index 927e5369..40b8b8a3 100644 --- a/commands-v2/hatchbot-inlined/subsystems/drivesubsystem.py +++ b/commands-v2/hatchbot-inlined/subsystems/drivesubsystem.py @@ -5,7 +5,7 @@ import constants -class DriveSubsystem(commands2.SubsystemBase): +class DriveSubsystem(commands2.Subsystem): def __init__(self) -> None: super().__init__() diff --git a/commands-v2/hatchbot-inlined/subsystems/hatchsubsystem.py b/commands-v2/hatchbot-inlined/subsystems/hatchsubsystem.py index 433be287..7977057a 100644 --- a/commands-v2/hatchbot-inlined/subsystems/hatchsubsystem.py +++ b/commands-v2/hatchbot-inlined/subsystems/hatchsubsystem.py @@ -5,7 +5,7 @@ import constants -class HatchSubsystem(commands2.SubsystemBase): +class HatchSubsystem(commands2.Subsystem): def __init__(self) -> None: super().__init__() @@ -18,11 +18,11 @@ def __init__(self) -> None: def grabHatch(self) -> commands2.Command: """Grabs the hatch""" return commands2.cmd.runOnce( - lambda: self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kForward), [self] + lambda: self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kForward), self ) def releaseHatch(self) -> commands2.Command: """Releases the hatch""" return commands2.cmd.runOnce( - lambda: self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kReverse), [self] + lambda: self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kReverse), self ) diff --git a/commands-v2/hatchbot/commands/defaultdrive.py b/commands-v2/hatchbot/commands/defaultdrive.py index 5a2a3aae..b6be4cf5 100644 --- a/commands-v2/hatchbot/commands/defaultdrive.py +++ b/commands-v2/hatchbot/commands/defaultdrive.py @@ -3,7 +3,7 @@ from subsystems.drivesubsystem import DriveSubsystem -class DefaultDrive(commands2.CommandBase): +class DefaultDrive(commands2.Command): def __init__( self, drive: DriveSubsystem, @@ -16,7 +16,7 @@ def __init__( self.forward = forward self.rotation = rotation - self.addRequirements([self.drive]) + self.addRequirements(self.drive) def execute(self) -> None: self.drive.arcadeDrive(self.forward(), self.rotation()) diff --git a/commands-v2/hatchbot/commands/drivedistance.py b/commands-v2/hatchbot/commands/drivedistance.py index f5d7fa77..8ace7949 100644 --- a/commands-v2/hatchbot/commands/drivedistance.py +++ b/commands-v2/hatchbot/commands/drivedistance.py @@ -3,7 +3,7 @@ from subsystems.drivesubsystem import DriveSubsystem -class DriveDistance(commands2.CommandBase): +class DriveDistance(commands2.Command): def __init__(self, inches: float, speed: float, drive: DriveSubsystem) -> None: super().__init__() self.distance = inches diff --git a/commands-v2/hatchbot/commands/grabhatch.py b/commands-v2/hatchbot/commands/grabhatch.py index c87a1277..719f8f8e 100644 --- a/commands-v2/hatchbot/commands/grabhatch.py +++ b/commands-v2/hatchbot/commands/grabhatch.py @@ -2,7 +2,7 @@ from subsystems.hatchsubsystem import HatchSubsystem -class GrabHatch(commands2.CommandBase): +class GrabHatch(commands2.Command): def __init__(self, hatch: HatchSubsystem) -> None: super().__init__() self.hatch = hatch diff --git a/commands-v2/hatchbot/commands/halvedrivespeed.py b/commands-v2/hatchbot/commands/halvedrivespeed.py index 1d77b0a4..0546d44d 100644 --- a/commands-v2/hatchbot/commands/halvedrivespeed.py +++ b/commands-v2/hatchbot/commands/halvedrivespeed.py @@ -3,7 +3,7 @@ from subsystems.drivesubsystem import DriveSubsystem -class HalveDriveSpeed(commands2.CommandBase): +class HalveDriveSpeed(commands2.Command): def __init__(self, drive: DriveSubsystem) -> None: super().__init__() self.drive = drive diff --git a/commands-v2/hatchbot/physics.py b/commands-v2/hatchbot/physics.py index ee916a51..1e869203 100644 --- a/commands-v2/hatchbot/physics.py +++ b/commands-v2/hatchbot/physics.py @@ -11,8 +11,7 @@ import wpilib import wpilib.simulation -from wpimath.system import LinearSystemId -from wpimath.system.plant import DCMotor +from wpimath.system.plant import DCMotor, LinearSystemId import constants diff --git a/commands-v2/hatchbot/robotcontainer.py b/commands-v2/hatchbot/robotcontainer.py index d7199905..62414c06 100644 --- a/commands-v2/hatchbot/robotcontainer.py +++ b/commands-v2/hatchbot/robotcontainer.py @@ -72,15 +72,15 @@ def configureButtonBindings(self): and then passing it to a JoystickButton. """ - commands2.button.JoystickButton(self.driverController, 1).whenPressed( + commands2.button.JoystickButton(self.driverController, 1).onTrue( GrabHatch(self.hatch) ) - commands2.button.JoystickButton(self.driverController, 2).whenPressed( + commands2.button.JoystickButton(self.driverController, 2).onTrue( ReleaseHatch(self.hatch) ) - commands2.button.JoystickButton(self.driverController, 3).whenHeld( + commands2.button.JoystickButton(self.driverController, 3).whileTrue( HalveDriveSpeed(self.drive) ) diff --git a/commands-v2/hatchbot/subsystems/drivesubsystem.py b/commands-v2/hatchbot/subsystems/drivesubsystem.py index 927e5369..40b8b8a3 100644 --- a/commands-v2/hatchbot/subsystems/drivesubsystem.py +++ b/commands-v2/hatchbot/subsystems/drivesubsystem.py @@ -5,7 +5,7 @@ import constants -class DriveSubsystem(commands2.SubsystemBase): +class DriveSubsystem(commands2.Subsystem): def __init__(self) -> None: super().__init__() diff --git a/commands-v2/hatchbot/subsystems/hatchsubsystem.py b/commands-v2/hatchbot/subsystems/hatchsubsystem.py index 78936582..df261645 100644 --- a/commands-v2/hatchbot/subsystems/hatchsubsystem.py +++ b/commands-v2/hatchbot/subsystems/hatchsubsystem.py @@ -4,7 +4,7 @@ import constants -class HatchSubsystem(commands2.SubsystemBase): +class HatchSubsystem(commands2.Subsystem): def __init__(self) -> None: super().__init__() diff --git a/commands-v2/ramsete/subsystems/drivetrain.py b/commands-v2/ramsete/subsystems/drivetrain.py index cdbc08ef..281ca67d 100644 --- a/commands-v2/ramsete/subsystems/drivetrain.py +++ b/commands-v2/ramsete/subsystems/drivetrain.py @@ -1,4 +1,4 @@ -from commands2 import SubsystemBase +from commands2 import Subsystem from wpilib import MotorControllerGroup, PWMSparkMax, Encoder, AnalogGyro from wpilib.drive import DifferentialDrive @@ -9,7 +9,7 @@ import constants -class Drivetrain(SubsystemBase): +class Drivetrain(Subsystem): def __init__(self): super().__init__() diff --git a/commands-v2/romi/commands/arcadedrive.py b/commands-v2/romi/commands/arcadedrive.py index 639d16f9..f1d7c907 100644 --- a/commands-v2/romi/commands/arcadedrive.py +++ b/commands-v2/romi/commands/arcadedrive.py @@ -7,7 +7,7 @@ from subsystems.drivetrain import Drivetrain -class ArcadeDrive(commands2.CommandBase): +class ArcadeDrive(commands2.Command): def __init__( self, drive: Drivetrain, diff --git a/commands-v2/romi/commands/drivedistance.py b/commands-v2/romi/commands/drivedistance.py index 30442e23..1f3cb01a 100644 --- a/commands-v2/romi/commands/drivedistance.py +++ b/commands-v2/romi/commands/drivedistance.py @@ -7,7 +7,7 @@ from subsystems.drivetrain import Drivetrain -class DriveDistance(commands2.CommandBase): +class DriveDistance(commands2.Command): def __init__(self, speed: float, inches: float, drive: Drivetrain) -> None: """Creates a new DriveDistance. This command will drive your your robot for a desired distance at a desired speed. diff --git a/commands-v2/romi/commands/drivetime.py b/commands-v2/romi/commands/drivetime.py index acb3892b..60e04736 100644 --- a/commands-v2/romi/commands/drivetime.py +++ b/commands-v2/romi/commands/drivetime.py @@ -8,7 +8,7 @@ from subsystems.drivetrain import Drivetrain -class DriveTime(commands2.CommandBase): +class DriveTime(commands2.Command): """Creates a new DriveTime. This command will drive your robot for a desired speed and time.""" def __init__(self, speed: float, time: float, drive: Drivetrain) -> None: diff --git a/commands-v2/romi/commands/turndegrees.py b/commands-v2/romi/commands/turndegrees.py index 30d8e8fa..82887561 100644 --- a/commands-v2/romi/commands/turndegrees.py +++ b/commands-v2/romi/commands/turndegrees.py @@ -8,7 +8,7 @@ from subsystems.drivetrain import Drivetrain -class TurnDegrees(commands2.CommandBase): +class TurnDegrees(commands2.Command): def __init__(self, speed: float, degrees: float, drive: Drivetrain) -> None: """Creates a new TurnDegrees. This command will turn your robot for a desired rotation (in degrees) and rotational speed. diff --git a/commands-v2/romi/commands/turntime.py b/commands-v2/romi/commands/turntime.py index cb602d31..9cd135b3 100644 --- a/commands-v2/romi/commands/turntime.py +++ b/commands-v2/romi/commands/turntime.py @@ -8,7 +8,7 @@ from subsystems.drivetrain import Drivetrain -class TurnTime(commands2.CommandBase): +class TurnTime(commands2.Command): """Creates a new TurnTime command. This command will turn your robot for a desired rotational speed and time. """ diff --git a/commands-v2/romi/robotcontainer.py b/commands-v2/romi/robotcontainer.py index 23821c63..28ec7e3e 100644 --- a/commands-v2/romi/robotcontainer.py +++ b/commands-v2/romi/robotcontainer.py @@ -73,7 +73,7 @@ def _configureButtonBindings(self): self.chooser.addOption("Auto Routine Time", AutonomousTime(self.drivetrain)) wpilib.SmartDashboard.putData(self.chooser) - def getAutonomousCommand(self) -> typing.Optional[commands2.CommandBase]: + def getAutonomousCommand(self) -> typing.Optional[commands2.Command]: return self.chooser.getSelected() def getArcadeDriveCommand(self) -> ArcadeDrive: diff --git a/commands-v2/romi/subsystems/drivetrain.py b/commands-v2/romi/subsystems/drivetrain.py index 133f555f..20ee1daf 100644 --- a/commands-v2/romi/subsystems/drivetrain.py +++ b/commands-v2/romi/subsystems/drivetrain.py @@ -10,7 +10,7 @@ import romi -class Drivetrain(commands2.SubsystemBase): +class Drivetrain(commands2.Subsystem): kCountsPerRevolution = 1440.0 kWheelDiameterInch = 2.75591 diff --git a/commands-v2/scheduler-event-logging/robot.py b/commands-v2/scheduler-event-logging/robot.py old mode 100644 new mode 100755 index 3e996977..c57d8a18 --- a/commands-v2/scheduler-event-logging/robot.py +++ b/commands-v2/scheduler-event-logging/robot.py @@ -1,4 +1,4 @@ -#!usr/bin/env python3 +#!/usr/bin/env python3 # Copyright (c) FIRST and other WPILib contributors. # Open Source Software; you can modify and/or share it under the terms of diff --git a/commands-v2/selectcommand/robot.py b/commands-v2/selectcommand/robot.py old mode 100644 new mode 100755 index 3e996977..c57d8a18 --- a/commands-v2/selectcommand/robot.py +++ b/commands-v2/selectcommand/robot.py @@ -1,4 +1,4 @@ -#!usr/bin/env python3 +#!/usr/bin/env python3 # Copyright (c) FIRST and other WPILib contributors. # Open Source Software; you can modify and/or share it under the terms of diff --git a/commands-v2/selectcommand/robotcontainer.py b/commands-v2/selectcommand/robotcontainer.py index 12eca8f3..800083c4 100644 --- a/commands-v2/selectcommand/robotcontainer.py +++ b/commands-v2/selectcommand/robotcontainer.py @@ -32,22 +32,19 @@ def __init__(self) -> None: # selector does not have to be an enum; it could be any desired type (string, integer, # boolean, double...) self.example_select_command = commands2.SelectCommand( - self.select, # Maps selector values to commands - [ - ( - self.CommandSelector.ONE, - commands2.PrintCommand("Command one was selected!"), + { + self.CommandSelector.ONE: commands2.PrintCommand( + "Command one was selected!" ), - ( - self.CommandSelector.TWO, - commands2.PrintCommand("Command two was selected!"), + self.CommandSelector.TWO: commands2.PrintCommand( + "Command two was selected!" ), - ( - self.CommandSelector.THREE, - commands2.PrintCommand("Command three was selected!"), + self.CommandSelector.THREE: commands2.PrintCommand( + "Command three was selected!" ), - ], + }, + self.select, ) # Configure the button bindings diff --git a/elevator-simulation/physics.py b/elevator-simulation/physics.py index 55024a9b..e4d9c82c 100644 --- a/elevator-simulation/physics.py +++ b/elevator-simulation/physics.py @@ -46,6 +46,7 @@ def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): robot.kMinElevatorHeight, robot.kMaxElevatorHeight, True, + 0, [0.01], ) self.encoderSim = wpilib.simulation.EncoderSim(robot.encoder) diff --git a/run_tests.sh b/run_tests.sh index d53086fc..3e0dbb94 100755 --- a/run_tests.sh +++ b/run_tests.sh @@ -7,16 +7,8 @@ BASE_TESTS=" addressableled arcade-drive arm-simulation - commands-v2/armbot - commands-v2/armbotoffboard - commands-v2/drive-distance-offboard - commands-v2/frisbee-bot - commands-v2/gyro-drive-commands commands-v2/hatchbot commands-v2/hatchbot-inlined - commands-v2/ramsete - commands-v2/scheduler-event-logging - commands-v2/selectcommand cscore-intermediate-vision cscore-quick-vision elevator-profiled-pid @@ -42,6 +34,14 @@ BASE_TESTS=" " IGNORED_TESTS=" + commands-v2/armbot + commands-v2/armbotoffboard + commands-v2/drive-distance-offboard + commands-v2/frisbee-bot + commands-v2/gyro-drive-commands + commands-v2/ramsete + commands-v2/scheduler-event-logging + commands-v2/selectcommand commands-v2/romi physics-camsim/src " diff --git a/timed/tests/basic_test.py b/timed/tests/basic_test.py index e7f0eef0..933b7e45 100644 --- a/timed/tests/basic_test.py +++ b/timed/tests/basic_test.py @@ -36,9 +36,9 @@ def test_operator_control(control, robot): # Enable the robot, check to see if it was set control.step_timing(seconds=0.1, autonomous=False, enabled=True) - assert 0.5 == pytest.approx(motorsim.getSpeed()) + assert 0.5 == pytest.approx(motorsim.getSpeed(), rel=0.01) # change it, see if it's still set joysim.setY(0.2) control.step_timing(seconds=0.1, autonomous=False, enabled=True) - assert 0.2 == pytest.approx(motorsim.getSpeed()) + assert 0.2 == pytest.approx(motorsim.getSpeed(), rel=0.01)