feat: shooter testing changes and sim fixes

flamingchickens1540 · Feb 9, 2024 · 87a7633 · 87a7633
1 parent 4173384
commit 87a7633
Show file tree

Hide file tree

Showing 5 changed files with 27 additions and 14 deletions.
diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
@@ -82,8 +82,6 @@ public static class Indexer {
         public static final double INTAKE_MOI = 0.025;
         public static final double FEEDER_MOI = 0.025;
         public static final int VELOCITY_ERR_TOLERANCE_RPM = 10;
-
-
     }
 
 
@@ -121,7 +119,7 @@ public static class Flywheels {
             public static final double SIM_MOI = 4.08232288e-4;
 
             // TODO: if it's tuned in simulation, it's tuned in real life
-            public static final double KP = 0.4;
+            public static final double KP = 0.1;
             public static final double KI = 0.0;
             public static final double KD = 0.0;
             public static final double KS = 0.26925;
@@ -143,7 +141,7 @@ public static class Pivot {
             public static final double TOTAL_GEAR_RATIO = MOTOR_TO_CANCODER * CANCODER_TO_PIVOT;
             public static final double SIM_LENGTH_METERS = Units.inchesToMeters(12.910);
             // TODO: find the moi
-            public static final double SIM_MOI = 0.22552744227754662;
+            public static final double SIM_MOI = 0.04064471269;
 
             public static final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(60.0);
             public static final Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(8.0);
@@ -156,8 +154,15 @@ public static class Pivot {
             public static final double KG = 0.1;
             public static final double KV = 0.1;
 
-            public static final double CRUISE_VELOCITY_RPS = 4.0;
-            public static final double MAX_ACCEL_RPS2 = 40.0;
+            public static final double SIM_KP = 254;
+            public static final double SIM_KI = 0.0;
+            public static final double SIM_KD = 0.0;
+            public static final double SIM_KS = 0.0;
+            public static final double SIM_KG = 0.15;
+            public static final double SIM_KV = 0.187;
+
+            public static final double CRUISE_VELOCITY_RPS = 1.0;
+            public static final double MAX_ACCEL_RPS2 = 10.0;
             public static final double JERK_RPS3 = 2000;
 
             public static final Rotation2d ERROR_TOLERANCE = Rotation2d.fromDegrees(0.2);

diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java
@@ -45,6 +45,10 @@ public void setFeederVelocity(double setpointRPM) {
         io.setFeederVelocity(setpointRPM);
     }
 
+    public void setFeederPercent(double percent) {
+        io.setFeederVoltage(12.0 * percent);
+    }
+
     public Command feedToAmp() {
         return Commands.runOnce(() -> io.setFeederVelocity(-600), this);
     }
@@ -70,6 +74,7 @@ public boolean isFeederAtSetpoint() {
     public void stopFeeder() {
         io.setFeederVoltage(0);
     }
+
     public void stopIntake() {
         io.setIntakeVoltage(0);
     }

diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java
@@ -25,8 +25,9 @@ public IndexerIOSparkMax() {
         intakeMotor.setSmartCurrentLimit(30);
 
         feederMotor.setIdleMode(CANSparkBase.IdleMode.kBrake);
+        feederMotor.setInverted(true);
         feederMotor.enableVoltageCompensation(12.0);
-        feederMotor.setSmartCurrentLimit(30);
+        feederMotor.setSmartCurrentLimit(40);
 
         feederPID = feederMotor.getPIDController();
         feederPID.setP(FEEDER_KP, 0);

diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java
@@ -7,6 +7,7 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.Logger;
 import org.team1540.robot2024.util.LoggedTunableNumber;
+import org.team1540.robot2024.util.MechanismVisualiser;
 import org.team1540.robot2024.util.math.AverageFilter;
 
 import java.util.function.Supplier;
@@ -27,7 +28,7 @@ public class Shooter extends SubsystemBase {
 
     private double leftFlywheelSetpointRPM;
     private double rightFlywheelSetpointRPM;
-    private Rotation2d pivotSetpoint;
+    private Rotation2d pivotSetpoint = new Rotation2d();
 
     private final LoggedTunableNumber flywheelsKP = new LoggedTunableNumber("Shooter/Flywheels/kP", Flywheels.KP);
     private final LoggedTunableNumber flywheelsKI = new LoggedTunableNumber("Shooter/Flywheels/kI", Flywheels.KI);
@@ -49,6 +50,7 @@ public void periodic() {
         flywheelsIO.updateInputs(flywheelInputs);
         Logger.processInputs("Shooter/Pivot", pivotInputs);
         Logger.processInputs("Shooter/Flywheels", flywheelInputs);
+        MechanismVisualiser.setShooterPivotRotation(getPivotPosition());
 
         // Update tunable numbers
         if (flywheelsKP.hasChanged(hashCode()) || flywheelsKI.hasChanged(hashCode()) || flywheelsKD.hasChanged(hashCode())) {
@@ -62,6 +64,7 @@ public void periodic() {
         leftSpeedFilter.add(getLeftFlywheelSpeed());
         rightSpeedFilter.add(getRightFlywheelSpeed());
         pivotPositionFilter.add(getPivotPosition().getRotations());
+        Logger.recordOutput("Shooter/Pivot/Setpoint", pivotSetpoint.getDegrees());
     }
 
     /**
@@ -180,5 +183,4 @@ public Command setPivotPositionCommand(Rotation2d setpoint) {
                 this::isPivotAtSetpoint,
                 this);
     }
-
 }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOSim.java
@@ -24,8 +24,8 @@ public class ShooterPivotIOSim implements ShooterPivotIO {
                     MIN_ANGLE.getRadians());
 
     private final ProfiledPIDController controller =
-            new ProfiledPIDController(KP, KI, KD, new TrapezoidProfile.Constraints(CRUISE_VELOCITY_RPS, MAX_ACCEL_RPS2));
-    private final ArmFeedforward feedforward = new ArmFeedforward(KS, KG, KV);
+            new ProfiledPIDController(SIM_KP, SIM_KI, SIM_KD, new TrapezoidProfile.Constraints(CRUISE_VELOCITY_RPS, MAX_ACCEL_RPS2));
+    private final ArmFeedforward feedforward = new ArmFeedforward(SIM_KS, SIM_KG, SIM_KV);
 
     private boolean isClosedLoop;
     private TrapezoidProfile.State goalState;
@@ -38,8 +38,8 @@ public void updateInputs(ShooterPivotIOInputs inputs) {
             appliedVolts =
                     controller.calculate(Units.radiansToRotations(sim.getAngleRads()), goalState)
                             + feedforward.calculate(
-                            Units.rotationsToRadians(controller.getSetpoint().position),
-                            controller.getSetpoint().velocity);
+                                    Units.rotationsToRadians(controller.getSetpoint().position),
+                                    controller.getSetpoint().velocity);
         }
 
         sim.setInputVoltage(appliedVolts);
@@ -58,7 +58,7 @@ public void setPosition(Rotation2d position) {
                 Units.radiansToRotations(sim.getVelocityRadPerSec())
         );
         isClosedLoop = true;
-        goalState = new TrapezoidProfile.State(position.getRadians(), 0);
+        goalState = new TrapezoidProfile.State(position.getRotations(), 0);
     }
 
     @Override