Merge branch '1.0.0' of https://github.com/FIRSTTeam6423/CompetitionRobot2025 into 1.0.0

Author: dabeycorn

src/main/java/wmironpatriots/Robot.java

@@ -225,7 +225,7 @@ public Robot() {
 
     // // * ELEVATOR LEVEL 3 COMMAND
     operatorController
-        .b()
+        .y()
         .whileTrue(
             tail.setTargetPoseCmmd(Tail.POSE_MOVE_ANGLE)
                 .until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
@@ -235,11 +235,11 @@ public Robot() {
 
     // // * ELEVATOR LEVEL 4 COMMAND
     operatorController
-        .y()
+        .b()
         .whileTrue(
             tail.setTargetPoseCmmd(Tail.POSE_MOVE_ANGLE)
                 .until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
-                .andThen(tail.setTargetPoseCmmd(Tail.POSE_L4)))
+                .andThen(tail.setTargetPoseCmmd(Tail.POSE_L4).until(joystick.leftBumper()).andThen(tail.setTargetPoseCmmd(Tail.POSE_IN_ANGLE))))
         .whileTrue(
             elevator.setTargetPoseCmmd(Elevator.POSE_L4).onlyIf(() -> tail.inSetpointRange()));
 
@@ -253,6 +253,16 @@ public Robot() {
         .whileFalse(tail.setRollerSpeedCmmd(0))
         .whileFalse(chute.runChuteSpeedCmmd(0.0));
 
+    // * AUTO CENTERING COMMAND
+
+    operatorController
+        .povRight()
+        .onTrue(
+            tail.setRollerSpeedCmmd(.5)
+                .alongWith(chute.runChuteSpeedCmmd(-.1))
+                .until(() -> tail.beamTripped = true)
+                .andThen(tail.setRollerTimecmmd(.5, 1)).alongWith(chute.runChuteSpeedCmmd(0)));
+
     // // * OUTTAKING CORAL COMMAND
     operatorController
         .rightBumper()
@@ -269,6 +279,42 @@ public Robot() {
         .whileTrue(tail.setRollerSpeedCmmd(Tail.OUTPUTTING_SPEEDS))
         .onFalse(tail.setRollerSpeedCmmd(0.0));
 
+    // * ALGAE DESCORING
+
+    operatorController
+        .povUp()
+        .whileTrue(
+            tail.setRollerSpeedCmmd(1)
+                .alongWith(
+                    tail.setTargetPoseCmmd(Tail.POSE_MOVE_ANGLE)
+                        .until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
+                        .andThen(tail.setTargetPoseCmmd(Tail.POSE_ALGAE_HIGH))))
+        .whileTrue(
+            elevator
+                .setTargetPoseCmmd(Elevator.POSE_ALGAE_HIGH)
+                .onlyIf(() -> tail.inSetpointRange()))
+        .whileFalse(tail.setRollerSpeedCmmd(0));
+
+    operatorController
+        .povDown()
+        .whileTrue(
+            tail.setRollerSpeedCmmd(1)
+                .alongWith(
+                    tail.setTargetPoseCmmd(Tail.POSE_MOVE_ANGLE)
+                        .until(() -> tail.inSetpointRange() && elevator.inSetpointRange())
+                        .andThen(tail.setTargetPoseCmmd(Tail.POSE_ALGAE_LOW))))
+        .whileTrue(
+            elevator
+                .setTargetPoseCmmd(Elevator.POSE_ALGAE_LOW)
+                .onlyIf(() -> tail.inSetpointRange()))
+        .whileFalse(tail.setRollerSpeedCmmd(0));
+
+    operatorController
+        .povDown()
+        .whileTrue(
+            tail.setTargetPoseCmmd(Tail.POSE_ALGAE_LOW).alongWith(tail.setRollerSpeedCmmd(1)))
+        .whileFalse(tail.setRollerSpeedCmmd(0));
+
     // * SUPERSTRUCTURE INIT
     Map<Requests, Trigger> triggerMap = new HashMap<Superstructure.Requests, Trigger>();
 
@@ -308,7 +354,7 @@ private void configureBindings() {
                             * MaxAngularRate) // Drive counterclockwise with negative X (left)
             ));
 
-    joystick.a().whileTrue(drivetrain.applyRequest(() -> brake));
+    //joystick.a().whileTrue(drivetrain.applyRequest(() -> brake));
     joystick
         .b()
         .whileTrue(
@@ -325,7 +371,7 @@ private void configureBindings() {
     // joystick.povUp().and(joystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse));
 
     // reset the field-centric heading on left bumper press
-    joystick.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
+    joystick.a().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
 
     drivetrain.registerTelemetry(logger::telemeterize);
   }

src/main/java/wmironpatriots/subsystems/elevator/Elevator.java

@@ -28,10 +28,13 @@ public abstract class Elevator extends SubsystemBase implements Logged {
   public static final double POSE_MAX_CARRIAGE_STAGE_ONE = 1.39903980829;
 
   public static final double POSE_L1 = 0.0;
-  public static final double POSE_L2 = 3.16;
-  public static final double POSE_L3 = 7;
+  public static final double POSE_L2 = (5.5 / 0.87835 / 3.14159 / 2 * 3); // 3.16;
+  public static final double POSE_L3 = (13.5 / 0.87835 / 3.14159 / 2 * 3); // 7;
   public static final double POSE_L4 = 12.75;
 
+  public static final double POSE_ALGAE_HIGH = (10.5 / 0.87835 / 3.14159 / 2 * 3);
+  public static final double POSE_ALGAE_LOW = (2.5 / 0.87835 / 3.14159 / 2 * 3);
+
   // Visualizer constants
   public static final double MAX_ELEVATOR_HEIGHT_INCHES = 57;
   public static final double STAGE_ZERO_HEIGHT_INCHES = 32;

src/main/java/wmironpatriots/subsystems/tail/Tail.java

@@ -6,6 +6,7 @@
 
 package wmironpatriots.subsystems.tail;
 
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import monologue.Annotations.Log;
 import wmironpatriots.util.mechanismUtil.LoggedSubsystem;
@@ -27,9 +28,12 @@ public abstract class Tail implements LoggedSubsystem {
   public static final double POSE_MOVE_ANGLE = 8.2;
 
   public static final double POSE_L1 = 5.56;
-  public static final double POSE_L2 = 5;
-  public static final double POSE_L3 = 5.56;
-  public static final double POSE_L4 = 4;
+  public static final double POSE_L2 = (20 * 5 / 36); // 5;
+  public static final double POSE_L3 = (20 * 5 / 36); // 5.56;
+  public static final double POSE_L4 = (30 * 5 / 36); // 4;
+
+  public static final double POSE_ALGAE_HIGH = (70 * 5 / 36); // uses formula
+  public static final double POSE_ALGAE_LOW = (70 * 5 / 36); // uses formula
 
   // Roller speeds
   public static final double INTAKING_SPEEDS = 2;
@@ -48,7 +52,7 @@ public abstract class Tail implements LoggedSubsystem {
   @Log protected boolean pivotMotorOk = false;
   @Log protected boolean rollerMotorOk = false;
 
-  @Log protected boolean beamTriggered = false;
+  @Log public boolean beamTripped = true;
 
   @Log protected double pivotSetpointRevs;
   @Log protected double pivotPoseRevs;
@@ -60,6 +64,8 @@ public abstract class Tail implements LoggedSubsystem {
   @Log protected double rollerAppliedVolts;
   @Log protected double rollerSupplyCurrentAmps;
 
+  Timer timer = new Timer();
+
   /** Runs target position in radians from current zeroed pose */
   public Command setTargetPoseCmmd(double poseRes) {
     return this.run(
@@ -77,6 +83,17 @@ public Command setRollerSpeedCmmd(double speed) {
         });
   }
 
+  public Command setRollerTimecmmd(double speed, double time){
+    return this.run(()->{
+      runRollerSpeed(speed);
+      timer.reset();
+      timer.start();
+      if (timer.hasElapsed(1)){
+        runRollerSpeed(0);
+      }
+    });
+  }
+
   /** Zeroes tail pivot at current pose */
   public Command zeroPoseCmmd() {
     return this.run(
@@ -99,6 +116,13 @@ public Command runPoseZeroingCmmd() {
             });
   }
 
+  public Command setRollerPositionCommand(double revs) {
+    return this.run(
+        () -> {
+          setRollerPosition(revs);
+        });
+  }
+
   /** Stop all elevator motor input */
   public Command stopMotorInputCmmd() {
     return this.runOnce(() -> runPivotVolts(0.0));
@@ -126,7 +150,7 @@ public boolean inSetpointRange() {
 
   /** Checks if both tail beambreaks are triggered */
   public boolean hasCoral() {
-    return beamTriggered;
+    return beamTripped;
   }
 
   /** HARDWARE METHODS */
@@ -139,6 +163,8 @@ public boolean hasCoral() {
   /** Run roller motor with speed request */
   protected abstract void runRollerSpeed(double speed);
 
+  protected abstract void setRollerPosition(double revs);
+
   /** Reset encoder to specific pose in rads */
   protected abstract void setEncoderPose(double poseRevs);
 

src/main/java/wmironpatriots/subsystems/tail/TailIOComp.java

@@ -14,17 +14,23 @@
 import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkMaxConfig;
+import edu.wpi.first.wpilibj.DigitalInput;
 
 public class TailIOComp extends Tail {
   private final TalonFX pivot;
   private final SparkMax roller;
 
+  private final RelativeEncoder encoder;
+
+  private final DigitalInput beam;
+
   private final TalonFXConfiguration pivotConf;
   private final SparkMaxConfig rollerConf;
 
@@ -37,6 +43,10 @@ public TailIOComp() {
     pivot = new TalonFX(13, "rio");
     roller = new SparkMax(1, MotorType.kBrushless);
 
+    beam = new DigitalInput(8);
+
+    encoder = roller.getEncoder();
+
     pivotConf = new TalonFXConfiguration();
     pivotConf.MotorOutput.NeutralMode = NeutralModeValue.Brake;
     pivotConf.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
@@ -72,6 +82,8 @@ public TailIOComp() {
   public void periodic() {
     pivotMotorOk = BaseStatusSignal.refreshAll(sigPose, sigVel, sigVolts, sigCurrent).isOK();
 
+    beamTripped = !beam.get();
+
     pivotPoseRevs = sigPose.getValueAsDouble();
     pivotVelRPM = sigPose.getValueAsDouble();
     pivotAppliedVolts = sigVolts.getValueAsDouble();
@@ -96,6 +108,10 @@ protected void runRollerSpeed(double speed) {
     roller.setVoltage(speed);
   }
 
+  protected void setRollerPosition(double poseRevs) {
+    encoder.setPosition(poseRevs);
+  }
+
   @Override
   protected void setEncoderPose(double poseRevs) {
     pivot.setPosition(poseRevs);

src/main/java/wmironpatriots/subsystems/tail/TailIOSim.java

@@ -45,8 +45,9 @@ public void periodic() {
     pivotMotorOk = true;
     rollerMotorOk = true;
 
-    beamTriggered = true;
+    beamTripped = true;
 
+    
     pivotPoseRevs = tailSim.getAngleRads();
     pivotVelRPM = Units.radiansPerSecondToRotationsPerMinute(tailSim.getAngleRads());
     pivotAppliedVolts = pivotInputVoltage;
@@ -83,4 +84,10 @@ protected void stopRollers() {} // no rollers
 
   @Override
   protected void pivotCoastingEnabled(boolean enabled) {} // Sim motors can't coast
+
+  @Override
+  protected void setRollerPosition(double revs) {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'setRollerPosition'");
+  }
 }