Getting parking to work again as path 6b. Original path during competition was not working properly.

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAuto.java

@@ -43,6 +43,7 @@ public class SpecimenAuto extends OpMode {
          wrist.InitAuto();
          arm.initAuto();
 
+
         follower.setMaxPower(.45);
 
 
@@ -50,42 +51,49 @@ public class SpecimenAuto extends OpMode {
 
     @Override
     public void loop() {
+        if(runtime != null){
+            telemetry.addData("Runtime (seconds)", runtime.seconds());
+        }
         telemetry.addData("state", state);
          switch(state) {
              case 0:
+                 runtime.reset();
                  wrist.toSpecimenPrep();
                  arm.toSpecimenPrep();
-                 state = 1;
-                 new SleepAction(5);
+                 if(runtime.seconds() > 0.25){state = 1;}
                  break;
              case 1:
-                state = 2;
+                 //line 1
+                 if(runtime.seconds() > 3){state = 2;}
                  break;
              case 2:
-                 new SleepAction(5);
-                 state = 3;
+                 lift.toSpecimanHangHeight();
+                 if(runtime.seconds() > 3.75){state = 3;}
                  break;
              case 3:
-
-                 new SleepAction(5);
-                 state = 4;
+                wrist.toSpecimenHang();
+                 if(runtime.seconds() > 4){state = 4;}
                  break;
              case 4:
-
-                 new SleepAction(5);
-                 state = 5;
+                lift.toSpecimanReleaseHeight();
+                 if(runtime.seconds() > 4.5){state = 5;}
                  break;
              case 5:
-                 //specimen drop
+                 claw.switchState();
+                 if(runtime.seconds() > 4.65){state = 6;}
                  break;
              case 6:
-                 //path 4
+                 lift.toFloorPosition();
+                 if(runtime.seconds() > 4.75){state = 7;}
                  break;
              case 7:
-                 //specimen pick up
+                 arm.toParkPosition();
+                 wrist.toTravelPosition();
+                 //line 2
+                 if(runtime.seconds() > 0.){state = 8;}
                  break;
              case 8:
-                 //path 5
+                 if(runtime.seconds() > 0.25){state = 9;}
                  break;
              case 9:
                  //specimen drop

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAutoLineTest.java

@@ -13,7 +13,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
 
-@Autonomous(name = "Specimen Test", group = "Competition")
+@Autonomous(name = "Specimen Line Test", group = "Competition")
 public class SpecimenAutoLineTest extends OpMode {
     private Telemetry telemetryA;
 
@@ -40,15 +40,15 @@ public class SpecimenAutoLineTest extends OpMode {
                                 new Point(39.500, 60.000, Point.CARTESIAN)
                         )
                 )
-               .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
-/*                .addPath(
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0))
+                .addPath(
                         // Line 2
                         new BezierLine(
                                 new Point(39.500, 60.000, Point.CARTESIAN),
                                 new Point(37.000, 60.000, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .setConstantHeadingInterpolation(Math.toRadians(0))
                 .addPath(
                         // Line 3
                         new BezierCurve(
@@ -58,7 +58,7 @@ public class SpecimenAutoLineTest extends OpMode {
                                 new Point(58.000, 23.500, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .setConstantHeadingInterpolation(Math.toRadians(0))
                 .addPath(
                         // Line 4
                         new BezierLine(
@@ -66,7 +66,7 @@ public class SpecimenAutoLineTest extends OpMode {
                                 new Point(14.000, 23.500, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .setConstantHeadingInterpolation(Math.toRadians(0))
                 .addPath(
                         // Line 5
                         new BezierLine(
@@ -155,7 +155,7 @@ public class SpecimenAutoLineTest extends OpMode {
                                 new Point(37.000, 68.000, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(180))*/.build();
+                .setConstantHeadingInterpolation(Math.toRadians(180)).build();
         follower.followPath(path);
 
         telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleOpDevelopment.java

@@ -188,7 +188,7 @@ public class CometBotTeleOpDevelopment {
                 throw new RuntimeException(e);
             }
             goClaw = true;
-            dualSlides.toFixedPosition(2200);
+            dualSlides.toFixedPosition(2100);
             dualSlides.update();
         }
         if (gamepad1.right_bumper) {

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

@@ -15,7 +15,7 @@ import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
 import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
 import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
 
-@Autonomous(name = "Auto Test Competition V2", group = "Dev")
+@Autonomous(name = "Auto Competition V2", group = "A")
 public class CometBotDriveV2 extends OpMode {
     private Follower follower;
     private int state;
@@ -118,7 +118,7 @@ public class CometBotDriveV2 extends OpMode {
                 moveToPark();
                 break;
             case 99:
-                wrist.toTravelPosition();
+                wrist.toFloorPosition();
                 break;
             default:
                 telemetry.addLine("default");
@@ -192,7 +192,7 @@ public class CometBotDriveV2 extends OpMode {
                 wrist.toReverseBucket();
             }
             if(runtime.seconds() > 12.75) {
-                claw.openClaw();
+                claw.autoOpenClaw();
                 state = 6;
             }
         }
@@ -205,7 +205,7 @@ public class CometBotDriveV2 extends OpMode {
                 wrist.toReverseBucket();
             }
             if(runtime.seconds() > 23.00) {
-                claw.openClaw();
+                claw.autoOpenClaw();
                 state = 10;
             }
         }
@@ -217,7 +217,7 @@ public class CometBotDriveV2 extends OpMode {
             wrist.toReverseBucket();
         }
         if (runtime.seconds() > 3.5) {
-            claw.openClaw();
+            claw.autoOpenClaw();
         }
         if (runtime.seconds() > 4) {
             wrist.toTravelPosition();
@@ -247,9 +247,7 @@ public class CometBotDriveV2 extends OpMode {
         }
     }
 
-    private void moveToPickupAgainPath5() {
 
-    }
 
     private void moveToParkPath6() {
         if (runtime.seconds() > 24.5) {

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

@@ -0,0 +1,333 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.Action;
+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.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
+
+@Autonomous(name = "Copy of Auto Competition V2", group = "A")
+public class CopyofCometBotDriveV2 extends OpMode {
+    private Follower follower;
+    private int state;
+
+    private HighBasketPath1 path1;
+    private HighBasketPath2 path2;
+    private HighBasketPath3 path3;
+    private HighBasketPath4 path4;
+    private HighBasketPath5 path5;
+    //private HighBasketPath6 path6;
+    private HighBasketPath6b path6;
+
+
+    //private CometBotTeleopCompetition comp;
+    private static ElapsedTime runtime;
+    private static boolean initalized = false;
+    private static boolean followingPath = false;
+    private DualMotorSliderSubsystem lift;
+    private ClawSubsystem claw;
+    private WristSubsystem wrist;
+    private ArmSubsystem arm;
+
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+        follower.setMaxPower(.75);
+
+        path1 = new HighBasketPath1();
+        path2 = new HighBasketPath2();
+        path3 = new HighBasketPath3();
+        path4 = new HighBasketPath4();
+        path5 = new HighBasketPath5();
+        path6 = new HighBasketPath6b();
+        //path6 = new HighBasketPath6();
+        lift = new DualMotorSliderSubsystem(hardwareMap);
+        arm = new ArmSubsystem(hardwareMap);
+        wrist = new WristSubsystem(hardwareMap);
+        claw = new ClawSubsystem(hardwareMap);
+
+
+        lift.init();
+        arm.initAuto();
+        wrist.InitAuto();
+        claw.init();
+
+        //comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
+        // comp.initCloseClaw();
+        runtime = new ElapsedTime();
+    }
+
+
+    public void loop() {
+        telemetry.addData("state", state);
+//        telemetry.addData("arm", arm);
+//        telemetry.addData("lift", lift);
+//        telemetry.addData("wrist", wrist);
+//        telemetry.addData("claw", claw);
+        telemetry.addData("followingPath", followingPath);
+        telemetry.addData("busy", follower.isBusy());
+
+        if (runtime != null) {
+            telemetry.addData("Runtime (seconds)", runtime.seconds());
+        }
+        switch (state) {
+            case 0:
+
+                runtime.reset();
+                moveToPathOneAndHighBucket();
+                break;
+            case 1:
+                doArmThing();
+                state = 2;
+                break;
+            case 2:
+                moveToPathTwoAndPickSampleUp();
+                break;
+            case 3:
+                doPickUpThing();
+                break;
+            case 4:
+                moveToBasketPath3();
+                break;
+            case 5:
+                theArmThing();
+                break;
+            case 6:
+                moveToPickupAgainPath4();
+                break;
+            case 7:
+                doPickUpThingAgain();
+                break;
+            case 8:
+                moveToBasketPath5();
+                break;
+            case 9:
+                theArmThingAgain();
+                break;
+            case 10:
+                moveToParkPath6();
+                break;
+            case 11:
+                moveToPark();
+                break;
+            case 99:
+                wrist.toFloorPosition();
+                break;
+            default:
+                telemetry.addLine("default");
+
+        }
+        telemetry.update();
+        lift.update();
+        follower.update();
+
+
+    }
+
+    public void moveToPark() {
+        if (runtime.seconds() > 26) {
+            path6.moveToBasketPath6(follower);
+            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 {
+
+        private int value;
+
+        public SetStateAction(int value) {
+            this.value = value;
+        }
+
+        @Override
+        public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+            state = value;
+            return false;
+        }
+    }
+
+    private Action setStateValue(int value) {
+        return new SetStateAction(value);
+    }
+
+    private void doArmThing() {
+
+        claw.thumbOutOfTheWay();
+        wrist.toTravelPosition();
+        lift.toHighBucketReverseDrop();
+        //arm.toReverseBucket();
+        //wrist.toReverseBucket();
+        //claw.openClaw();
+        //wrist.toFloorPosition();
+
+    }
+
+
+    private void theArmThing() {
+        if(lift.getFixedPosition() >= 4280){
+            arm.toReverseBucket();
+            if(runtime.seconds() > 12) {
+                wrist.toReverseBucket();
+            }
+            if(runtime.seconds() > 12.75) {
+                claw.autoOpenClaw();
+                state = 6;
+            }
+        }
+    }
+
+    private void theArmThingAgain() {
+        if(lift.getFixedPosition() >= 4280){
+            arm.toReverseBucket();
+            if(runtime.seconds() > 22) {
+                wrist.toReverseBucket();
+            }
+            if(runtime.seconds() > 23.00) {
+                claw.autoOpenClaw();
+                state = 10;
+            }
+        }
+    }
+
+
+    private void moveToPathTwoAndPickSampleUp() {
+        if (runtime.seconds() > 3.25 && runtime.seconds() < 4) {
+            wrist.toReverseBucket();
+        }
+        if (runtime.seconds() > 3.5) {
+            claw.autoOpenClaw();
+        }
+        if (runtime.seconds() > 4) {
+            wrist.toTravelPosition();
+            lift.toFloorPosition();
+            path2.moveToPath2(follower);
+            state = 3;
+        }
+    }
+
+    //
+    private void moveToBasketPath3() {
+        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;
+        }
+    }
+
+
+
+    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) {
+//            path3.moveToBasketPath3(follower);
+//            followingPath = true;
+//        }
+//        if (runtime != null) {
+//            telemetry.addData("Runtime (seconds)", runtime.seconds());
+//            if (follower.atParametricEnd() || runtime.seconds() > 10.0) {
+//                state = 5;
+//                followingPath = false;
+//            }
+//        }
+//    }
+//
+//    private void thePickUpAuto() {
+//        Actions.runBlocking(new SequentialAction(
+//                new SleepAction(.1),
+//                comp.claw.openClaw(),
+//                new SleepAction(.2),
+//                comp.wrist.toPickupPosition(),
+//                new SleepAction(.2),
+//                comp.arm.toSubmarinePosition(),
+//                new SleepAction(.5),
+//                comp.claw.closeClaw(),
+//                new SleepAction(.3),
+//                comp.wrist.toFloorPosition(),
+//                new SleepAction(.2),
+//                comp.arm.toParkPosition(),
+//                new SleepAction(.2)
+//        ));
+//    }
+//    private void thePickUp() {
+//        Actions.runBlocking(new SequentialAction(
+//                new SleepAction(.5),
+//                comp.wrist.toPickupPosition(),
+//                new SleepAction(.5),
+//                comp.arm.toSubmarinePosition(),
+//                new SleepAction(.7),
+//                comp.claw.closeClaw(),
+//                new SleepAction(.7),
+//                comp.wrist.toFloorPosition(),
+//                new SleepAction(.5),
+//                comp.arm.toParkPosition(),
+//                new SleepAction(.5)
+//        ));
+//    }
+//
+    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/HighBasketPath6b.java

@@ -0,0 +1,44 @@
+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 HighBasketPath6b {
+
+
+    public void moveToBasketPath6(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(27.000, 128.000, Point.CARTESIAN),
+                                new Point(92.448, 125.671, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(315), Math.toRadians(270))
+                .addPath(
+                        // Line 2
+                        new BezierLine(
+                                new Point(92.448, 125.671, Point.CARTESIAN),
+                                new Point(93.000, 94.000, Point.CARTESIAN)
+                        )
+                )
+                .setTangentHeadingInterpolation();
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

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

@@ -6,7 +6,7 @@ import com.acmerobotics.dashboard.config.Config;
 public class RobotConstants {
     public final static double clawClose = 0.95;
     public final static double clawOpen = 0.35;
-
+    public final static int autoClawOpen = 0;
 
 
     public final static double armFloor = 0.7;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ClawSubsystem.java

@@ -4,6 +4,7 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.CLAW_NAME;
 import static org.firstinspires.ftc.teamcode.PedroConstants.THUMB_SERVO;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawClose;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawOpen;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.autoClawOpen;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.grabBlueberry;
 
 import com.qualcomm.robotcore.hardware.HardwareMap;
@@ -12,7 +13,7 @@ import com.qualcomm.robotcore.hardware.Servo;
 public class ClawSubsystem {
 
     public enum ClawState {
-        CLOSED, OPEN, GRAB_BLUEBERRY
+        CLOSED, OPEN, GRAB_BLUEBERRY, AUTO_OPEN
     }
 
     public enum ThumbState {
@@ -32,6 +33,7 @@ public class ClawSubsystem {
         claw.setPosition(grabBlueberry);
         state = ClawState.GRAB_BLUEBERRY;
     }
+
     public void closeClaw() {
         claw.setPosition(clawClose);
 
@@ -43,6 +45,11 @@ public class ClawSubsystem {
 
         state = ClawState.OPEN;
     }
+    public void autoOpenClaw() {
+        claw.setPosition(autoClawOpen);
+
+        state = ClawState.AUTO_OPEN;
+    }
 
     public ClawState getState() {
         return state;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HangMotorSubsystem.java

@@ -22,10 +22,10 @@ public class HangMotorSubsystem {
     }
     public void init(){
         //init hook, set runmodes
-        hang.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        //hang.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         hang.setDirection(DcMotorSimple.Direction.FORWARD);
         hang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        hang.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        hang.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
     }
     public void hangRobot(){