diff --git a/publish.gradle b/publish.gradle index 4c256cf..a57438f 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.0065' +def pubVersion = '2025.0.0083' def outputsFolder = file("$buildDir/outputs") diff --git a/src/main/java/tagalong/subsystems/micro/Elevator.java b/src/main/java/tagalong/subsystems/micro/Elevator.java index 9273e00..2fdaa0a 100644 --- a/src/main/java/tagalong/subsystems/micro/Elevator.java +++ b/src/main/java/tagalong/subsystems/micro/Elevator.java @@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; +import org.littletonrobotics.junction.Logger; import tagalong.TagalongConfiguration; import tagalong.math.AlgebraicUtils; import tagalong.measurements.Height; @@ -161,6 +162,7 @@ public void followLastProfile() { } _curState = nextState; + Logger.recordOutput(_conf.name + " next state", nextState.position); } /** @@ -318,7 +320,7 @@ public void setElevatorProfile( _goalState.velocity = goalVelocityMPS; _trapProfile = new TrapezoidProfile( - (maxVelocityMPS >= _maxVelocityMPS || maxAccelerationMPS2 >= _maxAccelerationMPS2) + (maxVelocityMPS > _maxVelocityMPS || maxAccelerationMPS2 > _maxAccelerationMPS2) ? _elevatorConf.trapezoidalLimits : new TrapezoidProfile.Constraints(maxVelocityMPS, maxAccelerationMPS2) ); @@ -408,6 +410,9 @@ public void periodic() { } else if (_isFFTuningMicro && _trapProfile.isFinished(_profileTimer.get())) { _primaryMotor.setControl(_requestedPositionVoltage.withFeedForward(_elevatorFF.getKs())); } + Logger.recordOutput(_conf.name + " motor reset config", motorResetConfig()); + Logger.recordOutput(_conf.name + " profile velocity", _curState.velocity); + Logger.recordOutput(_conf.name + " profile state", _curState.position); if (_followProfile) { followLastProfile(); } diff --git a/src/main/java/tagalong/subsystems/micro/Microsystem.java b/src/main/java/tagalong/subsystems/micro/Microsystem.java index d8fef33..5d1a418 100644 --- a/src/main/java/tagalong/subsystems/micro/Microsystem.java +++ b/src/main/java/tagalong/subsystems/micro/Microsystem.java @@ -338,6 +338,13 @@ public TalonFX getPrimaryMotor() { return _primaryMotor; } + public TalonFX getOtherMotor(int id) { + if (_isMicrosystemDisabled) { + return new TalonFX(id); + } + return _allMotors[id]; + } + /** * @return position of the primary motor, 0.0 if system is disabled */ @@ -461,7 +468,7 @@ public void onTeleopDisable() { * @param followProfile whether or not to follow the trapezoidal profile */ public void setFollowProfile(boolean followProfile) { - if (!(_isMicrosystemDisabled)) { + if (!_isMicrosystemDisabled) { _followProfile = followProfile; } } diff --git a/src/main/java/tagalong/subsystems/micro/Pivot.java b/src/main/java/tagalong/subsystems/micro/Pivot.java index 1ed7393..aaad246 100644 --- a/src/main/java/tagalong/subsystems/micro/Pivot.java +++ b/src/main/java/tagalong/subsystems/micro/Pivot.java @@ -10,13 +10,11 @@ import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.IterativeRobotBase; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color8Bit; -import tagalong.TagalongConfiguration; import tagalong.math.AlgebraicUtils; import tagalong.measurements.Angle; import tagalong.subsystems.micro.confs.PivotConf; @@ -47,12 +45,12 @@ public class Pivot extends Microsystem { /** * Absolute range of pivot movement in rotations */ - public final double _absoluteRangeRot; + public double _absoluteRangeRot; /** * Minimum position of the pivot in rotations, * Maximum position of the pivot in rotations */ - public final double _minPositionRot, _maxPositionRot; + public double _minPositionRot, _maxPositionRot; /** * Maximum velocity of the pivot in rotations per second, * Maximum acceleration of the pivot in rotations per second squared @@ -121,9 +119,6 @@ public Pivot(PivotConf conf) { if (_configuredMicrosystemDisable) { _defaultPivotLowerToleranceRot = 0.0; _defaultPivotUpperToleranceRot = 0.0; - _absoluteRangeRot = 0.0; - _minPositionRot = 0.0; - _maxPositionRot = 0.0; _maxVelocityRPS = 0.0; _maxAccelerationRPS2 = 0.0; _profileTargetOffset = 0.0; @@ -138,19 +133,7 @@ public Pivot(PivotConf conf) { // 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; _profileTargetOffset = _pivotConf.profileOffsetValue; @@ -162,30 +145,33 @@ public Pivot(PivotConf conf) { _motorToEncoderRatio = _pivotConf.motorToEncoderRatio; _encoderToPivotRatio = _pivotConf.encoderToPivotRatio; - double minAbs = AlgebraicUtils.cppMod(_minPositionRot, 1.0); - double maxAbs = AlgebraicUtils.cppMod(_maxPositionRot, 1.0); - double halfUnusedRange = (1.0 - _absoluteRangeRot) / 2.0; - double midUnused = maxAbs + halfUnusedRange; - - if (midUnused > 1.0) { - _values = new double[] {midUnused - 1.0, minAbs, maxAbs, 1.0}; - _ids = new int[] {2, 0, 1, 2}; - } else if (_minPositionRot > 0.0) { - _values = new double[] {minAbs, maxAbs, midUnused, 1.0}; - _ids = new int[] {0, 1, 2, 0}; - } else { - _values = new double[] {maxAbs, midUnused, minAbs, 1.0}; - _ids = new int[] {1, 2, 0, 1}; - } - if (IterativeRobotBase.isReal()) { - int count = 0; - // while (!_primaryMotor.isAlive() && count <= 1000) { - // System.out.println(_pivotConf.name + " not alive " + (count++)); - // } - if (count >= 1000) { - System.out.println(_pivotConf.name + " failed to initialize!"); - } - } + _minPositionRot = _pivotConf.rotationalMin; + _maxPositionRot = _pivotConf.rotationalMax; + + // double minAbs = AlgebraicUtils.cppMod(_minPositionRot, 1.0); + // double maxAbs = AlgebraicUtils.cppMod(_maxPositionRot, 1.0); + // double halfUnusedRange = (1.0 - _absoluteRangeRot) / 2.0; + // double midUnused = maxAbs + halfUnusedRange; + + // if (midUnused > 1.0) { + // _values = new double[] {midUnused - 1.0, minAbs, maxAbs, 1.0}; + // _ids = new int[] {2, 0, 1, 2}; + // } else if (_minPositionRot > 0.0) { + // _values = new double[] {minAbs, maxAbs, midUnused, 1.0}; + // _ids = new int[] {0, 1, 2, 0}; + // } else { + // _values = new double[] {maxAbs, midUnused, minAbs, 1.0}; + // _ids = new int[] {1, 2, 0, 1}; + // } + // if (IterativeRobotBase.isReal()) { + // int count = 0; + // // while (!_primaryMotor.isAlive() && count <= 1000) { + // // System.out.println(_pivotConf.name + " not alive " + (count++)); + // // } + // if (count >= 1000) { + // System.out.println(_pivotConf.name + " failed to initialize!"); + // } + // } } // Override to ensure the position config happens after the devices are configured @@ -547,7 +533,7 @@ public void setPivotProfile( // + _profileTargetOffset; _trapProfile = new TrapezoidProfile( - (maxVelocityRPS >= _maxVelocityRPS || maxAccelerationRPS2 >= _maxAccelerationRPS2) + (maxVelocityRPS > _maxVelocityRPS || maxAccelerationRPS2 > _maxAccelerationRPS2) ? _pivotConf.trapezoidalLimits : new TrapezoidProfile.Constraints(maxVelocityRPS, maxAccelerationRPS2) ); diff --git a/src/main/java/tagalong/subsystems/micro/PivotFused.java b/src/main/java/tagalong/subsystems/micro/PivotFused.java index 59126a7..0fbdbea 100644 --- a/src/main/java/tagalong/subsystems/micro/PivotFused.java +++ b/src/main/java/tagalong/subsystems/micro/PivotFused.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.BatterySim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import org.littletonrobotics.junction.Logger; import tagalong.TagalongConfiguration; import tagalong.subsystems.micro.confs.PivotConf; @@ -41,18 +42,35 @@ public PivotFused(PivotConf conf) { if (_configuredMicrosystemDisable) { return; } - setupFusedCancoder(); - } - - private void setupFusedCancoder() { - if(!_fusedCancoderSetup) { - _pivotCancoder = new CANcoder(_pivotConf.encoderDeviceID, _pivotConf.encoderCanBus); - _pivotCancoderConfiguration = _pivotConf.encoderConfig; - configCancoder(); - configAllDevices(); - configMotor(); - _fusedCancoderSetup = true; + _pivotCancoder = new CANcoder(_pivotConf.encoderDeviceID, _pivotConf.encoderCanBus); + _pivotCancoderConfiguration = _pivotConf.encoderConfig; + configCancoder(); + configAllDevices(); + configMotor(); + _pivotCancoder.getAbsolutePosition().waitForUpdate(0.5); + _primaryMotor.getPosition().waitForUpdate(0.5); + // int count = 0; + // while (_primaryMotor.getPosition().getStatus() != StatusCode.OK && count <= 1000) { + // System.out.println(_pivotConf.name + " not ok " + count++); + // } + // if (count > 1000) { + // System.out.println(_pivotConf.name + " failed to initialize!"); + // } + double pivotPosition = getPivotPosition(); + Logger.recordOutput(_conf.name + " initial pivot position", pivotPosition); + double average = (_pivotConf.rotationalMin + _pivotConf.rotationalMax) / 2.0; + double min = average - 0.5; + double max = average + 0.5; + while (min + _scopeOffset > pivotPosition) { + _scopeOffset -= 1.0; } + while (max + _scopeOffset < pivotPosition) { + _scopeOffset += 1.0; + } + _minPositionRot = min + _scopeOffset; + _maxPositionRot = max + _scopeOffset; + + _absoluteRangeRot = _maxPositionRot - _minPositionRot; } @Override @@ -76,6 +94,7 @@ public void followLastProfile() { } _curState = nextState; + Logger.recordOutput(_conf.name + "pivot next state", nextState.position); } @Override @@ -102,7 +121,6 @@ public void setPivotVelocity(double rps, boolean withFF) { @Override public double getPivotPosition() { - setupFusedCancoder(); return getPrimaryMotorPosition(); } @@ -122,6 +140,10 @@ protected void configCancoder() { _pivotCancoder.getConfigurator().apply(_pivotCancoderConfiguration); } + public CANcoder getPivotCancoder() { + return _pivotCancoder; + } + @Override protected void configMotor() { for (int i = 0; i < _conf.numMotors; i++) { diff --git a/src/main/java/tagalong/subsystems/micro/Roller.java b/src/main/java/tagalong/subsystems/micro/Roller.java index 62ef4d9..0e39742 100644 --- a/src/main/java/tagalong/subsystems/micro/Roller.java +++ b/src/main/java/tagalong/subsystems/micro/Roller.java @@ -460,7 +460,7 @@ public void setRollerProfile( _goalState.velocity = goalVelocityRPS; _trapProfile = new TrapezoidProfile( - (maxVelocityRPS >= _maxVelocityRPS || maxAccelerationRPS2 >= _maxAccelerationRPS2) + (maxVelocityRPS > _maxVelocityRPS || maxAccelerationRPS2 > _maxAccelerationRPS2) ? _rollerConf.trapezoidalLimits : new TrapezoidProfile.Constraints(maxVelocityRPS, maxAccelerationRPS2) );