AutoV2 Fixes (#116)

* [fix] motor coefficients

* [av1] autonv1 basics

* code

* Complete Auton Rewrite & Simplification

* Manual Starting State Selection & TestingMode

* Trajectory fixes

* bad solution, but hopefully fixes

* boo

* im sorry

* [roadrunner] DriveConstants update

* Updates to roadrunner tuning

* auton fixes, hopefully

* Fix Auton Trajectories

* Add Moving after Yellow Pixel Delivery

* FTC Dashboard Telemetry

* Blue Side Trajectories

* Telemetry Fix

* Slow trajectory fixes

* things

* Merge Master

* Auton mostly works, we are conisdering a different method of detecting

* AutonV1 Rewrite + AutonV2 Red Only

* AutonV1 Edits + AutonV2 Massive Changes

* Fix blue side trajectories for autov1

* Intake Detection While Driving + New Experimental Sweeping Idea

* Pixel Stack -> Backboard Trajectory WORKS

* woops

* HWC Changes

* things

* Comp Changes!

* V2 changes

* V2 Rewrite

* Auton things ig idk

* woop!

* Mostly functioning auto- inconsistent stack

* idk what

* omar here

* wooh

* minor

* Slide Fix

* things

* working cycles i hope

end

* half functional far

* push

* mostly functioning autos
end

* night

* Merge Master + Uncommited changes

* center auton and mods

* Commit before switch to old

* last omar update hehehehe

* Final Reformat

---------

Co-authored-by: Milo Banks <[email protected]>
Co-authored-by: Om0r <[email protected]>

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/AutonomousV1.java

@@ -8,10 +8,8 @@
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 
-import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.firstinspires.ftc.teamcode.auton.enums.AllianceColor;
 import org.firstinspires.ftc.teamcode.subsystems.HWC;
-import org.firstinspires.ftc.teamcode.subsystems.roadrunner.trajectorysequence.TrajectorySequence;
 
 @Autonomous(name = "AutonomousV1")
 public class AutonomousV1 extends OpMode {
@@ -90,7 +88,7 @@ public void init_loop() {
 
 
         // ------ Set Trajectories based on Alliance Color ------ //
-        switch(allianceColor) {
+        switch (allianceColor) {
             case RED:
                 // ------ Set Robot Start Pose ------ //
                 robot.drive.setPoseEstimate(START_POSE_RED);
@@ -108,7 +106,7 @@ public void init_loop() {
 
                 // Drive to Left Line
                 toDepositLeft = robot.drive.trajectoryBuilder(START_POSE_RED)
-                        .splineToLinearHeading(new Pose2d(8, -39, Math.toRadians(135)), Math.toRadians(135))
+                        .splineToLinearHeading(new Pose2d(7, -39, Math.toRadians(135)), Math.toRadians(135))
                         .build();
 
                 // Drive to Backboard from Center
@@ -159,7 +157,7 @@ public void init_loop() {
 
                 // Drive to Left Line
                 toDepositRight = robot.drive.trajectoryBuilder(START_POSE_BLUE)
-                        .splineToLinearHeading(new Pose2d(8, 39, Math.toRadians(225)), Math.toRadians(225))
+                        .splineToLinearHeading(new Pose2d(7, 39, Math.toRadians(225)), Math.toRadians(225))
                         .build();
 
                 // Drive to Backboard from Center
@@ -169,7 +167,7 @@ public void init_loop() {
 
                 // Drive to Backboard from Right
                 toBackboardFromLeft = robot.drive.trajectoryBuilder(toDepositLeft.end())
-                        .lineToLinearHeading(new Pose2d(49, 42, Math.toRadians(180)))
+                        .lineToLinearHeading(new Pose2d(49, 41, Math.toRadians(180)))
                         .build();
 
                 // Drive to Backboard from Left
@@ -250,9 +248,9 @@ public void loop() {
     }
 
     // ------ State Methods ------ //
-   private void drivingToDepositPurplePixel() {
+    private void drivingToDepositPurplePixel() {
         // ------ Select Trajectory ------ //
-        if(firstRun) {
+        if (firstRun) {
             firstRun = false;
             if (elementLocation == HWC.Location.CENTER) {
                 activeTrajectory = "toDepositCenter";
@@ -271,7 +269,7 @@ private void drivingToDepositPurplePixel() {
             state = State.DEPOSITING_PURPLE_PIXEL;
             firstRun = true;
         }
-   }
+    }
 
     // Deposit Purple Pixel
     private void depositingPurplePixel() {
@@ -287,7 +285,7 @@ private void depositingPurplePixel() {
     // Drive to Backboard
     private void drivingToBackboard() {
         // ------ Select Trajectory ------ //
-        if(firstRun) {
+        if (firstRun) {
             firstRun = false;
             if (elementLocation == HWC.Location.CENTER) {
                 activeTrajectory = "toBackboardFromInitial";
@@ -332,7 +330,7 @@ private void depositingYellowPixel() {
     // Drive to Park
     private void drivingToPark() {
         // ------ Select Trajectory ------ //
-        if(firstRun) {
+        if (firstRun) {
             firstRun = false;
             activeTrajectory = "toParkFromBackboard2";
             if (elementLocation == HWC.Location.CENTER) {
@@ -359,8 +357,8 @@ private void deliver() {
         robot.passoverArmLeft.setPosition(HWC.passoverDeliveryPos);
         robot.passoverArmRight.setPosition(HWC.passoverDeliveryPos);
         robot.wrist.setPosition(HWC.wristDeliveryPos);
-        robot.pulleyLComponent.setTarget(HWC.slidePositions[1] / 4.0);
-        robot.pulleyRComponent.setTarget(HWC.slidePositions[1] / 4.0);
+        robot.pulleyLComponent.setTarget(-170);
+        robot.pulleyRComponent.setTarget(-170);
     }
 
     // Method to move to Intake Position
@@ -371,21 +369,4 @@ private void intake() {
         robot.pulleyLComponent.setTarget(HWC.slidePositions[0]);
         robot.pulleyRComponent.setTarget(HWC.slidePositions[0]);
     }
-
-    // Method to Check Claws & Close
-    private void checkClaws() {
-        // Close Claws when Pixel Detected
-        if (robot.colorLeft.getDistance(DistanceUnit.CM) <= 2) {
-            robot.clawL.setPosition(0.5);
-        }
-
-        if (robot.colorRight.getDistance(DistanceUnit.CM) <= 2) {
-            robot.clawR.setPosition(0.5);
-        }
-
-        // If both Pixels are Detected, Stop Intake
-        if (robot.colorLeft.getDistance(DistanceUnit.CM) <= 1 && robot.colorRight.getDistance(DistanceUnit.CM) <= 1) {
-            robot.intakeMotor.setPower(0);
-        }
-    }
 }