diff --git a/build.gradle b/build.gradle index 7d3cee78..6deddbf8 100644 --- a/build.gradle +++ b/build.gradle @@ -116,7 +116,7 @@ sonar { // AdvantageKit log replay from the command line. Set the // value to "true" to enable the sim GUI by default (this // is the standard WPILib behavior). -wpi.sim.addGui().defaultEnabled = true +wpi.sim.addGui() wpi.sim.addDriverstation() // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') diff --git a/networktables.json b/networktables.json index 6cc7f9ce..1e88e928 100644 --- a/networktables.json +++ b/networktables.json @@ -126,5 +126,53 @@ "properties": { "persistent": true } + }, + { + "name": "/Preferences/Sim Turn PID.mm.kp", + "type": "double", + "value": 0.1, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Sim Turn PID.mm.ki", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Sim Turn PID.mm.kd", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Sim Drive PID.mm.kp", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Sim Drive PID.mm.ki", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Sim Drive PID.mm.kd", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } } ] diff --git a/networktables.json.bck b/networktables.json.bck index 4dc34172..6cc7f9ce 100644 --- a/networktables.json.bck +++ b/networktables.json.bck @@ -94,5 +94,37 @@ "properties": { "persistent": true } + }, + { + "name": "/Preferences/Drive/Module3/Drive Pid Property.mm.kp", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module3/Turn Pid Property.mm.kp", + "type": "double", + "value": 7.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module3/Turn Pid Property.mm.ki", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module3/Turn Pid Property.mm.kd", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } } ] diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9b00c72c..b527bf9f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -64,7 +64,7 @@ private DriveConstants() { DRIVE_FF_GAINS, DRIVE_FB_GAINS, TURN_FB_GAINS, - Units.rotationsToDegrees(-0.017578), // offset 0.457764 + Units.rotationsToDegrees(0.230713), // offset 0.457764 true, // inversion ModuleConstants.GearRatios.L3 ); @@ -75,7 +75,7 @@ private DriveConstants() { DRIVE_FF_GAINS, DRIVE_FB_GAINS, TURN_FB_GAINS, - Units.rotationsToDegrees(0.201172), + Units.rotationsToDegrees(0.203857), true, ModuleConstants.GearRatios.L3 ); @@ -86,7 +86,7 @@ private DriveConstants() { DRIVE_FF_GAINS, DRIVE_FB_GAINS, TURN_FB_GAINS, - Units.rotationsToDegrees(0.457764), + Units.rotationsToDegrees(0.212158), true, ModuleConstants.GearRatios.L3 ); @@ -97,7 +97,7 @@ private DriveConstants() { DRIVE_FF_GAINS, DRIVE_FB_GAINS, TURN_FB_GAINS, - Units.rotationsToDegrees(-0.263916), + Units.rotationsToDegrees(-0.263184), true, ModuleConstants.GearRatios.L3 ); 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 7ecf28b4..2a90d2b1 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/module/Module.java @@ -13,8 +13,6 @@ package frc.robot.subsystems.drive.module; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -28,12 +26,8 @@ public class Module { private final ModuleIO m_io; private final ModuleIOInputsAutoLogged m_inputs = new ModuleIOInputsAutoLogged(); - private final ModuleConstants m_moduleConstants; private final int m_index; - private final SimpleMotorFeedforward m_driveFeedforward; - private final PIDController m_driveFeedback; - private final PIDController m_turnFeedback; 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 @@ -42,32 +36,11 @@ public class Module { public Module(ModuleIO io) { this.m_io = io; - this.m_moduleConstants = m_io.getModuleConstants(); - this.m_index = m_moduleConstants.MODULE_INDEX(); + ModuleConstants moduleConstants = m_io.getModuleConstants(); + this.m_index = moduleConstants.MODULE_INDEX(); // delay to initialize all hardware Timer.delay(0.5); - // Switch constants based on mode (the physics simulator is treated as a - // separate robot with different tuning) - switch (Constants.currentMode) { - case REAL, REPLAY -> { - m_driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13); - m_driveFeedback = new PIDController(0.05, 0.0, 0.0); - m_turnFeedback = new PIDController(0.1, 0.0, 0.0); - } - case SIM -> { - m_driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13); - m_driveFeedback = new PIDController(0.1, 0.0, 0.0); - m_turnFeedback = new PIDController(10.0, 0.0, 0.0); - } - default -> { - m_driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); - m_driveFeedback = new PIDController(0.0, 0.0, 0.0); - m_turnFeedback = new PIDController(0.0, 0.0, 0.0); - } - } - - m_turnFeedback.enableContinuousInput(-Math.PI, Math.PI); setBrakeMode(true); } @@ -90,23 +63,13 @@ public void periodic() { // Run closed loop turn control if (m_angleSetpoint != null) { - m_io.setTurnPositionDegs(m_angleSetpoint.minus(m_turnRelativeOffset).getRotations()); + 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) { - // Scale velocity based on turn error - // - // When the error is 90°, the velocity setpoint should be 0. As the wheel turns - // towards the setpoint, its velocity should increase. This is achieved by - // taking the component of the velocity in the direction of the setpoint. - double adjustSpeedSetpoint = m_speedSetpoint * Math.cos(m_turnFeedback.getPositionError()); - // Run drive controller - m_io.setDriveVelocityMPS(adjustSpeedSetpoint); -// m_io.setDriveVelocityMPS( -// m_driveFeedforward.calculate(velocityRadPerSec) -// + m_driveFeedback.calculate(m_inputs.getDriveVelocityRadPerSec(), velocityRadPerSec)); + m_io.setDriveVelocityMPS(m_speedSetpoint); } } @@ -149,7 +112,7 @@ public void runCharacterization(double volts) { /** Disables all outputs to motors. */ public void stop() { - m_io.setTurnPositionDegs(0.0); + m_io.setTurnPositionRots(0.0); m_io.setDriveVelocityMPS(0.0); // Disable closed loop control for turn and drive 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 e364670f..451a79f6 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java @@ -129,7 +129,7 @@ default void updateInputs(ModuleIOInputs inputs) {} default void setDriveVelocityMPS(double mps) {} /** Run the turn motor at the specified voltage. */ - default void setTurnPositionDegs(double degrees) {} + default void setTurnPositionRots(double degrees) {} /** Enable or disable brake mode on the drive motor. */ default void setDriveBrakeMode(boolean enable) {} 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 b3a15eb7..1a7ab440 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java @@ -104,7 +104,7 @@ public void setDriveVelocityMPS(double mps) { } @Override - public void setTurnPositionDegs(double degrees) { + public void setTurnPositionRots(double degrees) { double rots = degrees / 360; double volts = m_turnPid.calculate(m_turnSim.getAngularPositionRotations(), rots); turnAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); 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 37f92d00..291f87e7 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java @@ -28,10 +28,7 @@ import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import lib.properties.phoenix6.Phoenix6PidPropertyBuilder; -import lib.properties.phoenix6.PidPropertyPublic; -import org.littletonrobotics.junction.Logger; import java.util.Queue; @@ -79,6 +76,7 @@ public class ModuleIOTalonFX implements ModuleIO { public ModuleIOTalonFX(ModuleConstants moduleConstants) { m_driveTalon = new TalonFX(moduleConstants.DRIVE_MOTOR_ID()); m_turnTalon = new TalonFX(moduleConstants.TURN_MOTOR_ID()); + /* * this is technically the proper way of using any class that * implements the "Closeable" or "AutoClosable", typically things @@ -87,8 +85,8 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) { // run factory default on cancoder var encoderConfig = new CANcoderConfiguration(); encoderConfig.MagnetSensor.SensorDirection = - moduleConstants.ENCODER_INVERTED() ? SensorDirectionValue.Clockwise_Positive - : SensorDirectionValue.CounterClockwise_Positive; + moduleConstants.ENCODER_INVERTED() ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive; encoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; cancoder.getConfigurator().apply(encoderConfig); @@ -142,6 +140,7 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) { m_posRequest = new PositionVoltage(0, 0, false, 0, 0, false, false, false); + // Fancy multithreaded odometry update stuff // setup drive values m_driveTalon.setPosition(0.0); @@ -205,9 +204,9 @@ public void updateInputs(ModuleIOInputs inputs) { m_turnPid.updateIfChanged(); inputs.drivePositionRad = - Units.rotationsToRadians(m_drivePosition.getValueAsDouble()) / m_moduleConstants.DRIVE_GEAR_RATIO(); + Units.rotationsToRadians(m_drivePosition.getValueAsDouble()); inputs.driveVelocityRadPerSec = - Units.rotationsToRadians(m_driveVelocity.getValueAsDouble()) / m_moduleConstants.DRIVE_GEAR_RATIO(); + Units.rotationsToRadians(m_driveVelocity.getValueAsDouble()); inputs.driveAppliedVolts = m_driveAppliedVolts.getValueAsDouble(); inputs.driveCurrentAmps = new double[] {m_driveCurrent.getValueAsDouble()}; @@ -216,28 +215,19 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.setTurnPosition(Rotation2d.fromRotations(( m_turnPosition.getValueAsDouble()))); inputs.setTurnVelocityRadPerSec( - Units.rotationsToRadians(m_turnVelocity.getValueAsDouble()) / m_moduleConstants.TURNING_GEAR_RATIO()); + Units.rotationsToRadians(m_turnVelocity.getValueAsDouble())); inputs.setTurnAppliedVolts(m_turnAppliedVolts.getValueAsDouble()); inputs.setTurnCurrentAmps(new double[] {m_turnCurrent.getValueAsDouble()}); inputs.setOdometryDrivePositionsRad(m_drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value) / m_moduleConstants.DRIVE_GEAR_RATIO()) + .mapToDouble(Units::rotationsToRadians) .toArray()); inputs.setOdometryTurnPositions(m_turnPositionQueue.stream() - .map((Double value) -> Rotation2d.fromRotations(value / m_moduleConstants.DRIVE_GEAR_RATIO())) + .map(Rotation2d::fromRotations) .toArray(Rotation2d[]::new)); + m_drivePositionQueue.clear(); m_turnPositionQueue.clear(); - - SmartDashboard.putNumber("Module " + m_moduleConstants.MODULE_INDEX() + - "/Closed Loop Output", m_turnClosedLoopOutput.getValueAsDouble()); - SmartDashboard.putNumber("Module " + m_moduleConstants.MODULE_INDEX() + - "/Closed Loop Error", m_turnClosedLoopError.getValueAsDouble()); - SmartDashboard.putNumber("Module " + m_moduleConstants.MODULE_INDEX() + - "/Closed Loop Reference", m_turnClosedLoopReference.getValueAsDouble()); - - SmartDashboard.putNumber("Module " + m_moduleConstants.MODULE_INDEX() + - "/Recorded kP", m_turnPid.getSlotConfigs().kP); } @Override @@ -248,9 +238,8 @@ public void setDriveVelocityMPS(double mps) { } @Override - public void setTurnPositionDegs(double degrees) { -// double rots = degrees / 360; - m_turnTalon.setControl(m_posRequest.withPosition(degrees)); + public void setTurnPositionRots(double rotations) { + m_turnTalon.setControl(m_posRequest.withPosition(rotations)); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java index 2a6bde0a..eb9cb8d2 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java @@ -95,6 +95,7 @@ public void run() { } } catch (InterruptedException e) { DriverStation.reportError(e.toString(), true); + Thread.currentThread().interrupt(); } finally { signalsLock.unlock(); } diff --git a/src/main/java/lib/properties/phoenix6/Phoenix6PidPropertyBuilder.java b/src/main/java/lib/properties/phoenix6/Phoenix6PidPropertyBuilder.java index fafc623a..c0ffafc0 100644 --- a/src/main/java/lib/properties/phoenix6/Phoenix6PidPropertyBuilder.java +++ b/src/main/java/lib/properties/phoenix6/Phoenix6PidPropertyBuilder.java @@ -6,7 +6,6 @@ import com.ctre.phoenix6.configs.SlotConfigs; import com.ctre.phoenix6.hardware.core.CoreTalonFX; import com.ctre.phoenix6.signals.GravityTypeValue; -import com.gos.lib.properties.BaseHeavyProperty; import com.gos.lib.properties.GosDoubleProperty; import com.gos.lib.properties.HeavyDoubleProperty; @@ -15,7 +14,7 @@ import java.util.function.DoubleConsumer; public final class Phoenix6PidPropertyBuilder { - private final CoreTalonFX m_motor; + private final CoreTalonFX m_motor; private final SlotConfigs m_slot; private final boolean m_isConstant; private final String m_baseName; @@ -42,7 +41,9 @@ public Phoenix6PidPropertyBuilder(String baseName, boolean isConstant, CoreTalon } } - private HeavyDoubleProperty createDoubleProperty(String propertyNameSuffix, double defaultValue, DoubleConsumer setter) { + private HeavyDoubleProperty createDoubleProperty(String propertyNameSuffix, + double defaultValue, + DoubleConsumer setter) { String propertyName = m_baseName + ".mm." + propertyNameSuffix; GosDoubleProperty prop = new GosDoubleProperty(m_isConstant, propertyName, defaultValue); diff --git a/src/main/java/lib/properties/phoenix6/PidPropertyPublic.java b/src/main/java/lib/properties/phoenix6/PidPropertyPublic.java index b9529f39..7c720646 100644 --- a/src/main/java/lib/properties/phoenix6/PidPropertyPublic.java +++ b/src/main/java/lib/properties/phoenix6/PidPropertyPublic.java @@ -5,19 +5,19 @@ import java.util.List; public class PidPropertyPublic { - private final List m_properties; + private final List m_properties; - public PidPropertyPublic(List properties) { - m_properties = properties; - } + public PidPropertyPublic(List properties) { + m_properties = properties; + } - public final void updateIfChanged() { - updateIfChanged(false); - } + public final void updateIfChanged() { + updateIfChanged(false); + } - public final void updateIfChanged(boolean forceUpdate) { - for (HeavyDoubleProperty property : m_properties) { - property.updateIfChanged(forceUpdate); - } + public final void updateIfChanged(boolean forceUpdate) { + for (HeavyDoubleProperty property : m_properties) { + property.updateIfChanged(forceUpdate); } + } }