Merge remote-tracking branch 'origin/main' into sop26/fixvers

team1868 · Feb 5, 2025 · 2e52163 · 2e52163
2 parents 4e0d9c2 + 9bc0f3b
commit 2e52163
Show file tree

Hide file tree

Showing 6 changed files with 16 additions and 35 deletions.
diff --git a/src/main/java/tagalong/subsystems/micro/Elevator.java b/src/main/java/tagalong/subsystems/micro/Elevator.java
@@ -411,6 +411,8 @@ public void periodic() {
       setFollowProfile(false);
       setElevatorProfile(getElevatorHeightM(), 0.0);
       _primaryMotor.set(0.0);
+    } else if (_isFFTuningMicro && _trapProfile.isFinished(_profileTimer.get())) {
+      _primaryMotor.setControl(_requestedPositionVoltage.withFeedForward(_elevatorFF.getKs()));
     }
     if (_followProfile) {
       followLastProfile();

diff --git a/src/main/java/tagalong/subsystems/micro/Pivot.java b/src/main/java/tagalong/subsystems/micro/Pivot.java
@@ -182,7 +182,7 @@ public void periodic() {
       _primaryMotor.set(0.0);
     } else if (_isFFTuningMicro && _trapProfile.isFinished(_profileTimer.get())) {
       _primaryMotor.setControl(_requestedPositionVoltage.withFeedForward(
-          _pivotFF.getKs() + _pivotFF.getKs() * Math.cos(getFFPositionRad())
+          _pivotFF.getKs() + _pivotFF.getKg() * Math.cos(getFFPositionRad())
       ));
     }
 
@@ -218,7 +218,7 @@ public void onEnable() {
           _KAEntry.getDouble(_pivotFF.getKa())
       );
       _primaryMotor.setControl(_requestedPositionVoltage.withFeedForward(
-          _pivotFF.getKs() + _pivotFF.getKs() * Math.cos(getFFPositionRad())
+          _pivotFF.getKs() + _pivotFF.getKg() * Math.cos(getFFPositionRad())
       ));
     }
   }

diff --git a/src/main/java/tagalong/subsystems/micro/PivotFused.java b/src/main/java/tagalong/subsystems/micro/PivotFused.java
@@ -55,11 +55,8 @@ public void followLastProfile() {
 
     TrapezoidProfile.State nextState =
         _trapProfile.calculate(TagalongConfiguration.LOOP_PERIOD_S, _curState, _goalState);
-    // FUTURE DEV: modify to allow for unfused or not 1:1 with pivot, convert to motor units
     _primaryMotor.setControl(
-        _requestedPositionVoltage
-            .withPosition(nextState.position)
-            // FeedForward must know the pivot rotation and other arguments in radians
+        _requestedPositionVoltage.withPosition(nextState.position)
             .withFeedForward(
                 _pivotFF.calculate(getFFPositionRad(), Units.rotationsToRadians(nextState.velocity))
             )
@@ -79,7 +76,6 @@ public double getFFPositionRad() {
       return 0.0;
     }
 
-    // FUTURE DEV: modify to allow for unfused or not 1:1 with pivot
     return Units.rotationsToRadians(_pivotCancoder.getPosition().getValueAsDouble())
         + _ffCenterOfMassOffsetRad;
   }
@@ -91,14 +87,9 @@ public void setPivotVelocity(double rps, boolean withFF) {
     }
     setFollowProfile(false);
 
-    _primaryMotor.setControl(
-        _requestedVelocityVoltage
-            // FUTURE DEV: modify to allow for unfused or not 1:1 with pivot
-            .withVelocity(rps)
-            .withFeedForward(
-                withFF ? _pivotFF.calculate(getFFPositionRad(), Units.rotationsToRadians(rps)) : 0.0
-            )
-    );
+    _primaryMotor.setControl(_requestedVelocityVoltage.withVelocity(rps).withFeedForward(
+        withFF ? _pivotFF.calculate(getFFPositionRad(), Units.rotationsToRadians(rps)) : 0.0
+    ));
   }
 
   @Override
@@ -173,7 +164,6 @@ public void simulationPeriodic() {
     _pivotSim.setInputVoltage(_primaryMotor.get() * RobotController.getBatteryVoltage());
     _pivotSim.update(TagalongConfiguration.LOOP_PERIOD_S);
 
-    // FUTURE DEV: modify to allow for unfused or not 1:1 with pivot
     double prevSimVelo = _simVeloRPS;
     _simVeloRPS = Units.radiansToRotations(_pivotSim.getVelocityRadPerSec());
     _simRotations += Units.radiansToRotations(_pivotSim.getAngleRads());

diff --git a/src/main/java/tagalong/subsystems/micro/PivotNoCancoder.java b/src/main/java/tagalong/subsystems/micro/PivotNoCancoder.java
@@ -40,11 +40,8 @@ public void followLastProfile() {
 
     TrapezoidProfile.State nextState =
         _trapProfile.calculate(TagalongConfiguration.LOOP_PERIOD_S, _curState, _goalState);
-    // FUTURE DEV: modify to allow for unfused or not 1:1 with pivot, convert to motor units
     _primaryMotor.setControl(
-        _requestedPositionVoltage
-            .withPosition(pivotRotToMotor(nextState.position))
-            // FeedForward must know the pivot rotation and other arguments in radians
+        _requestedPositionVoltage.withPosition(pivotRotToMotor(nextState.position))
             .withFeedForward(
                 _pivotFF.calculate(getFFPositionRad(), Units.rotationsToRadians(nextState.velocity))
             )
@@ -64,7 +61,6 @@ public double getFFPositionRad() {
       return 0.0;
     }
 
-    // FUTURE DEV: modify to allow for unfused or not 1:1 with pivot
     return Units.rotationsToRadians(getPivotPosition()) + _ffCenterOfMassOffsetRad;
   }
 
@@ -76,9 +72,7 @@ public void setPivotVelocity(double rps, boolean withFF) {
     setFollowProfile(false);
 
     _primaryMotor.setControl(
-        _requestedVelocityVoltage
-            // FUTURE DEV: modify to allow for unfused or not 1:1 with pivot
-            .withVelocity(pivotRotToMotor(rps))
+        _requestedVelocityVoltage.withVelocity(pivotRotToMotor(rps))
             .withFeedForward(
                 withFF ? _pivotFF.calculate(getFFPositionRad(), Units.rotationsToRadians(rps)) : 0.0
             )
@@ -127,7 +121,6 @@ public void simulationPeriodic() {
     _pivotSim.setInputVoltage(_primaryMotor.get() * RobotController.getBatteryVoltage());
     _pivotSim.update(TagalongConfiguration.LOOP_PERIOD_S);
 
-    // FUTURE DEV: modify to allow for unfused or not 1:1 with pivot
     double prevSimVelo = _simVeloRPS;
     _simVeloRPS = Units.radiansToRotations(_pivotSim.getVelocityRadPerSec());
     _simRotations += Units.radiansToRotations(_pivotSim.getAngleRads());

diff --git a/src/main/java/tagalong/subsystems/micro/PivotUnfused.java b/src/main/java/tagalong/subsystems/micro/PivotUnfused.java
@@ -60,11 +60,8 @@ public void followLastProfile() {
 
     TrapezoidProfile.State nextState =
         _trapProfile.calculate(TagalongConfiguration.LOOP_PERIOD_S, _curState, _goalState);
-    // FUTURE DEV: modify to allow for unfused or not 1:1 with pivot, convert to motor units
     _primaryMotor.setControl(
-        _requestedPositionVoltage
-            .withPosition(pivotRotToMotor(nextState.position))
-            // FeedForward must know the pivot rotation and other arguments in radians
+        _requestedPositionVoltage.withPosition(pivotRotToMotor(nextState.position))
             .withFeedForward(
                 _pivotFF.calculate(getFFPositionRad(), Units.rotationsToRadians(nextState.velocity))
             )
@@ -97,9 +94,7 @@ public void setPivotVelocity(double rps, boolean withFF) {
     setFollowProfile(false);
 
     _primaryMotor.setControl(
-        _requestedVelocityVoltage
-            // FUTURE DEV: modify to allow for unfused or not 1:1 with pivot
-            .withVelocity(pivotRotToMotor(rps))
+        _requestedVelocityVoltage.withVelocity(pivotRotToMotor(rps))
             .withFeedForward(
                 withFF ? _pivotFF.calculate(getFFPositionRad(), Units.rotationsToRadians(rps)) : 0.0
             )

diff --git a/src/main/java/tagalong/subsystems/micro/Roller.java b/src/main/java/tagalong/subsystems/micro/Roller.java
@@ -156,8 +156,9 @@ public void periodic() {
     } else if (motorResetConfig()) {
       setRollerProfile(getRollerPosition(), 0.0);
       _primaryMotor.set(0.0);
+    } else if (_isFFTuningMicro && _trapProfile.isFinished(_profileTimer.get())) {
+      _primaryMotor.setControl(_requestedPositionVoltage.withFeedForward(_rollerFF.getKs()));
     }
-
     if (_followProfile) {
       followLastProfile();
     }
@@ -281,8 +282,8 @@ public double rollerRotToMotor(double rollerRot) {
   }
 
   /**
-   * Calculates the next state according to the trapezoidal profile and requests the roller motor(s)
-   * to arrive at the next position with feedforward
+   * Calculates the next state according to the trapezoidal profile and requests the roller
+   * motor(s) to arrive at the next position with feedforward
    */
   public void followLastProfile() {
     if (_isMicrosystemDisabled) {