diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f3c478b..98be733 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -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; @@ -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()); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOThrifty.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOThrifty.java new file mode 100644 index 0000000..94681e7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOThrifty.java @@ -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. + * + *

Device configuration and other behaviors not exposed by TunerConstants can be customized here. + */ +public class ModuleIOThrifty implements ModuleIO { + private final SwerveModuleConstants 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 timestampQueue; + + // Inputs from drive motor + private final StatusSignal drivePosition; + private final Queue drivePositionQueue; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + // Inputs from turn motor + private final StatusSignal turnAbsolutePosition; + private final StatusSignal turnPosition; + private final Queue turnPositionQueue; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal 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 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()); + }); + } +}