Merge pull request #18 from FRC2713/shooterPID-on-sparks

FRC2713 · Feb 19, 2024 · f6d898d · f6d898d
2 parents 8a2b857 + 9c2b563
commit f6d898d
Show file tree

Hide file tree

Showing 13 changed files with 32 additions and 44 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -128,7 +128,7 @@ public static final class ShooterPivotConstants {
 
   public static final class ElevatorConstants {
     public static final PIDFFGains ELEVATOR_GAINS =
-        PIDFFGains.builder().name("Elevator Controller").kP(0.14).kD(0.0).kG(0.31).build();
+        PIDFFGains.builder().name("Elevator Controller").kP(0.0).kD(0.0).kG(0.0).build();
     public static final double GEARING = 5.0;
     public static final double CARRIAGE_MASS_KG = 0.3;
     public static final double DRUM_RADIUS_METERS = Units.inchesToMeters(1);

diff --git a/src/main/java/frc/robot/subsystems/feederIO/Feeder.java b/src/main/java/frc/robot/subsystems/feederIO/Feeder.java
@@ -6,6 +6,7 @@
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
+import frc.robot.Constants;
 import frc.robot.Robot;
 import frc.robot.subsystems.intakeIO.Intake;
 import frc.robot.util.LoggableMotor;
@@ -80,7 +81,7 @@ public void periodic() {
   }
 
   public boolean hasGamepiece() {
-    return IO.hasGamepiece();
+    return inputs.sensorRange < Constants.FeederConstants.SENSOR_THRESHOLD;
   }
 
   public boolean atTarget() {

diff --git a/src/main/java/frc/robot/subsystems/feederIO/FeederIO.java b/src/main/java/frc/robot/subsystems/feederIO/FeederIO.java
@@ -11,6 +11,8 @@ public static class FeederInputs {
     public double tempCelcius = 0.0;
     public double currentAmps = 0.0;
     public double positionDeg = 0.0;
+    public double sensorRange = 0.0;
+    public String sensorStatus = "";
   }
 
   public void setSetpoint(double setpointRPM);
@@ -20,6 +22,4 @@ public static class FeederInputs {
   public boolean atTarget();
 
   public void updateInputs(FeederInputsAutoLogged inputs);
-
-  public boolean hasGamepiece();
 }
diff --git a/src/main/java/frc/robot/subsystems/feederIO/FeederIOSim.java b/src/main/java/frc/robot/subsystems/feederIO/FeederIOSim.java
@@ -38,16 +38,13 @@ public void updateInputs(FeederInputsAutoLogged inputs) {
     inputs.tempCelcius = 0;
     inputs.velocityRPM = sim.getAngularVelocityRPM();
     inputs.positionDeg = Units.rotationsToDegrees(sim.getAngularPositionRotations());
+    inputs.sensorRange = 0.0;
+    inputs.sensorStatus = "Short";
   }
 
   @Override
   public void setSetpoint(double setpointRPM) {
     feederController.setSetpoint(setpointRPM);
     this.setpointRPM = setpointRPM;
   }
-
-  @Override
-  public boolean hasGamepiece() {
-    return false;
-  }
 }
diff --git a/src/main/java/frc/robot/subsystems/feederIO/FeederIOSparks.java b/src/main/java/frc/robot/subsystems/feederIO/FeederIOSparks.java
@@ -5,7 +5,6 @@
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
 import edu.wpi.first.math.util.Units;
-import frc.robot.Constants;
 import frc.robot.Constants.RobotMap;
 
 public class FeederIOSparks implements FeederIO {
@@ -43,10 +42,7 @@ public void updateInputs(FeederInputsAutoLogged inputs) {
     inputs.tempCelcius = motor.getMotorTemperature();
     inputs.velocityRPM = motor.getEncoder().getVelocity();
     inputs.positionDeg = Units.rotationsToDegrees(motor.getEncoder().getPosition());
-  }
-
-  @Override
-  public boolean hasGamepiece() {
-    return (sensor.getRange() < Constants.FeederConstants.SENSOR_THRESHOLD);
+    inputs.sensorRange = sensor.getRange();
+    inputs.sensorStatus = sensor.getStatus().name();
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/intakeIO/Intake.java b/src/main/java/frc/robot/subsystems/intakeIO/Intake.java
@@ -62,7 +62,7 @@ public void setRPM(double leftRpm, double rightRPM) {
   }
 
   public boolean hasGamepiece() {
-    return IO.hasGamepiece();
+    return inputs.sensorRange < Constants.IntakeConstants.SENSOR_THRESHOLD;
   }
 
   public void periodic() {
@@ -72,8 +72,6 @@ public void periodic() {
     boolean leftIsAtTarget = leftIsAtTarget();
     boolean rightIsAtTarget = rightIsAtTarget();
 
-    Logger.recordOutput("Intake/Sensor Range", this.inputs.sensorRange);
-
     Logger.recordOutput("Intake/Left Target RPM", leftTargetRPM);
     Logger.recordOutput("Intake/Left Has reached target", leftIsAtTarget);
 

diff --git a/src/main/java/frc/robot/subsystems/intakeIO/IntakeIO.java b/src/main/java/frc/robot/subsystems/intakeIO/IntakeIO.java
@@ -28,6 +28,4 @@ public static class IntakeInputs {
   public void setCurrentLimit(int currentLimit);
 
   public void setVoltage(double leftVolts, double rightVolts);
-
-  public boolean hasGamepiece();
 }
diff --git a/src/main/java/frc/robot/subsystems/intakeIO/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intakeIO/IntakeIOSim.java
@@ -52,11 +52,6 @@ public void updateInputs(IntakeInputsAutoLogged inputs) {
     inputs.sensorStatus = "Short";
   }
 
-  @Override
-  public boolean hasGamepiece() {
-    return false;
-  }
-
   @Override
   public void setVoltage(double leftVolts, double rightVolts) {
     simLeft.setInputVoltage(leftVolts);

diff --git a/src/main/java/frc/robot/subsystems/intakeIO/IntakeIOSparks.java b/src/main/java/frc/robot/subsystems/intakeIO/IntakeIOSparks.java
@@ -60,12 +60,6 @@ public void updateInputs(IntakeInputsAutoLogged inputs) {
     inputs.sensorStatus = sensor.getStatus().toString();
   }
 
-  @Override
-  public boolean hasGamepiece() {
-    // return (sensor.getRange() < Constants.IntakeConstants.SENSOR_THRESHOLD);
-    return false;
-  }
-
   @Override
   public void setVoltage(double leftVolts, double rightVolts) {
     leftMotor.setVoltage(leftVolts);

diff --git a/src/main/java/frc/robot/subsystems/shooterPivot/ShooterPivot.java b/src/main/java/frc/robot/subsystems/shooterPivot/ShooterPivot.java
@@ -1,6 +1,8 @@
 package frc.robot.subsystems.shooterPivot;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -25,14 +27,16 @@ public enum MotionMode {
   public MotionMode mode;
 
   public final ShooterPivotInputsAutoLogged inputs;
+  private ArmFeedforward feedforward;
   private final ShooterPivotIO IO;
   private double targetDegs = 0;
 
   public ShooterPivot(ShooterPivotIO IO) {
     this.inputs = new ShooterPivotInputsAutoLogged();
     this.IO = IO;
-    this.IO.updateInputs(inputs);
+    this.IO.updateInputs(inputs, 0);
     mode = MotionMode.CLOSED_LOOP;
+    feedforward = Constants.ShooterPivotConstants.SHOOTER_PIVOT_GAINS.createArmFeedForward();
   }
 
   public void setTargetAngle(double angleInDegrees) {
@@ -73,7 +77,12 @@ public void periodic() {
         break;
     }
 
-    IO.updateInputs(inputs);
+    var ffVolts =
+        feedforward.calculate(
+            Units.degreesToRadians(inputs.absoluteEncoderAdjustedAngle),
+            Units.degreesToRadians(inputs.velocityDegreesPerSecondOne));
+    IO.updateInputs(inputs, ffVolts);
+    Logger.recordOutput("ShooterPivot/FFVolts", ffVolts);
     Logger.recordOutput("ShooterPivot/isAtTarget", this.isAtTargetAngle());
     Logger.recordOutput("ShooterPivot/TargetAngleDegs", targetDegs);
   }

diff --git a/src/main/java/frc/robot/subsystems/shooterPivot/ShooterPivotIO.java b/src/main/java/frc/robot/subsystems/shooterPivot/ShooterPivotIO.java
@@ -16,7 +16,7 @@ public static class ShooterPivotInputs {
     public double targetAngle = 0.0;
   }
 
-  public void updateInputs(ShooterPivotInputs inputs);
+  public void updateInputs(ShooterPivotInputs inputs, double ffVolts);
 
   public void reseedPosition(double angleDeg);
 

diff --git a/src/main/java/frc/robot/subsystems/shooterPivot/ShooterPivotIOSim.java b/src/main/java/frc/robot/subsystems/shooterPivot/ShooterPivotIOSim.java
@@ -38,7 +38,7 @@ public ShooterPivotIOSim() {
   }
 
   @Override
-  public void updateInputs(ShooterPivotInputs inputs) {
+  public void updateInputs(ShooterPivotInputs inputs, double ffVolts) {
     if (DriverStation.isDisabled()) {
       sim.setInputVoltage(0.0);
     }
@@ -58,6 +58,7 @@ public void updateInputs(ShooterPivotInputs inputs) {
     inputs.currentDrawOne = sim.getCurrentDrawAmps();
 
     double effort = motorController.calculate(inputs.absoluteEncoderAdjustedAngle, targetAngle);
+    effort += ffVolts;
     effort = MathUtil.clamp(effort, -12, 12);
     setVoltage(effort);
   }

diff --git a/src/main/java/frc/robot/subsystems/shooterPivot/ShooterPivotIOSparks.java b/src/main/java/frc/robot/subsystems/shooterPivot/ShooterPivotIOSparks.java
@@ -6,10 +6,9 @@
 import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.SparkAnalogSensor;
+import com.revrobotics.SparkPIDController;
 import edu.wpi.first.math.util.Units;
 import frc.robot.Constants;
-import frc.robot.rhr.RHRFeedForward;
-import frc.robot.rhr.RHRPIDFFController;
 import frc.robot.util.RedHawkUtil;
 import java.util.HashMap;
 
@@ -20,10 +19,6 @@ public class ShooterPivotIOSparks implements ShooterPivotIO {
 
   private double targetAngle;
 
-  private RHRPIDFFController motorController;
-
-  private RHRFeedForward feedforward;
-
   public ShooterPivotIOSparks() {
     spark = new CANSparkMax(Constants.RobotMap.PIVOT_ID, MotorType.kBrushless);
     analogSensor = spark.getAnalog(SparkAnalogSensor.Mode.kAbsolute);
@@ -50,12 +45,15 @@ public ShooterPivotIOSparks() {
     spark.setIdleMode(IdleMode.kBrake);
     spark.setInverted(false);
 
-    motorController = Constants.ShooterPivotConstants.SHOOTER_PIVOT_GAINS.createRHRController();
-    feedforward = Constants.ShooterPivotConstants.SHOOTER_PIVOT_GAINS.createRHRFeedForward();
+    SparkPIDController pid = spark.getPIDController();
+    pid.setP(Constants.ShooterPivotConstants.SHOOTER_PIVOT_GAINS.getKP());
+    pid.setI(Constants.ShooterPivotConstants.SHOOTER_PIVOT_GAINS.getKI());
+    pid.setD(Constants.ShooterPivotConstants.SHOOTER_PIVOT_GAINS.getKD());
   }
 
   @Override
-  public void updateInputs(ShooterPivotInputs inputs) {
+  public void updateInputs(ShooterPivotInputs inputs, double ffVolts) {
+
     inputs.absoluteEncoderAdjustedAngle =
         Units.rotationsToDegrees(spark.getEncoder().getPosition());
 
@@ -70,6 +68,7 @@ public void updateInputs(ShooterPivotInputs inputs) {
     inputs.currentDrawOne = spark.getOutputCurrent();
 
     inputs.outputVoltage = spark.getBusVoltage();
+    spark.getPIDController().setReference(targetAngle, ControlType.kPosition, 0, ffVolts);
   }
 
   @Override