Fine tuning to autonomous prior to switching to new robot

overtime

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

@@ -78,8 +78,8 @@ public class BlueBasketAuto extends OpMode {
                 if (runtime.seconds() > 15) {
                     telemetry.addData("case2", "case2");
 
-                    new SleepAction(.5);
+                   // new SleepAsction(.5);
-                    path2.moveToPath1(follower);
+                    //path2.moveToPath1(follower);
 
                     //For next time, add encoder control to skyhook and extend here
                     //comp.moveSkyHook();

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

@@ -1,9 +1,14 @@
 package org.firstinspires.ftc.teamcode;
 
+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;
@@ -13,6 +18,10 @@ import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
 import org.firstinspires.ftc.teamcode.subsystem.AutoPark;
 import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath1;
 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;
 
@@ -24,6 +33,12 @@ public class ComeBotDriveDevV2 extends OpMode {
 
     private HighBasketAutoPath1 path1;
     private HighBasketAutoPath2 path2;
+
+    private HighBasketAutoPath3 path3;
+    private HighBasketAutoPath4 path4;
+    private HighBasketAutoPath5 path5;
+    private HighBasketAutoPath6 path6;
+
     private AutoPark pathPark;
     private SkyHookSubsystem hook;
     private CometBotTeleopCompetition comp;
@@ -38,18 +53,26 @@ public class ComeBotDriveDevV2 extends OpMode {
         follower.setMaxPower(.75);
         path1 = new HighBasketAutoPath1();
         path2 = new HighBasketAutoPath2();
+        path3 = new HighBasketAutoPath3();
+        path4 = new HighBasketAutoPath4();
+        path5 = new HighBasketAutoPath5();
+        path6 = new HighBasketAutoPath6();
+
         pathPark = new AutoPark();
 
         comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
         comp.initCloseClaw();
         hook = new SkyHookSubsystem(hardwareMap);
         state = 0;
-
     }
 
 
     public void loop() {
         telemetry.addData("state", state);
+        telemetry.addData("followingPath", followingPath);
+        if (runtime != null) {
+            telemetry.addData("Runtime (seconds)", runtime.seconds());
+        }
         switch (state) {
             case 0:
                 moveToPathOneAndHighBucket();
@@ -60,6 +83,33 @@ public class ComeBotDriveDevV2 extends OpMode {
             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:
+                moveToPickupAgainPath5();
+                break;
+            case 9:
+                theArmThingAgain();
+                break;
+            case 10:
+                //moveToParkPath6();
+                break;
+            case 11:
+
+
         }
         telemetry.update();
         follower.update();
@@ -73,7 +123,7 @@ public class ComeBotDriveDevV2 extends OpMode {
         }
         if (runtime != null) {
             telemetry.addData("Runtime (seconds)", runtime.seconds());
-            if (follower.atParametricEnd() || runtime.seconds() > 6.0) {
+            if (follower.atParametricEnd() || runtime.seconds() > 4) {
                 state = 1;
                 followingPath = false;
             }
@@ -100,10 +150,27 @@ public class ComeBotDriveDevV2 extends OpMode {
     }
 
     private void doArmThing() {
-        comp.highBucketDrop();
+        comp.highBucketDropAuto();
         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 theArmThingAgain() {
+        follower.breakFollowing();
+        comp.highBucketDropAuto();
+        state = 10;
+    }
+
     private void moveToPathTwoAndPickSampleUp() {
         if (!followingPath) {
             path2.moveToPath2(follower);
@@ -111,13 +178,113 @@ public class ComeBotDriveDevV2 extends OpMode {
         }
         if (runtime != null) {
             telemetry.addData("Runtime (seconds)", runtime.seconds());
-            if (follower.atParametricEnd() || runtime.seconds() > 12.0) {
+            if (follower.atParametricEnd() || runtime.seconds() > 22.0) {
                 state = 3;
                 followingPath = false;
             }
         }
     }
 
+    private void moveToPickupAgainPath4() {
+        if (!followingPath) {
+            path4.moveToPickupAgainPath4(follower);
+            followingPath = true;
+        }
+        if (runtime != null) {
+            telemetry.addData("Runtime (seconds)", runtime.seconds());
+            if (follower.atParametricEnd() || runtime.seconds() > 27.0) {
+                state = 7;
+                followingPath = false;
+            }
+        }
+    }
+
+    private void moveToPickupAgainPath5() {
+        if (!followingPath) {
+            path5.moveToPickupAgainPath5(follower);
+            followingPath = true;
+        }
+        if (runtime != null) {
+            telemetry.addData("Runtime (seconds)", runtime.seconds());
+            if (follower.atParametricEnd() || runtime.seconds() > 36.0) {
+                state = 9;
+                followingPath = false;
+            }
+        }
+    }
+    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 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() {
+        follower.breakFollowing();
+        thePickUpAuto();
+        state = 4;
+    }
+
+    private void doPickUpThingAgain() {
+        follower.breakFollowing();
+        thePickUpAuto();
+        state = 8;
+    }
+
 
 }
 

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

@@ -31,7 +31,7 @@ public class PedroConstants {
     /*
         Motor Max Power
      */
-    public static final double MAX_POWER = .675;
+    public static final double MAX_POWER = .8;
 
     /*
         IMU

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

@@ -105,7 +105,7 @@ public class CometBotTeleopCompetition {
         this.currentGP1.copy(this.GP1);
         this.previousGP2.copy(currentGP2);
         this.currentGP2.copy(this.GP2);
-
+        this.moveSkyHook();
         this.toHighBucketScore();
         this.toLowBucketScore();
         this.toArmParkPosition();
@@ -157,6 +157,18 @@ public class CometBotTeleopCompetition {
             this.follower.setMaxPower(this.currentPower);
         }
     }
+    public void moveSkyHook() {
+        if (this.currentGP2.dpad_down) {
+            hook.moveSkyHook(-1.00);
+        }
+        else if (this.currentGP2.dpad_up) {
+            hook.moveSkyHook(1.00);
+        }
+        else{
+            hook.moveSkyHook(0.00);
+        }
+
+    }
 
     /*
     Type: PS4 / Logitech
@@ -200,20 +212,20 @@ public class CometBotTeleopCompetition {
     }
     public void highBucketDropAuto() {
         Actions.runBlocking(new SequentialAction(
-                new SleepAction(.5),
+                new SleepAction(.1),
                 this.lift.toHighBucketPosition(),
-                new SleepAction(.5),
+                new SleepAction(.25),
                 this.arm.toBucketPosition(),
-                new SleepAction(.5),
+                new SleepAction(.25),
                 this.wrist.toBucketPosition(),
-                new SleepAction(.5),
+                new SleepAction(.25),
                 this.claw.openClaw(),
-                new SleepAction(.5),
+                new SleepAction(.25),
                 this.wrist.toFloorPosition(),
-                new SleepAction(.5),
+                new SleepAction(.25),
                 this.arm.toParkPosition(),
                 this.lift.toZeroPosition(),
-                new SleepAction(.5)
+                new SleepAction(.25)
         ));
     }
     /*

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftWristArmTest.java

@@ -115,11 +115,13 @@ public class LiftWristArmTest extends LinearOpMode {
             }
 
             if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
-                lift.setPosition(lift.getPosition() + 175);
+                lift.setPosition(lift.getPosition() + 100);
             }
 
             if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
-                lift.setPosition(lift.getPosition() - 25);
+                if (lift.getPosition() > 1000) {
+                    lift.setPosition(lift.getPosition()-1000);
+                }
             }
 
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SpecimenTest.java

@@ -49,7 +49,6 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
 import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.AutoLine1;
-import org.firstinspires.ftc.teamcode.subsystem.AutoLine2;
 import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem;

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

@@ -5,7 +5,7 @@ import com.acmerobotics.dashboard.config.Config;
 @Config
 public class RobotConstants {
     public final static double clawClose = 0.8;
-    public final static double clawOpen = 0.05;
+    public final static double clawOpen = 0.15;
 
     public final static double armFloor = 0.7;
     public final static double armSubmarine = 0.55;
@@ -20,7 +20,7 @@ public class RobotConstants {
     public final static double wristSpeciemen = 0.1;
 
 
-    public final static int liftToFloorPos = 550;
+    public final static int liftToFloorPos = 500;
     public final static int liftToLowBucketPos = 2650;
     public final static int liftToHighRung = 2100;
     public final static int dropToHighRung = 1675;

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

@@ -1,40 +0,0 @@
-package org.firstinspires.ftc.teamcode.subsystem;
-
-import androidx.annotation.NonNull;
-
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-import com.acmerobotics.roadrunner.Action;
-
-import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
-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 AutoLine2 {
-
-
-    private PathChain pathChain;
-
-    private PathChain goToStore;
-
-    private Pose startPose = new Pose(37.5, 72);
-
-    public AutoLine2(Follower robot) {
-        robot.setStartingPose(startPose);
-        pathChain  = robot.pathBuilder().addPath(
-                new BezierLine(
-                        new Point(37.500, 72.000, Point.CARTESIAN),
-                        new Point(36.000, 72.000, Point.CARTESIAN)
-                )
-        ).setConstantHeadingInterpolation(Math.toRadians(0)).build();
-        robot.followPath(pathChain);
-    }
-}
-
-

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

@@ -29,7 +29,7 @@ public class HighBasketAutoPath1 {
                                 new Point(18.000, 126.000, Point.CARTESIAN)
                         )
                 )
-                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)).build();
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
         pathChain = builder.build();
         robot.followPath(pathChain);
     }

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

@@ -4,6 +4,7 @@ package org.firstinspires.ftc.teamcode.subsystem;
 import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
 import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
 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;
@@ -15,19 +16,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
  */
 public class HighBasketAutoPath2 {
     
-    public void moveToPath1(Follower robot) {
+    public void moveToPath2(Follower robot) {
         PathChain pathChain;
         PathBuilder builder = new PathBuilder();
         builder
                 .addPath(
-                        // Line 2
+                        // Line 1
-                        new BezierCurve(
+                        new BezierLine(
                                 new Point(18.000, 126.000, Point.CARTESIAN),
-                                new Point(85.000, 132.750, Point.CARTESIAN),
+                                new Point(25.500, 120.000, Point.CARTESIAN)
-                                new Point(84.000, 97.000, Point.CARTESIAN)
                         )
                 )
-                .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(135));
+                .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0));
         pathChain = builder.build();
         robot.followPath(pathChain);
     }

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

@@ -0,0 +1,35 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
+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 HighBasketAutoPath3 {
+
+    public void moveToBasketPath3(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(29.000, 120.000, Point.CARTESIAN),
+                                new Point(18.000, 126.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

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

@@ -0,0 +1,34 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+
+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 HighBasketAutoPath4 {
+    
+    public void moveToPickupAgainPath4(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(18.000, 126.000, Point.CARTESIAN),
+                                new Point(25.000, 129.900, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

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

@@ -0,0 +1,34 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+
+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 HighBasketAutoPath5 {
+    
+    public void moveToPickupAgainPath5(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(24.000, 131.000, Point.CARTESIAN),
+                                new Point(18.000, 126.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

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

@@ -0,0 +1,50 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+
+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 HighBasketAutoPath6 {
+    
+    public void moveToParkPath6(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(18.000, 126.000, Point.CARTESIAN),
+                                new Point(18.000, 126.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(270))
+                .addPath(
+                        // Line 2
+                        new BezierLine(
+                                new Point(18.000, 126.000, Point.CARTESIAN),
+                                new Point(81.000, 125.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(270), Math.toRadians(270))
+                .addPath(
+                        // Line 3
+                        new BezierLine(
+                                new Point(81.000, 125.000, Point.CARTESIAN),
+                                new Point(81.000, 100.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(270), Math.toRadians(270));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+