From afc4ae73165c76d1b4c8f8f798076efae152b843 Mon Sep 17 00:00:00 2001
From: ImposterVarunoverlord <rajagopalva@student.saviochs.org>
Date: Thu, 30 Nov 2023 19:47:38 -0600
Subject: [PATCH] new push

---
 .../ftc/teamcode/hardware/Arm.java            |  6 ++---
 .../ftc/teamcode/hardware/Robot.java          |  2 +-
 .../BlueRightAuto_Drivepathonly_.java         | 24 ++++++++++---------
 3 files changed, 17 insertions(+), 15 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java
index 25ea42f..b20b986 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java
@@ -39,12 +39,12 @@ public class Arm {
         setWristPos(Position.INTAKE);
     }
 
-    public static double doorOpenpos = 0.5;
+    public static double doorOpenpos = 0.1;
     public static double doorClosePos = 0.85;
 
-    public static double armStart = 0.22;
+    public static double armStart = 0.15;
     public static double armScore = 1;
-    public static double wristStart = 0.9;
+    public static double wristStart = 0.93;
     public static double wristScore = 0.98;
 
     public void setArmPos(Position pos) {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
index 6d9e1ee..2f61aa6 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
@@ -71,7 +71,7 @@ public class Robot {
                 break;
                 //Ind_sleeper
             case(1):
-                if (runTime > macroStartTime + 2) {
+                if (runTime > macroStartTime + 1) {
                     macroState ++;
                 }
                 break;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueRightAuto_Drivepathonly_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueRightAuto_Drivepathonly_.java
index 4c6c6a4..9df0943 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueRightAuto_Drivepathonly_.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueRightAuto_Drivepathonly_.java
@@ -13,14 +13,14 @@ public class BlueRightAuto_Drivepathonly_ extends LinearOpMode {
     public void runOpMode() throws InterruptedException {
         SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
 //      add more trajectories here
-        Pose2d start = new Pose2d(-24, 61.5, Math.toRadians(-90));
+        Pose2d start = new Pose2d(-36, 61.5, Math.toRadians(-90));
 
 //        Pose2d dropLeft = new Pose2d(24, 60, Math.toRadians(-90));
         Pose2d dropMiddle = new Pose2d(-36, 40.5, Math.toRadians(-90));
 //
-        Pose2d nine = new Pose2d(-36, 40.5, Math.toRadians(-180));
+    //    Pose2d nine = new Pose2d(-36, 40.5, Math.toRadians(-180));
 
-        Pose2d stack1 = new Pose2d(-68, 40.5, Math.toRadians(-180));
+        Pose2d stack1 = new Pose2d(-50, 40.5, Math.toRadians(-180));
 
         Pose2d dropMiddle2 = new Pose2d(-36, 40.5, Math.toRadians(-180));
 
@@ -28,9 +28,9 @@ public class BlueRightAuto_Drivepathonly_ extends LinearOpMode {
 
         Pose2d bmid2 = new Pose2d(36, 5, Math.toRadians(-180));
 
-        Pose2d alimb = new Pose2d(60, 40.5, Math.toRadians(-180));
+        Pose2d alimb = new Pose2d(54, 40.5, Math.toRadians(-180));
 
-        Pose2d score = new Pose2d(60, 36, Math.toRadians(180));
+        Pose2d score = new Pose2d(54, 36, Math.toRadians(180));
 
         drive.setPoseEstimate(start);
 //      add this per trajectories
@@ -39,18 +39,18 @@ public class BlueRightAuto_Drivepathonly_ extends LinearOpMode {
                 .build();
 
         Trajectory mid_drop = drive.trajectoryBuilder(dropMiddle)
-                .lineToLinearHeading(nine)
-                .build();
-        Trajectory stack = drive.trajectoryBuilder(nine)
                 .lineToLinearHeading(stack1)
                 .build();
-        Trajectory back_to_mid = drive.trajectoryBuilder(start)
+        Trajectory back_to_mid = drive.trajectoryBuilder(stack1)
                 .lineToLinearHeading(dropMiddle2)
                 .build();
         Trajectory front_gate = drive.trajectoryBuilder(dropMiddle2)
                 .lineToLinearHeading(bmid)
                 .build();
-        Trajectory front_gate_pt_2 = drive.trajectoryBuilder(bmid2)
+        Trajectory front_gate_pt_2 = drive.trajectoryBuilder(bmid)
+                .lineToLinearHeading(bmid2)
+                .build();
+        Trajectory front_gate_pt_3 = drive.trajectoryBuilder(bmid2)
                 .lineToLinearHeading(alimb)
                 .build();
         Trajectory about_to_score = drive.trajectoryBuilder(alimb)
@@ -66,11 +66,13 @@ public class BlueRightAuto_Drivepathonly_ extends LinearOpMode {
         if(isStopRequested()) return;
 //      add this per trajectories
         drive.followTrajectory(scorePurple);
+        drive.turn(-90);
         drive.followTrajectory(mid_drop);
-        drive.followTrajectory(stack);
+    //    drive.followTrajectory(stack);
         drive.followTrajectory(back_to_mid);
         drive.followTrajectory(front_gate);
         drive.followTrajectory(front_gate_pt_2);
+        drive.followTrajectory(front_gate_pt_3);
         drive.followTrajectory(about_to_score);
         drive.followTrajectory(scoreYellow);
     }