[copybara] Import robotpy examples (#8608)

GitOrigin-RevId: 9ba4bc3040fa7e772f5a594039e78fc6c43d114e

robotpyExamples/ElevatorExponentialProfile/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 enum
+
+
+class ExampleSmartMotorController:
+    """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.
+
+        :param port: The port for the controller.
+        """
+        self._port = port
+        self._inverted = False
+
+    def setPID(self, kp: float, ki: float, kd: float) -> None:
+        """Example method for setting the PID gains of the smart controller.
+
+        :param kp: The proportional gain.
+        :param ki: The integral gain.
+        :param kd: The derivative gain.
+        """
+        pass
+
+    def setSetpoint(
+        self,
+        mode: "ExampleSmartMotorController.PIDMode",
+        setpoint: float,
+        arbFeedforward: float,
+    ) -> None:
+        """Example method for setting the setpoint of the smart controller in PID mode.
+
+        :param mode: The mode of the PID controller.
+        :param setpoint: The controller setpoint.
+        :param arbFeedforward: An arbitrary feedforward output (from -1 to 1).
+        """
+        pass
+
+    def follow(self, leader: "ExampleSmartMotorController") -> None:
+        """Places this motor controller in follower mode.
+
+        :param 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:
+        return 0
+
+    def setInverted(self, isInverted: bool) -> None:
+        self._inverted = isInverted
+
+    def getInverted(self) -> bool:
+        return self._inverted
+
+    def disable(self) -> None:
+        pass
+
+    def stopMotor(self) -> None:
+        pass

robotpyExamples/ElevatorExponentialProfile/robot.py

@@ -0,0 +1,54 @@
+#!/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
+
+from examplesmartmotorcontroller import ExampleSmartMotorController
+
+
+class MyRobot(wpilib.TimedRobot):
+    kDt = 0.02
+
+    def __init__(self) -> None:
+        super().__init__()
+        self.joystick = wpilib.Joystick(1)
+        self.motor = ExampleSmartMotorController(1)
+        # Note: These gains are fake, and will have to be tuned for your robot.
+        self.feedforward = wpimath.SimpleMotorFeedforwardMeters(1, 1, 1)
+
+        # Create a motion profile with the given maximum voltage and characteristics kV, kA
+        # These gains should match your feedforward kV, kA for best results.
+        self.profile = wpimath.ExponentialProfileMeterVolts(
+            wpimath.ExponentialProfileMeterVolts.Constraints.fromCharacteristics(
+                10, 1, 1
+            )
+        )
+        self.goal = wpimath.ExponentialProfileMeterVolts.State(0, 0)
+        self.setpoint = wpimath.ExponentialProfileMeterVolts.State(0, 0)
+
+        # 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) -> None:
+        if self.joystick.getRawButtonPressed(2):
+            self.goal = wpimath.ExponentialProfileMeterVolts.State(5, 0)
+        elif self.joystick.getRawButtonPressed(3):
+            self.goal = wpimath.ExponentialProfileMeterVolts.State(0, 0)
+
+        # Retrieve the profiled setpoint for the next timestep. This setpoint moves
+        # toward the goal while obeying the constraints.
+        next_state = self.profile.calculate(self.kDt, self.setpoint, self.goal)
+
+        # Send setpoint to offboard controller PID
+        self.motor.setSetpoint(
+            ExampleSmartMotorController.PIDMode.kPosition,
+            self.setpoint.position,
+            self.feedforward.calculate(next_state.velocity) / 12.0,
+        )
+
+        self.setpoint = next_state