added tail comp io logging

FIRSTTeam6423 · Feb 20, 2025 · b3be924 · b3be924
1 parent 8fd07fe
commit b3be924
Show file tree

Hide file tree

Showing 3 changed files with 81 additions and 4 deletions.
diff --git a/src/main/java/wmironpatriots/subsystems/elevator/Elevator.java b/src/main/java/wmironpatriots/subsystems/elevator/Elevator.java
@@ -28,6 +28,7 @@ public abstract class Elevator extends SubsystemBase {
   public static final double POSE_ALGAE_L = 0.0; // Remove algae low
 
   // Scoring poses
+  // TODO CONVERT POSES TO RADIANS
   public static final double POSE_L2 = 3.16;
   public static final double POSE_L3 = 10.81;
   public static final double POSE_L4 = 24;

diff --git a/src/main/java/wmironpatriots/subsystems/tail/Tail.java b/src/main/java/wmironpatriots/subsystems/tail/Tail.java
@@ -1,5 +1,6 @@
 package wmironpatriots.subsystems.tail;
 
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import monologue.Annotations.Log;
 
@@ -13,23 +14,64 @@ public abstract class Tail extends SubsystemBase {
     public static final double CURRENT_LIMIT = 0.0;
 
     /** LOGGED VALUES */
+    @Log protected boolean isZeroed = false;
+
     @Log protected boolean pivotMotorOk = false;
     @Log protected boolean rollerMotorOk = false;
 
-    @Log protected boolean beamUno = false;
-    @Log protected boolean beamDos = false;
+    @Log protected boolean beamUnoTriggered = false;
+    @Log protected boolean beamDosTriggered = false;
 
     @Log protected double pivotSetpointRads;
     @Log protected double pivotPoseRads;
     @Log protected double pivotVelRPM;
     @Log protected double pivotAppliedVolts;
     @Log protected double pivotSupplyCurrentAmps;
-    @Log protected double pivotTorqueCurrentAmps;
 
     @Log protected double rollerVelRPM;
     @Log protected double rollerAppliedVolts;
     @Log protected double rollerSupplyCurrentAmps;
-    @Log protected double rollerTorqueCurrentAmps;
+
+    /** Runs target position in radians from current zeroed pose */
+    public Command runTargetPoseCommand(double pose) {
+        return this.run(() -> {
+            pivotSetpointRads = pose;
+            runPivotSetpoint(pose);
+        });
+    }
+
+    /** Runs rollers at specific speed */
+    public Command runRollersCommand(double speed) {
+        return this.run(() -> {
+            runRollerSpeed(speed);
+        });
+    }
+
+    /** Zeroes tail pivot at current pose */
+    public Command zeroPoseCommmand() {
+        return this.run(() -> {
+            isZeroed = true;
+            resetEncoderPose();
+        });
+    }
+
+    /** Runs pivot backwards until current spikes above threshold */
+    public Command runPoseZeroingCommand() {
+        return this.run(() -> runPivotVolts(0.5))
+            .until(() -> pivotSupplyCurrentAmps > 20.0)
+            .finallyDo(
+                (interrupted) -> {
+                    runPivotVolts(0.0);
+                    resetEncoderPose();
+                    isZeroed = true;
+                }
+            );
+    }
+
+    /** Checks if both tail beambreaks are triggered */
+    public boolean hasCoral() {
+        return beamUnoTriggered && beamDosTriggered;
+    }
 
     /** HARDWARE METHODS */
     /** Run pivot voltage */
@@ -49,6 +91,9 @@ private void resetEncoderPose() {
         setEncoderPose(0.0);
     }
 
+    /** Stop pivot motor */
+    protected abstract void stopPivot();
+
     /** Stop roller motor */
     protected abstract void stopRollers();
 

diff --git a/src/main/java/wmironpatriots/subsystems/tail/TailIOComp.java b/src/main/java/wmironpatriots/subsystems/tail/TailIOComp.java
@@ -10,16 +10,23 @@
 import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
+import edu.wpi.first.wpilibj.DigitalOutput;
+
 public class TailIOComp extends Tail {
     private final SparkMax pivot, roller;
     private final SparkMaxConfig pivotConf, rollerConf;
 
+    private final DigitalOutput beamUno, beamDos;
+
     private final SparkClosedLoopController pivotFeedback;
 
     public TailIOComp() {
         pivot = new SparkMax(1, MotorType.kBrushless);
         roller = new SparkMax(2, MotorType.kBrushless);
 
+        beamUno = new DigitalOutput(0);
+        beamDos = new DigitalOutput(0); // TODO SET CHANNELS
+
         // Configure pivot
         pivotConf = new SparkMaxConfig();
 
@@ -61,6 +68,25 @@ public TailIOComp() {
         pivotFeedback = pivot.getClosedLoopController();
     }
 
+    @Override
+    public void periodic() {
+        super.periodic();
+        pivotMotorOk = pivot.hasStickyFault();
+        rollerMotorOk = roller.hasStickyFault();
+
+        beamUnoTriggered = beamUno.get();
+        beamDosTriggered = beamDos.get();
+
+        pivotPoseRads = pivot.getEncoder().getPosition();
+        pivotVelRPM = pivot.getEncoder().getVelocity();
+        pivotAppliedVolts = pivot.getAppliedOutput() * pivot.getBusVoltage();
+        pivotSupplyCurrentAmps = pivot.getOutputCurrent();
+
+        rollerVelRPM = roller.getEncoder().getVelocity();
+        rollerAppliedVolts = roller.getAppliedOutput() * roller.getBusVoltage();
+        rollerSupplyCurrentAmps = roller.getOutputCurrent();
+    }
+
     @Override
     protected void runPivotVolts(double volts) {
         pivot.setVoltage(volts);
@@ -81,6 +107,11 @@ protected void setEncoderPose(double poseMeters) {
         pivot.getEncoder().setPosition(poseMeters);
     }
 
+    @Override
+    protected void stopPivot() {
+        pivot.stopMotor();
+    }
+
     @Override
     protected void stopRollers() {
         roller.stopMotor();