Add elevator trapezoid profile example

commands-v2/armbotoffboard/examplesmartmotorcontroller.py

@@ -34,7 +34,9 @@ def setPID(self, kp: float, ki: float, kd: float) -> None:
         """
         pass
 
-    def setSetPoint(mode: PIDMode, setpoint: float, arbfeedforward: float) -> None:
+    def setSetPoint(
+        self, mode: PIDMode, setpoint: float, arbfeedforward: float
+    ) -> None:
         """Example method for setting the setpoint of the smart controller in PID mode.
 
         Args:

elevator-trapezoid-profile/examplesmartmotorcontroller.py

@@ -0,0 +1,93 @@
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+
+import wpilib.interfaces
+import enum
+
+
+class ExampleSmartMotorController(wpilib.interfaces.MotorController):
+    """A simplified stub class that simulates the API of a common "smart" motor controller.
+    Has no actual functionality.
+    """
+
+    class PIDMode(enum.Enum):
+        kPosition = enum.auto()
+        kVelocity = enum.auto()
+        kMovementWitchcraft = enum.auto()
+
+    def __init__(self, port: int) -> None:
+        """Creates a new ExampleSmartMotorController.
+
+        Args:
+            port: The port for the controller.
+        """
+        super().__init__()
+
+    def setPID(self, kp: float, ki: float, kd: float) -> None:
+        """Example method for setting the PID gains of the smart controller.
+
+        Args:
+            kp: The proportional gain.
+            ki: The integral gain.
+            kd: The derivative gain.
+        """
+        pass
+
+    def setSetPoint(
+        self, mode: PIDMode, setpoint: float, arbfeedforward: float
+    ) -> None:
+        """Example method for setting the setpoint of the smart controller in PID mode.
+
+        Args:
+            mode: The mode of the PID controller.
+            setpoint: The controller setpoint.
+            arbfeedforward: An arbitrary feedforward output (from -1 to 1).
+        """
+        pass
+
+    def follow(self, leader: "ExampleSmartMotorController") -> None:
+        """Places this motor controller in follower mode.
+
+        Args:
+            leader: The leader to follow.
+        """
+        pass
+
+    def getEncoderDistance(self) -> float:
+        """Returns the encoder distance.
+
+        Returns:
+            The current encoder distance.
+        """
+        return 0
+
+    def getEncoderRate(self) -> float:
+        """Returns the encoder rate.
+
+        Returns:
+            The current encoder rate.
+        """
+        return 0
+
+    def resetEncoder(self) -> None:
+        """Resets the encoder to zero distance."""
+        pass
+
+    def set(self, speed: float) -> None:
+        pass
+
+    def get(self) -> float:
+        pass
+
+    def setInverted(isInverted: bool) -> None:
+        pass
+
+    def getInverted(self) -> bool:
+        pass
+
+    def disable(self) -> None:
+        pass
+
+    def stopMotor(self) -> None:
+        pass

elevator-trapezoid-profile/robot.py

@@ -0,0 +1,58 @@
+#!/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
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+import wpimath.controller
+import wpimath.trajectory
+import examplesmartmotorcontroller
+
+
+class MyRobot(wpilib.TimedRobot):
+
+    kDt = 0.02
+
+    def robotInit(self):
+        self.joystick = wpilib.Joystick(1)
+        self.motor = examplesmartmotorcontroller.ExampleSmartMotorController(1)
+        # Note: These gains are fake, and will have to be tuned for your robot.
+        self.feedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 1.5)
+
+        self.constraints = wpimath.trajectory.TrapezoidProfile.Constraints(1.75, 0.75)
+
+        self.goal = wpimath.trajectory.TrapezoidProfile.State()
+        self.setpoint = wpimath.trajectory.TrapezoidProfile.State()
+
+        # Note: These gains are fake, and will have to be tuned for your robot.
+        self.motor.setPID(1.3, 0.0, 0.7)
+
+    def teleopPeriodic(self):
+        if self.joystick.getRawButtonPressed(2):
+            self.goal = wpimath.trajectory.TrapezoidProfile.State(5, 0)
+        elif self.joystick.getRawButtonPressed(3):
+            self.goal = wpimath.trajectory.TrapezoidProfile.State(0, 0)
+
+        # Create a motion profile with the given maximum velocity and maximum
+        # acceleration constraints for the next setpoint, the desired goal, and the
+        # current setpoint.
+        profile = wpimath.trajectory.TrapezoidProfile(
+            self.constraints, self.goal, self.setpoint
+        )
+
+        # Retrieve the profiled setpoint for the next timestep. This setpoint moves
+        # toward the goal while obeying the constraints.
+        self.setpoint = profile.calculate(self.kDt)
+
+        # Send setpoint to offboard controller PID
+        self.motor.setSetPoint(
+            examplesmartmotorcontroller.ExampleSmartMotorController.PIDMode.kPosition,
+            self.setpoint.position,
+            self.feedforward.calculate(self.setpoint.velocity) / 12,
+        )
+
+
+if __name__ == "__main__":
+    wpilib.run(MyRobot)

run_tests.sh

@@ -27,6 +27,7 @@ BASE_TESTS="
   tank-drive
   timed/src
   elevator-profiled-pid
+  elevator-trapezoid-profile
 "
 
 IGNORED_TESTS="