Skip to content

Commit

Permalink
refactor to use CANCoder onboard encoder offsets
Browse files Browse the repository at this point in the history
  • Loading branch information
@GearBoxFox
GearBoxFox committed Apr 28, 2024
1 parent 92798ba commit cb13387
Show file tree
Hide file tree
Showing 5 changed files with 66 additions and 129 deletions.
39 changes: 2 additions & 37 deletions src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -139,7 +139,7 @@ public DriveSubsystem(
modules[2] = new Module(blModuleIO);
modules[3] = new Module(brModuleIO);

// PhoenixOdometryThread.getInstance().start();
PhoenixOdometryThread.getInstance().start();

m_thetaPid = new PIDController(0.0, 0.0, 0.0);
m_thetaPid.enableContinuousInput(0, 360);
Expand Down Expand Up @@ -234,9 +234,6 @@ public void periodic() {
}
odometryLock.unlock();
Logger.processInputs("Drive/Gyro", gyroInputs);
for (var module : modules) {
module.periodic();
}

// Stop moving when disabled
if (DriverStation.isDisabled()) {
Expand All @@ -251,37 +248,6 @@ public void periodic() {
}

// Update odometry
// double[] sampleTimestamps =
// modules[0].getOdometryTimestamps(); // All signals are sampled together
// int sampleCount = sampleTimestamps.length;
// for (int i = 0; i < sampleCount; i++) {
// Read wheel positions and deltas from each module
// SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];
// SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4];
// for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) {
// modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i];
// moduleDeltas[moduleIndex] =
// new SwerveModulePosition(
// modulePositions[moduleIndex].distanceMeters
// - m_lastModulePositions[moduleIndex].distanceMeters,
// modulePositions[moduleIndex].angle);
// m_lastModulePositions[moduleIndex] = modulePositions[moduleIndex];
// }
//
// Update gyro angle
// if (gyroInputs.connected) {
// Use the real gyro angle
// m_rawGyroRotation = gyroInputs.odometryYawPositions[i];
// } else {
// Use the angle delta from the kinematics and module deltas
// Twist2d twist = kinematics.toTwist2d(moduleDeltas);
// m_rawGyroRotation = m_rawGyroRotation.plus(new Rotation2d(twist.dtheta));
// }

// Apply update
// m_wpiPoseEstimator.updateWithTime(sampleTimestamps[i], m_rawGyroRotation, modulePositions);
// }

for (VisionSubsystem camera : m_cameras) {
// make sure we're not moving too fast before trying to update vision poses
if ((MathUtil.applyDeadband(kinematics.toChassisSpeeds(getModuleStates()).vxMetersPerSecond,
Expand All @@ -304,8 +270,8 @@ public void periodic() {
m_wheelOnlyPoseEstimator.update(gyroInputs.yawPosition, getModulePositions());
m_thetaPidProperty.updateIfChanged();

// log field positioning data
Logger.recordOutput("Odometry/Wheel Only Odometry", m_wheelOnlyPoseEstimator.getEstimatedPosition());

Logger.recordOutput("Drive/DistanceToTarget",
Units.metersToInches(AimbotUtils.getDistanceFromSpeaker(getVisionPose())));
Logger.recordOutput("Drive/AngleToTarget",
Expand All @@ -327,7 +293,6 @@ public void periodic() {
public void runVelocity(ChassisSpeeds speeds) {
// Calculate module setpoints
SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(speeds);
// SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, DriveConstants.MAX_LINEAR_SPEED);

// Send setpoints to modules
for (int i = 0; i < 4; i++) {
Expand Down
71 changes: 18 additions & 53 deletions src/main/java/frc/robot/subsystems/drive/module/Module.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,11 +28,6 @@ public class Module {
private final ModuleIOInputsAutoLogged m_inputs = new ModuleIOInputsAutoLogged();
private final int m_index;

private Rotation2d m_angleSetpoint = null; // Setpoint for closed loop control, null for open loop
private Double m_speedSetpoint = null; // Setpoint for closed loop control, null for open loop
private Rotation2d m_turnRelativeOffset = null; // Relative + Offset = Absolute
private SwerveModulePosition[] m_odometryPositions = new SwerveModulePosition[] {};

public Module(ModuleIO io) {
this.m_io = io;
ModuleConstants moduleConstants = m_io.getModuleConstants();
Expand All @@ -50,40 +45,19 @@ public Module(ModuleIO io) {
public void updateInputs() {
m_io.updateInputs(m_inputs);
Logger.processInputs("Swerve/Module" + m_index, m_inputs);

// On first cycle, reset relative turn encoder
// Wait until absolute angle is nonzero in case it wasn't initialized yet
if (m_turnRelativeOffset == null) {
m_turnRelativeOffset = m_inputs.getTurnAbsolutePosition().minus(m_inputs.getTurnPosition());
}

// Run closed loop turn control
if (m_angleSetpoint != null) {
m_io.setTurnPositionRots(m_angleSetpoint.minus(m_turnRelativeOffset).getRotations());

// Run closed loop drive control
// Only allowed if closed loop turn control is running
if (m_speedSetpoint != null) {
// Run drive controller
m_io.setDriveVelocityMPS(m_speedSetpoint);
}
}

// Calculate positions for odometry
// int sampleCount = m_inputs.odometryTimestamps.length; // All signals are sampled together
// m_odometryPositions = new SwerveModulePosition[sampleCount];
// for (int i = 0; i < sampleCount; i++) {
// double positionMeters = m_inputs.odometryDrivePositionsRad[i] * WHEEL_RADIUS;
// Rotation2d angle =
// m_inputs.odometryTurnPositions[i].plus(
// m_turnRelativeOffset != null ? m_turnRelativeOffset : new Rotation2d());
// m_odometryPositions[i] = new SwerveModulePosition(positionMeters, angle);
// }
}

/** Returns the module positions received this cycle. */
public SwerveModulePosition[] getOdometryPositions() {
return m_odometryPositions;
int minOdometryPositions =
Math.min(m_inputs.odometryDrivePositionsMeters.length, m_inputs.odometryTurnPositions.length);
SwerveModulePosition[] positions = new SwerveModulePosition[minOdometryPositions];
for (int i = 0; i < minOdometryPositions; i++) {
positions[i] =
new SwerveModulePosition(
m_inputs.odometryDrivePositionsMeters[i], m_inputs.odometryTurnPositions[i]);
}
return positions;
}

/** Returns the timestamps of the samples received this cycle. */
Expand All @@ -98,30 +72,25 @@ public SwerveModuleState runSetpoint(SwerveModuleState state) {
var optimizedState = SwerveModuleState.optimize(state, getAngle());

// Update setpoints, controllers run in "periodic"
m_angleSetpoint = optimizedState.angle;
m_speedSetpoint = optimizedState.speedMetersPerSecond;
double angleSetpoint = optimizedState.angle.getRotations();
double speedSetpoint = optimizedState.speedMetersPerSecond;

m_io.setTurnPositionRots(angleSetpoint);
m_io.setDriveVelocityMPS(speedSetpoint);

return optimizedState;
}

/** Runs the module with the specified voltage while controlling to zero degrees. */
public void runCharacterization(double volts) {
// Closed loop turn control
m_angleSetpoint = new Rotation2d();

// Open loop drive control
m_io.setDriveVelocityMPS(volts);
m_speedSetpoint = null;
}

/** Disables all outputs to motors. */
public void stop() {
m_io.setTurnPositionRots(0.0);
m_io.setDriveVelocityMPS(0.0);

// Disable closed loop control for turn and drive
m_angleSetpoint = null;
m_speedSetpoint = null;
}

/** Sets whether brake mode is enabled. */
Expand All @@ -132,21 +101,17 @@ public void setBrakeMode(boolean enabled) {

/** Returns the current turn angle of the module. */
public Rotation2d getAngle() {
if (m_turnRelativeOffset == null) {
return new Rotation2d();
} else {
return m_inputs.getTurnPosition().plus(m_turnRelativeOffset);
}
return m_inputs.getTurnPosition();
}

/** Returns the current drive position of the module in meters. */
public double getPositionMeters() {
return m_inputs.getDrivePositionRots() * WHEEL_RADIUS;
return m_inputs.getDrivePositionMeters() * WHEEL_RADIUS;
}

/** Returns the current drive velocity of the module in meters per second. */
public double getVelocityMetersPerSec() {
return m_inputs.getDriveVelocityRotsPerSec() * WHEEL_RADIUS;
return m_inputs.getDriveVelocityMPS() * WHEEL_RADIUS;
}

/** Returns the module position (turn angle and drive position). */
Expand All @@ -161,6 +126,6 @@ public SwerveModuleState getState() {

/** Returns the drive velocity in radians/sec. */
public double getCharacterizationVelocity() {
return m_inputs.getDriveVelocityRotsPerSec();
return m_inputs.getDriveVelocityMPS();
}
}
30 changes: 15 additions & 15 deletions src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,8 @@
public interface ModuleIO {
@AutoLog
class ModuleIOInputs {
protected double drivePositionRots = 0.0;
protected double driveVelocityRotsPerSec = 0.0;
protected double drivePositionMeters = 0.0;
protected double driveVelocityMPS = 0.0;
protected double driveAppliedVolts = 0.0;
protected double[] driveCurrentAmps = new double[] {};

Expand All @@ -30,25 +30,25 @@ class ModuleIOInputs {
protected double turnAppliedVolts = 0.0;
protected double[] turnCurrentAmps = new double[] {};

protected double[] odometryDrivePositionsRad = new double[] {};
protected double[] odometryDrivePositionsMeters = new double[] {};
protected Rotation2d[] odometryTurnPositions = new Rotation2d[] {};

protected double[] odometryTimestamps = new double[] {};

public double getDrivePositionRots() {
return drivePositionRots;
public double getDrivePositionMeters() {
return drivePositionMeters;
}

public void setDrivePositionRots(double drivePositionRots) {
this.drivePositionRots = drivePositionRots;
public void setDrivePositionMeters(double drivePositionMeters) {
this.drivePositionMeters = drivePositionMeters;
}

public double getDriveVelocityRotsPerSec() {
return driveVelocityRotsPerSec;
public double getDriveVelocityMPS() {
return driveVelocityMPS;
}

public void setDriveVelocityRotsPerSec(double driveVelocityRotsPerSec) {
this.driveVelocityRotsPerSec = driveVelocityRotsPerSec;
public void setDriveVelocityMPS(double driveVelocityMPS) {
this.driveVelocityMPS = driveVelocityMPS;
}

public double getDriveAppliedVolts() {
Expand Down Expand Up @@ -107,12 +107,12 @@ public void setTurnCurrentAmps(double[] turnCurrentAmps) {
this.turnCurrentAmps = turnCurrentAmps;
}

public double[] getOdometryDrivePositionsRad() {
return odometryDrivePositionsRad;
public double[] getOdometryDrivePositionsMeters() {
return odometryDrivePositionsMeters;
}

public void setOdometryDrivePositionsRad(double[] odometryDrivePositionsRad) {
this.odometryDrivePositionsRad = odometryDrivePositionsRad;
public void setOdometryDrivePositionsMeters(double[] odometryDrivePositionsRad) {
this.odometryDrivePositionsMeters = odometryDrivePositionsRad;
}

public Rotation2d[] getOdometryTurnPositions() {
Expand Down
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,8 +59,8 @@ public void updateInputs(ModuleIOInputsAutoLogged inputs) {
driveSim.update(LOOP_PERIOD_SECS);
turnSim.update(LOOP_PERIOD_SECS);

inputs.drivePositionRots = driveSim.getAngularPositionRad();
inputs.driveVelocityRotsPerSec = driveSim.getAngularVelocityRadPerSec();
inputs.drivePositionMeters = driveSim.getAngularPositionRad();
inputs.driveVelocityMPS = driveSim.getAngularVelocityRadPerSec();
inputs.driveAppliedVolts = driveAppliedVolts;
inputs.driveCurrentAmps = new double[] {Math.abs(driveSim.getCurrentDrawAmps())};

Expand All @@ -72,7 +72,7 @@ public void updateInputs(ModuleIOInputsAutoLogged inputs) {
inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())};

inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()};
inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRots};
inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionMeters};
inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition};
}

Expand Down
49 changes: 28 additions & 21 deletions src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,7 @@
public class ModuleIOTalonFX implements ModuleIO {
private final TalonFX m_driveTalon;
private final TalonFX m_turnTalon;
private final CANcoder m_cancoder;

// GoSProperties for motor controllers
private final Phoenix6PidPropertyBuilder m_drivePid;
Expand Down Expand Up @@ -82,24 +83,20 @@ public class ModuleIOTalonFX implements ModuleIO {

public ModuleIOTalonFX(ModuleConstants moduleConstants) {
String canbus = "canivore";

/*
* this is technically the proper way of using any class that
* implements the "Closeable" or "AutoClosable", typically things
* like files or network ports, but also robot hardware */
try (CANcoder cancoder = new CANcoder(moduleConstants.ENCODER_ID(), canbus)) {
// run factory default on cancoder
var encoderConfig = new CANcoderConfiguration();
encoderConfig.MagnetSensor.SensorDirection =
moduleConstants.ENCODER_INVERTED() ? SensorDirectionValue.Clockwise_Positive
: SensorDirectionValue.CounterClockwise_Positive;
encoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf;
cancoder.getConfigurator().apply(encoderConfig);
m_moduleConstants = moduleConstants;

m_turnAbsolutePosition = cancoder.getAbsolutePosition();
}
m_cancoder = new CANcoder(moduleConstants.ENCODER_ID(), canbus);

m_moduleConstants = moduleConstants;
// run factory default on cancoder
var encoderConfig = new CANcoderConfiguration();
encoderConfig.MagnetSensor.SensorDirection =
moduleConstants.ENCODER_INVERTED() ? SensorDirectionValue.Clockwise_Positive
: SensorDirectionValue.CounterClockwise_Positive;
encoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf;
encoderConfig.MagnetSensor.MagnetOffset = m_moduleConstants.ENCODER_OFFSET().getRotations();
m_cancoder.getConfigurator().apply(encoderConfig);

m_turnAbsolutePosition = m_cancoder.getAbsolutePosition();

// run configs on drive motor
var driveConfig = new TalonFXConfiguration();
Expand Down Expand Up @@ -171,7 +168,7 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) {
m_driveCurrent = m_driveTalon.getStatorCurrent();

// setup turn values
m_turnTalon.setPosition(0.0);
m_turnTalon.setPosition(m_turnAbsolutePosition.getValueAsDouble());
m_turnVelocity = m_turnTalon.getVelocity();
m_turnAppliedVolts = m_turnTalon.getMotorVoltage();
m_turnCurrent = m_turnTalon.getStatorCurrent();
Expand Down Expand Up @@ -217,21 +214,31 @@ public void updateInputs(ModuleIOInputsAutoLogged inputs) {
m_turnPid.updateIfChanged();

// update the logged values of the drive motor
inputs.drivePositionRots = m_drivePosition.getValueAsDouble();
inputs.driveVelocityRotsPerSec = m_driveVelocity.getValueAsDouble();
inputs.drivePositionMeters = m_drivePosition.getValueAsDouble() * m_moduleConstants.WHEEL_CURCUMFERENCE_METERS();
inputs.driveVelocityMPS = m_driveVelocity.getValueAsDouble() * m_moduleConstants.WHEEL_CURCUMFERENCE_METERS();
inputs.driveAppliedVolts = m_driveAppliedVolts.getValueAsDouble();
inputs.driveCurrentAmps = new double[] {m_driveCurrent.getValueAsDouble()};

// update the logged value of the encoder
inputs.setTurnAbsolutePosition(Rotation2d.fromRotations(m_turnAbsolutePosition.getValueAsDouble())
.minus(m_moduleConstants.ENCODER_OFFSET()));
inputs.setTurnAbsolutePosition(Rotation2d.fromRotations(m_turnAbsolutePosition.getValueAsDouble()));

// update the logged values of the azimuth motor
inputs.setTurnPosition(Rotation2d.fromRotations((m_turnPosition.getValueAsDouble())));
inputs.setTurnVelocityRadPerSec(
Units.rotationsToRadians(m_turnVelocity.getValueAsDouble()));
inputs.setTurnAppliedVolts(m_turnAppliedVolts.getValueAsDouble());
inputs.setTurnCurrentAmps(new double[] {m_turnCurrent.getValueAsDouble()});

// update high speed odometry inputs
inputs.odometryDrivePositionsMeters =
m_drivePositionQueue.stream()
.mapToDouble(
signalValue -> signalValue * m_moduleConstants.WHEEL_CURCUMFERENCE_METERS())
.toArray();
inputs.odometryTurnPositions =
m_turnPositionQueue.stream().map(Rotation2d::fromRotations).toArray(Rotation2d[]::new);
m_drivePositionQueue.clear();
m_turnPositionQueue.clear();
}

@Override
Expand Down

0 comments on commit cb13387

Please sign in to comment.