Remove setVoltageImpls, move all controllers into implementations

StuyPulse · Jan 26, 2024 · 3239617 · 3239617
1 parent 1b021ee
commit 3239617
Show file tree

Hide file tree

Showing 9 changed files with 165 additions and 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
@@ -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()));
-    }
-
 }
diff --git 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
@@ -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
@@ -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() {}
 }
diff --git 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
@@ -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());