[wpimath] Add Exponential motion profile (#5720)

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/ExampleSmartMotorController.java

@@ -0,0 +1,96 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.elevatorexponentialprofile;
+
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+
+/**
+ * A simplified stub class that simulates the API of a common "smart" motor controller.
+ *
+ * <p>Has no actual functionality.
+ */
+public class ExampleSmartMotorController implements MotorController {
+  public enum PIDMode {
+    kPosition,
+    kVelocity,
+    kMovementWitchcraft
+  }
+
+  /**
+   * Creates a new ExampleSmartMotorController.
+   *
+   * @param port The port for the controller.
+   */
+  @SuppressWarnings("PMD.UnusedFormalParameter")
+  public ExampleSmartMotorController(int port) {}
+
+  /**
+   * 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.
+   */
+  public void setPID(double kp, double ki, double kd) {}
+
+  /**
+   * 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).
+   */
+  public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
+
+  /**
+   * Places this motor controller in follower mode.
+   *
+   * @param leader The leader to follow.
+   */
+  public void follow(ExampleSmartMotorController leader) {}
+
+  /**
+   * Returns the encoder distance.
+   *
+   * @return The current encoder distance.
+   */
+  public double getEncoderDistance() {
+    return 0;
+  }
+
+  /**
+   * Returns the encoder rate.
+   *
+   * @return The current encoder rate.
+   */
+  public double getEncoderRate() {
+    return 0;
+  }
+
+  /** Resets the encoder to zero distance. */
+  public void resetEncoder() {}
+
+  @Override
+  public void set(double speed) {}
+
+  @Override
+  public double get() {
+    return 0;
+  }
+
+  @Override
+  public void setInverted(boolean isInverted) {}
+
+  @Override
+  public boolean getInverted() {
+    return false;
+  }
+
+  @Override
+  public void disable() {}
+
+  @Override
+  public void stopMotor() {}
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.elevatorexponentialprofile;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java

@@ -0,0 +1,53 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.elevatorexponentialprofile;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.trajectory.ExponentialProfile;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.TimedRobot;
+
+public class Robot extends TimedRobot {
+  private static double kDt = 0.02;
+
+  private final Joystick m_joystick = new Joystick(1);
+  private final ExampleSmartMotorController m_motor = new ExampleSmartMotorController(1);
+  // Note: These gains are fake, and will have to be tuned for your robot.
+  private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(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.
+  private final ExponentialProfile m_profile =
+      new ExponentialProfile(ExponentialProfile.Constraints.fromCharacteristics(10, 1, 1));
+  private ExponentialProfile.State m_goal = new ExponentialProfile.State(0, 0);
+  private ExponentialProfile.State m_setpoint = new ExponentialProfile.State(0, 0);
+
+  @Override
+  public void robotInit() {
+    // Note: These gains are fake, and will have to be tuned for your robot.
+    m_motor.setPID(1.3, 0.0, 0.7);
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    if (m_joystick.getRawButtonPressed(2)) {
+      m_goal = new ExponentialProfile.State(5, 0);
+    } else if (m_joystick.getRawButtonPressed(3)) {
+      m_goal = new ExponentialProfile.State(0, 0);
+    }
+
+    // Retrieve the profiled setpoint for the next timestep. This setpoint moves
+    // toward the goal while obeying the constraints.
+    ExponentialProfile.State next = m_profile.calculate(kDt, m_setpoint, m_goal);
+
+    // Send setpoint to offboard controller PID
+    m_motor.setSetpoint(
+        ExampleSmartMotorController.PIDMode.kPosition,
+        m_setpoint.position,
+        m_feedforward.calculate(m_setpoint.velocity, next.velocity, 0.02) / 12.0);
+
+    m_setpoint = next;
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Constants.java

@@ -0,0 +1,39 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.elevatorexponentialsimulation;
+
+import edu.wpi.first.math.util.Units;
+
+public class Constants {
+  public static final int kMotorPort = 0;
+  public static final int kEncoderAChannel = 0;
+  public static final int kEncoderBChannel = 1;
+  public static final int kJoystickPort = 0;
+
+  public static final double kElevatorKp = 0.75;
+  public static final double kElevatorKi = 0;
+  public static final double kElevatorKd = 0;
+
+  public static final double kElevatorMaxV = 10.0; // volts (V)
+  public static final double kElevatorkS = 0.0; // volts (V)
+  public static final double kElevatorkG = 0.62; // volts (V)
+  public static final double kElevatorkV = 3.9; // volts (V)
+  public static final double kElevatorkA = 0.06; // volts (V)
+
+  public static final double kElevatorGearing = 5.0;
+  public static final double kElevatorDrumRadius = Units.inchesToMeters(1.0);
+  public static final double kCarriageMass = Units.lbsToKilograms(12); // kg
+
+  public static final double kSetpointMeters = Units.inchesToMeters(42.875);
+  public static final double kLowerkSetpointMeters = Units.inchesToMeters(15);
+  // Encoder is reset to measure 0 at the bottom, so minimum height is 0.
+  public static final double kMinElevatorHeightMeters = 0.0;
+  public static final double kMaxElevatorHeightMeters = Units.inchesToMeters(50);
+
+  // distance per pulse = (distance per revolution) / (pulses per revolution)
+  //  = (Pi * D) / ppr
+  public static final double kElevatorEncoderDistPerPulse =
+      2.0 * Math.PI * kElevatorDrumRadius / 4096;
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.elevatorexponentialsimulation;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Robot.java

@@ -0,0 +1,62 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.elevatorexponentialsimulation;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.examples.elevatorexponentialsimulation.subsystems.Elevator;
+
+/** This is a sample program to demonstrate the use of elevator simulation. */
+public class Robot extends TimedRobot {
+  private final Joystick m_joystick = new Joystick(Constants.kJoystickPort);
+  private final Elevator m_elevator = new Elevator();
+
+  public Robot() {
+    super(0.020);
+  }
+
+  @Override
+  public void robotInit() {}
+
+  @Override
+  public void robotPeriodic() {
+    // Update the telemetry, including mechanism visualization, regardless of mode.
+    m_elevator.updateTelemetry();
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    // Update the simulation model.
+    m_elevator.simulationPeriodic();
+  }
+
+  @Override
+  public void teleopInit() {
+    m_elevator.reset();
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    if (m_joystick.getTrigger()) {
+      // Here, we set the constant setpoint of 10 meters.
+      m_elevator.reachGoal(Constants.kSetpointMeters);
+    } else {
+      // Otherwise, we update the setpoint to 1 meter.
+      m_elevator.reachGoal(Constants.kLowerkSetpointMeters);
+    }
+  }
+
+  @Override
+  public void disabledInit() {
+    // This just makes sure that our simulation code knows that the motor's off.
+    m_elevator.stop();
+  }
+
+  @Override
+  public void close() {
+    m_elevator.close();
+    super.close();
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java

@@ -0,0 +1,142 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.elevatorexponentialsimulation.subsystems;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.trajectory.ExponentialProfile;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.examples.elevatorexponentialsimulation.Constants;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.PWMSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class Elevator implements AutoCloseable {
+  // This gearbox represents a gearbox containing 4 Vex 775pro motors.
+  private final DCMotor m_elevatorGearbox = DCMotor.getNEO(2);
+
+  private final ExponentialProfile m_profile =
+      new ExponentialProfile(
+          ExponentialProfile.Constraints.fromCharacteristics(
+              Constants.kElevatorMaxV, Constants.kElevatorkV, Constants.kElevatorkA));
+
+  private ExponentialProfile.State m_setpoint = new ExponentialProfile.State(0, 0);
+
+  // Standard classes for controlling our elevator
+  private final PIDController m_pidController =
+      new PIDController(Constants.kElevatorKp, Constants.kElevatorKi, Constants.kElevatorKd);
+
+  ElevatorFeedforward m_feedforward =
+      new ElevatorFeedforward(
+          Constants.kElevatorkS,
+          Constants.kElevatorkG,
+          Constants.kElevatorkV,
+          Constants.kElevatorkA);
+  private final Encoder m_encoder =
+      new Encoder(Constants.kEncoderAChannel, Constants.kEncoderBChannel);
+  private final PWMSparkMax m_motor = new PWMSparkMax(Constants.kMotorPort);
+
+  // Simulation classes help us simulate what's going on, including gravity.
+  private final ElevatorSim m_elevatorSim =
+      new ElevatorSim(
+          m_elevatorGearbox,
+          Constants.kElevatorGearing,
+          Constants.kCarriageMass,
+          Constants.kElevatorDrumRadius,
+          Constants.kMinElevatorHeightMeters,
+          Constants.kMaxElevatorHeightMeters,
+          true,
+          0,
+          VecBuilder.fill(0.005));
+  private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
+  private final PWMSim m_motorSim = new PWMSim(m_motor);
+
+  // Create a Mechanism2d visualization of the elevator
+  private final Mechanism2d m_mech2d =
+      new Mechanism2d(Units.inchesToMeters(10), Units.inchesToMeters(51));
+  private final MechanismRoot2d m_mech2dRoot =
+      m_mech2d.getRoot("Elevator Root", Units.inchesToMeters(5), Units.inchesToMeters(0.5));
+  private final MechanismLigament2d m_elevatorMech2d =
+      m_mech2dRoot.append(
+          new MechanismLigament2d("Elevator", m_elevatorSim.getPositionMeters(), 90));
+
+  /** Subsystem constructor. */
+  public Elevator() {
+    m_encoder.setDistancePerPulse(Constants.kElevatorEncoderDistPerPulse);
+
+    // Publish Mechanism2d to SmartDashboard
+    // To view the Elevator visualization, select Network Tables -> SmartDashboard -> Elevator Sim
+    SmartDashboard.putData("Elevator Sim", m_mech2d);
+  }
+
+  /** Advance the simulation. */
+  public void simulationPeriodic() {
+    // In this method, we update our simulation of what our elevator is doing
+    // First, we set our "inputs" (voltages)
+    m_elevatorSim.setInput(m_motorSim.getSpeed() * RobotController.getBatteryVoltage());
+
+    // Next, we update it. The standard loop time is 20ms.
+    m_elevatorSim.update(0.020);
+
+    // Finally, we set our simulated encoder's readings and simulated battery voltage
+    m_encoderSim.setDistance(m_elevatorSim.getPositionMeters());
+    // SimBattery estimates loaded battery voltages
+    RoboRioSim.setVInVoltage(
+        BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps()));
+  }
+
+  /**
+   * Run control loop to reach and maintain goal.
+   *
+   * @param goal the position to maintain
+   */
+  public void reachGoal(double goal) {
+    var goalState = new ExponentialProfile.State(goal, 0);
+
+    var next = m_profile.calculate(0.020, m_setpoint, goalState);
+
+    // With the setpoint value we run PID control like normal
+    double pidOutput = m_pidController.calculate(m_encoder.getDistance(), m_setpoint.position);
+    double feedforwardOutput = m_feedforward.calculate(m_setpoint.velocity, next.velocity, 0.020);
+
+    m_motor.setVoltage(pidOutput + feedforwardOutput);
+
+    m_setpoint = next;
+  }
+
+  /** Stop the control loop and motor output. */
+  public void stop() {
+    m_motor.set(0.0);
+  }
+
+  /** Reset Exponential profile to begin from current position on enable. */
+  public void reset() {
+    m_setpoint = new ExponentialProfile.State(m_encoder.getDistance(), 0);
+  }
+
+  /** Update telemetry, including the mechanism visualization. */
+  public void updateTelemetry() {
+    // Update elevator visualization with position
+    m_elevatorMech2d.setLength(m_encoder.getDistance());
+  }
+
+  @Override
+  public void close() {
+    m_encoder.close();
+    m_motor.close();
+    m_mech2d.close();
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json

@@ -173,6 +173,21 @@
     "mainclass": "Main",
     "commandversion": 2
   },
+  {
+    "name": "Elevator with exponential profile",
+    "description": "Reach elevator position setpoints with exponential profiles and smart motor controller PID.",
+    "tags": [
+      "Basic Robot",
+      "Elevator",
+      "Exponential Profile",
+      "Smart Motor Controller",
+      "Joystick"
+    ],
+    "foldername": "elevatorexponentialprofile",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
   {
     "name": "Elevator with profiled PID controller",
     "description": "Reach elevator position setpoints with an encoder and profiled PID control.",
@@ -802,6 +817,23 @@
     "mainclass": "Main",
     "commandversion": 2
   },
+  {
+    "name": "Elevator Exponential Profile Simulation",
+    "description": "Simulate an elevator.",
+    "tags": [
+      "Basic Robot",
+      "Elevator",
+      "State-Space",
+      "Simulation",
+      "Mechanism2d",
+      "PID",
+      "Exponential Profile"
+    ],
+    "foldername": "elevatorexponentialsimulation",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
   {
     "name": "ArmSimulation",
     "description": "Simulate a single-jointed arm.",