Skip to content
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

add position latency compensation and immediate talon output #17

Merged
merged 2 commits into from
Jan 30, 2025
Merged

add position latency compensation and immediate talon output #17

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
3b33df4
speed up CTRE signal refresh, add position latency compensation
truher Jan 30, 2025
e395714
make talon output synchronous
truher Jan 30, 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
91 changes: 68 additions & 23 deletions lib/src/main/java/org/team100/lib/motor/Talon6Motor.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,25 +10,38 @@
import org.team100.lib.util.Memo;
import org.team100.lib.util.Util;

import com.ctre.phoenix6.BaseStatusSignal;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.TalonFXConfigurator;
import com.ctre.phoenix6.controls.DutyCycleOut;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.hardware.TalonFX;

import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Current;
import edu.wpi.first.units.measure.Temperature;
import edu.wpi.first.units.measure.Voltage;
import edu.wpi.first.util.datalog.DoubleLogEntry;
import edu.wpi.first.wpilibj.RobotController;

/**
* Superclass for TalonFX motors.
*/
public abstract class Talon6Motor implements BareMotor {
// speeding up the updates is a tradeoff between latency and CAN utilization.
// 254 seems to think that 100 is a good compromise?
// see
// https://github.com/Team254/FRC-2024-Public/blob/040f653744c9b18182be5f6bc51a7e505e346e59/src/main/java/com/team254/lib/ctre/swerve/SwerveDrivetrain.java#L382
private static final int SIGNAL_UPDATE_FREQ_HZ = 100;
private final TalonFX m_motor;
private final Feedforward100 m_ff;

// CACHES
// Two levels of caching here: the cotemporal cache caches the value
// and also the supplier
/** position is latency-compensated. */
protected final DoubleSupplier m_position;
protected final DoubleSupplier m_velocity;
protected final DoubleSupplier m_dutyCycle;
Expand All @@ -40,9 +53,9 @@ public abstract class Talon6Motor implements BareMotor {
protected final DoubleSupplier m_torque;

// caching the control requests saves allocation
private final VelocityVoltage m_velocityVoltage = new VelocityVoltage(0);
private final DutyCycleOut m_dutyCycleOut = new DutyCycleOut(0);
private final PositionVoltage m_PositionVoltage = new PositionVoltage(0);
private final VelocityVoltage m_velocityVoltage;
private final DutyCycleOut m_dutyCycleOut;
private final PositionVoltage m_positionVoltage;

private final double m_supplyLimit;

Expand Down Expand Up @@ -73,6 +86,15 @@ protected Talon6Motor(
double statorLimit,
PIDConstants lowLevelVelocityConstants,
Feedforward100 ff) {
m_velocityVoltage = new VelocityVoltage(0);
m_dutyCycleOut = new DutyCycleOut(0);
m_positionVoltage = new PositionVoltage(0);
// make control synchronous.
// see https://github.com/Team254/FRC-2024-Public/blob/040f653744c9b18182be5f6bc51a7e505e346e59/src/main/java/com/team254/lib/ctre/swerve/SwerveModule.java#L210
m_velocityVoltage.UpdateFreqHz = 0;
m_dutyCycleOut.UpdateFreqHz = 0;
m_positionVoltage.UpdateFreqHz = 0;

LoggerFactory child = parent.child(this);
m_motor = new TalonFX(canId);
m_ff = ff;
Expand All @@ -85,20 +107,43 @@ protected Talon6Motor(
Phoenix100.currentConfig(talonFXConfigurator, supplyLimit, statorLimit);
Phoenix100.pidConfig(talonFXConfigurator, lowLevelVelocityConstants);

Phoenix100.crash(() -> m_motor.getPosition().setUpdateFrequency(50));
Phoenix100.crash(() -> m_motor.getVelocity().setUpdateFrequency(50));
Phoenix100.crash(() -> m_motor.getTorqueCurrent().setUpdateFrequency(50));

// each memo refresh calls the motor refresh method
m_position = Memo.ofDouble(() -> m_motor.getPosition().refresh().getValueAsDouble());
m_velocity = Memo.ofDouble(() -> m_motor.getVelocity().refresh().getValueAsDouble());
m_dutyCycle = Memo.ofDouble(() -> m_motor.getDutyCycle().refresh().getValueAsDouble());
m_error = Memo.ofDouble(() -> m_motor.getClosedLoopError().refresh().getValueAsDouble());
m_supply = Memo.ofDouble(() -> m_motor.getSupplyCurrent().refresh().getValueAsDouble());
m_supplyVoltage = Memo.ofDouble(() -> m_motor.getSupplyVoltage().refresh().getValueAsDouble());
m_stator = Memo.ofDouble(() -> m_motor.getStatorCurrent().refresh().getValueAsDouble());
m_temp = Memo.ofDouble(() -> m_motor.getDeviceTemp().refresh().getValueAsDouble());
m_torque = Memo.ofDouble(() -> m_motor.getTorqueCurrent().refresh().getValueAsDouble());
Phoenix100.crash(() -> m_motor.getPosition().setUpdateFrequency(SIGNAL_UPDATE_FREQ_HZ));
Phoenix100.crash(() -> m_motor.getVelocity().setUpdateFrequency(SIGNAL_UPDATE_FREQ_HZ));
Phoenix100.crash(() -> m_motor.getTorqueCurrent().setUpdateFrequency(SIGNAL_UPDATE_FREQ_HZ));

// Cache the status signal getters.
final StatusSignal<Angle> motorPosition = m_motor.getPosition();
final StatusSignal<AngularVelocity> motorVelocity = m_motor.getVelocity();
final StatusSignal<Double> motorDutyCycle = m_motor.getDutyCycle();
final StatusSignal<Double> motorClosedLoopError = m_motor.getClosedLoopError();
final StatusSignal<Current> motorSupplyCurrent = m_motor.getSupplyCurrent();
final StatusSignal<Voltage> motorSupplyVoltage = m_motor.getSupplyVoltage();
final StatusSignal<Current> motorStatorCurrent = m_motor.getStatorCurrent();
final StatusSignal<Temperature> motorDeviceTemp = m_motor.getDeviceTemp();
final StatusSignal<Current> motorTorqueCurrent = m_motor.getTorqueCurrent();

// The memoizer refreshes all the signals at once.
Memo.registerSignal(motorPosition);
Memo.registerSignal(motorVelocity);
Memo.registerSignal(motorDutyCycle);
Memo.registerSignal(motorClosedLoopError);
Memo.registerSignal(motorSupplyCurrent);
Memo.registerSignal(motorSupplyVoltage);
Memo.registerSignal(motorStatorCurrent);
Memo.registerSignal(motorDeviceTemp);
Memo.registerSignal(motorTorqueCurrent);

// None of these need to refresh.
m_position = Memo
.ofDouble(() -> BaseStatusSignal.getLatencyCompensatedValueAsDouble(motorPosition, motorVelocity));
m_velocity = Memo.ofDouble(() -> motorVelocity.getValueAsDouble());
m_dutyCycle = Memo.ofDouble(() -> motorDutyCycle.getValueAsDouble());
m_error = Memo.ofDouble(() -> motorClosedLoopError.getValueAsDouble());
m_supply = Memo.ofDouble(() -> motorSupplyCurrent.getValueAsDouble());
m_supplyVoltage = Memo.ofDouble(() -> motorSupplyVoltage.getValueAsDouble());
m_stator = Memo.ofDouble(() -> motorStatorCurrent.getValueAsDouble());
m_temp = Memo.ofDouble(() -> motorDeviceTemp.getValueAsDouble());
m_torque = Memo.ofDouble(() -> motorTorqueCurrent.getValueAsDouble());

m_log_desired_duty = child.doubleLogger(Level.TRACE, "desired duty cycle [-1,1]");
m_log_desired_position = child.doubleLogger(Level.DEBUG, "desired position (rev)");
Expand Down Expand Up @@ -196,7 +241,7 @@ public void setPosition(double motorPositionRad, double motorVelocityRad_S, doub
// PositionVoltage has a velocity field for kV feedforward but we use arbitrary
// feedforward for that.
Phoenix100.warn(() -> m_motor.setControl(
m_PositionVoltage
m_positionVoltage
.withPosition(motorRev)
.withFeedForward(kFFVolts)));

Expand Down Expand Up @@ -263,7 +308,7 @@ public double getVelocityRev_S() {
return m_velocity.getAsDouble();
}

/** Cached. */
/** Position is latency-compensated. Cached. */
public double getPositionRev() {
return m_position.getAsDouble();
}
Expand All @@ -284,10 +329,10 @@ protected void log() {
m_log_torque.log(this::getMotorTorque);
m_log_temp.log(m_temp);
// if (RobotController.getBatteryVoltage() - m_supplyVoltage.getAsDouble() > 1)
// Util.warnf("Motor voltage %d low, bad connection? motor: %f, battery: %f\n",
// m_motor.getDeviceID(),
// m_supplyVoltage.getAsDouble(),
// RobotController.getBatteryVoltage());
// Util.warnf("Motor voltage %d low, bad connection? motor: %f, battery: %f\n",
// m_motor.getDeviceID(),
// m_supplyVoltage.getAsDouble(),
// RobotController.getBatteryVoltage());
}

private double getMotorTorque() {
Expand Down
15 changes: 14 additions & 1 deletion lib/src/main/java/org/team100/lib/util/Memo.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,8 @@
import java.util.function.DoubleSupplier;
import java.util.function.Supplier;

import com.ctre.phoenix6.BaseStatusSignal;

/**
* Cache a supplier until reset().
*
Expand All @@ -20,6 +22,7 @@
*/
public class Memo {
private static final List<Runnable> resetters = new ArrayList<>();
private static final List<BaseStatusSignal> signals = new ArrayList<>();

public static <T> CotemporalCache<T> of(Supplier<T> delegate) {
CotemporalCache<T> cache = new CotemporalCache<>(delegate);
Expand All @@ -33,10 +36,20 @@ public static DoubleCache ofDouble(DoubleSupplier delegate) {
return cache;
}

/**
* There's a "resetter" that calls CTRE's refreshAll; add the supplied signal to
* the list in the refresh.
*/
public static void registerSignal(BaseStatusSignal signal) {
signals.add(signal);
}

/**
* This should be run in Robot.robotPeriodic().
*/
public static void resetAll() {
if (!signals.isEmpty())
BaseStatusSignal.refreshAll(signals.toArray(new BaseStatusSignal[0]));
for (Runnable r : resetters) {
r.run();
}
Expand All @@ -56,7 +69,7 @@ public synchronized T get() {
// synchronized adds ~20ns.
if (m_value == null)
m_value = m_delegate.get();

return m_value;
}

Expand Down
Loading