tweaked auto and slides

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java

@@ -75,7 +75,6 @@ public class Robot {
             case(0):
                 macroStartTime = runTime;
                 arm.setDoor(OPEN);
-                slides.setTarget(slides.getTarget()+70);
                 macroState++;
                 break;
             case(1):

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java

@@ -5,18 +5,20 @@ import com.arcrobotics.ftclib.controller.PIDController;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
 
 @Config
 public class Slides {
     private DcMotor slide;
     private DcMotor slide2;
 
-    public static double p = 0.0014;
+//    public static double p = 0.0014;
-    public static double i = 0.02;
+//    public static double i = 0.02;
-    public static double d = 0;
+//    public static double d = 0;
-    public static double f = 0.01;
+//    public static double f = 0.01;
+    public static PIDFCoefficients coefficients = new PIDFCoefficients(0.0014,0.02,0,0.01);
     public static double pTolerance = 20;
-    public static PIDController controller = new PIDController(p, i, d);
+    private PIDController controller = new PIDController(coefficients.p, coefficients.i, coefficients.d);
 
     public static int targetMin = -10;
     public static int targetMax = 830;
@@ -102,15 +104,15 @@ public class Slides {
 //                slide2.setPower(0);
 //            } else {
         double pid, ff;
-        controller.setPID(p, i, d);
+        controller.setPID(coefficients.p, coefficients.i, coefficients.d);
         controller.setTolerance(pTolerance);
 
         pid = controller.calculate(slide.getCurrentPosition(), target);
-        ff = f;
+        ff = coefficients.f;
         slide.setPower(pid + ff);
 
         pid = controller.calculate(slide2.getCurrentPosition(), target);
-        ff = f;
+        ff = coefficients.f;
         slide2.setPower(pid + ff);
 //            }
 //        }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java

@@ -1,6 +1,7 @@
 package org.firstinspires.ftc.teamcode.opmode.autonomous;
 
 import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
 import com.acmerobotics.roadrunner.trajectory.Trajectory;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 
@@ -20,6 +21,10 @@ public class RedBackStageAuto extends AutoBase {
     public Trajectory parkRobot2;
     public Trajectory parkRobot3;
 
+    public Trajectory stackrun1b1;
+    public Trajectory stackrun1b2;
+    public Trajectory stackrun1b3;
+
     @Override
     public void createTrajectories() {
         // set start position
@@ -41,6 +46,10 @@ public class RedBackStageAuto extends AutoBase {
         Pose2d park2 = new Pose2d(48, -12, Math.toRadians(180));
         Pose2d park3 = new Pose2d(48, -12, Math.toRadians(180));
 
+        Pose2d stack_1x1 = new Pose2d(-56, -12, Math.toRadians(180));
+        Pose2d stack_2x1 = new Pose2d(-56, -12, Math.toRadians(180));
+        Pose2d stack_3x1 = new Pose2d(-56, -12, Math.toRadians(180));
+
         // create trajectories
         scorePurple1 = robot.drive.trajectoryBuilder(start)
                 .lineToLinearHeading(drop1)
@@ -71,6 +80,19 @@ public class RedBackStageAuto extends AutoBase {
         parkRobot3 = robot.drive.trajectoryBuilder(scoreYellow3.end())
                 .lineToLinearHeading(park3)
                 .build();
+
+        stackrun1b1 = robot.drive.trajectoryBuilder(scoreYellow1.end())
+                .splineToConstantHeading(new Vector2d(30, -12), Math.toRadians(0))
+                .lineToLinearHeading(stack_1x1)
+                .build();
+        stackrun1b2 = robot.drive.trajectoryBuilder(scoreYellow2.end())
+                .splineToConstantHeading(new Vector2d(30, -12), Math.toRadians(0))
+                .lineToLinearHeading(stack_2x1)
+                .build();
+        stackrun1b3 = robot.drive.trajectoryBuilder(scoreYellow1.end())
+                .splineToConstantHeading(new Vector2d(30, -12), Math.toRadians(0))
+                .lineToLinearHeading(stack_1x1)
+                .build();
     }
 
     @Override
@@ -117,21 +139,30 @@ public class RedBackStageAuto extends AutoBase {
             case 4:
                 robot.resetMacro(0, getRuntime());
                 if (robot.macroState >= 2){
-                    robot.drive.followTrajectoryAsync(teamPropLocation==1?parkRobot1:(teamPropLocation==2?parkRobot2:parkRobot3));
+                   // robot.drive.followTrajectoryAsync(teamPropLocation==1?parkRobot1:(teamPropLocation==2?parkRobot2:parkRobot3));
+                    robot.drive.followTrajectoryAsync(stackrun1b2);
                     macroState++;
                 }
                 break;
-            // PARK ROBOT
+
             case 5:
-                // reset macro'
+                if(!robot.drive.isBusy()){
-                if (robot.macroState != 0) {
+                    macroState =-1;
-                    robot.resetMacro(0, getRuntime());
-                }
-                // if macro and drive are done, end auto
-                if (robot.macroState == 0 && !robot.drive.isBusy()) {
-                    macroState=-1;
                 }
+
+                //macroState++;
                 break;
+                // PARK ROBOT
+//            case 6:
+//                // reset macro'
+//                if (robot.macroState != 0) {
+//                    robot.resetMacro(0, getRuntime());
+//                }
+//                // if macro and drive are done, end auto
+//                if (robot.macroState == 0 && !robot.drive.isBusy()) {
+//                    macroState=-1;
+//                }
+//                break;
         }
     }
 }