diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index ede23655..4c7f9999 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -28,7 +28,7 @@ "BAD AUTONS" ], "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxAccel": 5.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 65822d85..93b42cc0 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -49,6 +49,7 @@ public class RobotTypes { intakeSubsystem).addAprilTagCamera(CameraConstants.primeLeftCamera) .addAprilTagCamera(CameraConstants.primeRightCamera) .addAprilTagCamera(CameraConstants.primeBackCamera); + public static final RobotType deux26 = new RobotType("Deux26", driveSubsystem); // , kickerSubsystem, // spindexerSubsystem, intakeSubsystem, intakeRollerSubsystem, turretSubsystem, // shooterSubsystem, @@ -189,7 +190,7 @@ public static class WarningThresholdConstants { public class AutopilotConstants { private static final APConstraints kConstraints = new APConstraints() - .withAcceleration(DriveConstants.accelLimit).withVelocity(DriveConstants.maxSpeed).withJerk(2.0); + .withAcceleration(DriveConstants.moduleAccelLimit).withVelocity(DriveConstants.maxSpeed).withJerk(2.0); private static final APProfile kProfile = new APProfile(kConstraints).withErrorXY(Centimeters.of(2)) .withErrorTheta(Degrees.of(0.5)).withBeelineRadius(Centimeters.of(8)); diff --git a/src/main/java/frc/robot/constants/RobotPIDConstants.java b/src/main/java/frc/robot/constants/RobotPIDConstants.java index c510cc98..754f1d21 100644 --- a/src/main/java/frc/robot/constants/RobotPIDConstants.java +++ b/src/main/java/frc/robot/constants/RobotPIDConstants.java @@ -24,7 +24,7 @@ public class RobotPIDConstants { public static final PIDConstants drivePid = new PIDConstants(0.17189, 0.0, 0); public static final FeedForwardConstants driveFF = new FeedForwardConstants(0.12506, 2, 0.27879); public static final PIDConstants steerPid = new PIDConstants(0.725, 0.0, 0.005); - // TODO compare results across modules, run one by one + // TODO deux public static final FeedForwardConstants[] steerFFs = new FeedForwardConstants[]{ new FeedForwardConstants(0.43147, 0.46232, 0.072917), new FeedForwardConstants(0.52027, 0.44734, 0.048812), new FeedForwardConstants(0.33446, 0.44561, 0.071012), new FeedForwardConstants(0.53387, 0.46702, 0.063417)}; diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 70fdb988..8174d5d5 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.drive; +import static frc.robot.subsystems.drive.DriveConstants.useTunedSwerve; + import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.util.Units; @@ -43,15 +45,26 @@ public static enum PivotId { public static final double maxSpeed = 4.25; public static final double maxNorm = DriveSubsystem.computeMaxNorm(DriveConstants.positions, new Translation2d()); public static final double maxOmega = (maxSpeed / maxNorm); - public static final double accelLimit = 20; - public static final double deaccelLimit = 11; + public static final double maxTranslationalDecel = 10; + public static final double maxRotationalDecel = Double.POSITIVE_INFINITY; + + public static final double moduleAccelLimit = 20; + public static final double moduleDecelLimit = 11; + + public static final double steerSetpointContinuityDeltaRadPerSec = Double.POSITIVE_INFINITY; // (Math.PI / 8) / + // 0.02; + public static final boolean useTunedSwerve = true; + + public static final boolean useSteerVelocitySetpoint = useTunedSwerve; + + public static final double defaultDreamLevel = DriveConstants.useTunedSwerve ? 1 : 3; public static final double wheelRadius = Units.inchesToMeters(2); public static final double initalSlope = 3.125; public static final double finalSlope = 4.375; - public static final double maxSteerSpeed = 50; // rad per second + public static final double maxSteerSpeed = Double.POSITIVE_INFINITY; // rad per second public static final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(frontLeftLocation, frontRightLocation, backLeftLocation, backRightLocation); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 6de1f992..cc940abf 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -123,7 +123,7 @@ public void configureBLine() { FollowPath.setTranslationListLoggingConsumer((pair) -> Logger.recordOutput(pair.getFirst(), pair.getSecond())); this.pathBuilder = new FollowPath.Builder((SubsystemBase) this, () -> RobotOdometry.instance.getPose("Main"), - this::getChassisSpeeds, (speeds) -> runVelocity(speeds, false, 3, () -> false), + this::getChassisSpeeds, (speeds) -> runVelocity(speeds, false, () -> false), new PIDController(5.0, 0.0, 2.0), new PIDController(6, 0.0, 1.3), new PIDController(4, 0.0, 1)) .withDefaultShouldFlip().withPoseReset((pose) -> { CommandScheduler.getInstance().schedule(new InstantCommand(() -> { @@ -220,6 +220,10 @@ public SwerveModulePosition[] getModulePositions() { return states; } + public void runVelocity(ChassisSpeeds speeds, boolean fieldCentric, BooleanSupplier limitSpeeds) { + runVelocity(speeds, fieldCentric, DriveConstants.defaultDreamLevel, limitSpeeds); + } + public void runVelocity(ChassisSpeeds speeds, boolean fieldCentric, double dreamLevel, BooleanSupplier limitSpeeds) { @@ -241,7 +245,14 @@ public void runVelocity(ChassisSpeeds speeds, boolean fieldCentric, double dream doubleCone.vyMetersPerSecond * DriveConstants.maxSpeed * scale, doubleCone.omegaRadiansPerSecond * DriveConstants.maxOmega * scale); - previousSetpoint = setpointGenerator.generateSetpoint(previousSetpoint, speedsOptimized, 0.02); + previousSetpoint = setpointGenerator.generateSetpoint(previousSetpoint, // The previous setpoint + speedsOptimized, // The desired target speeds + 0.02 // The loop time of the robot code, in seconds + ); + // replace above w/ below to bypass generator + // previousSetpoint = new SwerveSetpoint(speedsOptimized, + // DriveConstants.kinematics.toSwerveModuleStates(speedsOptimized), + // DriveFeedforwards.zeros(4)); Logger.recordOutput("Drive/SwerveStates/SetpointStates", previousSetpoint.moduleStates()); @@ -250,6 +261,9 @@ public void runVelocity(ChassisSpeeds speeds, boolean fieldCentric, double dream Logger.recordOutput("Drive/SwerveStates/DoubleCone", DriveConstants.kinematics.toSwerveModuleStates(speedsOptimized)); + Logger.recordOutput("Drive/SwerveStates/InputStates", DriveConstants.kinematics.toSwerveModuleStates(speeds)); + // speedsOptimized = ChassisSpeeds.discretize(speedsOptimized, 0.02); + SwerveModuleState[] directStates = DriveConstants.kinematics.toSwerveModuleStates(speedsOptimized); Logger.recordOutput("Drive/SwerveStates/DirectStates", directStates); @@ -342,11 +356,11 @@ public Command sysIdDynamicSteer(SysIdRoutine.Direction direction) { } public Command runVelocityCommand(Supplier speeds, BooleanSupplier limitSpeeds) { - return new RunCommand(() -> runVelocity(speeds.get(), true, 3, limitSpeeds), this).finallyDo(() -> stop()); + return new RunCommand(() -> runVelocity(speeds.get(), true, limitSpeeds), this).finallyDo(() -> stop()); } public Consumer runVelocityConsumer() { - return (speeds) -> runVelocity(speeds, true, 3, () -> false); + return (speeds) -> runVelocity(speeds, true, () -> false); } public static ModuleIO getIOByMode(ModuleInfo modInfo) { diff --git a/src/main/java/frc/robot/subsystems/drive/weights/JoystickDriveWeight.java b/src/main/java/frc/robot/subsystems/drive/weights/JoystickDriveWeight.java index b4eb9cd8..f3097e1c 100644 --- a/src/main/java/frc/robot/subsystems/drive/weights/JoystickDriveWeight.java +++ b/src/main/java/frc/robot/subsystems/drive/weights/JoystickDriveWeight.java @@ -68,8 +68,8 @@ public ChassisSpeeds getSpeeds() { linearVelocity = linearVelocity.div(linearVelocity.getNorm()); } omega = MathUtil.clamp(omega, -1, 1); - double xyMult = 0.90; - double omegaMult = 0.7; + double xyMult = DriveConstants.useTunedSwerve ? 1 : 0.9; + double omegaMult = DriveConstants.useTunedSwerve ? 1 : 0.7; if (slowMode.getAsBoolean()) { xyMult = 0.3; omegaMult = 0.4; diff --git a/src/main/java/frc/robot/subsystems/module/Module.java b/src/main/java/frc/robot/subsystems/module/Module.java index bd38ae24..8e38d7b9 100644 --- a/src/main/java/frc/robot/subsystems/module/Module.java +++ b/src/main/java/frc/robot/subsystems/module/Module.java @@ -20,8 +20,8 @@ public class Module { PivotId id; ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); - SlewRateLimiter accelLimiter = new SlewRateLimiter(DriveConstants.accelLimit); - SlewRateLimiter deaccelLimiter = new SlewRateLimiter(DriveConstants.deaccelLimit); + SlewRateLimiter accelLimiter = new SlewRateLimiter(DriveConstants.moduleAccelLimit); + SlewRateLimiter deaccelLimiter = new SlewRateLimiter(DriveConstants.moduleDecelLimit); public Module(ModuleIO io, PivotId id) { this.io = io; @@ -48,10 +48,13 @@ public Module(ModuleIO io, PivotId id) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Drive/Modules/" + id, inputs); - Logger.recordOutput("Subsystems/Module/" + id + "/setpointVel", io.velocitySetpoint(), Units.MetersPerSecond); + Logger.recordOutput("Subsystems/Module/" + id + "/setpointVel", io.driveVelocitySetpoint(), + Units.MetersPerSecond); Logger.recordOutput("Subsystems/Module/" + id + "/currentVel", getVelocity(), Units.MetersPerSecond); Logger.recordOutput("Subsystems/Module/" + id + "/velocityError", - Math.abs(io.velocitySetpoint() - getVelocity()), Units.MetersPerSecond); + Math.abs(io.driveVelocitySetpoint() - getVelocity()), Units.MetersPerSecond); + Logger.recordOutput("Subsystems/Module/" + id + "/steerSetpointVel", io.steerVelocitySetpoint(), + Units.RadiansPerSecond); } public void setDesiredStateMetersPerSecond(SwerveModuleState state) { diff --git a/src/main/java/frc/robot/subsystems/module/ModuleIO.java b/src/main/java/frc/robot/subsystems/module/ModuleIO.java index 7d713737..172b5c5c 100644 --- a/src/main/java/frc/robot/subsystems/module/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/module/ModuleIO.java @@ -52,7 +52,11 @@ public default void setSteerPosition(Rotation2d angle, ModuleIOInputs inputs) { default void close() { } - public default double velocitySetpoint() { + public default double driveVelocitySetpoint() { + return 0.0; + } + + public default double steerVelocitySetpoint() { return 0.0; } diff --git a/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java index 6b429e7d..2378063b 100644 --- a/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java @@ -2,6 +2,8 @@ import java.util.Queue; +import org.littletonrobotics.junction.Logger; + import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkMax; @@ -10,6 +12,7 @@ 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.units.Units; import frc.robot.constants.RobotPIDConstants; import frc.robot.sensors.odometry.SparkOdometryThread; import frc.robot.sensors.resolvers.ResolverPWM; @@ -17,8 +20,11 @@ import frc.robot.util.spark.SparkConfigurer; import frc.robot.util.spark.SparkConstants; +import static frc.robot.subsystems.drive.DriveConstants.useSteerVelocitySetpoint; + public class ModuleIOReal implements ModuleIO { - private double velocitySetpoint = 0; + private double driveVelocitySetpoint = 0; + private double steerVelocitySetpoint = 0; private final Queue timestampQueue; private final Queue drivePositionQueue; private final Queue turnPositionQueue; @@ -36,16 +42,23 @@ public class ModuleIOReal implements ModuleIO { private final PIDController steerPID; private final SimpleMotorFeedforward steerFF; + private final double maxSteerRateRadiansPerSecond; + + private Rotation2d lastSteerSetpoint = new Rotation2d(); + private int lastSteerSetpointCounter = 0; + + private final String logPath; + public ModuleIOReal(ModuleInfo id) { drivePID = RobotPIDConstants.constructPID(RobotPIDConstants.drivePid, "drivePID" + id.id.toString()); driveFF = RobotPIDConstants.constructFFSimpleMotor(RobotPIDConstants.driveFF, "driveFF" + id.id.toString()); steerPID = RobotPIDConstants.constructPID(RobotPIDConstants.steerPid, "steerPID" + id.id.toString()); + steerFF = RobotPIDConstants.constructFFSimpleMotor(id.steerFF); driveSpark = SparkConfigurer.configSparkFlex(id.driveID, SparkConstants.driveConfig); steerSpark = SparkConfigurer.configSparkMax(id.steerID, SparkConstants.steerConfig); timestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue(); drivePositionQueue = SparkOdometryThread.getInstance().registerSignal(driveSpark, () -> driveSpark.getEncoder().getPosition()); - steerFF = RobotPIDConstants.constructFFSimpleMotor(id.steerFF); driveEncoder = driveSpark.getEncoder(); steeringEncoder = new ResolverPWM(id.resolverChannel, id.angleOffset); // steeringEncoder = @@ -61,6 +74,9 @@ public ModuleIOReal(ModuleInfo id) { turnPositionQueue = SparkOdometryThread.getInstance().registerSignal(steerSpark, () -> steeringEncoder.getDegrees() % 360); + + logPath = "Subsystems/Module/" + id.id.toString(); + maxSteerRateRadiansPerSecond = 10 / steerFF.getKv(); } @Override @@ -105,6 +121,8 @@ public void updateInputs(ModuleIOInputs inputs) { driveVelocityQueue.clear(); // inputs.rawEncoderValue = steeringEncoder.getRawValue(); + + lastSteerSetpointCounter = Math.max(0, lastSteerSetpointCounter - 1); } @Override @@ -112,7 +130,7 @@ public void setDriveVelocity(double velocity, ModuleIOInputs inputs) { double pidSpeed = driveFF.calculate(velocity); pidSpeed += drivePID.calculate(inputs.driveVelocityMetersPerSecond, velocity); setDriveVoltage(pidSpeed); - velocitySetpoint = velocity; + driveVelocitySetpoint = velocity; } @Override @@ -124,7 +142,39 @@ public void setDriveVoltage(double voltage) { public void setSteerPosition(Rotation2d angle, ModuleIOInputs inputs) { Rotation2d delta = angle.minus(Rotation2d.fromDegrees(inputs.steerAngleDegrees)); double sin = Math.sin(delta.getRadians()); - setSteerVoltage(steerPID.calculate(sin, 0) * 6); + double pidVoltage = steerPID.calculate(sin, 0) * 6; + + Logger.recordOutput(logPath + "/errorRadians", + (Math.abs(inputs.steerAngleDegrees - angle.getDegrees()) % 180) * Math.PI / 180); + + // calculate setpoint velocity using last input + double setpointDerivative; + if (!isLastSteerSetpointValid() || !useSteerVelocitySetpoint) + setpointDerivative = 0; // do not set velocity if estimate is stale + else { // if last setpoint is fresh (last robot loop), approximate derivative of + // setpoint + setpointDerivative = angle.minus(lastSteerSetpoint).getRadians() / 0.02; + } // clamp velocity setpoint + Logger.recordOutput(logPath + "/steerSetpointDerivative", setpointDerivative, Units.RadiansPerSecond); + + double angularVelocityRadPerSec; + // force velocity setpoint to zero if setpoint discontinuous + if (MathUtil.isNear(0, setpointDerivative, DriveConstants.steerSetpointContinuityDeltaRadPerSec)) + angularVelocityRadPerSec = MathUtil.clamp(setpointDerivative, -maxSteerRateRadiansPerSecond, + maxSteerRateRadiansPerSecond); + else + angularVelocityRadPerSec = 0; + angularVelocityRadPerSec = Math.abs(angularVelocityRadPerSec) * Math.max(0, Math.signum(pidVoltage)); + + steerVelocitySetpoint = angularVelocityRadPerSec; + // update last setpoint + updateLastSteerSetpoint(angle); + + // compute output voltage + double ffVoltage = steerFF.calculate(angularVelocityRadPerSec); + Logger.recordOutput(logPath + "/steerPIDVoltage", pidVoltage); + Logger.recordOutput(logPath + "/steerFFVoltage", ffVoltage); + setSteerVoltage(ffVoltage + pidVoltage); } @Override @@ -133,7 +183,21 @@ public void setSteerVoltage(double voltage) { } @Override - public double velocitySetpoint() { - return velocitySetpoint; + public double driveVelocitySetpoint() { + return driveVelocitySetpoint; + } + + @Override + public double steerVelocitySetpoint() { + return steerVelocitySetpoint; + } + + private void updateLastSteerSetpoint(Rotation2d lastSteerSetpoint) { + this.lastSteerSetpoint = lastSteerSetpoint; + lastSteerSetpointCounter = 2; + } + + private boolean isLastSteerSetpointValid() { + return lastSteerSetpointCounter > 0; } } diff --git a/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java index 5d673bd2..b3b46107 100644 --- a/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java @@ -6,13 +6,19 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.constants.RobotPIDConstants; import frc.robot.subsystems.drive.DriveConstants; +import static frc.robot.subsystems.drive.DriveConstants.useSteerVelocitySetpoint; + +import org.littletonrobotics.junction.Logger; + public class ModuleIOSim implements ModuleIO { - private double velocitySetpoint = 0; + private double driveVelocitySetpoint = 0; + private double steerVelocitySetpoint = 0; private final DCMotorSim driveSim; private final DCMotorSim turnSim; private double driveAppliedVolts = 0.0; @@ -21,11 +27,20 @@ public class ModuleIOSim implements ModuleIO { private final PIDController drivePID; private final SimpleMotorFeedforward driveFF; private final PIDController steerPID; + private final SimpleMotorFeedforward steerFF; + + private final double maxSteerRateRadiansPerSecond; + + private Rotation2d lastSteerSetpoint = new Rotation2d(); + private int lastSteerSetpointCounter = 0; + + private final String logPath; public ModuleIOSim(ModuleInfo id) { drivePID = RobotPIDConstants.constructPID(RobotPIDConstants.drivePid, "drivePID" + id.id.toString()); driveFF = RobotPIDConstants.constructFFSimpleMotor(RobotPIDConstants.driveFF, "driveFF" + id.id.toString()); steerPID = RobotPIDConstants.constructPID(RobotPIDConstants.steerPid, "steerPID" + id.id.toString()); + steerFF = RobotPIDConstants.constructFFSimpleMotor(id.steerFF, "steerFF" + id.id.toString()); DCMotor driveGearbox = DCMotor.getNeoVortex(1); DCMotor turnGearbox = DCMotor.getNeo550(1); driveSim = new DCMotorSim( @@ -34,6 +49,9 @@ public ModuleIOSim(ModuleInfo id) { turnSim = new DCMotorSim( LinearSystemId.createDCMotorSystem(turnGearbox, 0.002174375, DriveConstants.steerGearRatio), turnGearbox); + + logPath = "Subsystems/Module/" + id.id.toString(); + maxSteerRateRadiansPerSecond = 10 / steerFF.getKv(); } @Override @@ -41,7 +59,7 @@ public void setDriveVelocity(double velocity, ModuleIOInputs inputs) { double pidSpeed = driveFF.calculate(velocity); pidSpeed += drivePID.calculate(inputs.driveVelocityMetersPerSecond, velocity); setDriveVoltage(pidSpeed); - velocitySetpoint = velocity; + driveVelocitySetpoint = velocity; } @Override @@ -53,12 +71,42 @@ public void setDriveVoltage(double voltage) { public void setSteerPosition(Rotation2d angle, ModuleIOInputs inputs) { Rotation2d delta = angle.minus(Rotation2d.fromDegrees(inputs.steerAngleDegrees)); double sin = Math.sin(delta.getRadians()); - setSteerVoltage(MathUtil.clamp(steerPID.calculate(sin, 0) * 12, -12, 12)); + + // calculate setpoint velocity using last input + double setpointDerivative; + if (!isLastSteerSetpointValid() || !useSteerVelocitySetpoint) + setpointDerivative = 0; // do not set velocity if estimate is stale + else { // if last setpoint is fresh (last robot loop), approximate derivative of + // setpoint + setpointDerivative = angle.minus(lastSteerSetpoint).getRadians() / 0.02; + } // clamp velocity setpoint + Logger.recordOutput(logPath + "/steerSetpointDerivative", setpointDerivative, Units.RadiansPerSecond); + + double angularVelocityRadPerSec; + // force velocity setpoint to zero if setpoint discontinuous + if (MathUtil.isNear(0, setpointDerivative, DriveConstants.steerSetpointContinuityDeltaRadPerSec)) + angularVelocityRadPerSec = MathUtil.clamp(setpointDerivative, -maxSteerRateRadiansPerSecond, + maxSteerRateRadiansPerSecond); + else + angularVelocityRadPerSec = 0; + angularVelocityRadPerSec = Math.abs(angularVelocityRadPerSec) + * -Math.signum(MathUtil.applyDeadband(sin, Math.PI / 36)); + + steerVelocitySetpoint = angularVelocityRadPerSec; + // update last setpoint + updateLastSteerSetpoint(angle); + + // compute output voltage + double pidVoltage = steerPID.calculate(sin, 0) * 12; + double ffVoltage = steerFF.calculate(angularVelocityRadPerSec); + Logger.recordOutput(logPath + "/steerPIDVoltage", pidVoltage); + Logger.recordOutput(logPath + "/steerFFVoltage", ffVoltage); + setSteerVoltage(ffVoltage + pidVoltage); } @Override public void setSteerVoltage(double voltage) { - turnAppliedVolts = -voltage; + turnAppliedVolts = -MathUtil.clamp(voltage, -12, 12); } @Override @@ -86,10 +134,26 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.odometryDrivePositionsMeters = new double[]{inputs.drivePositionMeters}; inputs.odometryTurnPositions = new Rotation2d[]{Rotation2d.fromDegrees(inputs.steerAngleDegrees)}; inputs.driveVelocities = new double[]{inputs.driveVelocityMetersPerSecond}; + + lastSteerSetpointCounter = Math.max(0, lastSteerSetpointCounter - 1); + } + + @Override + public double driveVelocitySetpoint() { + return driveVelocitySetpoint; } @Override - public double velocitySetpoint() { - return velocitySetpoint; + public double steerVelocitySetpoint() { + return steerVelocitySetpoint; + } + + private void updateLastSteerSetpoint(Rotation2d lastSteerSetpoint) { + this.lastSteerSetpoint = lastSteerSetpoint; + lastSteerSetpointCounter = 2; + } + + private boolean isLastSteerSetpointValid() { + return lastSteerSetpointCounter > 0; } }