Skip to content

scope offset and other fixes #23

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
sop26 wants to merge 2 commits into
base: main
Choose a base branch
from
Open

scope offset and other fixes #23

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
611db16
fixes pre ventura
sop26 Mar 7, 2025
a4eefdc
scope offset fixes
sop26 Mar 31, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion publish.gradle
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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")

Expand Down
7 changes: 6 additions & 1 deletion src/main/java/tagalong/subsystems/micro/Elevator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -161,6 +162,7 @@ public void followLastProfile() {
}

_curState = nextState;
Logger.recordOutput(_conf.name + " next state", nextState.position);
}

/**
Expand Down Expand Up @@ -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)
);
Expand Down Expand Up @@ -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();
}
Expand Down
9 changes: 8 additions & 1 deletion src/main/java/tagalong/subsystems/micro/Microsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
*/
Expand Down Expand Up @@ -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;
}
}
Expand Down
74 changes: 30 additions & 44 deletions src/main/java/tagalong/subsystems/micro/Pivot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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
Expand Down Expand Up @@ -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)
);
Expand Down
46 changes: 34 additions & 12 deletions src/main/java/tagalong/subsystems/micro/PivotFused.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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);
Copy link
Contributor Author

@sop26 sop26 May 29, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

waiting for update fixed scope offset issue

// 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
Expand All @@ -76,6 +94,7 @@ public void followLastProfile() {
}

_curState = nextState;
Logger.recordOutput(_conf.name + "pivot next state", nextState.position);
}

@Override
Expand All @@ -102,7 +121,6 @@ public void setPivotVelocity(double rps, boolean withFF) {

@Override
public double getPivotPosition() {
setupFusedCancoder();
return getPrimaryMotorPosition();
}

Expand All @@ -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++) {
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/tagalong/subsystems/micro/Roller.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
);
Expand Down