From de4ccb7f1b2742d54eb96ac247ef7f8fb218c57f Mon Sep 17 00:00:00 2001 From: Sophie L Date: Sat, 8 Feb 2025 00:33:12 -0800 Subject: [PATCH 01/10] to test fixes --- publish.gradle | 2 +- .../commands/base/PivotToAbsoluteCmd.java | 6 ++++- .../java/tagalong/subsystems/micro/Pivot.java | 25 ++++++++++++++++--- 3 files changed, 28 insertions(+), 5 deletions(-) diff --git a/publish.gradle b/publish.gradle index 1a466c4..a67c8e1 100644 --- a/publish.gradle +++ b/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.0054' def outputsFolder = file("$buildDir/outputs") diff --git a/src/main/java/tagalong/commands/base/PivotToAbsoluteCmd.java b/src/main/java/tagalong/commands/base/PivotToAbsoluteCmd.java index d1bd040..79da00a 100644 --- a/src/main/java/tagalong/commands/base/PivotToAbsoluteCmd.java +++ b/src/main/java/tagalong/commands/base/PivotToAbsoluteCmd.java @@ -73,6 +73,10 @@ 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); if (_scopedGoalPositionRot < _pivot._minPositionRot) { _scopedGoalPositionRot += 1.0; @@ -80,7 +84,7 @@ public void execute() { if (_scopedGoalPositionRot > _pivot._maxPositionRot) { _scopedGoalPositionRot -= 1.0; } - + System.out.println("\ngoal scoped: " + _scopedGoalPositionRot + "\n "); _pivot.setPivotProfile( _pivot.clampPivotPosition(_scopedGoalPositionRot), 0.0, _maxVelocityRPS ); diff --git a/src/main/java/tagalong/subsystems/micro/Pivot.java b/src/main/java/tagalong/subsystems/micro/Pivot.java index 7633544..3baf5a1 100644 --- a/src/main/java/tagalong/subsystems/micro/Pivot.java +++ b/src/main/java/tagalong/subsystems/micro/Pivot.java @@ -131,8 +131,24 @@ public Pivot(PivotConf conf) { _pivotFF = _pivotConf.feedForward; _defaultPivotLowerToleranceRot = _pivotConf.defaultLowerTolerance; _defaultPivotUpperToleranceRot = _pivotConf.defaultUpperTolerance; - _minPositionRot = _pivotConf.rotationalMin; - _maxPositionRot = _pivotConf.rotationalMax; + // NOTE: This (temporarily) resolves an issue with an absolute encoder that boots out of range + // TODO: generalize this logic to better handle the CTRE encoder boot location and how it tends + // to play jump rope with 0 and is seemingly unpredictable + // Needs to deal with > 360 range and booting where the min AND max can never contain the + // current position + double min = _pivotConf.rotationalMin; + double max = _pivotConf.rotationalMax; + while (min >= getPivotPosition()) { + min -= 1.0; + max -= 1.0; + } + while (max <= getPivotPosition()) { + min += 1.0; + max += 1.0; + } + _minPositionRot = min; + _maxPositionRot = max; + _absoluteRangeRot = _maxPositionRot - _minPositionRot; _maxVelocityRPS = _pivotConf.trapezoidalLimitsVelocity; _maxAccelerationRPS2 = _pivotConf.trapezoidalLimitsAcceleration; @@ -514,7 +530,10 @@ public void setPivotProfile( _goalState.position = (_absoluteRangeRot < 1.0 ? absoluteClamp(goalPositionRot) : clampPivotPosition(goalPositionRot)) + _profileTargetOffset; - + System.out.println( + "goal: " + goalPositionRot + "\ngoal clamped: " + absoluteClamp(goalPositionRot) + + "\nabs range: " + (_absoluteRangeRot < 1.0) + ); _trapProfile = new TrapezoidProfile( (maxVelocityRPS >= _maxVelocityRPS || maxAccelerationRPS2 >= _maxAccelerationRPS2) ? _pivotConf.trapezoidalLimits From 96753245c00302368e58b64170d5972c2152e7f5 Mon Sep 17 00:00:00 2001 From: Sophie L Date: Sat, 8 Feb 2025 00:38:31 -0800 Subject: [PATCH 02/10] take out set control --- .../java/tagalong/subsystems/micro/PivotFused.java | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/java/tagalong/subsystems/micro/PivotFused.java b/src/main/java/tagalong/subsystems/micro/PivotFused.java index 709251c..178be6a 100644 --- a/src/main/java/tagalong/subsystems/micro/PivotFused.java +++ b/src/main/java/tagalong/subsystems/micro/PivotFused.java @@ -55,12 +55,13 @@ public void followLastProfile() { TrapezoidProfile.State nextState = _trapProfile.calculate(TagalongConfiguration.LOOP_PERIOD_S, _curState, _goalState); - _primaryMotor.setControl( - _requestedPositionVoltage.withPosition(nextState.position) - .withFeedForward( - _pivotFF.calculate(getFFPositionRad(), Units.rotationsToRadians(nextState.velocity)) - ) - ); + // _primaryMotor.setControl( + // _requestedPositionVoltage.withPosition(nextState.position) + // .withFeedForward( + // _pivotFF.calculate(getFFPositionRad(), + // Units.rotationsToRadians(nextState.velocity)) + // ) + // ); if (_isShuffleboardMicro) { _targetPositionEntry.setDouble(nextState.position); From 9517690731deb6464e3561b8c9c2ecaa29c47ae0 Mon Sep 17 00:00:00 2001 From: Sophie L Date: Sun, 9 Feb 2025 14:53:38 -0800 Subject: [PATCH 03/10] proper scoping works --- publish.gradle | 2 +- .../tagalong/commands/base/PivotToAbsoluteCmd.java | 4 ++++ src/main/java/tagalong/subsystems/micro/Pivot.java | 2 +- .../java/tagalong/subsystems/micro/PivotFused.java | 13 ++++++------- 4 files changed, 12 insertions(+), 9 deletions(-) diff --git a/publish.gradle b/publish.gradle index a67c8e1..69f9513 100644 --- a/publish.gradle +++ b/publish.gradle @@ -2,7 +2,7 @@ apply plugin: 'maven-publish' ext.licenseFile = files("$rootDir/LICENSE.txt") -def pubVersion = '2025.0.0054' +def pubVersion = '2025.0.0059' def outputsFolder = file("$buildDir/outputs") diff --git a/src/main/java/tagalong/commands/base/PivotToAbsoluteCmd.java b/src/main/java/tagalong/commands/base/PivotToAbsoluteCmd.java index 79da00a..157bc68 100644 --- a/src/main/java/tagalong/commands/base/PivotToAbsoluteCmd.java +++ b/src/main/java/tagalong/commands/base/PivotToAbsoluteCmd.java @@ -78,6 +78,10 @@ public void execute() { // 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; } diff --git a/src/main/java/tagalong/subsystems/micro/Pivot.java b/src/main/java/tagalong/subsystems/micro/Pivot.java index 3baf5a1..3aae959 100644 --- a/src/main/java/tagalong/subsystems/micro/Pivot.java +++ b/src/main/java/tagalong/subsystems/micro/Pivot.java @@ -635,7 +635,7 @@ public double absoluteClamp(double value) { case 2: return _maxPositionRot; case 1: - return abs; + return placePivotInClosestRot(getPivotPosition(), abs); case 0: default: return _minPositionRot; diff --git a/src/main/java/tagalong/subsystems/micro/PivotFused.java b/src/main/java/tagalong/subsystems/micro/PivotFused.java index 178be6a..709251c 100644 --- a/src/main/java/tagalong/subsystems/micro/PivotFused.java +++ b/src/main/java/tagalong/subsystems/micro/PivotFused.java @@ -55,13 +55,12 @@ public void followLastProfile() { TrapezoidProfile.State nextState = _trapProfile.calculate(TagalongConfiguration.LOOP_PERIOD_S, _curState, _goalState); - // _primaryMotor.setControl( - // _requestedPositionVoltage.withPosition(nextState.position) - // .withFeedForward( - // _pivotFF.calculate(getFFPositionRad(), - // Units.rotationsToRadians(nextState.velocity)) - // ) - // ); + _primaryMotor.setControl( + _requestedPositionVoltage.withPosition(nextState.position) + .withFeedForward( + _pivotFF.calculate(getFFPositionRad(), Units.rotationsToRadians(nextState.velocity)) + ) + ); if (_isShuffleboardMicro) { _targetPositionEntry.setDouble(nextState.position); From 4c07702fb70c430fba042f0447374351b65d8afb Mon Sep 17 00:00:00 2001 From: Sophie L Date: Sun, 9 Feb 2025 17:55:37 -0800 Subject: [PATCH 04/10] :') --- publish.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/publish.gradle b/publish.gradle index 69f9513..4c256cf 100644 --- a/publish.gradle +++ b/publish.gradle @@ -2,7 +2,7 @@ apply plugin: 'maven-publish' ext.licenseFile = files("$rootDir/LICENSE.txt") -def pubVersion = '2025.0.0059' +def pubVersion = '2025.0.0065' def outputsFolder = file("$buildDir/outputs") From 7d7b47384161c108ff9a6bf6eb53f7dc246542f6 Mon Sep 17 00:00:00 2001 From: Sophie L Date: Sat, 1 Mar 2025 10:56:53 -0800 Subject: [PATCH 05/10] remove prints --- .../tagalong/commands/base/PivotToAbsoluteCmd.java | 10 +++++----- src/main/java/tagalong/subsystems/micro/Pivot.java | 8 ++++---- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/tagalong/commands/base/PivotToAbsoluteCmd.java b/src/main/java/tagalong/commands/base/PivotToAbsoluteCmd.java index 157bc68..7644972 100644 --- a/src/main/java/tagalong/commands/base/PivotToAbsoluteCmd.java +++ b/src/main/java/tagalong/commands/base/PivotToAbsoluteCmd.java @@ -78,17 +78,17 @@ public void execute() { // 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 - ); + // 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 "); + // System.out.println("\ngoal scoped: " + _scopedGoalPositionRot + "\n "); _pivot.setPivotProfile( _pivot.clampPivotPosition(_scopedGoalPositionRot), 0.0, _maxVelocityRPS ); diff --git a/src/main/java/tagalong/subsystems/micro/Pivot.java b/src/main/java/tagalong/subsystems/micro/Pivot.java index 3aae959..4ec6e09 100644 --- a/src/main/java/tagalong/subsystems/micro/Pivot.java +++ b/src/main/java/tagalong/subsystems/micro/Pivot.java @@ -530,10 +530,10 @@ public void setPivotProfile( _goalState.position = (_absoluteRangeRot < 1.0 ? absoluteClamp(goalPositionRot) : clampPivotPosition(goalPositionRot)) + _profileTargetOffset; - System.out.println( - "goal: " + goalPositionRot + "\ngoal clamped: " + absoluteClamp(goalPositionRot) - + "\nabs range: " + (_absoluteRangeRot < 1.0) - ); + // System.out.println( + // "goal: " + goalPositionRot + "\ngoal clamped: " + absoluteClamp(goalPositionRot) + // + "\nabs range: " + (_absoluteRangeRot < 1.0) + // ); _trapProfile = new TrapezoidProfile( (maxVelocityRPS >= _maxVelocityRPS || maxAccelerationRPS2 >= _maxAccelerationRPS2) ? _pivotConf.trapezoidalLimits From fb4fa9cdbca638b5801572e929fcee500ac38361 Mon Sep 17 00:00:00 2001 From: Sophie L Date: Sat, 1 Mar 2025 15:04:11 -0800 Subject: [PATCH 06/10] set position 0.0 on boot --- src/main/java/tagalong/subsystems/micro/Pivot.java | 10 ++++++++++ src/main/java/tagalong/subsystems/micro/Roller.java | 10 ++++++++++ 2 files changed, 20 insertions(+) diff --git a/src/main/java/tagalong/subsystems/micro/Pivot.java b/src/main/java/tagalong/subsystems/micro/Pivot.java index 4ec6e09..697354f 100644 --- a/src/main/java/tagalong/subsystems/micro/Pivot.java +++ b/src/main/java/tagalong/subsystems/micro/Pivot.java @@ -187,6 +187,16 @@ public Pivot(PivotConf conf) { } } + // Override to ensure the position config happens after the devices are configured + @Override + public void configAllDevices() { + super.configAllDevices(); + + // FUTURE DEV: Look into if all motors or just the leader need their positions set? + // for (var motor : _allMotors) motor.setPosition(0.0); + _primaryMotor.setPosition(0.0); + } + /** * Periodic update function */ diff --git a/src/main/java/tagalong/subsystems/micro/Roller.java b/src/main/java/tagalong/subsystems/micro/Roller.java index a163a13..3254cb7 100644 --- a/src/main/java/tagalong/subsystems/micro/Roller.java +++ b/src/main/java/tagalong/subsystems/micro/Roller.java @@ -121,6 +121,16 @@ public Roller(RollerConf conf) { configAllDevices(); } + // Override to ensure the position config happens after the devices are configured + @Override + public void configAllDevices() { + super.configAllDevices(); + + // FUTURE DEV: Look into if all motors or just the leader need their positions set? + // for (var motor : _allMotors) motor.setPosition(0.0); + _primaryMotor.setPosition(0.0); + } + @Override public void onEnable() { if (_isMicrosystemDisabled) { From 77a4e43b9557972b8b633a29b878e1683feb8c45 Mon Sep 17 00:00:00 2001 From: Sophie L Date: Mon, 3 Mar 2025 00:55:23 -0800 Subject: [PATCH 07/10] commands fixes - pivot logic + isfinished condition for all + acceleration param in pivotto --- .../tagalong/commands/base/ElevateToCmd.java | 2 +- .../tagalong/commands/base/ElevateXCmd.java | 2 +- .../commands/base/PivotToAbsoluteCmd.java | 2 +- .../tagalong/commands/base/PivotToCmd.java | 106 ++++++++++-------- .../base/PivotToDynamicAbsoluteCmd.java | 3 +- .../commands/base/PivotToDynamicCmd.java | 2 +- .../tagalong/commands/base/PivotXCmd.java | 4 +- .../tagalong/commands/base/RollToCmd.java | 2 +- .../java/tagalong/commands/base/RollXCmd.java | 2 +- .../java/tagalong/subsystems/micro/Pivot.java | 39 ++++--- .../tagalong/subsystems/micro/PivotFused.java | 19 +++- 11 files changed, 103 insertions(+), 80 deletions(-) diff --git a/src/main/java/tagalong/commands/base/ElevateToCmd.java b/src/main/java/tagalong/commands/base/ElevateToCmd.java index 45e7a81..518cde8 100644 --- a/src/main/java/tagalong/commands/base/ElevateToCmd.java +++ b/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), diff --git a/src/main/java/tagalong/commands/base/ElevateXCmd.java b/src/main/java/tagalong/commands/base/ElevateXCmd.java index 45890ea..5f6f7dc 100644 --- a/src/main/java/tagalong/commands/base/ElevateXCmd.java +++ b/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), diff --git a/src/main/java/tagalong/commands/base/PivotToAbsoluteCmd.java b/src/main/java/tagalong/commands/base/PivotToAbsoluteCmd.java index 7644972..a000ed2 100644 --- a/src/main/java/tagalong/commands/base/PivotToAbsoluteCmd.java +++ b/src/main/java/tagalong/commands/base/PivotToAbsoluteCmd.java @@ -90,7 +90,7 @@ public void execute() { } // System.out.println("\ngoal scoped: " + _scopedGoalPositionRot + "\n "); _pivot.setPivotProfile( - _pivot.clampPivotPosition(_scopedGoalPositionRot), 0.0, _maxVelocityRPS + _scopedGoalPositionRot, 0.0, _maxVelocityRPS ); } diff --git a/src/main/java/tagalong/commands/base/PivotToCmd.java b/src/main/java/tagalong/commands/base/PivotToCmd.java index bdf3c8c..3712ef4 100644 --- a/src/main/java/tagalong/commands/base/PivotToCmd.java +++ b/src/main/java/tagalong/commands/base/PivotToCmd.java @@ -49,6 +49,8 @@ public class PivotToCmd 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; diff --git a/src/main/java/tagalong/commands/base/PivotToDynamicAbsoluteCmd.java b/src/main/java/tagalong/commands/base/PivotToDynamicAbsoluteCmd.java index bee8e56..453b864 100644 --- a/src/main/java/tagalong/commands/base/PivotToDynamicAbsoluteCmd.java +++ b/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( diff --git a/src/main/java/tagalong/commands/base/PivotToDynamicCmd.java b/src/main/java/tagalong/commands/base/PivotToDynamicCmd.java index ddd6e02..a2f394c 100644 --- a/src/main/java/tagalong/commands/base/PivotToDynamicCmd.java +++ b/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(); diff --git a/src/main/java/tagalong/commands/base/PivotXCmd.java b/src/main/java/tagalong/commands/base/PivotXCmd.java index e0634db..ec5e7f5 100644 --- a/src/main/java/tagalong/commands/base/PivotXCmd.java +++ b/src/main/java/tagalong/commands/base/PivotXCmd.java @@ -75,7 +75,7 @@ public class PivotXCmd 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), diff --git a/src/main/java/tagalong/commands/base/RollToCmd.java b/src/main/java/tagalong/commands/base/RollToCmd.java index f78bc8a..4b7ea98 100644 --- a/src/main/java/tagalong/commands/base/RollToCmd.java +++ b/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), diff --git a/src/main/java/tagalong/commands/base/RollXCmd.java b/src/main/java/tagalong/commands/base/RollXCmd.java index 419c74b..9f6df2f 100644 --- a/src/main/java/tagalong/commands/base/RollXCmd.java +++ b/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), diff --git a/src/main/java/tagalong/subsystems/micro/Pivot.java b/src/main/java/tagalong/subsystems/micro/Pivot.java index 697354f..6285357 100644 --- a/src/main/java/tagalong/subsystems/micro/Pivot.java +++ b/src/main/java/tagalong/subsystems/micro/Pivot.java @@ -107,6 +107,8 @@ public class Pivot extends Microsystem { */ protected MechanismLigament2d _pivotLigament; + protected double _offset = 0.0; + /** * Constructs a pivot microsystem with the below configurations * @@ -138,16 +140,15 @@ public Pivot(PivotConf conf) { // current position double min = _pivotConf.rotationalMin; double max = _pivotConf.rotationalMax; - while (min >= getPivotPosition()) { - min -= 1.0; - max -= 1.0; + + while (min + _offset >= getPivotPosition()) { + _offset -= 1.0; } - while (max <= getPivotPosition()) { - min += 1.0; - max += 1.0; + while (max + _offset <= getPivotPosition()) { + _offset += 1.0; } - _minPositionRot = min; - _maxPositionRot = max; + _minPositionRot = min + _offset; + _maxPositionRot = max + _offset; _absoluteRangeRot = _maxPositionRot - _minPositionRot; _maxVelocityRPS = _pivotConf.trapezoidalLimitsVelocity; @@ -163,7 +164,7 @@ public Pivot(PivotConf conf) { double minAbs = AlgebraicUtils.cppMod(_minPositionRot, 1.0); double maxAbs = AlgebraicUtils.cppMod(_maxPositionRot, 1.0); - double halfUnusedRange = (_absoluteRangeRot) / 2.0; + double halfUnusedRange = (1.0 -_absoluteRangeRot) / 2.0; double midUnused = maxAbs + halfUnusedRange; if (midUnused > 1.0) { @@ -537,13 +538,15 @@ public void setPivotProfile( } _goalState.velocity = goalVelocityRPS; - _goalState.position = (_absoluteRangeRot < 1.0 ? absoluteClamp(goalPositionRot) - : clampPivotPosition(goalPositionRot)) - + _profileTargetOffset; - // System.out.println( - // "goal: " + goalPositionRot + "\ngoal clamped: " + absoluteClamp(goalPositionRot) - // + "\nabs range: " + (_absoluteRangeRot < 1.0) - // ); + _goalState.position = clampPivotPosition(goalPositionRot); + // NOTE + TODO: code removed due to compensating an already compensated value + // _goalState.position = (_absoluteRangeRot < 1.0 ? absoluteClamp(goalPositionRot + _offset) + // : clampPivotPosition(goalPositionRot + _offset)) + // + _profileTargetOffset; + System.out.println( + "goal: " + goalPositionRot + "\ngoal clamped: " + absoluteClamp(goalPositionRot + _offset) + + "\nabs range: " + (_absoluteRangeRot < 1.0) + " offset: " + _offset + ); _trapProfile = new TrapezoidProfile( (maxVelocityRPS >= _maxVelocityRPS || maxAccelerationRPS2 >= _maxAccelerationRPS2) ? _pivotConf.trapezoidalLimits @@ -697,4 +700,8 @@ public void holdCurrentPosition() { public MechanismLigament2d getPivotLigament() { return _pivotLigament; } + + public double getScopeOffset(){ + return _offset; + } } diff --git a/src/main/java/tagalong/subsystems/micro/PivotFused.java b/src/main/java/tagalong/subsystems/micro/PivotFused.java index 709251c..59126a7 100644 --- a/src/main/java/tagalong/subsystems/micro/PivotFused.java +++ b/src/main/java/tagalong/subsystems/micro/PivotFused.java @@ -30,6 +30,7 @@ public class PivotFused extends Pivot { * Configuration for the CANcoder */ protected CANcoderConfiguration _pivotCancoderConfiguration; + protected boolean _fusedCancoderSetup = false; /** * Constructs a pivot microsystem with the below configurations * @@ -40,11 +41,18 @@ public PivotFused(PivotConf conf) { if (_configuredMicrosystemDisable) { return; } - _pivotCancoder = new CANcoder(_pivotConf.encoderDeviceID, _pivotConf.encoderCanBus); - _pivotCancoderConfiguration = _pivotConf.encoderConfig; - configCancoder(); - configAllDevices(); - configMotor(); + setupFusedCancoder(); + } + + private void setupFusedCancoder() { + if(!_fusedCancoderSetup) { + _pivotCancoder = new CANcoder(_pivotConf.encoderDeviceID, _pivotConf.encoderCanBus); + _pivotCancoderConfiguration = _pivotConf.encoderConfig; + configCancoder(); + configAllDevices(); + configMotor(); + _fusedCancoderSetup = true; + } } @Override @@ -94,6 +102,7 @@ public void setPivotVelocity(double rps, boolean withFF) { @Override public double getPivotPosition() { + setupFusedCancoder(); return getPrimaryMotorPosition(); } From 2739188d6f7dcc70fa145b5d3d90ee500301d83c Mon Sep 17 00:00:00 2001 From: Sophie L Date: Mon, 3 Mar 2025 15:18:53 -0800 Subject: [PATCH 08/10] removed comments + renamed _offset to _scopeOffset --- .../commands/base/PivotToAbsoluteCmd.java | 13 +------- .../java/tagalong/subsystems/micro/Pivot.java | 31 +++++++++---------- 2 files changed, 16 insertions(+), 28 deletions(-) diff --git a/src/main/java/tagalong/commands/base/PivotToAbsoluteCmd.java b/src/main/java/tagalong/commands/base/PivotToAbsoluteCmd.java index a000ed2..093a8c6 100644 --- a/src/main/java/tagalong/commands/base/PivotToAbsoluteCmd.java +++ b/src/main/java/tagalong/commands/base/PivotToAbsoluteCmd.java @@ -73,25 +73,14 @@ 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( - _scopedGoalPositionRot, 0.0, _maxVelocityRPS - ); + _pivot.setPivotProfile(_scopedGoalPositionRot, 0.0, _maxVelocityRPS); } if (_startedMovement) { diff --git a/src/main/java/tagalong/subsystems/micro/Pivot.java b/src/main/java/tagalong/subsystems/micro/Pivot.java index 6285357..1ed7393 100644 --- a/src/main/java/tagalong/subsystems/micro/Pivot.java +++ b/src/main/java/tagalong/subsystems/micro/Pivot.java @@ -107,7 +107,7 @@ public class Pivot extends Microsystem { */ protected MechanismLigament2d _pivotLigament; - protected double _offset = 0.0; + protected double _scopeOffset = 0.0; /** * Constructs a pivot microsystem with the below configurations @@ -141,14 +141,14 @@ public Pivot(PivotConf conf) { double min = _pivotConf.rotationalMin; double max = _pivotConf.rotationalMax; - while (min + _offset >= getPivotPosition()) { - _offset -= 1.0; + while (min + _scopeOffset >= getPivotPosition()) { + _scopeOffset -= 1.0; } - while (max + _offset <= getPivotPosition()) { - _offset += 1.0; + while (max + _scopeOffset <= getPivotPosition()) { + _scopeOffset += 1.0; } - _minPositionRot = min + _offset; - _maxPositionRot = max + _offset; + _minPositionRot = min + _scopeOffset; + _maxPositionRot = max + _scopeOffset; _absoluteRangeRot = _maxPositionRot - _minPositionRot; _maxVelocityRPS = _pivotConf.trapezoidalLimitsVelocity; @@ -164,7 +164,7 @@ public Pivot(PivotConf conf) { double minAbs = AlgebraicUtils.cppMod(_minPositionRot, 1.0); double maxAbs = AlgebraicUtils.cppMod(_maxPositionRot, 1.0); - double halfUnusedRange = (1.0 -_absoluteRangeRot) / 2.0; + double halfUnusedRange = (1.0 - _absoluteRangeRot) / 2.0; double midUnused = maxAbs + halfUnusedRange; if (midUnused > 1.0) { @@ -540,13 +540,12 @@ public void setPivotProfile( _goalState.velocity = goalVelocityRPS; _goalState.position = clampPivotPosition(goalPositionRot); // NOTE + TODO: code removed due to compensating an already compensated value - // _goalState.position = (_absoluteRangeRot < 1.0 ? absoluteClamp(goalPositionRot + _offset) - // : clampPivotPosition(goalPositionRot + _offset)) + // _goalState.position = (_absoluteRangeRot < 1.0 ? absoluteClamp(goalPositionRot + + // _scopeOffset) + // : clampPivotPosition(goalPositionRot + + // _scopeOffset)) // + _profileTargetOffset; - System.out.println( - "goal: " + goalPositionRot + "\ngoal clamped: " + absoluteClamp(goalPositionRot + _offset) - + "\nabs range: " + (_absoluteRangeRot < 1.0) + " offset: " + _offset - ); + _trapProfile = new TrapezoidProfile( (maxVelocityRPS >= _maxVelocityRPS || maxAccelerationRPS2 >= _maxAccelerationRPS2) ? _pivotConf.trapezoidalLimits @@ -701,7 +700,7 @@ public MechanismLigament2d getPivotLigament() { return _pivotLigament; } - public double getScopeOffset(){ - return _offset; + public double getScopeOffset() { + return _scopeOffset; } } From 8617bb01cc4b33e9e8bc0504cd60548c093b955a Mon Sep 17 00:00:00 2001 From: Sophie L Date: Mon, 3 Mar 2025 15:33:59 -0800 Subject: [PATCH 09/10] fix overloaded constructors pivot to cmd --- .../tagalong/commands/base/PivotToCmd.java | 83 ++++++++++++++++--- 1 file changed, 70 insertions(+), 13 deletions(-) diff --git a/src/main/java/tagalong/commands/base/PivotToCmd.java b/src/main/java/tagalong/commands/base/PivotToCmd.java index 3712ef4..6ad903c 100644 --- a/src/main/java/tagalong/commands/base/PivotToCmd.java +++ b/src/main/java/tagalong/commands/base/PivotToCmd.java @@ -165,6 +165,31 @@ public PivotToCmd(int id, T pivot, Angle goalPosition, boolean holdPositionAfter * second, during this command */ + public PivotToCmd(T pivot, Angle goalPosition, boolean holdPositionAfter, double maxVelocityRPS) { + this( + pivot, + goalPosition, + holdPositionAfter, + maxVelocityRPS, + pivot.getPivot()._maxAccelerationRPS2, + pivot.getPivot()._defaultPivotLowerToleranceRot, + pivot.getPivot()._defaultPivotUpperToleranceRot + ); + } + + /** + * 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 maxAccelerationRPS2 The maximum acceleration of the pivot, in rotations per + * second squared, during this command + */ + public PivotToCmd( T pivot, Angle goalPosition, @@ -193,6 +218,8 @@ public PivotToCmd( * completes * @param maxVelocityRPS The maximum velocity of the pivot, in rotations per * second, during this command + * @param maxAccelerationRPS2 The maximum acceleration of the pivot, in rotations per + * second squared, during this command */ public PivotToCmd( @@ -224,6 +251,8 @@ public PivotToCmd( * completes * @param maxVelocityRPS The maximum velocity of the pivot, in rotations per * second, during this command + * @param maxAccelerationRPS2 The maximum acceleration of the pivot, in rotations per + * second squared, 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 @@ -250,6 +279,24 @@ public PivotToCmd( ); } + /** + * 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 maxAccelerationRPS2 The maximum acceleration of the pivot, in rotations per + * second squared, 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, @@ -274,19 +321,21 @@ public PivotToCmd( /** * 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 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 + * @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 maxAccelerationRPS2 The maximum acceleration of the pivot, in rotations per + * second squared, 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( @@ -323,6 +372,8 @@ public PivotToCmd( * @param maxVelocityRPS The maximum velocity of the pivot, in * rotations per second, during this * command + * @param maxAccelerationRPS2 The maximum acceleration of the pivot, in rotations per + * second squared, during this command * @param upperToleranceRot The number of rotations beyond the target * position the pivot can be * while still being considered in tolerance @@ -367,6 +418,8 @@ public PivotToCmd( * @param maxVelocityRPS The maximum velocity of the pivot, in * rotations per second, during this * command + * @param maxAccelerationRPS2 The maximum acceleration of the pivot, in rotations per + * second squared, during this command * @param lowerToleranceRot The number of rotations short of the * target position the pivot can be * while still being considered in tolerance @@ -412,6 +465,8 @@ public PivotToCmd( * @param maxVelocityRPS The maximum velocity of the pivot, in * rotations per second, during this * command + * @param maxAccelerationRPS2 The maximum acceleration of the pivot, in rotations per + * second squared, during this command * @param lowerToleranceRot The number of rotations short of the * target position the pivot can be * while still being considered in tolerance @@ -426,6 +481,7 @@ public PivotToCmd( double goalPosition, boolean holdPositionAfter, double maxVelocityRPS, + double maxAccelerationRPS2, double lowerToleranceRot, double upperToleranceRot, double requiredInToleranceDurationS @@ -434,6 +490,7 @@ public PivotToCmd( _goalPositionRot = goalPosition; _holdPositionAfter = holdPositionAfter; _maxVelocityRPS = maxVelocityRPS; + _maxAccelerationRPS2 = maxAccelerationRPS2; _lowerBoundRot = _goalPositionRot - Math.abs(lowerToleranceRot); _upperBoundRot = _goalPositionRot + Math.abs(upperToleranceRot); _requiredInToleranceDurationS = requiredInToleranceDurationS; From e5b4f0f95b504c1e355fd8570e7831ff1f7e341c Mon Sep 17 00:00:00 2001 From: Space Cookies Date: Mon, 3 Mar 2025 17:31:40 -0800 Subject: [PATCH 10/10] cleanup set position 0 --- src/main/java/tagalong/subsystems/micro/Elevator.java | 6 ------ src/main/java/tagalong/subsystems/micro/Microsystem.java | 1 - src/main/java/tagalong/subsystems/micro/Roller.java | 6 ------ 3 files changed, 13 deletions(-) diff --git a/src/main/java/tagalong/subsystems/micro/Elevator.java b/src/main/java/tagalong/subsystems/micro/Elevator.java index 41c0741..9273e00 100644 --- a/src/main/java/tagalong/subsystems/micro/Elevator.java +++ b/src/main/java/tagalong/subsystems/micro/Elevator.java @@ -133,12 +133,6 @@ public Elevator(ElevatorConf conf) { _elevatorZeroingDurationS = _elevatorConf.elevatorZeroingDurationS; configAllDevices(); - } - - // Override to ensure the position config happens after the devices are configured - @Override - public void configAllDevices() { - super.configAllDevices(); // FUTURE DEV: Look into if all motors or just the leader need their positions set? // for (var motor : _allMotors) motor.setPosition(0.0); diff --git a/src/main/java/tagalong/subsystems/micro/Microsystem.java b/src/main/java/tagalong/subsystems/micro/Microsystem.java index 4cb375e..5a5cd44 100644 --- a/src/main/java/tagalong/subsystems/micro/Microsystem.java +++ b/src/main/java/tagalong/subsystems/micro/Microsystem.java @@ -195,7 +195,6 @@ public void configAllDevices() { for (int i = 0; i < _conf.numMotors; i++) { _allMotors[i].getConfigurator().apply(_conf.motorConfig[i]); } - for (int i = 1; i < _conf.numMotors; i++) { _allMotors[i].setControl(new StrictFollower(_primaryMotor.getDeviceID())); } diff --git a/src/main/java/tagalong/subsystems/micro/Roller.java b/src/main/java/tagalong/subsystems/micro/Roller.java index 3254cb7..62ef4d9 100644 --- a/src/main/java/tagalong/subsystems/micro/Roller.java +++ b/src/main/java/tagalong/subsystems/micro/Roller.java @@ -119,12 +119,6 @@ public Roller(RollerConf conf) { _defaultRollerUpperToleranceRot = _rollerConf.defaultUpperTolerance; configAllDevices(); - } - - // Override to ensure the position config happens after the devices are configured - @Override - public void configAllDevices() { - super.configAllDevices(); // FUTURE DEV: Look into if all motors or just the leader need their positions set? // for (var motor : _allMotors) motor.setPosition(0.0);