added algae and modified positions

Author: arsamulyak

src/main/java/wmironpatriots/Robot.java

@@ -263,6 +263,35 @@ public Robot() {
         .rightBumper()
         .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>();

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

@@ -28,10 +28,14 @@ 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

@@ -27,9 +27,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;