From 038e317efbac371d2377fa0cb4a797edd74d28e9 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Thu, 25 Jan 2024 17:02:30 -0500 Subject: [PATCH 1/6] reimplement swerve modules to use onboard PID controllers --- src/main/java/frc/robot/Constants.java | 11 +- .../robot/subsystems/drive/module/Module.java | 10 +- .../subsystems/drive/module/ModuleIO.java | 15 +- .../subsystems/drive/module/ModuleIOSim.java | 45 ++++- .../drive/module/ModuleIOSparkMax.java | 177 ------------------ .../drive/module/ModuleIOTalonFX.java | 45 +++-- .../phoenix6/Phoenix6PidPropertyBuilder.java | 111 +++++++++++ .../phoenix6/PidPropertyPublic.java | 23 +++ 8 files changed, 226 insertions(+), 211 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java create mode 100644 src/main/java/lib/properties/phoenix6/Phoenix6PidPropertyBuilder.java create mode 100644 src/main/java/lib/properties/phoenix6/PidPropertyPublic.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d1b2241a..107c5455 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -24,9 +24,11 @@ * constants are needed, to reduce verbosity. */ public final class Constants { + private Constants() {} + public static final Mode currentMode = Mode.SIM; - public static enum Mode { + public enum Mode { /** Running on a real robot. */ REAL, @@ -38,13 +40,14 @@ public static enum Mode { } public static class DriveConstants { + private DriveConstants() {} // module constants public static final double WHEEL_RADIUS_METERS = Units.inchesToMeters(2.0); - public static final double[] DRIVE_FF_GAINS = new double[]{0.13, 0.1, 0.0}; - public static final double[] DRIVE_FB_GAINS = new double[]{0.05, 0.0, 0.0}; - public static final double[] TURN_FB_GAINS = new double[]{7.0, 0.0, 0.0}; + protected static final double[] DRIVE_FF_GAINS = new double[]{0.13, 0.1, 0.0}; + protected static final double[] DRIVE_FB_GAINS = new double[]{0.05, 0.0, 0.0}; + protected static final double[] TURN_FB_GAINS = new double[]{7.0, 0.0, 0.0}; public static final ModuleConstants FL_MOD_CONSTANTS = new ModuleConstants( 0, diff --git a/src/main/java/frc/robot/subsystems/drive/module/Module.java b/src/main/java/frc/robot/subsystems/drive/module/Module.java index 1a2b5077..f3fd1e8b 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/module/Module.java @@ -90,7 +90,7 @@ public void periodic() { // Run closed loop turn control if (m_angleSetpoint != null) { - m_io.setTurnVoltage( + m_io.setTurnPositionDegs( m_turnFeedback.calculate(getAngle().getRadians(), m_angleSetpoint.getRadians())); // Run closed loop drive control @@ -105,7 +105,7 @@ public void periodic() { // Run drive controller double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS; - m_io.setDriveVoltage( + m_io.setDriveVelocityMPS( m_driveFeedforward.calculate(velocityRadPerSec) + m_driveFeedback.calculate(m_inputs.driveVelocityRadPerSec, velocityRadPerSec)); } @@ -144,14 +144,14 @@ public void runCharacterization(double volts) { m_angleSetpoint = new Rotation2d(); // Open loop drive control - m_io.setDriveVoltage(volts); + m_io.setDriveVelocityMPS(volts); m_speedSetpoint = null; } /** Disables all outputs to motors. */ public void stop() { - m_io.setTurnVoltage(0.0); - m_io.setDriveVoltage(0.0); + m_io.setTurnPositionDegs(0.0); + m_io.setDriveVelocityMPS(0.0); // Disable closed loop control for turn and drive m_angleSetpoint = null; diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java index 7010f9f6..41d880c2 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java @@ -13,7 +13,6 @@ package frc.robot.subsystems.drive.module; -import com.gos.lib.properties.pid.PidProperty; import edu.wpi.first.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; @@ -36,20 +35,22 @@ public static class ModuleIOInputs { } /** Updates the set of loggable inputs. */ - public default void updateInputs(ModuleIOInputs inputs) {} + default void updateInputs(ModuleIOInputs inputs) {} /** Run the drive motor at the specified voltage. */ - public default void setDriveVoltage(double volts) {} + default void setDriveVelocityMPS(double mps) {} /** Run the turn motor at the specified voltage. */ - public default void setTurnVoltage(double volts) {} + default void setTurnPositionDegs(double degrees) {} /** Enable or disable brake mode on the drive motor. */ - public default void setDriveBrakeMode(boolean enable) {} + default void setDriveBrakeMode(boolean enable) {} /** Enable or disable brake mode on the turn motor. */ - public default void setTurnBrakeMode(boolean enable) {} + default void setTurnBrakeMode(boolean enable) {} // Used to pass moduleConstants - public default ModuleConstants getModuleConstants() {throw new RuntimeException("getModuleConstants() not implemented");} + default ModuleConstants getModuleConstants() { + throw new RuntimeException("getModuleConstants() not implemented"); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java index 5fb2d65d..f3da792b 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java @@ -13,7 +13,10 @@ package frc.robot.subsystems.drive.module; +import com.gos.lib.properties.pid.PidProperty; +import com.gos.lib.properties.pid.WpiPidPropertyBuilder; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.DCMotorSim; @@ -28,11 +31,16 @@ public class ModuleIOSim implements ModuleIO { private static final double LOOP_PERIOD_SECS = 0.02; - private final DCMotorSim m_driveSim = new DCMotorSim(DCMotor.getNEO(1), 6.75, 0.025); - private final DCMotorSim m_turnSim = new DCMotorSim(DCMotor.getNEO(1), 150.0 / 7.0, 0.004); + private final DCMotorSim m_driveSim; + private final DCMotorSim m_turnSim; private final ModuleConstants m_moduleConstants; // use this mainly for module id + private final PIDController m_turnPid = new PIDController(0.0, 0.0, 0.0); + private final PIDController m_drivePid = new PIDController(0.0, 0.0, 0.0); + + private final PidProperty m_turnProperty; + private final PidProperty m_driveProperty; private final Rotation2d turnAbsoluteInitPosition = new Rotation2d(Math.random() * 2.0 * Math.PI); private double driveAppliedVolts = 0.0; @@ -40,13 +48,38 @@ public class ModuleIOSim implements ModuleIO { public ModuleIOSim(ModuleConstants moduleConstants) { m_moduleConstants = moduleConstants; + // create sims for motors + m_driveSim = new DCMotorSim(DCMotor.getFalcon500(1), m_moduleConstants.DRIVE_GEAR_RATIO(), 0.025); + m_turnSim = new DCMotorSim(DCMotor.getFalcon500(1), m_moduleConstants.TURNING_GEAR_RATIO(), 0.004); + + // setup default PID values for "onboard" controllers + m_turnProperty = new WpiPidPropertyBuilder("Sim Turn PID", false, m_turnPid) + .addP(m_moduleConstants.TURN_KP()) + .addI(m_moduleConstants.TURN_KI()) + .addD(m_moduleConstants.TURN_KD()) + .build(); + + m_driveProperty = new WpiPidPropertyBuilder("Sim Drive PID", false, m_drivePid) + .addP(m_moduleConstants.DRIVE_KP()) + .addI(m_moduleConstants.DRIVE_KI()) + .addD(m_moduleConstants.DRIVE_KD()) + .build(); + + // force update the values + m_turnProperty.updateIfChanged(true); + m_driveProperty.updateIfChanged(true); } @Override public void updateInputs(ModuleIOInputs inputs) { + // update the simulation m_driveSim.update(LOOP_PERIOD_SECS); m_turnSim.update(LOOP_PERIOD_SECS); + // update pid controllers + m_turnProperty.updateIfChanged(); + m_driveProperty.updateIfChanged(); + inputs.drivePositionRad = m_driveSim.getAngularPositionRad(); inputs.driveVelocityRadPerSec = m_driveSim.getAngularVelocityRadPerSec(); inputs.driveAppliedVolts = driveAppliedVolts; @@ -64,13 +97,17 @@ public void updateInputs(ModuleIOInputs inputs) { } @Override - public void setDriveVoltage(double volts) { + public void setDriveVelocityMPS(double mps) { + double rps = (mps / m_moduleConstants.WHEEL_CURCUMFERENCE_METERS()) * m_moduleConstants.DRIVE_GEAR_RATIO(); + double volts = m_drivePid.calculate(m_driveSim.getAngularVelocityRPM() / 60, rps); driveAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); m_driveSim.setInputVoltage(driveAppliedVolts); } @Override - public void setTurnVoltage(double volts) { + public void setTurnPositionDegs(double degrees) { + double rots = degrees / 360; + double volts = m_turnPid.calculate(m_turnSim.getAngularPositionRotations(), rots); turnAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); m_turnSim.setInputVoltage(turnAppliedVolts); } diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java deleted file mode 100644 index 07c482c2..00000000 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java +++ /dev/null @@ -1,177 +0,0 @@ -// Copyright 2021-2024 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.module; - -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkLowLevel.PeriodicFrame; -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.AnalogInput; -import edu.wpi.first.wpilibj.RobotController; -import java.util.Queue; - -/** - * Module IO implementation for SparkMax drive motor controller, SparkMax turn motor controller (NEO - * or NEO 550), and analog absolute encoder connected to the RIO - * - *

NOTE: This implementation should be used as a starting point and adapted to different hardware - * configurations (e.g. If using a CANcoder, copy from "ModuleIOTalonFX") - * - *

To calibrate the absolute encoder offsets, point the modules straight (such that forward - * motion on the drive motor will propel the robot forward) and copy the reported values from the - * absolute encoders using AdvantageScope. These values are logged under - * "/Drive/ModuleX/TurnAbsolutePositionRad" - */ -public class ModuleIOSparkMax implements ModuleIO { - // Gear ratios for SDS MK4i L2, adjust as necessary - private static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - private static final double TURN_GEAR_RATIO = 150.0 / 7.0; - - private final CANSparkMax driveSparkMax; - private final CANSparkMax turnSparkMax; - - private final RelativeEncoder driveEncoder; - private final RelativeEncoder turnRelativeEncoder; - private final AnalogInput turnAbsoluteEncoder; - private final Queue drivePositionQueue; - private final Queue turnPositionQueue; - - private final boolean isTurnMotorInverted = true; - private final Rotation2d absoluteEncoderOffset; - - public ModuleIOSparkMax(int index) { - switch (index) { - case 0: - driveSparkMax = new CANSparkMax(1, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(2, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(0); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 1: - driveSparkMax = new CANSparkMax(3, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(4, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(1); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 2: - driveSparkMax = new CANSparkMax(5, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(6, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(2); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 3: - driveSparkMax = new CANSparkMax(7, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(8, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(3); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - default: - throw new RuntimeException("Invalid module index"); - } - - driveSparkMax.restoreFactoryDefaults(); - turnSparkMax.restoreFactoryDefaults(); - - driveSparkMax.setCANTimeout(250); - turnSparkMax.setCANTimeout(250); - - driveEncoder = driveSparkMax.getEncoder(); - turnRelativeEncoder = turnSparkMax.getEncoder(); - - turnSparkMax.setInverted(isTurnMotorInverted); - driveSparkMax.setSmartCurrentLimit(40); - turnSparkMax.setSmartCurrentLimit(30); - driveSparkMax.enableVoltageCompensation(12.0); - turnSparkMax.enableVoltageCompensation(12.0); - - driveEncoder.setPosition(0.0); - driveEncoder.setMeasurementPeriod(10); - driveEncoder.setAverageDepth(2); - - turnRelativeEncoder.setPosition(0.0); - turnRelativeEncoder.setMeasurementPeriod(10); - turnRelativeEncoder.setAverageDepth(2); - - driveSparkMax.setCANTimeout(0); - turnSparkMax.setCANTimeout(0); - - driveSparkMax.setPeriodicFramePeriod( - PeriodicFrame.kStatus2, (int) (1000.0 / Module.ODOMETRY_FREQUENCY)); - turnSparkMax.setPeriodicFramePeriod( - PeriodicFrame.kStatus2, (int) (1000.0 / Module.ODOMETRY_FREQUENCY)); - drivePositionQueue = - SparkMaxOdometryThread.getInstance().registerSignal(driveEncoder::getPosition); - turnPositionQueue = - SparkMaxOdometryThread.getInstance().registerSignal(turnRelativeEncoder::getPosition); - - driveSparkMax.burnFlash(); - turnSparkMax.burnFlash(); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - inputs.drivePositionRad = - Units.rotationsToRadians(driveEncoder.getPosition()) / DRIVE_GEAR_RATIO; - inputs.driveVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / DRIVE_GEAR_RATIO; - inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage(); - inputs.driveCurrentAmps = new double[] {driveSparkMax.getOutputCurrent()}; - - inputs.turnAbsolutePosition = - new Rotation2d( - turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V() * 2.0 * Math.PI) - .minus(absoluteEncoderOffset); - inputs.turnPosition = - Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO); - inputs.turnVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) - / TURN_GEAR_RATIO; - inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); - inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; - - inputs.odometryDrivePositionsRad = - drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) - .toArray(Rotation2d[]::new); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - } - - @Override - public void setDriveVoltage(double volts) { - driveSparkMax.setVoltage(volts); - } - - @Override - public void setTurnVoltage(double volts) { - turnSparkMax.setVoltage(volts); - } - - @Override - public void setDriveBrakeMode(boolean enable) { - driveSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } - - @Override - public void setTurnBrakeMode(boolean enable) { - turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java index ed183e4e..9b97bc11 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java @@ -18,6 +18,8 @@ import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; @@ -25,10 +27,11 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; -import com.gos.lib.phoenix6.properties.pid.Phoenix6TalonPidPropertyBuilder; -import com.gos.lib.properties.pid.PidProperty; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import lib.properties.phoenix6.Phoenix6PidPropertyBuilder; +import lib.properties.phoenix6.PidPropertyPublic; + import java.util.Queue; /** @@ -47,8 +50,8 @@ public class ModuleIOTalonFX implements ModuleIO { private final TalonFX m_driveTalon; private final TalonFX m_turnTalon; - private final PidProperty m_drivePid; - private final PidProperty m_turnPid; + private final PidPropertyPublic m_drivePid; + private final PidPropertyPublic m_turnPid; private final ModuleConstants m_moduleConstants; @@ -79,16 +82,20 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) { driveConfig.MotorOutput.Inverted = moduleConstants.DRIVE_MOTOR_INVERTED() ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; + driveConfig.Feedback.RotorToSensorRatio = m_moduleConstants.DRIVE_GEAR_RATIO(); m_driveTalon.getConfigurator().apply(driveConfig); setDriveBrakeMode(true); // setup pid gains for drive motor - m_drivePid = new Phoenix6TalonPidPropertyBuilder( + m_drivePid = new Phoenix6PidPropertyBuilder( "Drive/Module" + m_moduleConstants.MODULE_INDEX() + "/Drive Pid Property", - false, - m_driveTalon, - 0 - ).addP(m_moduleConstants.DRIVE_KP()).build(); + false, m_driveTalon, 0) + .addP(m_moduleConstants.DRIVE_KP()) + .addI(m_moduleConstants.DRIVE_KI()) + .addD(m_moduleConstants.DRIVE_KD()) + .addKV(m_moduleConstants.DRIVE_KV()) + .addKS(m_moduleConstants.DRIVE_KS()) + .build(); // run configs on turning motor var turnConfig = new TalonFXConfiguration(); @@ -97,10 +104,13 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) { turnConfig.MotorOutput.Inverted = moduleConstants.TURN_MOTOR_INVERTED() ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; + turnConfig.ClosedLoopGeneral.ContinuousWrap = true; + turnConfig.Feedback.RotorToSensorRatio = m_moduleConstants.TURNING_GEAR_RATIO(); m_turnTalon.getConfigurator().apply(turnConfig); setTurnBrakeMode(true); - m_turnPid = new Phoenix6TalonPidPropertyBuilder( + // setup pid gains for turn motor + m_turnPid = new Phoenix6PidPropertyBuilder( "Drive/Module" + m_moduleConstants.MODULE_INDEX() + "/Turn Pid Property", false, m_turnTalon, @@ -167,6 +177,9 @@ public void updateInputs(ModuleIOInputs inputs) { m_turnAppliedVolts, m_turnCurrent); + m_drivePid.updateIfChanged(); + m_turnPid.updateIfChanged(); + inputs.drivePositionRad = Units.rotationsToRadians(m_drivePosition.getValueAsDouble()) / m_moduleConstants.DRIVE_GEAR_RATIO(); inputs.driveVelocityRadPerSec = @@ -197,13 +210,17 @@ public void updateInputs(ModuleIOInputs inputs) { } @Override - public void setDriveVoltage(double volts) { - m_driveTalon.setControl(new VoltageOut(volts)); + public void setDriveVelocityMPS(double mps) { + double rps = (mps / m_moduleConstants.WHEEL_CURCUMFERENCE_METERS()) * m_moduleConstants.DRIVE_GEAR_RATIO(); + VelocityVoltage velRequest = new VelocityVoltage(rps).withSlot(0); + m_driveTalon.setControl(velRequest); } @Override - public void setTurnVoltage(double volts) { - m_turnTalon.setControl(new VoltageOut(volts)); + public void setTurnPositionDegs(double degrees) { + double rots = degrees / 360; + PositionVoltage posRequest = new PositionVoltage(rots).withSlot(0); + m_turnTalon.setControl(posRequest); } @Override diff --git a/src/main/java/lib/properties/phoenix6/Phoenix6PidPropertyBuilder.java b/src/main/java/lib/properties/phoenix6/Phoenix6PidPropertyBuilder.java new file mode 100644 index 00000000..478fc716 --- /dev/null +++ b/src/main/java/lib/properties/phoenix6/Phoenix6PidPropertyBuilder.java @@ -0,0 +1,111 @@ +package lib.properties.phoenix6; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.Slot1Configs; +import com.ctre.phoenix6.configs.Slot2Configs; +import com.ctre.phoenix6.configs.SlotConfigs; +import com.ctre.phoenix6.hardware.core.CoreTalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.gos.lib.properties.GosDoubleProperty; +import com.gos.lib.properties.HeavyDoubleProperty; + +import java.util.ArrayList; +import java.util.List; +import java.util.function.DoubleConsumer; + +public final class Phoenix6PidPropertyBuilder { + private final CoreTalonFX m_motor; + private final SlotConfigs m_slot; + private final boolean m_isConstant; + private final String m_baseName; + private final List m_props; + + @SuppressWarnings("PMD.ImplicitSwitchFallThrough") + public Phoenix6PidPropertyBuilder(String baseName, boolean isConstant, CoreTalonFX motor, int slot) { + m_isConstant = isConstant; + m_baseName = baseName; + m_motor = motor; + + m_props = new ArrayList<>(); + + switch (slot) { + case 1: + m_slot = SlotConfigs.from(new Slot1Configs()); + break; + case 2: + m_slot = SlotConfigs.from(new Slot2Configs()); + break; + case 0: + default: + m_slot = SlotConfigs.from(new Slot0Configs()); + } +} + + private HeavyDoubleProperty createDoubleProperty(String propertyNameSuffix, double defaultValue, DoubleConsumer setter) { + String propertyName = m_baseName + ".mm." + propertyNameSuffix; + GosDoubleProperty prop = new GosDoubleProperty(m_isConstant, propertyName, defaultValue); + + return new HeavyDoubleProperty(setter, prop); + } + + public Phoenix6PidPropertyBuilder addP(double defaultValue) { + m_props.add(createDoubleProperty("kp", defaultValue, (double gain) -> { + m_slot.kP = gain; + m_motor.getConfigurator().apply(m_slot); + })); + return this; + } + + public Phoenix6PidPropertyBuilder addI(double defaultValue) { + m_props.add(createDoubleProperty("ki", defaultValue, (double gain) -> { + m_slot.kI = gain; + m_motor.getConfigurator().apply(m_slot); + })); + return this; + } + + public Phoenix6PidPropertyBuilder addD(double defaultValue) { + m_props.add(createDoubleProperty("kD", defaultValue, (double gain) -> { + m_slot.kD = gain; + m_motor.getConfigurator().apply(m_slot); + })); + return this; + } + + public Phoenix6PidPropertyBuilder addKV(double defaultValue) { + m_props.add(createDoubleProperty("kV", defaultValue, (double gain) -> { + m_slot.kV = gain; + m_motor.getConfigurator().apply(m_slot); + })); + return this; + } + + public Phoenix6PidPropertyBuilder addKS(double defaultValue) { + m_props.add(createDoubleProperty("kS", defaultValue, (double gain) -> { + m_slot.kS = gain; + m_motor.getConfigurator().apply(m_slot); + })); + return this; + } + + public Phoenix6PidPropertyBuilder addKG(double defaultValue, GravityTypeValue gravityType) { + m_props.add(createDoubleProperty("kG", defaultValue, (double gain) -> { + m_slot.kG = gain; + m_slot.GravityType = gravityType; + m_motor.getConfigurator().apply(m_slot); + })); + return this; + } + + public Phoenix6PidPropertyBuilder addKA(double defaultValue) { + m_props.add(createDoubleProperty("kA", defaultValue, (double gain) -> { + m_slot.kA = gain; + m_motor.getConfigurator().apply(m_slot); + })); + return this; + } + + public PidPropertyPublic build() { + return new PidPropertyPublic(m_props); + } +} diff --git a/src/main/java/lib/properties/phoenix6/PidPropertyPublic.java b/src/main/java/lib/properties/phoenix6/PidPropertyPublic.java new file mode 100644 index 00000000..b9529f39 --- /dev/null +++ b/src/main/java/lib/properties/phoenix6/PidPropertyPublic.java @@ -0,0 +1,23 @@ +package lib.properties.phoenix6; + +import com.gos.lib.properties.HeavyDoubleProperty; + +import java.util.List; + +public class PidPropertyPublic { + private final List m_properties; + + public PidPropertyPublic(List properties) { + m_properties = properties; + } + + public final void updateIfChanged() { + updateIfChanged(false); + } + + public final void updateIfChanged(boolean forceUpdate) { + for (HeavyDoubleProperty property : m_properties) { + property.updateIfChanged(forceUpdate); + } + } +} From 112aae2505abb2451f0a6388f31d51129dc9a5e5 Mon Sep 17 00:00:00 2001 From: georgel735 Date: Thu, 25 Jan 2024 20:42:45 -0500 Subject: [PATCH 2/6] feat: Climbers PID --- .../robot/subsystems/climber/ClimberIO.java | 4 +-- .../climber/ClimberIOPrototype.java | 35 +++++++++++++++---- .../subsystems/climber/ClimberSubsystem.java | 4 +-- 3 files changed, 32 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index 53ce7717..c338ef15 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -2,6 +2,6 @@ public interface ClimberIO { - default void setLeftVoltage(double voltage) {} - default void setRightVoltage(double voltage) {} + default void setRightPosition(double degrees) {} + default void setLeftPosition(double degrees) {} } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOPrototype.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOPrototype.java index 416d599e..c4f89d10 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOPrototype.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOPrototype.java @@ -1,14 +1,19 @@ package frc.robot.subsystems.climber; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import edu.wpi.first.math.MathUtil; +import lib.properties.phoenix6.Phoenix6PidPropertyBuilder; +import lib.properties.phoenix6.PidPropertyPublic; public class ClimberIOPrototype implements ClimberIO { private final TalonFX m_leftTalon; private final TalonFX m_rightTalon; + private final PidPropertyPublic m_leftClimberPid; + private final PidPropertyPublic m_rightClimberPid; public ClimberIOPrototype() { m_leftTalon = new TalonFX(18); @@ -20,17 +25,33 @@ public ClimberIOPrototype() { var leftTalonConfig = new TalonFXConfiguration(); m_leftTalon.getConfigurator().apply(leftTalonConfig); + + m_leftClimberPid = new Phoenix6PidPropertyBuilder( + "Climber/Left PID", + false, m_leftTalon, 0) + .addP(0.0) + .addI(0.0) + .addD(0.0) + .build(); + + m_rightClimberPid = new Phoenix6PidPropertyBuilder( + "Climber/Right PID", + false, m_rightTalon, 0) + .addP(0.0) + .addI(0.0) + .addD(0.0) + .build(); + } @Override - public void setRightVoltage(double voltage) { - double clampedVoltage = MathUtil.clamp(voltage, -12, 12); - m_rightTalon.setVoltage(clampedVoltage); + public void setLeftPosition(double degrees) { + final PositionVoltage m_request = new PositionVoltage(0).withSlot(0); + m_leftTalon.setControl(m_request.withPosition(degrees / 360.0)); } - @Override - public void setLeftVoltage(double voltage) { - double clampedVoltage = MathUtil.clamp(voltage, -12, 12); - m_leftTalon.setVoltage(clampedVoltage); + public void setRightPosition(double degrees) { + final PositionVoltage m_request = new PositionVoltage(0).withSlot(0); + m_rightTalon.setControl(m_request.withPosition(degrees / 360.0)); } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index 03e291a9..366e45dd 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -11,8 +11,8 @@ public ClimberSubsystem(ClimberIO io) { } public void setClimberPower(double power) { - m_io.setLeftVoltage(power * 12.0); - m_io.setRightVoltage(power * 12.0); + m_io.setRightPosition(power * 12.0); + m_io.setLeftPosition(power * 12.0); } public Command setClimberPowerFactory(double power) { From ac8b1ac1dcdb50c59a61c401e5e13bcedfddd721 Mon Sep 17 00:00:00 2001 From: superdf407 <114831754+Steggoo@users.noreply.github.com> Date: Wed, 31 Jan 2024 20:36:37 -0500 Subject: [PATCH 3/6] feat: Climber IO Inputs Logged Values --- .../robot/subsystems/climber/ClimberIO.java | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index c338ef15..63c9550c 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -1,6 +1,24 @@ package frc.robot.subsystems.climber; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + public interface ClimberIO { + @AutoLog + class ClimberIOInputs { + public double leftClimberPosition = 0.0; + public double rightClimberPosition = 0.0; + public double leftClimberVelocity = 0.0; + public double rightClimberVelocity = 0.0; + public double leftClimberCurrentDraw = 0.0; + public double rightClimberCurrentDraw = 0.0; + public double leftClimberCurrentSetpoint = 0.0; + public double rightClimberCurrentSetpoint = 0.0; + public double leftClimberAppliedOutput = 0.0; + public double rightClimberAppliedOutput = 0.0; + + } default void setRightPosition(double degrees) {} default void setLeftPosition(double degrees) {} From d899bb374d39409b40bd62ed3000dc36b4a672e2 Mon Sep 17 00:00:00 2001 From: superdf407 <114831754+Steggoo@users.noreply.github.com> Date: Wed, 31 Jan 2024 21:13:26 -0500 Subject: [PATCH 4/6] feat: update inputs --- .../robot/subsystems/climber/ClimberIO.java | 1 + .../subsystems/climber/ClimberIOPrototype.java | 14 ++++++++++++++ .../subsystems/climber/ClimberSubsystem.java | 2 ++ .../subsystems/shooter/ShooterIOPrototype.java | 15 --------------- .../robot/subsystems/shooter/ShooterIOSim.java | 18 ------------------ 5 files changed, 17 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index 63c9550c..aac923ac 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -22,4 +22,5 @@ class ClimberIOInputs { default void setRightPosition(double degrees) {} default void setLeftPosition(double degrees) {} + default void updateInputs(ClimberIOInputs inputs) {} } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOPrototype.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOPrototype.java index c4f89d10..713586e2 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOPrototype.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOPrototype.java @@ -54,4 +54,18 @@ public void setRightPosition(double degrees) { final PositionVoltage m_request = new PositionVoltage(0).withSlot(0); m_rightTalon.setControl(m_request.withPosition(degrees / 360.0)); } + + @Override + public void updateInputs(ClimberIOInputs inputs) { + inputs.leftClimberPosition = m_leftTalon.getPosition().getValueAsDouble(); + inputs.rightClimberPosition = m_rightTalon.getPosition().getValueAsDouble(); + inputs.leftClimberVelocity = m_leftTalon.getVelocity().getValueAsDouble(); + inputs.rightClimberVelocity = m_rightTalon.getVelocity().getValueAsDouble(); + inputs.leftClimberCurrentDraw = m_leftTalon.getSupplyCurrent().getValueAsDouble(); + inputs.rightClimberCurrentDraw = m_rightTalon.getSupplyCurrent().getValueAsDouble(); + inputs.leftClimberCurrentSetpoint = m_leftTalon.getClosedLoopReference().getValueAsDouble(); + inputs.rightClimberCurrentSetpoint = m_rightTalon.getClosedLoopReference().getValueAsDouble(); + inputs.leftClimberAppliedOutput = m_leftTalon.getBridgeOutput().getValueAsDouble(); + inputs.rightClimberAppliedOutput = m_rightTalon.getBridgeOutput().getValueAsDouble(); + } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index 366e45dd..674b667b 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -6,8 +6,10 @@ public class ClimberSubsystem extends SubsystemBase { private final ClimberIO m_io; + private final ClimberIOInputsAutoLogged m_inputs; public ClimberSubsystem(ClimberIO io) { m_io = io; + m_inputs = new ClimberIOInputsAutoLogged(); } public void setClimberPower(double power) { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java index 1e2a3837..28b59b32 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java @@ -33,21 +33,6 @@ public ShooterIOPrototype() { m_intakeRight.burnFlash(); } - @Override - public void updateInputs(ShooterIOInputs inputs) { - inputs.tLAngularVelocity = m_topLeftMotor.getEncoder().getVelocity() * Math.PI; - inputs.tRAngularVelocity = m_topRightMotor.getEncoder().getVelocity() * Math.PI; - inputs.bLAngularVelocity = m_bottomLeftMotor.getEncoder().getVelocity() * Math.PI; - inputs.bRAngularVelocity = m_bottomRightMotor.getEncoder().getVelocity() * Math.PI; - inputs.kickerAngularVelocity = m_kickerMotor.getEncoder().getVelocity() * Math.PI; - - inputs.tLAppliedInputs = m_topLeftMotor.getAppliedOutput(); - inputs.tRAppliedInputs = m_topRightMotor.getAppliedOutput(); - inputs.bLAppliedInputs = m_bottomLeftMotor.getAppliedOutput(); - inputs.bRAppliedInputs = m_bottomRightMotor.getAppliedOutput(); - inputs.kickerAppliedInputs = m_kickerMotor.getAppliedOutput(); - } - @Override public void setMotorVoltageTL(double voltage) { m_topLeftMotor.setVoltage(voltage); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java index 492b7d64..c9dfffa7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java @@ -23,25 +23,7 @@ public class ShooterIOSim implements ShooterIO{ private double bRAppliedVolts = 0.0; private double kickerAppliedVolts = 0.0; - @Override - public void updateInputs(ShooterIOInputs inputs) { - m_simTL.update(LOOP_PERIOD_SECS); - m_simTR.update(LOOP_PERIOD_SECS); - m_simBL.update(LOOP_PERIOD_SECS); - m_simBR.update(LOOP_PERIOD_SECS); - - inputs.tLAngularVelocity = m_simTL.getAngularVelocityRadPerSec(); - inputs.tRAngularVelocity = m_simTR.getAngularVelocityRadPerSec(); - inputs.bLAngularVelocity = m_simBL.getAngularVelocityRadPerSec(); - inputs.bRAngularVelocity = m_simBR.getAngularVelocityRadPerSec(); - inputs.kickerAngularVelocity = m_simKicker.getAngularVelocityRadPerSec(); - inputs.tLAppliedInputs = tLAppliedVolts; - inputs.tRAppliedInputs = tRAppliedVolts; - inputs.bLAppliedInputs = bLAppliedVolts; - inputs.bRAppliedInputs = bRAppliedVolts; - inputs.kickerAppliedInputs = kickerAppliedVolts; - } @Override public void setMotorVoltageTL(double voltage) { From d7df891cd1a0f6eda27835c497f82322bdbdb459 Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Sat, 3 Feb 2024 10:14:30 -0500 Subject: [PATCH 5/6] fixed typo in kicker naming --- .../frc/robot/subsystems/shooter/ShooterIOPrototype.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java index 28b59b32..615f6370 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java @@ -9,7 +9,7 @@ public class ShooterIOPrototype implements ShooterIO { private final CANSparkFlex m_topRightMotor; private final CANSparkMax m_bottomLeftMotor; private final CANSparkMax m_bottomRightMotor; - private final CANSparkMax m_kickekMotor; + private final CANSparkMax m_kickerMotor; private final CANSparkMax m_intakeLeft; private final CANSparkMax m_intakeRight; public ShooterIOPrototype() { @@ -17,7 +17,7 @@ public ShooterIOPrototype() { m_topRightMotor = new CANSparkFlex(14, CANSparkLowLevel.MotorType.kBrushless); m_bottomLeftMotor = new CANSparkMax(15, CANSparkLowLevel.MotorType.kBrushless); m_bottomRightMotor = new CANSparkMax(16, CANSparkLowLevel.MotorType.kBrushless); - m_kickekMotor = new CANSparkMax(17, CANSparkLowLevel.MotorType.kBrushless); + m_kickerMotor = new CANSparkMax(17, CANSparkLowLevel.MotorType.kBrushless); m_intakeLeft = new CANSparkMax(20, CANSparkLowLevel.MotorType.kBrushless); m_intakeRight = new CANSparkMax(21, CANSparkLowLevel.MotorType.kBrushless); @@ -55,7 +55,7 @@ public void setMotorVoltageBR(double voltage) { @Override public void setKickerVoltage(double voltage) { - m_kickekMotor.setVoltage(voltage); + m_kickerMotor.setVoltage(voltage); } @Override From b916add9b244a446afefa428b3b6bcfac8cfb297 Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Sat, 3 Feb 2024 10:45:48 -0500 Subject: [PATCH 6/6] added back in voltage control and sonarqube cleanup --- build.gradle | 2 +- src/main/java/frc/robot/Constants.java | 20 +++ .../robot/subsystems/climber/ClimberIO.java | 119 ++++++++++++--- .../climber/ClimberIOPrototype.java | 137 ++++++++++-------- .../subsystems/climber/ClimberSubsystem.java | 33 +++-- vendordeps/AdvantageKit.json | 12 +- 6 files changed, 223 insertions(+), 100 deletions(-) diff --git a/build.gradle b/build.gradle index 48935bc8..7d3cee78 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.2.1" id "com.peterabeles.gversion" version "1.10" id "org.sonarqube" version "4.4.1.3373" } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3971679d..b9b903f4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -99,4 +99,24 @@ private DriveConstants() { ModuleConstants.GearRatios.L3 ); } + + public class ClimberConstants { + private ClimberConstants() { + throw new IllegalStateException("Static class should not be constructed"); + } + + public static final int LEFT_CLIMBER_ID = 18; + public static final int RIGHT_CLIMBER_ID = 19; + + public static final double CLIMBER_KP = 0.0; + public static final double CLIMBER_KI = 0.0; + public static final double CLIMBER_KD = 0.0; + + public static final double CLIMBER_KV = 0.0; + public static final double CLIMBER_KS = 0.0; + public static final double CLIMBER_KA = 0.0; + + public static final double CLIMBER_GEAR_RATIO = 0.0; + public static final double CLIMBER_CONVERSION_FACTOR_INCHES = 0.0; + } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index aac923ac..380f0b0b 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -1,26 +1,105 @@ package frc.robot.subsystems.climber; -import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; public interface ClimberIO { - @AutoLog - class ClimberIOInputs { - public double leftClimberPosition = 0.0; - public double rightClimberPosition = 0.0; - public double leftClimberVelocity = 0.0; - public double rightClimberVelocity = 0.0; - public double leftClimberCurrentDraw = 0.0; - public double rightClimberCurrentDraw = 0.0; - public double leftClimberCurrentSetpoint = 0.0; - public double rightClimberCurrentSetpoint = 0.0; - public double leftClimberAppliedOutput = 0.0; - public double rightClimberAppliedOutput = 0.0; - - } - - default void setRightPosition(double degrees) {} - default void setLeftPosition(double degrees) {} - default void updateInputs(ClimberIOInputs inputs) {} + @AutoLog + class ClimberIOInputs { + protected double leftClimberPosition = 0.0; + protected double rightClimberPosition = 0.0; + protected double leftClimberVelocity = 0.0; + protected double rightClimberVelocity = 0.0; + protected double leftClimberCurrentDraw = 0.0; + protected double rightClimberCurrentDraw = 0.0; + protected double leftClimberCurrentSetpoint = 0.0; + protected double rightClimberCurrentSetpoint = 0.0; + protected double leftClimberAppliedOutput = 0.0; + protected double rightClimberAppliedOutput = 0.0; + + public double getLeftClimberPosition() { + return leftClimberPosition; + } + + public void setLeftClimberPosition(double leftClimberPosition) { + this.leftClimberPosition = leftClimberPosition; + } + + public double getRightClimberPosition() { + return rightClimberPosition; + } + + public void setRightClimberPosition(double rightClimberPosition) { + this.rightClimberPosition = rightClimberPosition; + } + + public double getLeftClimberVelocity() { + return leftClimberVelocity; + } + + public void setLeftClimberVelocity(double leftClimberVelocity) { + this.leftClimberVelocity = leftClimberVelocity; + } + + public double getRightClimberVelocity() { + return rightClimberVelocity; + } + + public void setRightClimberVelocity(double rightClimberVelocity) { + this.rightClimberVelocity = rightClimberVelocity; + } + + public double getLeftClimberCurrentDraw() { + return leftClimberCurrentDraw; + } + + public void setLeftClimberCurrentDraw(double leftClimberCurrentDraw) { + this.leftClimberCurrentDraw = leftClimberCurrentDraw; + } + + public double getRightClimberCurrentDraw() { + return rightClimberCurrentDraw; + } + + public void setRightClimberCurrentDraw(double rightClimberCurrentDraw) { + this.rightClimberCurrentDraw = rightClimberCurrentDraw; + } + + public double getLeftClimberCurrentSetpoint() { + return leftClimberCurrentSetpoint; + } + + public void setLeftClimberCurrentSetpoint(double leftClimberCurrentSetpoint) { + this.leftClimberCurrentSetpoint = leftClimberCurrentSetpoint; + } + + public double getRightClimberCurrentSetpoint() { + return rightClimberCurrentSetpoint; + } + + public void setRightClimberCurrentSetpoint(double rightClimberCurrentSetpoint) { + this.rightClimberCurrentSetpoint = rightClimberCurrentSetpoint; + } + + public double getLeftClimberAppliedOutput() { + return leftClimberAppliedOutput; + } + + public void setLeftClimberAppliedOutput(double leftClimberAppliedOutput) { + this.leftClimberAppliedOutput = leftClimberAppliedOutput; + } + + public double getRightClimberAppliedOutput() { + return rightClimberAppliedOutput; + } + + public void setRightClimberAppliedOutput(double rightClimberAppliedOutput) { + this.rightClimberAppliedOutput = rightClimberAppliedOutput; + } + } + + default void setRightVoltage(double volts) {} + default void setLeftVoltage(double volts) {} + default void setRightPosition(double degrees) {} + default void setLeftPosition(double degrees) {} + default void updateInputs(ClimberIOInputsAutoLogged inputs) {} } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOPrototype.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOPrototype.java index 713586e2..2330b0e3 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOPrototype.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOPrototype.java @@ -2,70 +2,87 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; -import edu.wpi.first.math.MathUtil; +import frc.robot.Constants.ClimberConstants; import lib.properties.phoenix6.Phoenix6PidPropertyBuilder; import lib.properties.phoenix6.PidPropertyPublic; public class ClimberIOPrototype implements ClimberIO { - private final TalonFX m_leftTalon; - private final TalonFX m_rightTalon; - private final PidPropertyPublic m_leftClimberPid; - private final PidPropertyPublic m_rightClimberPid; - - public ClimberIOPrototype() { - m_leftTalon = new TalonFX(18); - m_rightTalon = new TalonFX(19); - - var rightTalonConfig = new TalonFXConfiguration(); - rightTalonConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - m_rightTalon.getConfigurator().apply(rightTalonConfig); - - var leftTalonConfig = new TalonFXConfiguration(); - m_leftTalon.getConfigurator().apply(leftTalonConfig); - - m_leftClimberPid = new Phoenix6PidPropertyBuilder( - "Climber/Left PID", - false, m_leftTalon, 0) - .addP(0.0) - .addI(0.0) - .addD(0.0) - .build(); - - m_rightClimberPid = new Phoenix6PidPropertyBuilder( - "Climber/Right PID", - false, m_rightTalon, 0) - .addP(0.0) - .addI(0.0) - .addD(0.0) - .build(); - - } - - @Override - public void setLeftPosition(double degrees) { - final PositionVoltage m_request = new PositionVoltage(0).withSlot(0); - m_leftTalon.setControl(m_request.withPosition(degrees / 360.0)); - } - @Override - public void setRightPosition(double degrees) { - final PositionVoltage m_request = new PositionVoltage(0).withSlot(0); - m_rightTalon.setControl(m_request.withPosition(degrees / 360.0)); - } - - @Override - public void updateInputs(ClimberIOInputs inputs) { - inputs.leftClimberPosition = m_leftTalon.getPosition().getValueAsDouble(); - inputs.rightClimberPosition = m_rightTalon.getPosition().getValueAsDouble(); - inputs.leftClimberVelocity = m_leftTalon.getVelocity().getValueAsDouble(); - inputs.rightClimberVelocity = m_rightTalon.getVelocity().getValueAsDouble(); - inputs.leftClimberCurrentDraw = m_leftTalon.getSupplyCurrent().getValueAsDouble(); - inputs.rightClimberCurrentDraw = m_rightTalon.getSupplyCurrent().getValueAsDouble(); - inputs.leftClimberCurrentSetpoint = m_leftTalon.getClosedLoopReference().getValueAsDouble(); - inputs.rightClimberCurrentSetpoint = m_rightTalon.getClosedLoopReference().getValueAsDouble(); - inputs.leftClimberAppliedOutput = m_leftTalon.getBridgeOutput().getValueAsDouble(); - inputs.rightClimberAppliedOutput = m_rightTalon.getBridgeOutput().getValueAsDouble(); - } + private final TalonFX m_leftTalon; + private final TalonFX m_rightTalon; + private final PidPropertyPublic m_leftClimberPid; + private final PidPropertyPublic m_rightClimberPid; + + public ClimberIOPrototype() { + m_leftTalon = new TalonFX(ClimberConstants.LEFT_CLIMBER_ID); + m_rightTalon = new TalonFX(ClimberConstants.RIGHT_CLIMBER_ID); + + var rightTalonConfig = new TalonFXConfiguration(); + rightTalonConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + m_rightTalon.getConfigurator().apply(rightTalonConfig); + + var leftTalonConfig = new TalonFXConfiguration(); + m_leftTalon.getConfigurator().apply(leftTalonConfig); + + m_leftClimberPid = new Phoenix6PidPropertyBuilder( + "Climber/Left PID", + false, m_leftTalon, 0) + .addP(ClimberConstants.CLIMBER_KP) + .addI(ClimberConstants.CLIMBER_KI) + .addD(ClimberConstants.CLIMBER_KD) + .build(); + + m_rightClimberPid = new Phoenix6PidPropertyBuilder( + "Climber/Right PID", + false, m_rightTalon, 0) + .addP(ClimberConstants.CLIMBER_KP) + .addI(ClimberConstants.CLIMBER_KI) + .addD(ClimberConstants.CLIMBER_KD) + .build(); + + } + + @Override + public void setLeftVoltage(double volts) { + m_leftTalon.setVoltage(volts); + } + + @Override + public void setRightVoltage(double volts) { + m_rightTalon.setVoltage(volts); + } + + @Override + public void setLeftPosition(double degrees) { + final PositionVoltage request = new PositionVoltage(0).withSlot(0); + m_leftTalon.setControl(request.withPosition(degrees / 360.0)); + } + + @Override + public void setRightPosition(double degrees) { + final PositionVoltage request = new PositionVoltage(0).withSlot(0); + m_rightTalon.setControl(request.withPosition(degrees / 360.0)); + } + + @Override + public void updateInputs(ClimberIOInputsAutoLogged inputs) { + m_leftClimberPid.updateIfChanged(); + m_rightClimberPid.updateIfChanged(); + + inputs.setLeftClimberPosition(m_leftTalon.getPosition().getValueAsDouble()); + inputs.setRightClimberPosition(m_rightTalon.getPosition().getValueAsDouble()); + + inputs.setLeftClimberVelocity(m_leftTalon.getVelocity().getValueAsDouble()); + inputs.setRightClimberVelocity(m_rightTalon.getVelocity().getValueAsDouble()); + + inputs.setLeftClimberCurrentDraw(m_leftTalon.getSupplyCurrent().getValueAsDouble()); + inputs.setRightClimberCurrentDraw(m_rightTalon.getSupplyCurrent().getValueAsDouble()); + + inputs.setLeftClimberCurrentSetpoint(m_leftTalon.getClosedLoopReference().getValueAsDouble()); + inputs.setRightClimberCurrentSetpoint(m_rightTalon.getClosedLoopReference().getValueAsDouble()); + + inputs.setLeftClimberAppliedOutput(m_leftTalon.getBridgeOutput().getValueAsDouble()); + inputs.setRightClimberAppliedOutput(m_rightTalon.getBridgeOutput().getValueAsDouble()); + } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index 674b667b..2797da39 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -3,22 +3,29 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; public class ClimberSubsystem extends SubsystemBase { - private final ClimberIO m_io; - private final ClimberIOInputsAutoLogged m_inputs; - public ClimberSubsystem(ClimberIO io) { - m_io = io; - m_inputs = new ClimberIOInputsAutoLogged(); - } + private final ClimberIO m_io; + private final ClimberIOInputsAutoLogged m_inputs; + public ClimberSubsystem(ClimberIO io) { + m_io = io; + m_inputs = new ClimberIOInputsAutoLogged(); + } - public void setClimberPower(double power) { - m_io.setRightPosition(power * 12.0); - m_io.setLeftPosition(power * 12.0); - } + @Override + public void periodic() { + m_io.updateInputs(m_inputs); + Logger.processInputs("Climber", m_inputs); + } - public Command setClimberPowerFactory(double power) { - return run(() -> setClimberPower(power)); - } + public void setClimberPower(double power) { + m_io.setRightVoltage(power * 12.0); + m_io.setLeftVoltage(power * 12.0); + } + + public Command setClimberPowerFactory(double power) { + return run(() -> setClimberPower(power)); + } } diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index a3a16b69..d0cc0d12 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "3.0.0", + "version": "3.0.2", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2024", "mavenUrls": [], @@ -10,24 +10,24 @@ { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "wpilib-shim", - "version": "3.0.0" + "version": "3.0.2" }, { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "junction-core", - "version": "3.0.0" + "version": "3.0.2" }, { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-api", - "version": "3.0.0" + "version": "3.0.2" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-wpilibio", - "version": "3.0.0", + "version": "3.0.2", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ @@ -39,4 +39,4 @@ } ], "cppDependencies": [] -} +} \ No newline at end of file