Merge pull request #14 from strykeforce/robot-state

Initial Robot State Subsystem

Author: mwitcpalek

src/main/java/frc/robot/constants/BiscuitConstants.java

@@ -19,14 +19,44 @@
 import edu.wpi.first.units.measure.Angle;
 
 public class BiscuitConstants {
-  // FIXME These are all wrong right now because we don't have any actual info
+  // These are all wrong right now because we don't have any actual info
 
   public static Angle kZero = Rotations.of(42); // Will need to be experimentally determined
   public static int talonID = 3;
   public static double kCloseEnough = 2137473647; // This is a little out of wack.
   public static final Angle kMaxFwd = Rotations.of(100);
   public static final Angle kMaxRev = Rotations.of(-100);
-  // public static Angle Level1 = ;
+
+  // Setpoints
+  // Idle
+  public static final Angle kStowSetpoint = Rotations.of(0.0);
+  public static final Angle kFunnelSetpoint = Rotations.of(0.0);
+  public static final Angle kPrestageSetpoint = Rotations.of(0.0);
+
+  // Algae removal
+  public static final Angle kL2AlgaeSetpoint = Rotations.of(0.0);
+  public static final Angle kL3AlgaeSetpoint = Rotations.of(0.0);
+
+  public static final Angle kL2AlgaeRemovalSetpoint = Rotations.of(0.0);
+  public static final Angle kL3AlgaeRemovalSetpoint = Rotations.of(0.0);
+
+  public static final Angle kSafeAlgaeRemovalSetpoint = Rotations.of(0.0);
+  public static final Angle kSafeAlgaeRemovalRotateSetpoint = Rotations.of(0.0);
+
+  // Coral score
+  public static final Angle kL1CoralSetpoint = Rotations.of(0.0);
+  public static final Angle kL2CoralSetpoint = Rotations.of(0.0);
+  public static final Angle kL3CoralSetpoint = Rotations.of(0.0);
+  public static final Angle kL4CoralSetpoint = Rotations.of(0.0);
+
+  // Algae obtaining
+  public static final Angle kFloorAlgaeSetpoint = Rotations.of(0.0);
+  public static final Angle kMicAlgaeSetpoint = Rotations.of(0.0);
+  public static final Angle kHpAlgaeSetpoint = Rotations.of(0.0);
+
+  // Algae scoring
+  public static final Angle kProcessorSetpoint = Rotations.of(0.0);
+  public static final Angle kBargeSetpoint = Rotations.of(0.0);
 
   // Disables the TalonFXS by setting it's voltage to zero. Not very shocking.
   public static VoltageConfigs disableTalon() {

src/main/java/frc/robot/constants/DriveConstants.java

@@ -13,6 +13,8 @@
 import edu.wpi.first.math.geometry.Translation2d;
 
 public class DriveConstants {
+  public static final double kAlgaeRemovalSpeed = 0;
+
   public static final double kDeadbandAllStick = 0.075;
   public static final double kExpoScaleYawFactor = 0.75;
   public static final double kRateLimitFwdStr = 3.5;

src/main/java/frc/robot/constants/ElevatorConstants.java

@@ -1,5 +1,7 @@
 package frc.robot.constants;
 
+import static edu.wpi.first.units.Units.Rotations;
+
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs;
 import com.ctre.phoenix6.configs.MotionMagicConfigs;
@@ -14,14 +16,15 @@
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.ctre.phoenix6.signals.ReverseLimitSourceValue;
 import com.ctre.phoenix6.signals.ReverseLimitTypeValue;
+import edu.wpi.first.units.measure.Angle;
 
 public class ElevatorConstants {
 
   public static final double kCloseEnoughRotations = 0.0083;
   public static final double kMaxFwd = 0; // TODO all of these fields need to be filled out
   public static final double kMaxRev = 0;
   public static final int kZeroMultiple =
-    0; // some constant to multiply, add by to turn the analog input into a position
+      0; // some constant to multiply, add by to turn the analog input into a position
   public static final double kZeroSpeed = -.05;
   public static final int kZeroCounter = 3;
   public static final double kZeroedThreshhold = .0001;
@@ -32,6 +35,37 @@ public class ElevatorConstants {
 
   public static final double kJogAmount = 0.1;
 
+  // Setpoints
+  // Idle
+  public static final Angle kStowSetpoint = Rotations.of(0.0);
+  public static final Angle kFunnelSetpoint = Rotations.of(0.0);
+  public static final Angle kPrestageSetpoint = Rotations.of(0.0);
+
+  // Algae removal
+  public static final Angle kL2AlgaeSetpoint = Rotations.of(0.0);
+  public static final Angle kL3AlgaeSetpoint = Rotations.of(0.0);
+
+  public static final Angle kL2AlgaeRemovalSetpoint = Rotations.of(0.0);
+  public static final Angle kL3AlgaeRemovalSetpoint = Rotations.of(0.0);
+
+  public static final Angle kSafeAlgaeRemovalSetpoint = Rotations.of(0.0);
+  public static final Angle kSafeAlgaeRemovalRotateSetpoint = Rotations.of(0.0);
+
+  // Coral score
+  public static final Angle kL1CoralSetpoint = Rotations.of(0.0);
+  public static final Angle kL2CoralSetpoint = Rotations.of(0.0);
+  public static final Angle kL3CoralSetpoint = Rotations.of(0.0);
+  public static final Angle kL4CoralSetpoint = Rotations.of(0.0);
+
+  // Algae obtaining
+  public static final Angle kFloorAlgaeSetpoint = Rotations.of(0.0);
+  public static final Angle kMicAlgaeSetpoint = Rotations.of(0.0);
+  public static final Angle kHpAlgaeSetpoint = Rotations.of(0.0);
+
+  // Algae scoring
+  public static final Angle kProcessorSetpoint = Rotations.of(0.0);
+  public static final Angle kBargeSetpoint = Rotations.of(0.0);
+
   public static TalonFXConfiguration getBothFXConfig() {
     TalonFXConfiguration fxConfig = new TalonFXConfiguration();
 

src/main/java/frc/robot/constants/RobotStateConstants.java

@@ -2,4 +2,8 @@
 
 public class RobotStateConstants {
   public static final double[] kNodeAngles = {0.0, 60.0, 120.0, 180.0, -120.0, -60.0};
+  public static final double kAlgaeRetreatDistance = 0;
+
+  public static final double kRedBargeSafeX = 9.5;
+  public static final double kBlueBargeSafeX = 8;
 }

src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java

@@ -1,3 +1,26 @@
 package frc.robot.subsystems.algae;
 
-public class AlgaeSubsystem {}
+public class AlgaeSubsystem {
+
+  public static final String kStowSpeed = null;
+
+  public boolean hasAlgae() {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'hasAlgae'");
+  }
+
+  public void intake() {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'intake'");
+  }
+
+  public void scoreProcessor() {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'scoreProcessor'");
+  }
+
+  public void scoreBarge() {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'scoreBarge'");
+  }
+}

src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java

@@ -4,33 +4,47 @@
 
 import static edu.wpi.first.units.Units.Rotations;
 
-import edu.wpi.first.units.measure.*;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
 import frc.robot.constants.BiscuitConstants;
+import frc.robot.standards.ClosedLoopPosSubsystem;
 import java.util.Set;
 import org.littletonrobotics.junction.Logger;
 import org.strykeforce.telemetry.TelemetryService;
 import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
 import org.strykeforce.telemetry.measurable.Measure;
 
-public class BiscuitSubsystem extends MeasurableSubsystem {
+public class BiscuitSubsystem extends MeasurableSubsystem implements ClosedLoopPosSubsystem {
 
   private BiscuitIO io;
   private BiscuitIOInputsAutoLogged inputs = new BiscuitIOInputsAutoLogged();
   private Angle setPoint;
 
+  @Override
   public void setPosition(Angle position) {
     io.setPosition(position);
     setPoint = position;
   }
 
-  public Angle getPosition(Angle position) {
+  @Override
+  public Angle getPosition() {
     return inputs.position;
   }
 
   public AngularVelocity getVelocity(AngularVelocity velocity) {
     return inputs.velocity;
   }
 
+  @Override
+  public void zero() {
+    io.zero();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return setPoint.minus(inputs.position).abs(Rotations) < BiscuitConstants.kCloseEnough;
+  }
+
   @Override
   public void periodic() {
     io.updateInputs(inputs);
@@ -45,18 +59,10 @@ public void registerWith(TelemetryService telemetry) {
     io.registerWith(telemetry);
   }
 
-  public boolean isFinished() {
-    return setPoint.minus(inputs.position).abs(Rotations) <= BiscuitConstants.kCloseEnough;
-  }
-
   @Override
   public Set<Measure> getMeasures() {
     return Set.of(
         new Measure("Is Biscuit Finished", () -> isFinished() ? 1.0 : 0.0),
         new Measure("Biscuit Set Point", () -> setPoint.in(Rotations)));
   }
-
-  public void zero() {
-    io.zero();
-  }
 }

src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java

@@ -1,3 +1,19 @@
 package frc.robot.subsystems.climb;
 
-public class ClimbSubsystem {}
+public class ClimbSubsystem {
+
+  public boolean isFinished() {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'isFinished'");
+  }
+
+  public void prepClimb() {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'prepClimb'");
+  }
+
+  public void climb() {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'climb'");
+  }
+}

src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java

@@ -65,6 +65,17 @@ public boolean isExitBeamBroken() {
     return inputs.isRevBeamBroken;
   }
 
+  public boolean hasCoral() {
+    switch (curState) {
+      case CORAL_LOADING, HAS_CORAL, EJECTING -> {
+        return true;
+      }
+      default -> {
+        return false;
+      }
+    }
+  }
+
   public void intake() {
     io.enableRevLimitSwitch(true);
     setSpeed(CoralConstants.kIntakingSpeed);

src/main/java/frc/robot/subsystems/elevator/ElevatorIOFX.java

@@ -55,9 +55,7 @@ public ElevatorIOFX() {
   public void updateInputs(ElevatorIOInputs inputs) {
     BaseStatusSignal.refreshAll(currVelocity, currPosition);
     inputs.velocity = currVelocity.getValueAsDouble();
-    inputs.position =
-        currPosition
-            .getValueAsDouble();
+    inputs.position = currPosition.getValueAsDouble();
   }
 
   @Override

src/main/java/frc/robot/subsystems/funnel/FunnelSubsystem.java

@@ -0,0 +1,9 @@
+package frc.robot.subsystems.funnel;
+
+public class FunnelSubsystem {
+
+  public boolean hasCoral() {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'hasCoral'");
+  }
+}