Skip to content

Feature thrifty encoder #7

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

Merged
cvanhoose merged 2 commits into from
Sep 6, 2025
Merged

Feature thrifty encoder #7

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
f150971
added thrifty encoder code (untested)
ahartnet Sep 6, 2025
1621416
Merge branch 'feature-thrifty-encoder' of https://github.com/average-…
Graceij Sep 6, 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
30 changes: 23 additions & 7 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@
import frc.robot.subsystems.drive.GyroIOSim;
import frc.robot.subsystems.drive.ModuleIOSim;
import frc.robot.subsystems.drive.ModuleIOTalonFX;
import frc.robot.subsystems.drive.ModuleIOThrifty;
import frc.robot.subsystems.elevator.Elevator;
import frc.robot.subsystems.elevator.ElevatorIO;
import frc.robot.subsystems.elevator.ElevatorIOReal;
Expand Down Expand Up @@ -122,13 +123,28 @@ public RobotContainer() {
// Real robot, instantiate hardware IO implementations

case REAL:
drive = new Drive(
new GyroIOPigeon2(),
new ModuleIOTalonFX(Constants.TUNER_CONSTANTS.FrontLeft),
new ModuleIOTalonFX(Constants.TUNER_CONSTANTS.FrontRight),
new ModuleIOTalonFX(Constants.TUNER_CONSTANTS.BackLeft),
new ModuleIOTalonFX(Constants.TUNER_CONSTANTS.BackRight),
(robotPose) -> {});

// Practice bot has one Thrifty Absolute Encoder; initiate the drivetrains differently

switch (RobotIdentity.getRobotIdentityString()) {
case "EVE":
drive = new Drive(
new GyroIOPigeon2(),
new ModuleIOTalonFX(Constants.TUNER_CONSTANTS.FrontLeft),
new ModuleIOTalonFX(Constants.TUNER_CONSTANTS.FrontRight),
new ModuleIOThrifty(Constants.TUNER_CONSTANTS.BackLeft),
new ModuleIOTalonFX(Constants.TUNER_CONSTANTS.BackRight),
(robotPose) -> {});
case "PEARRACUDA":
drive = new Drive(
new GyroIOPigeon2(),
new ModuleIOTalonFX(Constants.TUNER_CONSTANTS.FrontLeft),
new ModuleIOTalonFX(Constants.TUNER_CONSTANTS.FrontRight),
new ModuleIOTalonFX(Constants.TUNER_CONSTANTS.BackLeft),
new ModuleIOTalonFX(Constants.TUNER_CONSTANTS.BackRight),
(robotPose) -> {});
}

vision = new Vision(drive::accept, new VisionIOLimelight(camera0Name, drive::getRotation));
// new VisionIOLimelight(camera1Name, drive::getRotation));
elevator = new Elevator(new ElevatorIOReal());
Expand Down
249 changes: 249 additions & 0 deletions src/main/java/frc/robot/subsystems/drive/ModuleIOThrifty.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,249 @@
// Copyright 2021-2025 FRC 6328
// http://github.com/Mechanical-Advantage
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// version 3 as published by the Free Software Foundation or
// available in the root directory of this project.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.

package frc.robot.subsystems.drive;

import static frc.robot.util.PhoenixUtil.*;

import com.ctre.phoenix6.BaseStatusSignal;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.CANcoderConfiguration;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.controls.TorqueCurrentFOC;
import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.controls.VoltageOut;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.ParentDevice;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.ctre.phoenix6.signals.SensorDirectionValue;
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
import edu.wpi.first.math.filter.Debouncer;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
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.Voltage;
import edu.wpi.first.wpilibj.AnalogEncoder;
import frc.robot.Constants;
import java.util.Queue;

/**
* Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and CANcoder.
* Configured using a set of module constants from Phoenix.
*
* <p>Device configuration and other behaviors not exposed by TunerConstants can be customized here.
*/
public class ModuleIOThrifty implements ModuleIO {
private final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> constants;

// Hardware objects
private final TalonFX driveTalon;
private final TalonFX turnTalon;
private final CANcoder cancoder;
private final AnalogEncoder encoder;

// Voltage control requests
private final VoltageOut voltageRequest = new VoltageOut(0);
private final PositionVoltage positionVoltageRequest = new PositionVoltage(0.0);
private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0);

// Torque-current control requests
private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0);
private final PositionTorqueCurrentFOC positionTorqueCurrentRequest = new PositionTorqueCurrentFOC(0.0);
private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = new VelocityTorqueCurrentFOC(0.0);

// Timestamp inputs from Phoenix thread
private final Queue<Double> timestampQueue;

// Inputs from drive motor
private final StatusSignal<Angle> drivePosition;
private final Queue<Double> drivePositionQueue;
private final StatusSignal<AngularVelocity> driveVelocity;
private final StatusSignal<Voltage> driveAppliedVolts;
private final StatusSignal<Current> driveCurrent;

// Inputs from turn motor
private final StatusSignal<Angle> turnAbsolutePosition;
private final StatusSignal<Angle> turnPosition;
private final Queue<Double> turnPositionQueue;
private final StatusSignal<AngularVelocity> turnVelocity;
private final StatusSignal<Voltage> turnAppliedVolts;
private final StatusSignal<Current> turnCurrent;

// Connection debouncers
private final Debouncer driveConnectedDebounce = new Debouncer(0.5);
private final Debouncer turnConnectedDebounce = new Debouncer(0.5);
private final Debouncer turnEncoderConnectedDebounce = new Debouncer(0.5);

public ModuleIOThrifty(
SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> constants) {
this.constants = constants;
driveTalon = new TalonFX(constants.DriveMotorId, Constants.TUNER_CONSTANTS.DrivetrainConstants.CANBusName);
turnTalon = new TalonFX(constants.SteerMotorId, Constants.TUNER_CONSTANTS.DrivetrainConstants.CANBusName);
cancoder = new CANcoder(constants.EncoderId, Constants.TUNER_CONSTANTS.DrivetrainConstants.CANBusName);
encoder = new AnalogEncoder(0); // Placeholder port

// Configure drive motor
var driveConfig = constants.DriveMotorInitialConfigs;
driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
driveConfig.Slot0 = constants.DriveMotorGains;
driveConfig.Feedback.SensorToMechanismRatio = constants.DriveMotorGearRatio;
driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = constants.SlipCurrent;
driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -constants.SlipCurrent;
driveConfig.CurrentLimits.StatorCurrentLimit = constants.SlipCurrent;
driveConfig.CurrentLimits.StatorCurrentLimitEnable = true;
driveConfig.MotorOutput.Inverted = constants.DriveMotorInverted
? InvertedValue.Clockwise_Positive
: InvertedValue.CounterClockwise_Positive;
tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25));
tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25));

// Configure turn motor
var turnConfig = new TalonFXConfiguration();
turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
turnConfig.Slot0 = constants.SteerMotorGains;
turnConfig.Feedback.FeedbackRemoteSensorID = constants.EncoderId;
turnConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor;
turnConfig.Feedback.SensorToMechanismRatio = constants.SteerMotorGearRatio;
turnConfig.MotionMagic.MotionMagicCruiseVelocity = 100.0 / constants.SteerMotorGearRatio;
turnConfig.MotionMagic.MotionMagicAcceleration = turnConfig.MotionMagic.MotionMagicCruiseVelocity / 0.100;
turnConfig.MotionMagic.MotionMagicExpo_kV = 0.12 * constants.SteerMotorGearRatio;
turnConfig.MotionMagic.MotionMagicExpo_kA = 0.1;
turnConfig.ClosedLoopGeneral.ContinuousWrap = true;
turnConfig.MotorOutput.Inverted = constants.SteerMotorInverted
? InvertedValue.Clockwise_Positive
: InvertedValue.CounterClockwise_Positive;
tryUntilOk(5, () -> turnTalon.getConfigurator().apply(turnConfig, 0.25));
tryUntilOk(5, () -> turnTalon.setPosition(encoder.get(), 0.25));

// Configure CANCoder
CANcoderConfiguration cancoderConfig = constants.EncoderInitialConfigs;
cancoderConfig.MagnetSensor.MagnetOffset = constants.EncoderOffset;
cancoderConfig.MagnetSensor.SensorDirection = constants.EncoderInverted
? SensorDirectionValue.Clockwise_Positive
: SensorDirectionValue.CounterClockwise_Positive;
cancoder.getConfigurator().apply(cancoderConfig);

// Create timestamp queue
timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();

// Create drive status signals
drivePosition = driveTalon.getPosition();
drivePositionQueue = PhoenixOdometryThread.getInstance().registerSignal(driveTalon.getPosition());
driveVelocity = driveTalon.getVelocity();
driveAppliedVolts = driveTalon.getMotorVoltage();
driveCurrent = driveTalon.getStatorCurrent();

// Create turn status signals
turnAbsolutePosition = cancoder.getAbsolutePosition();
turnPosition = turnTalon.getPosition();
turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(turnTalon.getPosition());
turnVelocity = turnTalon.getVelocity();
turnAppliedVolts = turnTalon.getMotorVoltage();
turnCurrent = turnTalon.getStatorCurrent();

// Configure periodic frames
BaseStatusSignal.setUpdateFrequencyForAll(Drive.ODOMETRY_FREQUENCY, drivePosition, turnPosition);
BaseStatusSignal.setUpdateFrequencyForAll(
50.0,
driveVelocity,
driveAppliedVolts,
driveCurrent,
turnAbsolutePosition,
turnVelocity,
turnAppliedVolts,
turnCurrent);
ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnTalon);
}

@Override
public void updateInputs(ModuleIOInputs inputs) {
// Refresh all signals
var driveStatus = BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent);
var turnStatus = BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent);
var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition);

// Update drive inputs
inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK());
inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble());
inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble());
inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble();
inputs.driveCurrentAmps = driveCurrent.getValueAsDouble();

// Update turn inputs
inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK());
inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK());
inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble());
inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble());
inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble());
inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble();
inputs.turnCurrentAmps = turnCurrent.getValueAsDouble();

// Update odometry inputs
inputs.odometryTimestamps =
timestampQueue.stream().mapToDouble((Double value) -> value).toArray();
inputs.odometryDrivePositionsRad = drivePositionQueue.stream()
.mapToDouble((Double value) -> Units.rotationsToRadians(value))
.toArray();
inputs.odometryTurnPositions = turnPositionQueue.stream()
.map((Double value) -> Rotation2d.fromRotations(value))
.toArray(Rotation2d[]::new);
timestampQueue.clear();
drivePositionQueue.clear();
turnPositionQueue.clear();
}

@Override
public void setDriveOpenLoop(double output) {
driveTalon.setControl(
switch (constants.DriveMotorClosedLoopOutput) {
case Voltage -> voltageRequest.withOutput(output);
case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output);
});
}

@Override
public void setTurnOpenLoop(double output) {
turnTalon.setControl(
switch (constants.SteerMotorClosedLoopOutput) {
case Voltage -> voltageRequest.withOutput(output);
case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output);
});
}

@Override
public void setDriveVelocity(double velocityRadPerSec) {
double velocityRotPerSec = Units.radiansToRotations(velocityRadPerSec);
driveTalon.setControl(
switch (constants.DriveMotorClosedLoopOutput) {
case Voltage -> velocityVoltageRequest.withVelocity(velocityRotPerSec);
case TorqueCurrentFOC -> velocityTorqueCurrentRequest.withVelocity(velocityRotPerSec);
});
}

@Override
public void setTurnPosition(Rotation2d rotation) {
turnTalon.setControl(
switch (constants.SteerMotorClosedLoopOutput) {
case Voltage -> positionVoltageRequest.withPosition(rotation.getRotations());
case TorqueCurrentFOC -> positionTorqueCurrentRequest.withPosition(rotation.getRotations());
});
}
}