Sync ramsete example with java ramsete-command example (#96)

Author: fletch3555

ramsete/constants.py

@@ -37,25 +37,26 @@
 kEncoderCPR = 1024
 kWheelDiameterMeters = 0.15
 
-# The following works assuming the encoders are directly mounted to the wheel shafts.
+# Assumes the encoders are directly mounted on the wheel shafts
 kEncoderDistancePerPulse = (kWheelDiameterMeters * math.pi) / kEncoderCPR
 
-# NOTE: Please do NOT use these values on your robot. Rather, characterize your
-# drivetrain using the FRC Characterization tool. These are for demo purposes
-# only!
+# These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+# These characterization values MUST be determined either experimentally or theoretically
+# for *your* robot's drive.
+# The Robot Characterization Toolsuite provides a convenient tool for obtaining these
+# values for your robot.
 ksVolts = 0.22
 kvVoltSecondsPerMeter = 1.98
 kaVoltSecondsSquaredPerMeter = 0.2
 
-# The P gain for our turn controllers.
+# Example value only - as above, this must be tuned for your drive!
 kPDriveVel = 8.5
 
 # The max velocity and acceleration for our autonomous.
 kMaxSpeedMetersPerSecond = 3
-kMaxAccelerationMetersPerSecondSquared = 3
+kMaxAccelerationMetersPerSecondSquared = 1
 
-# Baseline values for a RAMSETE follower in units of meters
-# and seconds. These are recommended, but may be changes if wished.
+# Reasonable baseline values for a RAMSETE follower in units of meters and seconds.
 kRamseteB = 2
 kRamseteZeta = 0.7
 

ramsete/physics.py

@@ -22,8 +22,7 @@
     EncoderSim,
     AnalogGyroSim,
 )
-from wpimath.system import LinearSystemId
-from wpimath.system.plant import DCMotor
+from wpimath.system.plant import DCMotor, LinearSystemId
 
 import constants
 
@@ -74,10 +73,10 @@ def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"):
             (constants.kWheelDiameterMeters / 2),
         )
 
-        self.leftEncoderSim = EncoderSim(robot.container.robotDrive.leftEncoder)
-        self.rightEncoderSim = EncoderSim(robot.container.robotDrive.rightEncoder)
+        self.leftEncoderSim = EncoderSim(robot.robotContainer.robotDrive.leftEncoder)
+        self.rightEncoderSim = EncoderSim(robot.robotContainer.robotDrive.rightEncoder)
 
-        self.gyro = AnalogGyroSim(robot.container.robotDrive.gyro)
+        self.gyro = AnalogGyroSim(robot.robotContainer.robotDrive.gyro)
 
     def update_sim(self, now: float, tm_diff: float) -> None:
         """

ramsete/robot.py

@@ -30,20 +30,19 @@ def robotInit(self) -> None:
 
         # Instantiate our RobotContainer.  This will perform all our button bindings, and put our
         # autonomous chooser on the dashboard.
-        self.container = RobotContainer()
+        self.robotContainer = RobotContainer()
 
     def disabledInit(self) -> None:
         """This function is called once each time the robot enters Disabled mode."""
-        if self.autonomousCommand:
-            self.autonomousCommand.cancel()
 
     def disabledPeriodic(self) -> None:
         """This function is called periodically when disabled"""
 
     def autonomousInit(self) -> None:
         """This autonomous runs the autonomous command selected by your RobotContainer class."""
-        self.autonomousCommand = self.container.getAutonomousCommand()
+        self.autonomousCommand = self.robotContainer.getAutonomousCommand()
 
+        # schedule the autonomous command (example)
         if self.autonomousCommand:
             self.autonomousCommand.schedule()
 
@@ -65,6 +64,9 @@ def testInit(self) -> None:
         # Cancels all running commands at the start of test mode
         commands2.CommandScheduler.getInstance().cancelAll()
 
+    def testPeriodic(self) -> None:
+        """This function is called periodically during test mode"""
+
 
 if __name__ == "__main__":
     wpilib.run(MyRobot)

ramsete/robotcontainer.py

@@ -4,66 +4,73 @@
 # the WPILib BSD license file in the root directory of this project.
 #
 
-from commands2 import RunCommand, RamseteCommand
-from commands2.button import JoystickButton, Button
-
 from wpilib import XboxController
 from wpimath.controller import (
     RamseteController,
     PIDController,
     SimpleMotorFeedforwardMeters,
 )
-
-from wpimath.kinematics import ChassisSpeeds
-
-from wpilib.interfaces import GenericHID
-
-from wpimath.trajectory.constraint import DifferentialDriveVoltageConstraint
-from wpimath.trajectory import TrajectoryConfig, TrajectoryGenerator, Trajectory
-
 from wpimath.geometry import Pose2d, Rotation2d, Translation2d
+from wpimath.trajectory.constraint import DifferentialDriveVoltageConstraint
+from wpimath.trajectory import TrajectoryConfig, TrajectoryGenerator
 
-from subsystems.drivetrain import Drivetrain
+from commands2 import InstantCommand, RunCommand, RamseteCommand, cmd
+from commands2.button import JoystickButton
 
+from subsystems.driveSubsystem import DriveSubsystem
 import constants
 
 
 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.
+    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 {@link 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):
-        # Create the driver's controller.
-        self.driverController = XboxController(constants.kDriverControllerPort)
+        # The robot's subsystems
+        self.robotDrive = DriveSubsystem()
 
-        # Create an instance of the drivetrain subsystem.
-        self.robotDrive = Drivetrain()
+        # The driver's controller.
+        self.driverController = XboxController(constants.kDriverControllerPort)
 
-        # Configure and set the button bindings for the driver's controller.
+        # Configure the button bindings
         self.configureButtons()
 
-        # Set the default command for the drive subsystem. It's default command will allow
-        # the robot to drive with the controller.
-
+        # Configure default commands
+        # Set the default drive command to split-stick arcade drive
         self.robotDrive.setDefaultCommand(
+            # A split-stick arcade command, with forward/backward controlled by the left
+            # hand, and turning controlled by the right.
             RunCommand(
                 lambda: self.robotDrive.arcadeDrive(
-                    -self.driverController.getRawAxis(1),
-                    self.driverController.getRawAxis(2) * 0.65,
+                    -self.driverController.getLeftY(),
+                    -self.driverController.getRightX(),
                 ),
                 self.robotDrive,
             )
         )
 
+    def configureButtons(self):
+        """
+        Use this method to define your button->command mappings. Buttons can be created by
+        instantiating a GenericHID or one of its subclasses (Joystick or XboxController),
+        and then calling passing it to a JoystickButton.
+        """
+
+        # Drive at half speed when the right bumper is held
+        (
+            JoystickButton(self.driverController, XboxController.Button.kRightBumper)
+            .onTrue(InstantCommand(lambda: self.robotDrive.setMaxOutput(0.5)))
+            .onFalse(InstantCommand(lambda: self.robotDrive.setMaxOutput(1)))
+        )
+
     def getAutonomousCommand(self):
-        """Returns the command to be ran during the autonomous period."""
+        """Use this to pass the autonomous command to the main {@link Robot} class."""
 
         # Create a voltage constraint to ensure we don't accelerate too fast.
-
         autoVoltageConstraint = DifferentialDriveVoltageConstraint(
             SimpleMotorFeedforwardMeters(
                 constants.ksVolts,
@@ -74,84 +81,54 @@ def getAutonomousCommand(self):
             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.
+        # Create config for trajectory
         config = TrajectoryConfig(
             constants.kMaxSpeedMetersPerSecond,
             constants.kMaxAccelerationMetersPerSecondSquared,
         )
-
-        # Ensures that the max speed is actually obeyed.
+        # Add kinematics to ensure max speed is actually obeyed
         config.setKinematics(constants.kDriveKinematics)
-
-        # Apply the previously defined voltage constraint.
+        # Apply the 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,
+            # Start at the origin facing the +x direction.
+            Pose2d(0, 0, Rotation2d(0)),
+            # Pass through these two interior waypoints, making an 's' curve path
+            [Translation2d(1, 1), Translation2d(2, -1)],
+            # End 3 meters straight ahead of where we started, facing forward
+            Pose2d(3, 0, Rotation2d(0)),
+            # Pass config
             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.
+            # RamseteCommand passes volts to the callback
             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.
-
+        # Reset odometry to the initial pose of the trajectory, run path following
+        # command, then stop at the end.
         (
-            JoystickButton(self.driverController, XboxController.Button.kRightBumper)
-            .whenPressed(lambda: self.robotDrive.setSlowMaxOutput(0.5))
-            .whenReleased(lambda: self.robotDrive.setNormalMaxOutput(1))
+            cmd.runOnce(
+                lambda: self.robotDrive.resetOdometry(
+                    self.exampleTrajectory.initialPose()
+                )
+            )
+            .andThen(ramseteCommand)
+            .andThen(cmd.runOnce(lambda: self.robotDrive.tankDriveVolts(0, 0)))
         )