diff --git a/publish.gradle b/publish.gradle index 1a466c4..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.21.0204' +def pubVersion = '2025.0.0065' def outputsFolder = file("$buildDir/outputs") 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 d1bd040..093a8c6 100644 --- a/src/main/java/tagalong/commands/base/PivotToAbsoluteCmd.java +++ b/src/main/java/tagalong/commands/base/PivotToAbsoluteCmd.java @@ -80,10 +80,7 @@ public void execute() { if (_scopedGoalPositionRot > _pivot._maxPositionRot) { _scopedGoalPositionRot -= 1.0; } - - _pivot.setPivotProfile( - _pivot.clampPivotPosition(_scopedGoalPositionRot), 0.0, _maxVelocityRPS - ); + _pivot.setPivotProfile(_scopedGoalPositionRot, 0.0, _maxVelocityRPS); } if (_startedMovement) { diff --git a/src/main/java/tagalong/commands/base/PivotToCmd.java b/src/main/java/tagalong/commands/base/PivotToCmd.java index bdf3c8c..6ad903c 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,7 +144,14 @@ 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); + this( + id, + pivot, + goalPosition, + holdPositionAfter, + pivot.getPivot(id)._maxVelocityRPS, + pivot.getPivot(id)._maxAccelerationRPS2 + ); } /** @@ -154,6 +171,7 @@ public PivotToCmd(T pivot, Angle goalPosition, boolean holdPositionAfter, double goalPosition, holdPositionAfter, maxVelocityRPS, + pivot.getPivot()._maxAccelerationRPS2, pivot.getPivot()._defaultPivotLowerToleranceRot, pivot.getPivot()._defaultPivotUpperToleranceRot ); @@ -162,52 +180,65 @@ public PivotToCmd(T pivot, Angle goalPosition, boolean holdPositionAfter, double /** * 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 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( - 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 + * @param maxAccelerationRPS2 The maximum acceleration of the pivot, in rotations per + * second squared, during this command */ 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 + ); } /** @@ -220,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 @@ -231,33 +264,45 @@ 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 + * @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, boolean holdPositionAfter, double maxVelocityRPS, + double maxAccelerationRPS2, double lowerToleranceRot, double upperToleranceRot ) { @@ -266,6 +311,7 @@ public PivotToCmd( goalPosition, holdPositionAfter, maxVelocityRPS, + maxAccelerationRPS2, lowerToleranceRot, upperToleranceRot, -1.0 @@ -275,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( @@ -296,6 +344,7 @@ public PivotToCmd( Angle goalPosition, boolean holdPositionAfter, double maxVelocityRPS, + double maxAccelerationRPS2, double lowerToleranceRot, double upperToleranceRot ) { @@ -305,6 +354,7 @@ public PivotToCmd( goalPosition, holdPositionAfter, maxVelocityRPS, + maxAccelerationRPS2, lowerToleranceRot, upperToleranceRot, -1.0 @@ -322,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 @@ -336,15 +388,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 @@ -363,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 @@ -378,6 +435,7 @@ public PivotToCmd( Angle goalPosition, boolean holdPositionAfter, double maxVelocityRPS, + double maxAccelerationRPS2, double lowerToleranceRot, double upperToleranceRot, double requiredInToleranceDurationS @@ -388,6 +446,7 @@ public PivotToCmd( goalPosition.getRotations(), holdPositionAfter, maxVelocityRPS, + maxAccelerationRPS2, lowerToleranceRot, upperToleranceRot, requiredInToleranceDurationS @@ -406,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 @@ -420,6 +481,7 @@ public PivotToCmd( double goalPosition, boolean holdPositionAfter, double maxVelocityRPS, + double maxAccelerationRPS2, double lowerToleranceRot, double upperToleranceRot, double requiredInToleranceDurationS @@ -428,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; @@ -464,6 +527,7 @@ public PivotToCmd( double goalPosition, boolean holdPositionAfter, double maxVelocityRPS, + double maxAccelerationRPS2, double lowerToleranceRot, double upperToleranceRot, double requiredInToleranceDurationS @@ -472,6 +536,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/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/Pivot.java b/src/main/java/tagalong/subsystems/micro/Pivot.java index 7633544..1ed7393 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 _scopeOffset = 0.0; + /** * Constructs a pivot microsystem with the below configurations * @@ -131,8 +133,23 @@ 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 + _scopeOffset >= getPivotPosition()) { + _scopeOffset -= 1.0; + } + while (max + _scopeOffset <= getPivotPosition()) { + _scopeOffset += 1.0; + } + _minPositionRot = min + _scopeOffset; + _maxPositionRot = max + _scopeOffset; + _absoluteRangeRot = _maxPositionRot - _minPositionRot; _maxVelocityRPS = _pivotConf.trapezoidalLimitsVelocity; _maxAccelerationRPS2 = _pivotConf.trapezoidalLimitsAcceleration; @@ -147,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) { @@ -171,6 +188,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 */ @@ -511,9 +538,13 @@ public void setPivotProfile( } _goalState.velocity = goalVelocityRPS; - _goalState.position = (_absoluteRangeRot < 1.0 ? absoluteClamp(goalPositionRot) - : clampPivotPosition(goalPositionRot)) - + _profileTargetOffset; + _goalState.position = clampPivotPosition(goalPositionRot); + // NOTE + TODO: code removed due to compensating an already compensated value + // _goalState.position = (_absoluteRangeRot < 1.0 ? absoluteClamp(goalPositionRot + + // _scopeOffset) + // : clampPivotPosition(goalPositionRot + + // _scopeOffset)) + // + _profileTargetOffset; _trapProfile = new TrapezoidProfile( (maxVelocityRPS >= _maxVelocityRPS || maxAccelerationRPS2 >= _maxAccelerationRPS2) @@ -616,7 +647,7 @@ public double absoluteClamp(double value) { case 2: return _maxPositionRot; case 1: - return abs; + return placePivotInClosestRot(getPivotPosition(), abs); case 0: default: return _minPositionRot; @@ -668,4 +699,8 @@ public void holdCurrentPosition() { public MechanismLigament2d getPivotLigament() { return _pivotLigament; } + + public double getScopeOffset() { + return _scopeOffset; + } } 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(); } diff --git a/src/main/java/tagalong/subsystems/micro/Roller.java b/src/main/java/tagalong/subsystems/micro/Roller.java index a163a13..62ef4d9 100644 --- a/src/main/java/tagalong/subsystems/micro/Roller.java +++ b/src/main/java/tagalong/subsystems/micro/Roller.java @@ -119,6 +119,10 @@ public Roller(RollerConf conf) { _defaultRollerUpperToleranceRot = _rollerConf.defaultUpperTolerance; 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