Collision fixing

src/main/java/frc/robot/RobotStates.java

@@ -28,7 +28,8 @@ public class RobotStates {
 
     // intake Triggers
     public static final Trigger visionIntaking = Trigger.kFalse;
-    public static final Trigger intaking = pilot.intake_A.or(visionIntaking, autonSourceIntake);
+    public static final Trigger stationIntaking =
+            pilot.stationIntake_Y.or(visionIntaking, autonSourceIntake);
     public static final Trigger ejecting = pilot.eject_fA;
 
     // score Triggers

src/main/java/frc/robot/coralIntake/CoralIntakeStates.java

@@ -23,7 +23,7 @@ public static void setupDefaultCommand() {
     }
 
     public static void setStates() {
-        intaking.whileTrue(log(intake()));
+        stationIntaking.whileTrue(log(intake()));
         ejecting.whileTrue(log(eject()));
         score.whileTrue(log(intake()));
         handOffAlgae.whileTrue(log(handOffAlgae()));

src/main/java/frc/robot/elbow/Elbow.java

@@ -14,11 +14,13 @@
 import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import frc.robot.Robot;
 import frc.robot.RobotSim;
+import frc.robot.elevator.ElevatorStates;
 import frc.spectrumLib.Rio;
 import frc.spectrumLib.Telemetry;
 import frc.spectrumLib.mechanism.Mechanism;
 import frc.spectrumLib.sim.ArmConfig;
 import frc.spectrumLib.sim.ArmSim;
+import java.util.function.DoubleSupplier;
 import lombok.*;
 
 public class Elbow extends Mechanism {
@@ -183,6 +185,14 @@ public boolean ElbowHasError() {
         return false;
     }
 
+    public Command moveToPercentage(DoubleSupplier percent) {
+        return runHoldElbow()
+                .until(() -> (ElevatorStates.allowedPosition()))
+                .andThen(
+                        run(() -> setMMPosition(() -> percentToRotations(percent)))
+                                .withName(getName() + ".runPosePercentage"));
+    }
+
     // --------------------------------------------------------------------------------
     // Simulation
     // --------------------------------------------------------------------------------

src/main/java/frc/robot/elbow/ElbowStates.java

@@ -20,7 +20,7 @@ public static void setupDefaultCommand() {
     public static void setStates() {
         coastMode.onTrue(log(coastMode()));
         coastMode.onFalse(log(ensureBrakeMode()));
-        intaking.whileTrue(log(coralIntake()));
+        stationIntaking.whileTrue(log(coralIntake()));
         algaeFloorIntake.whileTrue(log(floorIntake()));
         L2Algae.whileTrue(log(l2Algae()));
         L3Algae.whileTrue(log(l3Algae()));

src/main/java/frc/robot/elevator/Elevator.java

@@ -13,6 +13,7 @@
 import frc.spectrumLib.mechanism.Mechanism;
 import frc.spectrumLib.sim.LinearConfig;
 import frc.spectrumLib.sim.LinearSim;
+import java.util.function.DoubleSupplier;
 import lombok.*;
 
 public class Elevator extends Mechanism {
@@ -169,6 +170,19 @@ public Command zeroElevatorRoutine() {
                 .withName("Elevator.zeroElevatorRoutine");
     }
 
+    public Command moveToRotations(DoubleSupplier rotations) {
+        return run(() -> stop())
+                .withName("Elevator.waitForElbow")
+                .until(
+                        () ->
+                                (ElevatorStates.getElbowShoulderPos().getAsDouble() < 50.0)
+                                        || ElevatorStates.getPosition().getAsDouble()
+                                                < rotations.getAsDouble()
+                                        || ElevatorStates.getPosition().getAsDouble()
+                                                > config.getL2())
+                .andThen(run(() -> setMMPosition(rotations)).withName("Elevator.moveToRotations"));
+    }
+
     // --------------------------------------------------------------------------------
     // Simulation
     // --------------------------------------------------------------------------------

src/main/java/frc/robot/elevator/ElevatorStates.java

@@ -9,6 +9,7 @@
 import frc.robot.elevator.Elevator.ElevatorConfig;
 import frc.robot.operator.Operator;
 import frc.robot.pilot.Pilot;
+import frc.robot.shoulder.ShoulderStates;
 import frc.spectrumLib.Telemetry;
 import frc.spectrumLib.TuneValue;
 import java.util.function.DoubleSupplier;
@@ -52,6 +53,27 @@ private static Command runElevator(DoubleSupplier speed) {
         return elevator.runPercentage(speed).withName("Elevator.runElevator");
     }
 
+    public static DoubleSupplier getPosition() {
+        return () -> elevator.getPositionRotations();
+    }
+
+    public static DoubleSupplier getElbowShoulderPos() {
+        double eToSratio = 2.0; // get actual elbow to shoulder length ratio
+        double e = Math.abs(ElbowStates.getPosition().getAsDouble());
+        double s = 100 - Math.abs(ShoulderStates.getPosition().getAsDouble());
+        return () -> (eToSratio * e + s) / 3;
+    }
+
+    public static boolean allowedPosition() {
+        if ((getPosition().getAsDouble() * 100 / config.getL3())
+                        - getElbowShoulderPos().getAsDouble()
+                > 0) {
+            return true;
+        } else {
+            return false;
+        }
+    }
+
     private static Command holdPosition() {
         return elevator.holdPosition().withName("Elevator.holdPosition");
     }

src/main/java/frc/robot/pilot/Pilot.java

@@ -17,7 +17,7 @@ public class Pilot extends Gamepad {
     public final Trigger noFn = fn.not();
 
     // TODO: Finalize Buttons
-    public final Trigger intake_A = Y.and(noFn, teleop);
+    public final Trigger stationIntake_Y = Y.and(noFn, teleop);
     public final Trigger eject_fA = A.and(fn, teleop);
     public final Trigger score_RB = rightBumper.and(noFn, teleop);
     public final Trigger climbPrep_RDP = rightDpad.and(noFn, teleop);

src/main/java/frc/robot/shoulder/ShoulderStates.java

@@ -20,11 +20,11 @@ public static void setupDefaultCommand() {
     public static void setStates() {
         coastMode.onTrue(log(coastMode()).ignoringDisable(true));
         coastMode.onFalse(log(ensureBrakeMode()));
-        intaking.whileTrue(log(coralIntake())); // using intake button to test
+        stationIntaking.whileTrue(log(coralIntake())); // using intake button to test
         score.whileTrue(home());
         algaeFloorIntake.whileTrue(log(floorIntake()));
         L2Algae.whileTrue(log(l2Algae()));
-        L3Algae.whileTrue(tuneShoulder()); // (log(l3Algae()));
+        L3Algae.whileTrue(log(l3Algae()));
         L2Coral.whileTrue(log(l2Coral()));
         L3Coral.whileTrue(log(l3Coral()));
         L4Coral.whileTrue(log(l4Coral()));
@@ -41,6 +41,10 @@ public static Command home() {
         return shoulder.moveToPercentage(config::getHome).withName("Shoulder.home");
     }
 
+    public static DoubleSupplier getPosition() {
+        return () -> shoulder.getPositionPercentage();
+    }
+
     public static Command climbHome() {
         return shoulder.moveToPercentage(config::getClimbHome).withName("Shoulder.climbHome");
     }

src/main/java/frc/robot/twist/TwistStates.java

@@ -18,15 +18,16 @@ public static void setupDefaultCommand() {
     }
 
     public static void setStates() {
-        homeAll.whileTrue(log(home()));
-        intaking.whileTrue(log(coralIntake()));
+        stationIntaking.whileTrue(log(coralIntake()));
+        // algaeFloor.whileTrue(tuneTwist());
         algaeFloorIntake.whileTrue(log(floorIntake()));
         L2Algae.whileTrue(log(l2Algae()));
         L3Algae.whileTrue(log(l3Algae()));
         L2Coral.whileTrue(log(l2Coral()));
         L3Coral.whileTrue(log(l3Coral()));
         L4Coral.whileTrue(log(l4Coral()));
         barge.whileTrue(log(barge()));
+        homeAll.whileTrue(log(home()));
         coastMode.onTrue(log(coastMode()));
         coastMode.onFalse(log(ensureBrakeMode()));
     }