Almost perfect, just need to get path 6 working properly

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java

@@ -1,35 +1,19 @@
 package org.firstinspires.ftc.teamcode.cometbots.paths;
 
-import static com.qualcomm.hardware.lynx.commands.core.LynxSetMotorPIDFControlLoopCoefficientsCommand.InternalMotorControlAlgorithm.Max;
 
 import androidx.annotation.NonNull;
 
 import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
 import com.acmerobotics.roadrunner.Action;
-import com.acmerobotics.roadrunner.SequentialAction;
-import com.acmerobotics.roadrunner.SleepAction;
-import com.acmerobotics.roadrunner.ftc.Actions;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
-//import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleopCompetition;
-//import org.firstinspires.ftc.teamcode.cometbots.paths.HighBasketPath1;
 import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-//import org.firstinspires.ftc.teamcode.subsystem.AutoPark;
-import org.firstinspires.ftc.teamcode.cometbots.paths.HighBasketPath1;
 import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
-//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath2;
-//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath3;
-//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath4;
-//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath5;
-//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath6;
-//import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
-//import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem;
-
 
 @Autonomous(name = "Auto Test Competition V2", group = "Dev")
 public class CometBotDriveV2 extends OpMode {
@@ -40,6 +24,8 @@ public class CometBotDriveV2 extends OpMode {
     private HighBasketPath2 path2;
     private HighBasketPath3 path3;
     private HighBasketPath4 path4;
+    private HighBasketPath5 path5;
+    private HighBasketPath6 path6;
 
 
     //private CometBotTeleopCompetition comp;
@@ -60,6 +46,8 @@ public class CometBotDriveV2 extends OpMode {
         path2 = new HighBasketPath2();
         path3 = new HighBasketPath3();
         path4 = new HighBasketPath4();
+        path5 = new HighBasketPath5();
+        path6 = new HighBasketPath6();
         lift = new DualMotorSliderSubsystem(hardwareMap);
         arm = new ArmSubsystem(hardwareMap);
         wrist = new WristSubsystem(hardwareMap);
@@ -91,61 +79,79 @@ public class CometBotDriveV2 extends OpMode {
         }
         switch (state) {
             case 0:
-//                claw.thumbOutOfTheWay();
+
+                runtime.reset();
                 moveToPathOneAndHighBucket();
                 break;
             case 1:
-                //doArmThing();
-                //lift.update();
+                doArmThing();
                 state = 2;
                 break;
             case 2:
                 moveToPathTwoAndPickSampleUp();
-                //lift.update();
                 break;
             case 3:
-//                doPickUpThing();
-//                runtime.reset();
-                state = 4;
+                doPickUpThing();
                 break;
             case 4:
                 moveToBasketPath3();
                 break;
             case 5:
-                //               theArmThing();
-                state = 6;
+                theArmThing();
                 break;
             case 6:
                 moveToPickupAgainPath4();
                 break;
-           case 7:
-//                doPickUpThingAgain();
+            case 7:
+                doPickUpThingAgain();
                 break;
             case 8:
-                moveToPickupAgainPath5();
+                moveToBasketPath5();
+                break;
+            case 9:
+                theArmThingAgain();
+                break;
+            case 10:
+                moveToParkPath6();
+                break;
+            case 11:
+                moveToPark();
+                break;
+            case 99:
                 break;
-//            case 9:
-//                //theArmThingAgain();
-//                break;
-//            case 10:
-//                //moveToParkPath6();
-//                break;
-//            case 11:
             default:
                 telemetry.addLine("default");
 
         }
         telemetry.update();
-
+        lift.update();
         follower.update();
 
 
     }
 
+    public void moveToPark() {
+        if (runtime.seconds() > 26) {
+            path6.moveToBasketPath6(follower);
+            wrist.toTravelPosition();
+            state = 99;
+        }
+    }
+
     private void moveToPathOneAndHighBucket() {
         path1.moveToPath1(follower);
         state = 1;
     }
+    private void moveToBasketPath5() {
+        if(runtime.seconds() > 19){
+            path5.moveToBasketPath5(follower);
+            lift.toHighBucketReverseDrop();
+            wrist.toTravelPosition();
+            state = 9;
+        }
+
+    }
+
 
     public class SetStateAction implements Action {
 
@@ -168,35 +174,54 @@ public class CometBotDriveV2 extends OpMode {
 
     private void doArmThing() {
 
+        claw.thumbOutOfTheWay();
+        wrist.toTravelPosition();
         lift.toHighBucketReverseDrop();
-        arm.toReverseBucket();
-        wrist.toReverseBucket();
-        claw.openClaw();
-        wrist.toFloorPosition();
+        //arm.toReverseBucket();
+        //wrist.toReverseBucket();
+        //claw.openClaw();
+        //wrist.toFloorPosition();
 
-        state = 2;
     }
 
 
-//    private void theArmThing() {
-//        telemetry.addData("busy?", follower.isBusy());
-//        telemetry.addData("end?", follower.atParametricEnd());
-//        if (follower.atParametricEnd()){
-//            follower.breakFollowing();
-//            comp.highBucketDropAuto();
-//            state = 6;
-//        }
-//        follower.breakFollowing();
-//    }
+    private void theArmThing() {
+        if(lift.getFixedPosition() >= 4280){
+            arm.toReverseBucket();
+            if(runtime.seconds() > 12) {
+                wrist.toReverseBucket();
+            }
+            if(runtime.seconds() > 12.75) {
+                claw.openClaw();
+                state = 6;
+            }
+        }
+    }
+
+    private void theArmThingAgain() {
+        if(lift.getFixedPosition() >= 4280){
+            arm.toReverseBucket();
+            if(runtime.seconds() > 22) {
+                wrist.toReverseBucket();
+            }
+            if(runtime.seconds() > 23.00) {
+                claw.openClaw();
+                state = 10;
+            }
+        }
+    }
 
-//    private void theArmThingAgain() {
-//        follower.breakFollowing();
-//        comp.highBucketDropAuto();
-//        state = 10;
-//    }
 
     private void moveToPathTwoAndPickSampleUp() {
-        if (runtime.seconds() > 6) {
+        if (runtime.seconds() > 3.25 && runtime.seconds() < 4) {
+            wrist.toReverseBucket();
+        }
+        if (runtime.seconds() > 3.5) {
+            claw.openClaw();
+        }
+        if (runtime.seconds() > 4) {
+            wrist.toTravelPosition();
+            lift.toFloorPosition();
             path2.moveToPath2(follower);
             state = 3;
         }
@@ -204,15 +229,19 @@ public class CometBotDriveV2 extends OpMode {
 
     //
     private void moveToBasketPath3() {
-        if (runtime.seconds() > 10)
-        {
+        if (runtime.seconds() > 7.25) {
+            lift.toHighBucketReverseDrop();
             path3.moveToBasketPath3(follower);
             state = 5;
         }
     }
 
     private void moveToPickupAgainPath4() {
+        if (runtime.seconds() > 13) {
+            wrist.toTravelPosition();
+        }
         if (runtime.seconds() > 14) {
+            lift.toFloorPosition();
             path4.moveToPickupPath4(follower);
             state = 7;
         }
@@ -222,19 +251,17 @@ public class CometBotDriveV2 extends OpMode {
 
     }
 
-//    private void moveToParkPath6() {
-//        if (!followingPath) {
-//            path6.moveToParkPath6(follower);
-//            followingPath = true;
-//        }
-//        if (runtime != null) {
-//            telemetry.addData("Runtime (seconds)", runtime.seconds());
-//            if (follower.atParametricEnd() || runtime.seconds() > 48.0) {
-//                state = 11;
-//                followingPath = false;
-//            }
-//        }
-//    }
+    private void moveToParkPath6() {
+        if (runtime.seconds() > 24.5) {
+            arm.toBucketPosition();
+            wrist.toTravelPosition();
+        }
+        if (runtime.seconds() > 25.) {
+            lift.toFloorPosition();
+            claw.closeClaw();
+            state = 11;
+        }
+    }
 //
 //    private void moveToBasketPath3() {
 //        if (!followingPath) {
@@ -283,18 +310,24 @@ public class CometBotDriveV2 extends OpMode {
 //        ));
 //    }
 //
-//    private void doPickUpThing() {
-//        follower.breakFollowing();
-//        thePickUpAuto();
-//        state = 4;
-//    }
-//
-//    private void doPickUpThingAgain() {
-//        follower.breakFollowing();
-//        thePickUpAuto();
-//        state = 8;
-//    }
-//
-//
+    private void doPickUpThing() {
+        wrist.toPickupPosition();
+        arm.toFloorPosition();
+        if (runtime.seconds() > 7){
+            claw.closeClaw();
+            state = 4;
+        }
+    }
+
+    private void doPickUpThingAgain() {
+       wrist.toPickupPosition();
+       arm.toFloorPosition();
+        if (runtime.seconds() > 18){
+            claw.closeClaw();
+            state = 8;
+        }
+    }
+
+
 }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath2.java

@@ -24,7 +24,7 @@ public class HighBasketPath2 {
                         // Line 1
                         new BezierLine(
                                 new Point(20.000, 130.000, Point.CARTESIAN),
-                                new Point(26.000, 117.000, Point.CARTESIAN)
+                                new Point(27.000, 117.000, Point.CARTESIAN)
                         )
                 )
                 .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0));

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath4.java

@@ -23,7 +23,7 @@ public class HighBasketPath4 {
                         // Line 1
                         new BezierLine(
                                 new Point(22.000, 132.000, Point.CARTESIAN),
-                                new Point(26.000, 127.500, Point.CARTESIAN)
+                                new Point(27.000, 128.500, Point.CARTESIAN)
                         )
                 )
                 .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0));

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath5.java

@@ -0,0 +1,35 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+
+/*
+    AutoLine# - This file does something of a path......
+
+ */
+public class HighBasketPath5 {
+
+
+    public void moveToBasketPath5(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(27.000, 127.000, Point.CARTESIAN),
+                                new Point(22.000, 132.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(320));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath6.java

@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+
+/*
+    AutoLine# - This file does something of a path......
+
+ */
+public class HighBasketPath6 {
+
+
+    public void moveToBasketPath6(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(27.000, 128.500, Point.CARTESIAN),
+                                new Point(104.00, 100.00, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java

@@ -21,18 +21,20 @@ public class RobotConstants {
     public final static int slideBelowFloor = -150;
     public final static int backwardBucketDrop = 4670;
     public final static double armBucket = 0.45;
-    public final static double armInit = 0.135;
-    public final static double wristInit = 0.25;
+    public final static double armInit = 0.13;
+    public final static double wristInit = 0.125;
     public final static double wristPickup = 0.475;
     public final static double wristBucket = 0.56;
     public final static double wristFloor = 0.75;
-    public final static double wristBackwardBucket = 0.775;
+    public final static double wristBackwardBucket = 0.725;
 
     public final static double wristRung = 0.55;
 
     public final static double wristSpeciemen = 0.1;
 
     public final static int toBar = 500;
+    public final static double wristtravel = 0.525;
+
     public final static int toFloor = 0;
     public final static int liftToFloorPos = 350;
     public final static int liftToSubmarinePos = 350;
@@ -43,6 +45,6 @@ public class RobotConstants {
     public final static int liftToHighRungAttach = 1050;
 
 
-    public final static int liftToHighBucketPos = 4000;
+    public final static int liftToHighBucketPos = 4500;
     public final static double liftPower = 1;
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java

@@ -9,6 +9,7 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristHangBlu
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristInit;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristPickup;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpeciemen;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristtravel;
 
 import androidx.annotation.NonNull;
 
@@ -21,7 +22,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx;
 public class WristSubsystem {
 
     public enum WristState {
-        FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT, REVERSE_BUCKET
+        FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT, REVERSE_BUCKET, TRAVEL
     }
 
     public ServoImplEx wrist;
@@ -97,6 +98,11 @@ public class WristSubsystem {
         wrist.setPosition(wristBackwardBucket);
         setState(WristState.REVERSE_BUCKET);
     }
+    public void toTravelPosition(){
+        wrist.setPosition(wristtravel);
+        setState(WristState.TRAVEL);
+    }
+
     public WristState getState() {
         return this.state;
     }