Repository: Team488/TeamXbot2025
Pull request #314: Motion magic on elevator
Author: login: junyu101
Created: 2025-03-20T20:00:19Z | Merged: 2025-03-21T02:22:15Z

# Why are we doing this?
Potentially faster elevator movement, I have no clue on how accurate the simulator reflects the speed of the elevator in real life, but the max acceleration and jerk values are properties so its tunable.
Asana task URL:

# Whats changing?

# Questions/notes for reviewers

# How this was tested
- [ ] tested on robot
- [x ] tested in simulator
- [ ] unit tests added

## Video/screenshots (from simulator or live robot)

https://github.com/user-attachments/assets/6ad9fb4a-c27e-4fbd-81ba-e55494a8b9cd


-----

PR feedback legend

 
| Symbol | Meaning                  |
|--------|--------------------------|
| :star: :star: :star:     | must be addressed                 |
| :star: :star:     | should be addressed        |
| :star:     | something to consider, a good idea                  |



Diff:
diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java
index c24ba54d8..99bba9504 100644
--- a/src/main/java/competition/operator_interface/OperatorCommandMap.java
+++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java
@@ -33,6 +33,7 @@
 import competition.subsystems.elevator.ElevatorSubsystem;
 import competition.subsystems.elevator.commands.ForceElevatorCalibratedCommand;
 import competition.subsystems.elevator.commands.SetElevatorTargetHeightCommand;
+import competition.subsystems.elevator.commands.ToggleElevatorMotionMagicCommand;
 import competition.subsystems.oracle.commands.DriveAccordingToOracleCommand;
 import competition.subsystems.oracle.commands.SuperstructureAccordingToOracleCommand;
 import competition.subsystems.pose.Cameras;
@@ -194,7 +195,8 @@ public void setupSuperstructureGamepadCommands(
             ForceElevatorCalibratedCommand forceElevatorCalibratedCommand,
             ForceCoralArmCalibratedCommand forceCoralArmCalibratedCommand,
             RepositionAlgaeArmDown repositionAlgaeArmDown,
-            RepositionAlgaeArmUp repositionAlgaeArmUp) {
+            RepositionAlgaeArmUp repositionAlgaeArmUp,
+            ToggleElevatorMotionMagicCommand toggleElevatorMotionMagicCommand) {
 
         var returnToBase = setElevatorTargetHeightCommandProvider.get();
         returnToBase.setHeight(Landmarks.CoralLevel.COLLECTING);
@@ -217,7 +219,8 @@ public void setupSuperstructureGamepadCommands(
         oi.superstructureGamepad.getifAvailable(XXboxController.XboxButton.RightBumper).onTrue(riseToScore);
 
         oi.superstructureGamepad.getifAvailable(XXboxController.XboxButton.Start).onTrue(forceElevatorCalibratedCommand);
-        oi.superstructureGamepad.getifAvailable(XXboxController.XboxButton.A).whileTrue(riseToL2);
+        oi.superstructureGamepad.getifAvailable(XXboxController.XboxButton.A).whileTrue(toggleElevatorMotionMagicCommand);
+        oi.superstructureGamepad.getifAvailable(XXboxController.XboxButton.Y).whileTrue(returnToBase);
         oi.superstructureGamepad.getifAvailable(XXboxController.XboxButton.B).whileTrue(riseToL3);
         oi.superstructureGamepad.getifAvailable(XXboxController.XboxButton.X).whileTrue(riseToL4);
 
diff --git a/src/main/java/competition/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/competition/subsystems/elevator/ElevatorSubsystem.java
index 65f92d4e7..d062ac796 100644
--- a/src/main/java/competition/subsystems/elevator/ElevatorSubsystem.java
+++ b/src/main/java/competition/subsystems/elevator/ElevatorSubsystem.java
@@ -18,6 +18,7 @@
 import xbot.common.controls.sensors.XDigitalInput;
 import xbot.common.controls.sensors.XLaserCAN;
 import xbot.common.math.MathUtils;
+import xbot.common.properties.BooleanProperty;
 import xbot.common.properties.DistanceProperty;
 import xbot.common.properties.DoubleProperty;
 import xbot.common.properties.PropertyFactory;
@@ -33,6 +34,7 @@
 import static edu.wpi.first.units.Units.Inches;
 import static edu.wpi.first.units.Units.Meters;
 import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
 import static edu.wpi.first.units.Units.Rotations;
 import static edu.wpi.first.units.Units.RotationsPerSecond;
 import static edu.wpi.first.units.Units.Second;
@@ -61,7 +63,10 @@ public class ElevatorSubsystem extends BaseSetpointSubsystem<Distance> {
     public final DoubleProperty powerNearUpperLimitThreshold;
     public final DoubleProperty powerWhenBottomSensorHit;
 
+    public final DoubleProperty motionMagicAcceleration;
+    public final DoubleProperty motionMagicJerk;
 
+    public final BooleanProperty motionMagicEnabled;
 
     public XCANMotorController masterMotor;
 
@@ -124,6 +129,11 @@ public ElevatorSubsystem(XCANMotorController.XCANMotorControllerFactory motorFac
         this.powerNearUpperLimitThreshold = pf.createPersistentProperty("powerNearUpperLimit", 0.0);
         this.powerNearLowerLimitThreshold = pf.createPersistentProperty("powerNearLowerLimit", 0.0);
         this.powerWhenBottomSensorHit = pf.createPersistentProperty("powerWhenBottomSensorHit", 0);
+
+        this.motionMagicAcceleration = pf.createPersistentProperty("motionMagicMaxAcceleration", 1);
+        this.motionMagicJerk = pf.createPersistentProperty("motionMagicMaxJerk", 0.1);
+
+        this.motionMagicEnabled = pf.createPersistentProperty("motionMagicEnabled", false);
         pf.setDefaultLevel(PropertyLevel.Important);
 
 
@@ -154,6 +164,7 @@ public ElevatorSubsystem(XCANMotorController.XCANMotorControllerFactory motorFac
                     );
             this.registerDataFrameRefreshable(masterMotor);
             masterMotor.setPositionAndVelocityUpdateFrequency(Hertz.of(50));
+            configureMotionMagicConstraints();
         }
 
         if (contract.isElevatorBottomSensorReady()) {
@@ -348,7 +359,9 @@ public void setElevatorDeltaFromCurrentHeight(Distance heightDelta) {
         var deltaRotations = Rotations.of(heightDelta.in(Meters) * rotationsPerMeter.get());
         masterMotor.setPositionTarget(
                 masterMotor.getPosition().plus(deltaRotations),
-                XCANMotorController.MotorPidMode.Voltage);
+                motionMagicEnabled.get()
+                ? XCANMotorController.MotorPidMode.TrapezoidalVoltage
+                : XCANMotorController.MotorPidMode.Voltage);
     }
 
     /**
@@ -379,11 +392,27 @@ public void trimElevatorDown(){
         trimValue.set(trimValue.get().minus((Inches.of(trimChangeAmount.get().in(Inches)))));
     }
 
+    public void toggleMotionMagic(){
+        motionMagicEnabled.set(!motionMagicEnabled.get());
+    }
+
+    public boolean isMotionMagicEnabled(){
+        return motionMagicEnabled.get();
+    }
+
+    public void configureMotionMagicConstraints(){
+        masterMotor.setTrapezoidalProfileAcceleration(RadiansPerSecondPerSecond.of(motionMagicAcceleration.get()));
+        masterMotor.setTrapezoidalProfileJerk(RadiansPerSecondPerSecond.of(motionMagicJerk.get()).per(Second));
+    }
+
     @Override
     public void periodic() {
         if (contract.isElevatorReady()) {
             masterMotor.periodic();
         }
+        if(motionMagicAcceleration.hasChangedSinceLastCheck() || motionMagicJerk.hasChangedSinceLastCheck()){
+            configureMotionMagicConstraints();
+        }
         //bandage case: isTouchingBottom flashes true for one tick on startup, investigate later?
         if (this.isTouchingBottom() && periodicTickCounter >= 3 && !isCalibrated()) {
             // Calibration may fail if the LaserCAN is not reporting a valid value
diff --git a/src/main/java/competition/subsystems/elevator/commands/ElevatorMaintainerCommand.java b/src/main/java/competition/subsystems/elevator/commands/ElevatorMaintainerCommand.java
index 418527461..00210e00e 100644
--- a/src/main/java/competition/subsystems/elevator/commands/ElevatorMaintainerCommand.java
+++ b/src/main/java/competition/subsystems/elevator/commands/ElevatorMaintainerCommand.java
@@ -111,15 +111,20 @@ protected void calibratedMachineControlAction() {
         aKitLog.record("PM-CurrentVelocity", elevator.getCurrentVelocity().in(MetersPerSecond));
         aKitLog.setLogLevel(AKitLogger.LogLevel.INFO);
 
-        profileManager.setTargetPosition(
-            elevator.getTargetValue().in(Meters),
-            currentValue.in(Meters),
-            elevator.getCurrentVelocity().in(MetersPerSecond)
-        );
-        var setpoint = profileManager.getRecommendedPositionForTime();
-        aKitLog.record("elevatorProfileTarget", setpoint);
-
-        elevator.setElevatorHeightGoalOnMotor(setpoint);
+        if (elevator.isMotionMagicEnabled()){
+            elevator.setElevatorHeightGoalOnMotor(elevator.getTargetValue().in(Meters));
+        }
+        else {
+            profileManager.setTargetPosition(
+                    elevator.getTargetValue().in(Meters),
+                    currentValue.in(Meters),
+                    elevator.getCurrentVelocity().in(MetersPerSecond)
+            );
+            var setpoint = profileManager.getRecommendedPositionForTime();
+            aKitLog.record("elevatorProfileTarget", setpoint);
+
+            elevator.setElevatorHeightGoalOnMotor(setpoint);
+        }
     }
 
     //defaults humanControlAction if there is no bottom sensor
diff --git a/src/main/java/competition/subsystems/elevator/commands/ToggleElevatorMotionMagicCommand.java b/src/main/java/competition/subsystems/elevator/commands/ToggleElevatorMotionMagicCommand.java
new file mode 100644
index 000000000..2fc04975b
--- /dev/null
+++ b/src/main/java/competition/subsystems/elevator/commands/ToggleElevatorMotionMagicCommand.java
@@ -0,0 +1,29 @@
+package competition.subsystems.elevator.commands;
+
+import competition.subsystems.elevator.ElevatorSubsystem;
+import xbot.common.command.BaseCommand;
+
+import javax.inject.Inject;
+
+public class ToggleElevatorMotionMagicCommand extends BaseCommand {
+    ElevatorSubsystem elevator;
+
+    @Inject
+    public ToggleElevatorMotionMagicCommand(ElevatorSubsystem elevator){
+        this.elevator = elevator;
+        addRequirements(elevator);
+    }
+
+    @Override
+    public void initialize() {
+        log.info("Initializing..");
+        elevator.toggleMotionMagic();
+        elevator.setTargetValue(elevator.getCurrentValue());
+        this.setRunsWhenDisabled(true);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}


Reviews:
user: login: JohnGilb
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-20T21:11:20Z
user: login: aschokking
body: Approving, but let's not actually merge this into `main` until after this competition just in case
state: APPROVED
author_association: CONTRIBUTOR
submitted_at: 2025-03-21T00:53:30Z
user: login: aschokking
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-21T01:18:32Z
user: login: stephenjust
state: APPROVED
author_association: CONTRIBUTOR
submitted_at: 2025-03-21T01:59:56Z

Review comments:
diff_hunk: @@ -379,6 +389,19 @@ public void trimElevatorDown(){
         trimValue.set(trimValue.get().minus((Inches.of(trimChangeAmount.get().in(Inches)))));
     }
 
+    public void toggleMotionMagic(){
+        motionMagicEnabled = !motionMagicEnabled;
+    }
+
+    public boolean isMotionMagicEnabled(){
+        return motionMagicEnabled;
+    }
+
+    public void configureMotionMagicConstraints(){
path: src/main/java/competition/subsystems/elevator/ElevatorSubsystem.java
user: login: JohnGilb
body: :star: I recommend looking at how we tune the P/I/D values for the motors, where property changes are picked up at runtime rather than having to reboot the robot with your current approach. It will make it much easier to tune the elevator.
created_at: 2025-03-20T21:11:20Z
author_association: CONTRIBUTOR
diff_hunk: @@ -348,7 +356,9 @@ public void setElevatorDeltaFromCurrentHeight(Distance heightDelta) {
         var deltaRotations = Rotations.of(heightDelta.in(Meters) * rotationsPerMeter.get());
         masterMotor.setPositionTarget(
                 masterMotor.getPosition().plus(deltaRotations),
-                XCANMotorController.MotorPidMode.Voltage);
+                motionMagicEnabled
+                ? XCANMotorController.MotorPidMode.TrapezoidalVoltage
path: src/main/java/competition/subsystems/elevator/ElevatorSubsystem.java
user: login: aschokking
body: ⭐ ⭐ We might want to try and S-curve rather than Trapezoid to have smoother starts/ends? https://v6.docs.ctr-electronics.com/en/latest/docs/api-reference/device-specific/talonfx/motion-magic.html
created_at: 2025-03-21T00:52:54Z
author_association: CONTRIBUTOR
diff_hunk: @@ -348,7 +356,9 @@ public void setElevatorDeltaFromCurrentHeight(Distance heightDelta) {
         var deltaRotations = Rotations.of(heightDelta.in(Meters) * rotationsPerMeter.get());
         masterMotor.setPositionTarget(
                 masterMotor.getPosition().plus(deltaRotations),
-                XCANMotorController.MotorPidMode.Voltage);
+                motionMagicEnabled
+                ? XCANMotorController.MotorPidMode.TrapezoidalVoltage
path: src/main/java/competition/subsystems/elevator/ElevatorSubsystem.java
user: login: aschokking
body: Ah apparently this is what we've mapped trapezoid voltage to, nvm
created_at: 2025-03-21T01:18:32Z
author_association: CONTRIBUTOR
diff_hunk: @@ -61,7 +62,10 @@ public class ElevatorSubsystem extends BaseSetpointSubsystem<Distance> {
     public final DoubleProperty powerNearUpperLimitThreshold;
     public final DoubleProperty powerWhenBottomSensorHit;
 
+    public final DoubleProperty motionMagicAcceleration;
+    public final DoubleProperty motionMagicJerk;
 
+    public boolean motionMagicEnabled;
path: src/main/java/competition/subsystems/elevator/ElevatorSubsystem.java
user: login: stephenjust
body: ⭐⭐Make this a boolean property so we can set it from Elastic
created_at: 2025-03-21T01:59:20Z
author_association: CONTRIBUTOR

PR comments:
user: login: stephenjust
created_at: 2025-03-21T02:00:25Z
body: > Approving, but let's not actually merge this into `main` until after this competition just in case

I would disagree on that. Since it's toggle-able, I think it's pretty safe to merge.
author_association: CONTRIBUTOR
user: login: aschokking
created_at: 2025-03-21T21:05:16Z
body: > > Approving, but let's not actually merge this into `main` until after this competition just in case
> 
> I would disagree on that. Since it's toggle-able, I think it's pretty safe to merge.

Balanced against the risk there was a bug with the toggleability and it actually regresses something for our first match 🙏🏻 
author_association: CONTRIBUTOR