Set motor init values

publish.gradle

@@ -2,7 +2,7 @@ apply plugin: 'maven-publish'
 
 ext.licenseFile = files("$rootDir/LICENSE.txt")
 
-def pubVersion = '2025.21.0204'
+def pubVersion = '2025.0.0065'
 
 def outputsFolder = file("$buildDir/outputs")
 

src/main/java/tagalong/commands/base/ElevateToCmd.java

@@ -90,7 +90,7 @@ public void end(boolean interrupted) {
   public boolean isFinished() {
     // Command is finished when the profile is finished AND
     // Either the tolerance is bypassed or in tolerance for the desired duration
-    return _elevator.isProfileFinished()
+    return _startedMovement && _elevator.isProfileFinished()
         && (!_requireInTolerance
             || _elevator.checkToleranceTime(
                 _elevator.isElevatorInTolerance(_lowerBoundM, _upperBoundM),

src/main/java/tagalong/commands/base/ElevateXCmd.java

@@ -104,7 +104,7 @@ public void end(boolean interrupted) {
   public boolean isFinished() {
     // Command is finished when the profile is finished AND
     // Either the tolerance is bypassed or in tolerance for the desired duration
-    return _elevator.isProfileFinished()
+    return _startedMovement && _elevator.isProfileFinished()
         && (!_requireInTolerance
             || _elevator.checkToleranceTime(
                 _elevator.isElevatorInTolerance(_lowerBoundM, _upperBoundM),

src/main/java/tagalong/commands/base/PivotToAbsoluteCmd.java

@@ -73,16 +73,24 @@ public void execute() {
     if (!_startedMovement && _pivot.isSafeToMove()) {
       _startedMovement = true;
       _scopedGoalPositionRot =
+          // goal 0.5, current -2.4, can't travel -2.4 to -2.5
+          // place in scope, would place this at -2.5
+          // but our legal movement for this example is -2.4 to -1.5
+
           AlgebraicUtils.placeInScopeRot(_pivot.getPivotPosition(), _goalPositionRot);
+      // System.out.println(
+      //     "\n pre goal scoped: " + _scopedGoalPositionRot + "\n pivot pos"
+      //     + _pivot.getPivotPosition() + "\n goal" + _goalPositionRot
+      // );
       if (_scopedGoalPositionRot < _pivot._minPositionRot) {
         _scopedGoalPositionRot += 1.0;
       }
       if (_scopedGoalPositionRot > _pivot._maxPositionRot) {
         _scopedGoalPositionRot -= 1.0;
       }
-
+      // System.out.println("\ngoal scoped: " + _scopedGoalPositionRot + "\n ");
       _pivot.setPivotProfile(
-          _pivot.clampPivotPosition(_scopedGoalPositionRot), 0.0, _maxVelocityRPS
+          _scopedGoalPositionRot, 0.0, _maxVelocityRPS
       );
     }
 

src/main/java/tagalong/commands/base/PivotToCmd.java

@@ -49,6 +49,8 @@ public class PivotToCmd<T extends TagalongSubsystemBase & PivotAugment> extends
    * The maximum velocity of the pivot, in rotations per second, during this command
    */
   private double _maxVelocityRPS;
+
+  private double _maxAccelerationRPS2;
   /**
    * Whether or not the pivot has started moving
    */
@@ -64,7 +66,9 @@ public void initialize() {
   public void execute() {
     if (!_startedMovement) {
       _startedMovement = true;
-      _pivot.setPivotProfile(_pivot.clampPivotPosition(_goalPositionRot), 0.0, _maxVelocityRPS);
+      _pivot.setPivotProfile(
+          _goalPositionRot + _pivot.getScopeOffset(), 0.0, _maxVelocityRPS, _maxAccelerationRPS2
+      );
     }
 
     if (_startedMovement) {
@@ -120,7 +124,13 @@ public PivotToCmd(int id, T pivot, Angle goalPosition) {
    *
    */
   public PivotToCmd(T pivot, Angle goalPosition, boolean holdPositionAfter) {
-    this(pivot, goalPosition, holdPositionAfter, pivot.getPivot()._maxVelocityRPS);
+    this(
+        pivot,
+        goalPosition,
+        holdPositionAfter,
+        pivot.getPivot()._maxVelocityRPS,
+        pivot.getPivot()._maxAccelerationRPS2
+    );
   }
 
   /**
@@ -134,35 +144,19 @@ public PivotToCmd(T pivot, Angle goalPosition, boolean holdPositionAfter) {
    *
    */
   public PivotToCmd(int id, T pivot, Angle goalPosition, boolean holdPositionAfter) {
-    this(id, pivot, goalPosition, holdPositionAfter, pivot.getPivot(id)._maxVelocityRPS);
-  }
-
-  /**
-   * Constructor that creates the command with the below parameters.
-   *
-   * @param pivot             Tagalong Subsystem containing a pivot microsystem
-   * @param goalPosition      Goal pivot positon
-   * @param holdPositionAfter If the pivot should hold position when the command
-   *                          completes
-   * @param maxVelocityRPS    The maximum velocity of the pivot, in rotations per
-   *                          second, during this command
-   */
-
-  public PivotToCmd(T pivot, Angle goalPosition, boolean holdPositionAfter, double maxVelocityRPS) {
     this(
+        id,
         pivot,
         goalPosition,
         holdPositionAfter,
-        maxVelocityRPS,
-        pivot.getPivot()._defaultPivotLowerToleranceRot,
-        pivot.getPivot()._defaultPivotUpperToleranceRot
+        pivot.getPivot(id)._maxVelocityRPS,
+        pivot.getPivot(id)._maxAccelerationRPS2
     );
   }
 
   /**
    * Constructor that creates the command with the below parameters.
    *
-   * @param id                The pivot integer ID
    * @param pivot             Tagalong Subsystem containing a pivot microsystem
    * @param goalPosition      Goal pivot positon
    * @param holdPositionAfter If the pivot should hold position when the command
@@ -172,42 +166,52 @@ public PivotToCmd(T pivot, Angle goalPosition, boolean holdPositionAfter, double
    */
 
   public PivotToCmd(
-      int id, T pivot, Angle goalPosition, boolean holdPositionAfter, double maxVelocityRPS
+      T pivot,
+      Angle goalPosition,
+      boolean holdPositionAfter,
+      double maxVelocityRPS,
+      double maxAccelerationRPS2
   ) {
     this(
-        id,
         pivot,
         goalPosition,
         holdPositionAfter,
         maxVelocityRPS,
-        pivot.getPivot(id)._defaultPivotLowerToleranceRot,
-        pivot.getPivot(id)._defaultPivotUpperToleranceRot
+        maxAccelerationRPS2,
+        pivot.getPivot()._defaultPivotLowerToleranceRot,
+        pivot.getPivot()._defaultPivotUpperToleranceRot
     );
   }
 
   /**
    * Constructor that creates the command with the below parameters.
    *
+   * @param id                The pivot integer ID
    * @param pivot             Tagalong Subsystem containing a pivot microsystem
    * @param goalPosition      Goal pivot positon
    * @param holdPositionAfter If the pivot should hold position when the command
    *                          completes
    * @param maxVelocityRPS    The maximum velocity of the pivot, in rotations per
    *                          second, during this command
-   * @param toleranceRot       The number of rotations short of or beyond the
-   *                          target position the
-   *                          pivot can be while still being considered in
-   *                          tolerance
    */
 
   public PivotToCmd(
+      int id,
       T pivot,
       Angle goalPosition,
       boolean holdPositionAfter,
       double maxVelocityRPS,
-      double toleranceRot
+      double maxAccelerationRPS2
   ) {
-    this(pivot, goalPosition, holdPositionAfter, maxVelocityRPS, toleranceRot, toleranceRot);
+    this(
+        id,
+        pivot,
+        goalPosition,
+        holdPositionAfter,
+        maxVelocityRPS,
+        maxAccelerationRPS2,
+        pivot.getPivot(id)._defaultPivotUpperToleranceRot
+    );
   }
 
   /**
@@ -231,33 +235,27 @@ public PivotToCmd(
       Angle goalPosition,
       boolean holdPositionAfter,
       double maxVelocityRPS,
+      double maxAccelerationRPS2,
       double toleranceRot
   ) {
-    this(id, pivot, goalPosition, holdPositionAfter, maxVelocityRPS, toleranceRot, toleranceRot);
+    this(
+        id,
+        pivot,
+        goalPosition,
+        holdPositionAfter,
+        maxVelocityRPS,
+        maxAccelerationRPS2,
+        toleranceRot,
+        toleranceRot
+    );
   }
 
-  /**
-   * Constructor that creates the command with the below parameters.
-   *
-   * @param pivot             Tagalong Subsystem containing a pivot microsystem
-   * @param goalPosition      Goal pivot positon
-   * @param holdPositionAfter If the pivot should hold position when the command
-   *                          completes
-   * @param maxVelocityRPS    The maximum velocity of the pivot, in rotations per
-   *                          second, during this command
-   * @param lowerToleranceRot The number of rotations short of the target position
-   *                          the pivot can be
-   *                          while still being considered in tolerance
-   * @param upperToleranceRot The number of rotations beyond the target position
-   *                          the pivot can be
-   *                          while still being considered in tolerance
-   */
-
   public PivotToCmd(
       T pivot,
       Angle goalPosition,
       boolean holdPositionAfter,
       double maxVelocityRPS,
+      double maxAccelerationRPS2,
       double lowerToleranceRot,
       double upperToleranceRot
   ) {
@@ -266,6 +264,7 @@ public PivotToCmd(
         goalPosition,
         holdPositionAfter,
         maxVelocityRPS,
+        maxAccelerationRPS2,
         lowerToleranceRot,
         upperToleranceRot,
         -1.0
@@ -296,6 +295,7 @@ public PivotToCmd(
       Angle goalPosition,
       boolean holdPositionAfter,
       double maxVelocityRPS,
+      double maxAccelerationRPS2,
       double lowerToleranceRot,
       double upperToleranceRot
   ) {
@@ -305,6 +305,7 @@ public PivotToCmd(
         goalPosition,
         holdPositionAfter,
         maxVelocityRPS,
+        maxAccelerationRPS2,
         lowerToleranceRot,
         upperToleranceRot,
         -1.0
@@ -336,15 +337,18 @@ public PivotToCmd(
       Angle goalPosition,
       boolean holdPositionAfter,
       double maxVelocityRPS,
+      double maxAccelerationRPS2,
       double lowerToleranceRot,
       double upperToleranceRot,
       double requiredInToleranceDurationS
   ) {
     this(
+        0,
         pivot,
         goalPosition.getRotations(),
         holdPositionAfter,
         maxVelocityRPS,
+        maxAccelerationRPS2,
         lowerToleranceRot,
         upperToleranceRot,
         requiredInToleranceDurationS
@@ -378,6 +382,7 @@ public PivotToCmd(
       Angle goalPosition,
       boolean holdPositionAfter,
       double maxVelocityRPS,
+      double maxAccelerationRPS2,
       double lowerToleranceRot,
       double upperToleranceRot,
       double requiredInToleranceDurationS
@@ -388,6 +393,7 @@ public PivotToCmd(
         goalPosition.getRotations(),
         holdPositionAfter,
         maxVelocityRPS,
+        maxAccelerationRPS2,
         lowerToleranceRot,
         upperToleranceRot,
         requiredInToleranceDurationS
@@ -464,6 +470,7 @@ public PivotToCmd(
       double goalPosition,
       boolean holdPositionAfter,
       double maxVelocityRPS,
+      double maxAccelerationRPS2,
       double lowerToleranceRot,
       double upperToleranceRot,
       double requiredInToleranceDurationS
@@ -472,6 +479,7 @@ public PivotToCmd(
     _goalPositionRot = goalPosition;
     _holdPositionAfter = holdPositionAfter;
     _maxVelocityRPS = maxVelocityRPS;
+    _maxAccelerationRPS2 = maxAccelerationRPS2;
     _lowerBoundRot = _goalPositionRot - Math.abs(lowerToleranceRot);
     _upperBoundRot = _goalPositionRot + Math.abs(upperToleranceRot);
     _requiredInToleranceDurationS = requiredInToleranceDurationS;

src/main/java/tagalong/commands/base/PivotToDynamicAbsoluteCmd.java

@@ -61,7 +61,7 @@ public void initialize() {
   @Override
   public void execute() {
     // if pivot has not started moving, check for legal states
-    _goalPositionRot = _pivot.clampPivotPosition(_goalSupplierRot.getAsDouble());
+    _goalPositionRot = _goalSupplierRot.getAsDouble();
     _scopedGoalPositionRot =
         AlgebraicUtils.placeInScopeRot(_pivot.getPivotPosition(), _goalPositionRot);
     if (_scopedGoalPositionRot < _pivot._minPositionRot) {
@@ -70,7 +70,6 @@ public void execute() {
     if (_scopedGoalPositionRot > _pivot._maxPositionRot) {
       _scopedGoalPositionRot -= 1.0;
     }
-    _scopedGoalPositionRot = _pivot.clampPivotPosition(_scopedGoalPositionRot);
 
     if (_startedMovement) {
       _pivot.setPivotProfile(

src/main/java/tagalong/commands/base/PivotToDynamicCmd.java

@@ -56,7 +56,7 @@ public void initialize() {
   @Override
   public void execute() {
     // if pivot has not started moving, check for legal states
-    _goalPositionRot = _pivot.clampPivotPosition(_goalSupplierRot.getAsDouble());
+    _goalPositionRot = _goalSupplierRot.getAsDouble();
     if (_startedMovement) {
       _pivot.setPivotProfile(_goalPositionRot, 0.0, _maxVelocityRPS);
       _pivot.followLastProfile();

src/main/java/tagalong/commands/base/PivotXCmd.java

@@ -75,7 +75,7 @@ public class PivotXCmd<T extends TagalongSubsystemBase & PivotAugment> extends T
   public void initialize() {
     _startedMovement = false;
     _startAngleRot = _pivot.getPivotPosition();
-    _goalAngleRot = _pivot.clampPivotPosition(_startAngleRot + _relativeMovementRot);
+    _goalAngleRot = _startAngleRot + _relativeMovementRot;
     _lowerBoundRot = _goalAngleRot - _lowerToleranceRot;
     _upperBoundRot = _goalAngleRot + _upperToleranceRot;
   }
@@ -102,7 +102,7 @@ public void end(boolean interrupted) {
   public boolean isFinished() {
     // Command is finished when the profile is finished AND
     // Either the tolerance is bypassed or in tolerance for the desired duration
-    return _pivot.isProfileFinished()
+    return _startedMovement && _pivot.isProfileFinished()
         && (!_requireInTolerance
             || _pivot.checkToleranceTime(
                 _pivot.isPivotInTolerance(_lowerBoundRot, _upperBoundRot),

src/main/java/tagalong/commands/base/RollToCmd.java

@@ -82,7 +82,7 @@ public void end(boolean interrupted) {
   public boolean isFinished() {
     // Command is finished when the profile is finished AND
     // Either the tolerance is bypassed or in tolerance for the desired duration
-    return _roller.isProfileFinished()
+    return _startedMovement && _roller.isProfileFinished()
         && (!_requireInTolerance
             || _roller.checkToleranceTime(
                 _roller.isRollerInTolerance(_lowerBoundRot, _upperBoundRot),

src/main/java/tagalong/commands/base/RollXCmd.java

@@ -100,7 +100,7 @@ public void end(boolean interrupted) {
   public boolean isFinished() {
     // Command is finished when the profile is finished AND
     // Either the tolerance is bypassed or in tolerance for the desired duration
-    return _roller.isProfileFinished()
+    return _startedMovement && _roller.isProfileFinished()
         && (!_requireInTolerance
             || _roller.checkToleranceTime(
                 _roller.isRollerInTolerance(_lowerBoundRot, _upperBoundRot),