From 8ff4bbdaa7aad249e148964b044f6447e946641a Mon Sep 17 00:00:00 2001 From: Talon540-root <122660543+Talon540-root@users.noreply.github.com> Date: Sat, 25 Jan 2025 16:11:58 -0500 Subject: [PATCH] skibidi --- .../frc/robot/commands/DriveCommands.java | 10 +++--- .../frc/robot/subsystems/drive/Drive.java | 9 +++++ .../subsystems/drive/DriveConstants.java | 33 ++++++++++++------- .../frc/robot/subsystems/drive/Module.java | 4 +++ .../frc/robot/subsystems/drive/ModuleIO.java | 1 + .../robot/subsystems/drive/ModuleIOSpark.java | 31 ++++++++++------- 6 files changed, 60 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 0a9079d..a4f822e 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.drive.Drive; @@ -44,7 +45,7 @@ public class DriveCommands { private static final double ANGLE_MAX_VELOCITY = 8.0; private static final double ANGLE_MAX_ACCELERATION = 20.0; private static final double FF_START_DELAY = 2.0; // Secs - private static final double FF_RAMP_RATE = 0.1; // Volts/Sec + private static final double FF_RAMP_RATE = 0.75; // Volts/Sec private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 @@ -90,6 +91,7 @@ public static Command joystickDrive( linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), omega * drive.getMaxAngularSpeedRadPerSec()); + boolean isFlipped = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; @@ -214,9 +216,9 @@ public static Command feedforwardCharacterization(Drive drive) { double kV = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX); NumberFormat formatter = new DecimalFormat("#0.00000"); - System.out.println("********** Drive FF Characterization Results **********"); - System.out.println("\tkS: " + formatter.format(kS)); - System.out.println("\tkV: " + formatter.format(kV)); + + SmartDashboard.putString("kS Output", formatter.format(kS)); + SmartDashboard.putString("kV Output", formatter.format(kV)); })); } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index a331a91..7eaf97a 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -225,6 +225,15 @@ private SwerveModuleState[] getModuleStates() { return states; } + @AutoLogOutput(key = "SwerveStates/MeasuredAbsolute") + private SwerveModuleState[] getAbsoluteModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getAbsoluteState(); + } + return states; + } + /** Returns the module positions (turn angles and drive positions) for all of the modules. */ private SwerveModulePosition[] getModulePositions() { SwerveModulePosition[] states = new SwerveModulePosition[4]; diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 3707279..019af0b 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -22,8 +22,8 @@ public class DriveConstants { public static final double maxSpeedMetersPerSec = 4.8; public static final double odometryFrequency = 100.0; // Hz - public static final double trackWidth = Units.inchesToMeters(26.5); - public static final double wheelBase = Units.inchesToMeters(26.5); + public static final double trackWidth = Units.inchesToMeters(22.75); + public static final double wheelBase = Units.inchesToMeters(22.75); public static final double driveBaseRadius = Math.hypot(trackWidth / 2.0, wheelBase / 2.0); public static final Translation2d[] moduleTranslations = new Translation2d[] { @@ -33,11 +33,17 @@ public class DriveConstants { new Translation2d(-trackWidth / 2.0, -wheelBase / 2.0) }; - // Zeroed rotation values for each module, see setup instructions - public static final Rotation2d frontLeftZeroRotation = new Rotation2d(0.1574540138244629); - public static final Rotation2d frontRightZeroRotation = new Rotation2d(-0.16320089995861053); - public static final Rotation2d backLeftZeroRotation = new Rotation2d(-2.942538261413574); - public static final Rotation2d backRightZeroRotation = new Rotation2d(2.5917508602142334); + // // Zeroed rotation values for each module, see setup instructions + // public static final Rotation2d frontLeftZeroRotation = new Rotation2d(0.07709961857367231); + // public static final Rotation2d frontRightZeroRotation = + // new Rotation2d(3.069966630478959); + // public static final Rotation2d backLeftZeroRotation = new Rotation2d(1.633997476530679); + // public static final Rotation2d backRightZeroRotation = new Rotation2d(1.273714169847585); + + public static final Rotation2d frontLeftZeroRotation = new Rotation2d(0.16028737150729522); + public static final Rotation2d frontRightZeroRotation = new Rotation2d(-0.1422097592800296); + public static final Rotation2d backLeftZeroRotation = new Rotation2d(-3.009554996093968); + public static final Rotation2d backRightZeroRotation = new Rotation2d(2.559973505647124); // Device CAN ID public static final int frontLeftTurnCanId = 2; @@ -72,15 +78,17 @@ public class DriveConstants { (2 * Math.PI) / 60.0 / driveMotorReduction; // Rotor RPM -> Wheel Rad/Sec // Drive PID configuration - public static final double driveKp = 0.0; + public static final double driveKp = 0.005; public static final double driveKd = 0.0; - public static final double driveKs = 0.0; - public static final double driveKv = 0.0; + public static final double driveKs = 0.19700; + public static final double driveKv = 0.12941; public static final double driveSimP = 0.05; public static final double driveSimD = 0.0; public static final double driveSimKs = 0.0; public static final double driveSimKv = 0.0789; + // 0.11891 0.13199 + // Turn motor configuration public static final boolean turnInverted = false; public static final int turnMotorCurrentLimit = 20; @@ -95,8 +103,9 @@ public class DriveConstants { (2 * Math.PI) / 60.0 / turnMotorReduction; // RPM -> Rad/Sec // Turn PID configuration - public static final double turnKp = 4000; - public static final double turnKd = 50; + public static final double turnKp = 2.0; + public static final double turnKd = 0.05; + public static final double turnSimP = 8.0; public static final double turnSimD = 0.0; public static final double turnPIDMinInput = 0; // Radians diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 24b19f4..e50615f 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -109,6 +109,10 @@ public SwerveModuleState getState() { return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); } + public SwerveModuleState getAbsoluteState() { + return new SwerveModuleState(getVelocityMetersPerSec(), inputs.turnAbsolutePosition); + } + /** Returns the module positions received this cycle. */ public SwerveModulePosition[] getOdometryPositions() { return odometryPositions; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 424a1be..f237702 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -26,6 +26,7 @@ public static class ModuleIOInputs { public double driveCurrentAmps = 0.0; public boolean turnConnected = false; + public Rotation2d turnAbsolutePosition = new Rotation2d(); public Rotation2d turnPosition = new Rotation2d(); public double turnVelocityRadPerSec = 0.0; public double turnAppliedVolts = 0.0; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index 3ba9c85..70760eb 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -59,6 +59,8 @@ public class ModuleIOSpark implements ModuleIO { private final Queue drivePositionQueue; private final Queue turnPositionQueue; + private boolean hasResetTurnPosition = false; + // Connection debouncers private final Debouncer driveConnectedDebounce = new Debouncer(0.5); private final Debouncer turnConnectedDebounce = new Debouncer(0.5); @@ -145,12 +147,11 @@ public ModuleIOSpark(int module) { // Configure turn motor var turnConfig = new SparkMaxConfig(); - turnConfig - .inverted(turnInverted) .idleMode(IdleMode.kCoast) - .smartCurrentLimit(turnMotorCurrentLimit) - .voltageCompensation(12.0); + .smartCurrentLimit(20) + .voltageCompensation(12.0) + .inverted(true); turnConfig .encoder .positionConversionFactor(turnEncoderPositionFactor) @@ -161,9 +162,8 @@ public ModuleIOSpark(int module) { .closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) .positionWrappingEnabled(true) - .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) + .positionWrappingInputRange(-Math.PI, Math.PI) // TODO .pidf(turnKp, 0.0, turnKd, 0.0); - turnConfig .signals .primaryEncoderPositionAlwaysOn(true) @@ -181,9 +181,6 @@ public ModuleIOSpark(int module) { turnSpark.configure( turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); - turnEncoder.setPosition( - Rotation2d.fromRadians(turnAbsoluteEncoder.get()).minus(zeroRotation).getRadians()); - // Create odometry queues timestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue(); drivePositionQueue = @@ -218,6 +215,7 @@ public void updateInputs(ModuleIOInputs inputs) { (values) -> inputs.turnAppliedVolts = values[0] * values[1]); ifOk(turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.turnCurrentAmps = value); inputs.turnConnected = turnConnectedDebounce.calculate(!sparkStickyFault); + inputs.turnAbsolutePosition = getOffsetAbsoluteAngle(); // Update odometry inputs inputs.odometryTimestamps = @@ -256,8 +254,17 @@ public void setDriveVelocity(double velocityRadPerSec) { @Override public void setTurnPosition(Rotation2d rotation) { - double setpoint = - MathUtil.inputModulus(rotation.plus(zeroRotation).getRadians(), -Math.PI, Math.PI); - turnController.setReference(setpoint, ControlType.kPosition); + if (!hasResetTurnPosition) { + turnEncoder.setPosition(getOffsetAbsoluteAngle().getRadians()); + hasResetTurnPosition = true; + } + + // Ensure Setpoint is within range + turnController.setReference( + MathUtil.angleModulus(rotation.getRadians()), ControlType.kPosition); + } + + private Rotation2d getOffsetAbsoluteAngle() { + return Rotation2d.fromRadians(turnAbsoluteEncoder.get()).minus(zeroRotation); } }