feat: grabber and sim yay

flamingchickens1540 · Jan 28, 2025 · 86a7175 · 86a7175
1 parent 8e967c2
commit 86a7175
Show file tree

Hide file tree

Showing 5 changed files with 60 additions and 43 deletions.
diff --git a/src/main/java/org/team1540/robot2025/Constants.java b/src/main/java/org/team1540/robot2025/Constants.java
@@ -14,6 +14,8 @@ public final class Constants {
     private static final Mode simMode = Mode.SIM;
     public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode;
 
+    public static final double LOOP_PERIOD_SECS = 0.02;
+
     private static final boolean tuningMode = true;
 
     public static boolean isTuningMode() {

diff --git a/src/main/java/org/team1540/robot2025/subsystems/grabber/Grabber.java b/src/main/java/org/team1540/robot2025/subsystems/grabber/Grabber.java
@@ -1,22 +1,15 @@
 package org.team1540.robot2025.subsystems.grabber;
 
-import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.Logger;
 
-import static org.team1540.robot2025.subsystems.grabber.GrabberConstants.GRABBER_GEAR_RATIO;
-
-
 public class Grabber extends SubsystemBase {
     private final GrabberIO io;
     private final GrabberIOInputsAutoLogged inputs = new GrabberIOInputsAutoLogged();
 
-    private final PIDController positionPID = new PIDController(1.540, 0.0, 0.0);
-    private boolean isClosedLoop = false;
-
     private Grabber(GrabberIO io) {
         this.io = io;
     }
@@ -38,25 +31,15 @@ public void periodic() {
         io.updateInputs(inputs);
         Logger.processInputs("Grabber", inputs);
 
-        if (isClosedLoop) {
-            io.setVoltage(positionPID.calculate(inputs.positionRots));
-        }
-
         if (RobotState.isDisabled()) {
             stop();
         }
     }
 
     public void setPercent(double percent) {
-        isClosedLoop = false;
         io.setVoltage(percent * 12.0);
     }
 
-    public void moveRotations(double rotations) {
-        isClosedLoop = true;
-        positionPID.setSetpoint(GRABBER_GEAR_RATIO * rotations + inputs.positionRots);
-    }
-
     public boolean hasCoral() {
         return inputs.hasCoral;
     }
@@ -65,11 +48,15 @@ public void stop() {
         io.setVoltage(0.0);
     }
 
+    public double getCurrent() {
+        return inputs.motorCurrentAmps;
+    }
+
+    public void setBrakeMode(boolean isBrakeMode) {
+        io.setBrakeMode(isBrakeMode);
+    }
+
     public Command commandRun(double percent) {
-        return Commands.startEnd(
-                () -> this.setPercent(percent),
-                () -> this.setPercent(0),
-                this
-        );
+        return Commands.startEnd(() -> this.setPercent(percent), () -> this.setPercent(0), this);
     }
 }
diff --git a/src/main/java/org/team1540/robot2025/subsystems/grabber/GrabberConstants.java b/src/main/java/org/team1540/robot2025/subsystems/grabber/GrabberConstants.java
@@ -1,9 +1,12 @@
 package org.team1540.robot2025.subsystems.grabber;
 
 public class GrabberConstants {
-    public static final int GRABBER_ID = 13; // TODO: Fix this
+    // TODO: Fix this
+    public static final int GRABBER_ID = 13;
     public static final int BEFORE_BREAK_CANDI_ID = 1;
     public static final int AFTER_BREAK_CANDI_ID = 2;
     public static final String CANDI_BUS = "canivore";
+
+    public static final double GRABBER_MOI = 0.025;
     public static final double GRABBER_GEAR_RATIO = 24.0 / 36.0;
 }
diff --git a/src/main/java/org/team1540/robot2025/subsystems/grabber/GrabberIOSim.java b/src/main/java/org/team1540/robot2025/subsystems/grabber/GrabberIOSim.java
@@ -0,0 +1,37 @@
+package org.team1540.robot2025.subsystems.grabber;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+
+import static org.team1540.robot2025.Constants.LOOP_PERIOD_SECS;
+import static org.team1540.robot2025.subsystems.grabber.GrabberConstants.*;
+
+public class GrabberIOSim implements GrabberIO {
+    private final DCMotorSim sim = new DCMotorSim(
+            LinearSystemId.createDCMotorSystem(
+                    DCMotor.getFalcon500Foc(1),
+                    GRABBER_MOI,
+                    GRABBER_GEAR_RATIO
+                    ),
+            DCMotor.getFalcon500Foc(1)
+    );
+
+    private double motorVoltage = 0.0;
+
+    @Override
+    public void updateInputs(GrabberIOInputs inputs) {
+        sim.setInputVoltage(motorVoltage);
+        sim.update(LOOP_PERIOD_SECS);
+        inputs.motorCurrentAmps = sim.getCurrentDrawAmps();
+        inputs.motorAppliedVolts = motorVoltage;
+        inputs.velocityRPM = sim.getAngularVelocityRPM();
+        inputs.positionRots = sim.getAngularPositionRotations();
+    }
+
+    @Override
+    public void setVoltage(double volts) {
+        motorVoltage = MathUtil.clamp(volts, -12.0, 12.0);
+    }
+}
diff --git a/src/main/java/org/team1540/robot2025/subsystems/grabber/GrabberIOTalonFX.java b/src/main/java/org/team1540/robot2025/subsystems/grabber/GrabberIOTalonFX.java
@@ -1,5 +1,7 @@
 package org.team1540.robot2025.subsystems.grabber;
 
+import static org.team1540.robot2025.subsystems.grabber.GrabberConstants.*;
+
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.CANdiConfiguration;
@@ -12,12 +14,9 @@
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.units.measure.*;
 
-import static org.team1540.robot2025.subsystems.grabber.GrabberConstants.*;
-
-
 public class GrabberIOTalonFX implements GrabberIO {
     private final TalonFX motor = new TalonFX(GRABBER_ID);
-    //TODO: Fix candi configuration stuff
+    // TODO: Fix candi configuration stuff
     private final CANdi beforeBreak = new CANdi(BEFORE_BREAK_CANDI_ID, CANDI_BUS);
     private final CANdi afterBreak = new CANdi(AFTER_BREAK_CANDI_ID, CANDI_BUS);
 
@@ -37,42 +36,31 @@ public GrabberIOTalonFX() {
         motorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
         motorConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast;
         motorConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
-        //TODO: Change all this maybe
+        // TODO: Change all this maybe
         motorConfig.CurrentLimits.SupplyCurrentLimit = 20;
         motorConfig.CurrentLimits.SupplyCurrentLowerLimit = 20;
         motorConfig.CurrentLimits.SupplyCurrentLowerTime = 0.1;
 
         motor.getConfigurator().apply(motorConfig);
 
         BaseStatusSignal.setUpdateFrequencyForAll(
-                50.0,
-                motorCurrent,
-                motorVoltage,
-                motorTemp,
-                motorVelocity,
-                motorAngle
-        );
+                50.0, motorCurrent, motorVoltage, motorTemp, motorVelocity, motorAngle);
 
         motor.optimizeBusUtilization();
     }
 
     @Override
     public void updateInputs(GrabberIOInputs inputs) {
-        BaseStatusSignal.refreshAll(
-                motorCurrent,
-                motorVoltage,
-                motorTemp,
-                motorVelocity,
-                motorAngle
-        );
+        BaseStatusSignal.refreshAll(motorCurrent, motorVoltage, motorTemp, motorVelocity, motorAngle);
 
         inputs.motorCurrentAmps = motorCurrent.getValueAsDouble();
         inputs.motorAppliedVolts = motorVoltage.getValueAsDouble();
         inputs.motorTempCelsius = motorTemp.getValueAsDouble();
         inputs.velocityRPM = motorVelocity.getValueAsDouble();
         inputs.positionRots = motorAngle.getValueAsDouble();
 
-        inputs.hasCoral = beforeBreak.getS1Closed().getValue() && afterBreak.getS2Closed().getValue(); //TODO: fix beam break logic
+        inputs.hasCoral = beforeBreak.getS1Closed().getValue()
+                && afterBreak.getS2Closed().getValue(); // TODO: fix beam break logic
     }
 
     @Override