From 3239617c89331c1cdcf67de3ab50a67d0330a7ed Mon Sep 17 00:00:00 2001 From: BenG49 Date: Fri, 26 Jan 2024 10:30:35 -0500 Subject: [PATCH] Remove setVoltageImpls, move all controllers into implementations --- .../robot/subsystems/amper/Amper.java | 13 +---- .../robot/subsystems/amper/AmperImpl.java | 24 ++++++---- .../robot/subsystems/amper/AmperSim.java | 22 +++++++-- .../robot/subsystems/shooter/Shooter.java | 29 ----------- .../robot/subsystems/shooter/ShooterImpl.java | 33 ++++++++----- .../robot/subsystems/shooter/SimShooter.java | 38 ++++++++++----- .../subsystems/swerve/modules/SimModule.java | 48 ++++++++++++++++--- .../swerve/modules/SwerveModule.java | 42 ---------------- .../swerve/modules/SwerveModuleImpl.java | 48 ++++++++++++++++--- 9 files changed, 165 insertions(+), 132 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/amper/Amper.java b/src/main/java/com/stuypulse/robot/subsystems/amper/Amper.java index 17717190..6c5f46b5 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/amper/Amper.java +++ b/src/main/java/com/stuypulse/robot/subsystems/amper/Amper.java @@ -1,9 +1,6 @@ package com.stuypulse.robot.subsystems.amper; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Settings.Amper.Lift; -import com.stuypulse.stuylib.control.Controller; -import com.stuypulse.stuylib.control.feedback.PIDController; import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.network.SmartNumber; @@ -33,11 +30,9 @@ public static Amper getInstance() { return instance; } - public final SmartNumber targetHeight; - public final Controller liftController; + protected final SmartNumber targetHeight; public Amper() { - liftController = new PIDController(Lift.PID.kP, Lift.PID.kI, Lift.PID.kD); targetHeight = new SmartNumber("Amp/Target Height", 0); // TODO: determine the default value } @@ -54,13 +49,7 @@ public void setTargetHeight(double height) { public abstract boolean liftAtBottom(); public abstract double getLiftHeight(); public abstract void stopLift(); - public abstract void setLiftVoltageImpl(double voltage); public abstract boolean touchingAmp(); - @Override - public void periodic() { - setLiftVoltageImpl(liftController.update(targetHeight.get(), getLiftHeight())); - } - } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java b/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java index 698c4480..8adaa7da 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java @@ -5,6 +5,9 @@ import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Amper.Lift; +import com.stuypulse.stuylib.control.Controller; +import com.stuypulse.stuylib.control.feedback.PIDController; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -20,7 +23,11 @@ public class AmperImpl extends Amper { private final DigitalInput minSwitch; private final DigitalInput ampIRSensor; + public final Controller liftController; + public AmperImpl() { + liftController = new PIDController(Lift.PID.kP, Lift.PID.kI, Lift.PID.kD); + scoreMotor = new CANSparkMax(Ports.Amper.SCORE, MotorType.kBrushless); liftMotor = new CANSparkMax(Ports.Amper.LIFT, MotorType.kBrushless); liftEncoder = liftMotor.getEncoder(); @@ -77,18 +84,17 @@ public void stopRoller() { } @Override - public void setLiftVoltageImpl(double voltage) { - if (liftAtBottom() && voltage < 0) { - stopLift(); - } - else { - liftMotor.setVoltage(voltage); - } - } - public void periodic() { super.periodic(); + liftController.update(targetHeight.get(), getLiftHeight()); + + if (liftAtBottom() && liftController.getOutput() < 0) { + stopLift(); + } else { + liftMotor.setVoltage(liftController.getOutput()); + } + SmartDashboard.putNumber("Amper/Intake Speed", scoreMotor.get()); SmartDashboard.putNumber("Amper/Lift Speed", liftMotor.get()); SmartDashboard.putNumber("Amper/Intake Current", scoreMotor.getOutputCurrent()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/amper/AmperSim.java b/src/main/java/com/stuypulse/robot/subsystems/amper/AmperSim.java index 10fcb23a..368b37c0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/amper/AmperSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/amper/AmperSim.java @@ -3,6 +3,11 @@ import static com.stuypulse.robot.constants.Settings.Amper.Lift.*; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Amper.Lift; +import com.stuypulse.robot.constants.Settings.Amper.Lift.Encoder; +import com.stuypulse.stuylib.control.Controller; +import com.stuypulse.stuylib.control.feedback.PIDController; + import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.ElevatorSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -11,7 +16,11 @@ public class AmperSim extends Amper { private final ElevatorSim sim; + public final Controller liftController; + public AmperSim() { + liftController = new PIDController(Lift.PID.kP, Lift.PID.kI, Lift.PID.kD); + sim = new ElevatorSim( DCMotor.getNEO(1), Encoder.GEARING, @@ -55,16 +64,19 @@ public double getLiftHeight() { return sim.getPositionMeters(); } - @Override - public void setLiftVoltageImpl(double voltage) { - sim.setInputVoltage(voltage); - } - @Override public void stopRoller() {} @Override public void simulationPeriodic() { + liftController.update(targetHeight.get(), getLiftHeight()); + + if (liftAtBottom() && liftController.getOutput() < 0) { + stopLift(); + } else { + sim.setInputVoltage(liftController.getOutput()); + } + sim.update(Settings.DT); SmartDashboard.putNumber("Amper/Lift Height", getLiftHeight()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java index 0ec9eb83..08fb6a45 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java @@ -1,14 +1,8 @@ package com.stuypulse.robot.subsystems.shooter; -import com.stuypulse.robot.constants.Settings.Shooter.Feedforward; -import com.stuypulse.robot.constants.Settings.Shooter.PID; -import com.stuypulse.stuylib.control.Controller; -import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; import com.stuypulse.stuylib.network.SmartNumber; -import com.stuypulse.stuylib.control.feedback.PIDController; import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public abstract class Shooter extends SubsystemBase { @@ -30,16 +24,9 @@ public static Shooter getInstance() { private final SmartNumber leftTargetRPM; private final SmartNumber rightTargetRPM; - private final Controller leftController; - private final Controller rightController; - public Shooter() { leftTargetRPM = new SmartNumber("Shooter/Left Target RPM", 0); rightTargetRPM = new SmartNumber("Shooter/Right Target RPM", 0); - leftController = new MotorFeedforward(Feedforward.kS, Feedforward.kV, Feedforward.kA).velocity() - .add(new PIDController(PID.kP, PID.kI, PID.kD)); - rightController = new MotorFeedforward(Feedforward.kS, Feedforward.kV, Feedforward.kA).velocity() - .add(new PIDController(PID.kP, PID.kI, PID.kD)); } public double getLeftTargetRPM() { @@ -61,20 +48,4 @@ public void setRightTargetRPM(Number rightTargetRPM) { public abstract void stop(); public abstract double getLeftShooterRPM(); public abstract double getRightShooterRPM(); - - protected abstract void setLeftMotorVoltageImpl(double voltage); - protected abstract void setRightMotorVoltageImpl(double voltage); - - @Override - public void periodic() { - - leftController.update(getLeftTargetRPM(), getLeftShooterRPM()); - rightController.update(getRightTargetRPM(), getRightShooterRPM()); - setLeftMotorVoltageImpl(leftController.getOutput()); - setRightMotorVoltageImpl(rightController.getOutput()); - - periodicallyCalled(); - } - - public void periodicallyCalled() {} } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java index 222d8bcd..7977c101 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -5,6 +5,12 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings.Shooter.Feedforward; +import com.stuypulse.robot.constants.Settings.Shooter.PID; +import com.stuypulse.stuylib.control.Controller; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; + import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class ShooterImpl extends Shooter { @@ -14,6 +20,8 @@ public class ShooterImpl extends Shooter { private final RelativeEncoder leftEncoder; private final RelativeEncoder rightEncoder; + private final Controller leftController; + private final Controller rightController; public ShooterImpl() { leftMotor = new CANSparkMax(Ports.Shooter.LEFT_MOTOR, MotorType.kBrushless); @@ -22,6 +30,11 @@ public ShooterImpl() { leftEncoder = leftMotor.getEncoder(); rightEncoder = rightMotor.getEncoder(); + leftController = new MotorFeedforward(Feedforward.kS, Feedforward.kV, Feedforward.kA).velocity() + .add(new PIDController(PID.kP, PID.kI, PID.kD)); + rightController = new MotorFeedforward(Feedforward.kS, Feedforward.kV, Feedforward.kA).velocity() + .add(new PIDController(PID.kP, PID.kI, PID.kD)); + Motors.Shooter.LEFT_SHOOTER.configure(leftMotor); Motors.Shooter.RIGHT_SHOOTER.configure(rightMotor); } @@ -43,17 +56,15 @@ public double getRightShooterRPM() { } @Override - public void setLeftMotorVoltageImpl(double voltage) { - leftMotor.setVoltage(voltage); - } - - @Override - public void setRightMotorVoltageImpl(double voltage) { - rightMotor.setVoltage(voltage); - } - - @Override - public void periodicallyCalled() { + public void periodic() { + super.periodic(); + + leftController.update(getLeftTargetRPM(), getLeftShooterRPM()); + rightController.update(getRightTargetRPM(), getRightShooterRPM()); + + leftMotor.setVoltage(leftController.getOutput()); + rightMotor.setVoltage(rightController.getOutput()); + SmartDashboard.putNumber("Shooter/Right RPM", getRightShooterRPM()); SmartDashboard.putNumber("Shooter/Left RPM", getLeftShooterRPM()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/SimShooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/SimShooter.java index 6a8dac67..64614377 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/SimShooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/SimShooter.java @@ -1,46 +1,60 @@ package com.stuypulse.robot.subsystems.shooter; -import com.stuypulse.stuylib.network.SmartNumber; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Shooter.Feedforward; +import com.stuypulse.robot.constants.Settings.Shooter.PID; +import com.stuypulse.stuylib.control.Controller; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; + import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class SimShooter extends Shooter { - private FlywheelSim leftWheel; - private FlywheelSim rightWheel; + + private final FlywheelSim leftWheel; + private final FlywheelSim rightWheel; + + private final Controller leftController; + private final Controller rightController; public SimShooter() { leftWheel = new FlywheelSim(DCMotor.getNEO(1), 1, Settings.Shooter.MOMENT_OF_INERTIA); rightWheel = new FlywheelSim(DCMotor.getNEO(1), 1, Settings.Shooter.MOMENT_OF_INERTIA); + + leftController = new MotorFeedforward(Feedforward.kS, Feedforward.kV, Feedforward.kA).velocity() + .add(new PIDController(PID.kP, PID.kI, PID.kD)); + rightController = new MotorFeedforward(Feedforward.kS, Feedforward.kV, Feedforward.kA).velocity() + .add(new PIDController(PID.kP, PID.kI, PID.kD)); } + @Override public void stop() { leftWheel.setInputVoltage(0); rightWheel.setInputVoltage(0); } + @Override public double getLeftShooterRPM() { return leftWheel.getAngularVelocityRPM(); } + @Override public double getRightShooterRPM() { return rightWheel.getAngularVelocityRPM(); } - public void setRightMotorVoltageImpl(double voltage) { - rightWheel.setInputVoltage(voltage); - } - - public void setLeftMotorVoltageImpl(double voltage) { - leftWheel.setInputVoltage(voltage); - } - @Override public void simulationPeriodic() { + leftController.update(getLeftTargetRPM(), getLeftShooterRPM()); + rightController.update(getRightTargetRPM(), getRightShooterRPM()); + + leftWheel.setInputVoltage(leftController.getOutput()); + rightWheel.setInputVoltage(rightController.getOutput()); + leftWheel.update(Settings.DT); rightWheel.update(Settings.DT); - SmartDashboard.putNumber("Shooter/Right RPM", getRightShooterRPM()); SmartDashboard.putNumber("Shooter/Left RPM", getLeftShooterRPM()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SimModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SimModule.java index 378d6b45..21921bfc 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SimModule.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SimModule.java @@ -1,8 +1,16 @@ package com.stuypulse.robot.subsystems.swerve.modules; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Swerve; import com.stuypulse.robot.constants.Settings.Swerve.Drive; import com.stuypulse.robot.constants.Settings.Swerve.Turn; import com.stuypulse.robot.util.PositionVelocitySystem; +import com.stuypulse.stuylib.control.Controller; +import com.stuypulse.stuylib.control.angle.AngleController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.streams.angles.filters.ARateLimit; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -14,15 +22,27 @@ import edu.wpi.first.wpilibj.simulation.BatterySim; import edu.wpi.first.wpilibj.simulation.LinearSystemSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class SimModule extends SwerveModule { + private final LinearSystemSim driveSim; private final LinearSystemSim turnSim; + private final Controller driveController; + private final AngleController angleController; + public SimModule(String id, Translation2d offset) { super(id, offset); + driveSim = new PositionVelocitySystem(Drive.kV, Drive.kA).getSim(); turnSim = new LinearSystemSim(LinearSystemId.identifyPositionSystem(Turn.kV.get(), Turn.kA.get())); + + driveController = new PIDController(Drive.kP, Drive.kI, Drive.kD) + .add(new MotorFeedforward(Drive.kS, Drive.kV, Drive.kA).velocity()); + + angleController = new AnglePIDController(Turn.kP, Turn.kI, Turn.kD) + .setSetpointFilter(new ARateLimit(Swerve.MAX_TURNING)); } @Override @@ -41,18 +61,34 @@ public SwerveModulePosition getModulePosition() { } @Override - public void setVoltageImpl(double driveVoltage, double turnVoltage) { - driveSim.setInput(driveVoltage); - turnSim.setInput(turnVoltage); + public void simulationPeriodic() { + driveController.update( + targetState.speedMetersPerSecond, + getVelocity() + ); + + angleController.update( + Angle.fromRotation2d(targetState.angle), + Angle.fromRotation2d(getAngle()) + ); + + if (Math.abs(driveController.getOutput()) < Settings.Swerve.MODULE_VELOCITY_DEADBAND.get()) { + driveSim.setInput(0); + turnSim.setInput(0); + } else { + driveSim.setInput(driveController.getOutput()); + turnSim.setInput(angleController.getOutput()); + } RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage( turnSim.getCurrentDrawAmps() + driveSim.getCurrentDrawAmps() )); - } - @Override - public void simulationPeriodic() { driveSim.update(Settings.DT); turnSim.update(Settings.DT); + + SmartDashboard.putNumber("Swerve/Modules/" + this.getId() + "/Drive Voltage", driveController.getOutput()); + SmartDashboard.putNumber("Swerve/Modules/" + this.getId() + "/Turn Voltage", angleController.getOutput()); + SmartDashboard.putNumber("Swerve/Modules/" + this.getId() + "/Angle Error", angleController.getError().toDegrees()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SwerveModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SwerveModule.java index 333b7a46..f7819a19 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SwerveModule.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SwerveModule.java @@ -1,17 +1,5 @@ package com.stuypulse.robot.subsystems.swerve.modules; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Settings.Swerve; -import com.stuypulse.robot.constants.Settings.Swerve.Drive; -import com.stuypulse.robot.constants.Settings.Swerve.Turn; -import com.stuypulse.stuylib.control.Controller; -import com.stuypulse.stuylib.control.angle.AngleController; -import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; -import com.stuypulse.stuylib.control.feedback.PIDController; -import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; -import com.stuypulse.stuylib.math.Angle; -import com.stuypulse.stuylib.streams.angles.filters.ARateLimit; - import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -27,20 +15,11 @@ public abstract class SwerveModule extends SubsystemBase { public SwerveModuleState targetState; - protected final Controller driveController; - protected final AngleController angleController; - public SwerveModule(String id, Translation2d offset) { this.id = id; this.offset = offset; targetState = new SwerveModuleState(); - - driveController = new PIDController(Drive.kP, Drive.kI, Drive.kD) - .add(new MotorFeedforward(Drive.kS, Drive.kV, Drive.kA).velocity()); - - angleController = new AnglePIDController(Turn.kP, Turn.kI, Turn.kD) - .setSetpointFilter(new ARateLimit(Swerve.MAX_TURNING)); } public String getId() { @@ -63,29 +42,8 @@ public void setState(SwerveModuleState state) { targetState = state; } - protected abstract void setVoltageImpl(double driveVoltage, double turnVoltage); - @Override public void periodic() { - driveController.update( - targetState.speedMetersPerSecond, - getVelocity() - ); - - angleController.update( - Angle.fromRotation2d(targetState.angle), - Angle.fromRotation2d(getAngle()) - ); - - if (Math.abs(driveController.getOutput()) < Settings.Swerve.MODULE_VELOCITY_DEADBAND.get()) { - setVoltageImpl(0, 0); - } else { - setVoltageImpl(driveController.getOutput(), angleController.getOutput()); - } - - SmartDashboard.putNumber("Swerve/Modules/" + this.getId() + "/Drive Voltage", driveController.getOutput()); - SmartDashboard.putNumber("Swerve/Modules/" + this.getId() + "/Turn Voltage", angleController.getOutput()); - SmartDashboard.putNumber("Swerve/Modules/" + this.getId() + "/Angle Error", angleController.getError().toDegrees()); SmartDashboard.putNumber("Swerve/Modules/" + this.getId() + "/Target Angle", targetState.angle.getDegrees()); SmartDashboard.putNumber("Swerve/Modules/" + this.getId() + "/Angle", getAngle().getDegrees()); SmartDashboard.putNumber("Swerve/Modules/" + this.getId()+ "/Target Speed", targetState.speedMetersPerSecond); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SwerveModuleImpl.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SwerveModuleImpl.java index fb847db4..6b4ecd49 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SwerveModuleImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SwerveModuleImpl.java @@ -5,7 +5,18 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.CANSparkLowLevel.MotorType; import com.stuypulse.robot.constants.Motors; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.constants.Settings.Swerve.Drive; import com.stuypulse.robot.constants.Settings.Swerve.Encoder; +import com.stuypulse.robot.constants.Settings.Swerve.Turn; +import com.stuypulse.stuylib.control.Controller; +import com.stuypulse.stuylib.control.angle.AngleController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.streams.angles.filters.ARateLimit; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -47,6 +58,9 @@ public class SwerveModuleImpl extends SwerveModule { private final RelativeEncoder driveEncoder; private final CANcoder turnEncoder; + private final Controller driveController; + private final AngleController angleController; + /** * Creates a new Swerve Module * @param id id of the module @@ -69,6 +83,12 @@ public SwerveModuleImpl(String id, Translation2d offset, int driveID, Rotation2d turnEncoder = new CANcoder(encoderID); + driveController = new PIDController(Drive.kP, Drive.kI, Drive.kD) + .add(new MotorFeedforward(Drive.kS, Drive.kV, Drive.kA).velocity()); + + angleController = new AnglePIDController(Turn.kP, Turn.kI, Turn.kD) + .setSetpointFilter(new ARateLimit(Swerve.MAX_TURNING)); + Motors.Swerve.DRIVE_CONFIG.configure(driveMotor); Motors.Swerve.TURN_CONFIG.configure(turnMotor); } @@ -88,15 +108,31 @@ public SwerveModulePosition getModulePosition() { return new SwerveModulePosition(driveEncoder.getPosition(), getAngle()); } - @Override - protected void setVoltageImpl(double driveVoltage, double turnVoltage) { - driveMotor.setVoltage(driveVoltage); - turnMotor.setVoltage(turnVoltage); - } - @Override public void periodic() { super.periodic(); + + driveController.update( + targetState.speedMetersPerSecond, + getVelocity() + ); + + angleController.update( + Angle.fromRotation2d(targetState.angle), + Angle.fromRotation2d(getAngle()) + ); + + if (Math.abs(driveController.getOutput()) < Settings.Swerve.MODULE_VELOCITY_DEADBAND.get()) { + driveMotor.setVoltage(0); + turnMotor.setVoltage(0); + } else { + driveMotor.setVoltage(driveController.getOutput()); + turnMotor.setVoltage(angleController.getOutput()); + } + + SmartDashboard.putNumber("Swerve/Modules/" + this.getId() + "/Drive Voltage", driveController.getOutput()); + SmartDashboard.putNumber("Swerve/Modules/" + this.getId() + "/Turn Voltage", angleController.getOutput()); + SmartDashboard.putNumber("Swerve/Modules/" + this.getId() + "/Angle Error", angleController.getError().toDegrees()); SmartDashboard.putNumber("Swerve/Modules/" + this.getId() + "/Raw Encoder Angle", Units.rotationsToDegrees(turnEncoder.getAbsolutePosition().getValueAsDouble())); } } \ No newline at end of file