competition code updates for blue basket auto. Move to blue basket, put block in basket, park.

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

@@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.SleepAction;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
@@ -9,6 +10,7 @@ import com.qualcomm.robotcore.util.ElapsedTime;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleopCompetition;
+import org.firstinspires.ftc.teamcode.configs.RobotConstants;
 import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
 import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
@@ -16,28 +18,39 @@ 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;
+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.LiftActionsSubsystem;
 
-@Disabled
+
-@Autonomous(name = "Auto Test", group = "Dev")
+@Autonomous(name = "Auto Test Competition", group = "Dev")
 public class BlueBasketAuto extends OpMode {
     private Follower follower;
     private int state;
 
     private HighBasketAutoPath1 path1;
     private HighBasketAutoPath2 path2;
+    private AutoPark pathPark;
+
     private CometBotTeleopCompetition comp;
     private ElapsedTime runtime;
 
+    private LiftActionsSubsystem liftActionsSubsystem;
+
     @Override
     public void init() {
         follower = new Follower(hardwareMap);
         follower.setMaxPower(.75);
-        path1 = new HighBasketAutoPath1();
+        path1    = new HighBasketAutoPath1();
-        path2 = new HighBasketAutoPath2();
+        path2    = new HighBasketAutoPath2();
+        pathPark = new AutoPark();
+
         comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
         comp.initCloseClaw();
+        runtime = new ElapsedTime();
+        state = 0;
+
     }
 
 
@@ -45,18 +58,41 @@ public class BlueBasketAuto extends OpMode {
     public void loop() {
         switch(state) {
             case 0:
+                telemetry.addData("case0", "case0");
+
                 path1.moveToPath1(follower);
                 state = 1;
                 runtime.reset();
             case 1:
                 if (runtime.seconds() > 5) {
-                    path2.moveToPath1(follower);
+                    telemetry.addData("case1", "case1");
+
+                    new SleepAction(.5);
+                    comp.highBucketDropAuto();
+
                     state = 2;
                 }
+            case 2:
+                if (runtime.seconds() > 15) {
+                    telemetry.addData("case2", "case2");
+
+                    new SleepAction(.5);
+                    path2.moveToPath1(follower);
+
+                    //For next time, add encoder control to skyhook and extend here
+                    //comp.moveSkyHook();
+
+                    //pathPark.moveToPark(follower);
+
+                    state = 5;
+                }
             default:
-                System.out.println("default");
+                //System.out.println("default");
+                //telemetry.addData("default", "default");
+                //telemetry.update();
         }
+        telemetry.update();
         follower.update();
-        follower.telemetryDebug(telemetry);
+        //follower.telemetryDebug(telemetry);
     }
 }

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

@@ -1,55 +0,0 @@
-package org.firstinspires.ftc.teamcode;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-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.PathChain;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
-@Disabled
-@Autonomous(name = "DKJBFWOFHBOUW   EHBF", group = "Dev")
-public class BlueBasketAutoWithDrop3 extends OpMode {
-    private Telemetry telemetryA;
-
-    private Follower follower;
-
-    private PathChain path;
-
-
-    @Override
-    public void init() {
-        follower = new Follower(hardwareMap);
-
-        follower.setMaxPower(.75);
-
-        path = follower.pathBuilder()
-                .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0))
-                .addPath(
-                        // Line 3
-                        new BezierCurve(
-                                new Point(32.000, 123.000, Point.CARTESIAN),
-                                new Point(16.000, 120.000, Point.CARTESIAN),
-                                new Point(16.000, 128.000, Point.CARTESIAN)
-                        )
-                )
-                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)).build();
-
-        follower.followPath(path);
-
-        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-        telemetryA.update();
-    }
-
-
-    @Override
-    public void loop() {
-        follower.update();
-        follower.telemetryDebug(telemetryA);
-    }
-}

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

@@ -1,55 +0,0 @@
-package org.firstinspires.ftc.teamcode;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-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.PathChain;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
-@Disabled
-@Autonomous(name = "rndnohsdnhngph", group = "Dev")
-public class BlueBasketAutoWithDrop4 extends OpMode {
-    private Telemetry telemetryA;
-
-    private Follower follower;
-
-    private PathChain path;
-
-
-    @Override
-    public void init() {
-        follower = new Follower(hardwareMap);
-
-        follower.setMaxPower(.75);
-
-        path = follower.pathBuilder()
-                .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0))
-                .addPath(
-                        // Line 4
-                        new BezierLine(
-                                new Point(16.000, 128.000, Point.CARTESIAN),
-                                new Point(32.000, 133.500, Point.CARTESIAN)
-                        )
-                )
-                .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0)).build();
-
-        follower.followPath(path);
-
-        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-        telemetryA.update();
-    }
-
-
-    @Override
-    public void loop() {
-        follower.update();
-        follower.telemetryDebug(telemetryA);
-    }
-}

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

@@ -1,56 +0,0 @@
-package org.firstinspires.ftc.teamcode;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-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.PathChain;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
-@Disabled
-@Autonomous(name = "rboeruhbgoeruhbg", group = "Dev")
-public class BlueBasketAutoWithDrop5 extends OpMode {
-    private Telemetry telemetryA;
-
-    private Follower follower;
-
-    private PathChain path;
-
-
-    @Override
-    public void init() {
-        follower = new Follower(hardwareMap);
-
-        follower.setMaxPower(.75);
-
-        path = follower.pathBuilder()
-                .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0))
-                .addPath(
-                        // Line 5
-                        new BezierLine(
-                                new Point(32.000, 133.500, Point.CARTESIAN),
-                                new Point(16.000, 128.000, Point.CARTESIAN)
-                        )
-                )
-                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)).build();
-
-
-        follower.followPath(path);
-
-        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-        telemetryA.update();
-    }
-
-
-    @Override
-    public void loop() {
-        follower.update();
-        follower.telemetryDebug(telemetryA);
-    }
-}

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

@@ -1,53 +0,0 @@
-package org.firstinspires.ftc.teamcode;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-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.PathChain;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
-@Disabled
-@Autonomous(name = "piqeripgqreipbripbipb", group = "Dev")
-public class BlueBasketAutoWithDrop6 extends OpMode {
-    private Telemetry telemetryA;
-
-    private Follower follower;
-
-    private PathChain path;
-
-    @Override
-    public void init() {
-        follower = new Follower(hardwareMap);
-
-        follower.setMaxPower(.75);
-
-        path = follower.pathBuilder()
-                .addPath(
-                        // Line 6
-                        new BezierCurve(
-                                new Point(16.000, 128.000, Point.CARTESIAN),
-                                new Point(85.000, 132.750, Point.CARTESIAN),
-                                new Point(84.000, 97.000, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation.build();
-
-        follower.followPath(path);
-
-        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-        telemetryA.update();
-    }
-
-
-    @Override
-    public void loop() {
-        follower.update();
-        follower.telemetryDebug(telemetryA);
-    }
-}

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

@@ -113,8 +113,8 @@ public class CometBotTeleopCompetition {
         this.clawControl();
         this.decreaseMaxPower();
         this.increaseMaxPower();
-        this.raiseSkyHook();
+        this.moveSkyHook();
-        //this.lowerSkyHook();
+
 
         Actions.runBlocking(this.lift.toFloorPosition());
 
@@ -199,7 +199,24 @@ public class CometBotTeleopCompetition {
                 new SleepAction(.5)
         ));
     }
-
+    public void highBucketDropAuto() {
+        Actions.runBlocking(new SequentialAction(
+                new SleepAction(.5),
+                this.lift.toHighBucketPosition(),
+                new SleepAction(.5),
+                this.arm.toBucketPosition(),
+                new SleepAction(.5),
+                this.wrist.toBucketPosition(),
+                new SleepAction(.5),
+                this.claw.openClaw(),
+                new SleepAction(.5),
+                this.wrist.toFloorPosition(),
+                new SleepAction(.5),
+                this.arm.toParkPosition(),
+                this.lift.toZeroPosition(),
+                new SleepAction(.5)
+        ));
+    }
     /*
     Type: PS4 / Logitech
     Controller: 2
@@ -268,26 +285,17 @@ public class CometBotTeleopCompetition {
         }
     }
 
-    public void lowerSkyHook() {
-        if (this.currentGP2.dpad_down) {
-            hook.lowerHook(1.00);
-        }
-        else{
-            hook.lowerHook(0.00);
-        }
 
-    }
 
-    public void raiseSkyHook() {
+    public void moveSkyHook() {
         if (this.currentGP2.dpad_down) {
-            hook.lowerHook(1.00);
+            hook.moveSkyHook(-1.00);
         }
         else if (this.currentGP2.dpad_up) {
-            hook.raiseHook(1.00);
+            hook.moveSkyHook(1.00);
         }
         else{
-            hook.raiseHook(0.00);
+            hook.moveSkyHook(0.00);
-            hook.lowerHook(0.00);
         }
 
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java

@@ -929,31 +929,31 @@ public class Follower {
      *                  method will use to output the debug data.
      */
     public void telemetryDebug(MultipleTelemetry telemetry) {
-        telemetry.addData("follower busy", isBusy());
+//        telemetry.addData("follower busy", isBusy());
-        telemetry.addData("heading error", headingError);
+//        telemetry.addData("heading error", headingError);
-        telemetry.addData("heading vector magnitude", headingVector.getMagnitude());
+//        telemetry.addData("heading vector magnitude", headingVector.getMagnitude());
-        telemetry.addData("corrective vector magnitude", correctiveVector.getMagnitude());
+//        telemetry.addData("corrective vector magnitude", correctiveVector.getMagnitude());
-        telemetry.addData("corrective vector heading", correctiveVector.getTheta());
+//        telemetry.addData("corrective vector heading", correctiveVector.getTheta());
-        telemetry.addData("translational error magnitude", getTranslationalError().getMagnitude());
+//        telemetry.addData("translational error magnitude", getTranslationalError().getMagnitude());
-        telemetry.addData("translational error direction", getTranslationalError().getTheta());
+//        telemetry.addData("translational error direction", getTranslationalError().getTheta());
-        telemetry.addData("translational vector magnitude", translationalVector.getMagnitude());
+//        telemetry.addData("translational vector magnitude", translationalVector.getMagnitude());
-        telemetry.addData("translational vector heading", translationalVector.getMagnitude());
+//        telemetry.addData("translational vector heading", translationalVector.getMagnitude());
-        telemetry.addData("centripetal vector magnitude", centripetalVector.getMagnitude());
+//        telemetry.addData("centripetal vector magnitude", centripetalVector.getMagnitude());
-        telemetry.addData("centripetal vector heading", centripetalVector.getTheta());
+//        telemetry.addData("centripetal vector heading", centripetalVector.getTheta());
-        telemetry.addData("drive error", driveError);
+//        telemetry.addData("drive error", driveError);
-        telemetry.addData("drive vector magnitude", driveVector.getMagnitude());
+//        telemetry.addData("drive vector magnitude", driveVector.getMagnitude());
-        telemetry.addData("drive vector heading", driveVector.getTheta());
+//        telemetry.addData("drive vector heading", driveVector.getTheta());
-        telemetry.addData("x", getPose().getX());
+//        telemetry.addData("x", getPose().getX());
-        telemetry.addData("y", getPose().getY());
+//        telemetry.addData("y", getPose().getY());
-        telemetry.addData("heading", getPose().getHeading());
+//        telemetry.addData("heading", getPose().getHeading());
-        telemetry.addData("total heading", poseUpdater.getTotalHeading());
+//        telemetry.addData("total heading", poseUpdater.getTotalHeading());
-        telemetry.addData("velocity magnitude", getVelocity().getMagnitude());
+//        telemetry.addData("velocity magnitude", getVelocity().getMagnitude());
-        telemetry.addData("velocity heading", getVelocity().getTheta());
+//        telemetry.addData("velocity heading", getVelocity().getTheta());
-        driveKalmanFilter.debug(telemetry);
+//        driveKalmanFilter.debug(telemetry);
-        telemetry.update();
+//        telemetry.update();
-        if (drawOnDashboard) {
+//        if (drawOnDashboard) {
-            Drawing.drawDebug(this);
+//            Drawing.drawDebug(this);
-        }
+//        }
     }
 
     /**

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

@@ -0,0 +1,36 @@
+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.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 AutoPark {
+
+
+    public void moveToPark(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        new BezierCurve(
+                                new Point(18.000, 126.000, Point.CARTESIAN),
+                                new Point(85.000, 132.750, Point.CARTESIAN),
+                                new Point(84.000, 97.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(135));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

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

@@ -23,14 +23,13 @@ public class HighBasketAutoPath1 {
         PathBuilder builder = new PathBuilder();
         builder
                 .addPath(
-                        // Line 1
                         new BezierCurve(
                                 new Point(8.000, 89.000, Point.CARTESIAN),
                                 new Point(24.000, 96.000, Point.CARTESIAN),
-                                new Point(16.000, 128.000, Point.CARTESIAN)
+                                new Point(18.000, 126.000, Point.CARTESIAN)
                         )
                 )
-                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)).build();
         pathChain = builder.build();
         robot.followPath(pathChain);
     }

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

@@ -22,12 +22,12 @@ public class HighBasketAutoPath2 {
                 .addPath(
                         // Line 2
                         new BezierCurve(
-                                new Point(16.000, 128.000, Point.CARTESIAN),
+                                new Point(18.000, 126.000, Point.CARTESIAN),
-                                new Point(16.000, 120.000, Point.CARTESIAN),
+                                new Point(85.000, 132.750, Point.CARTESIAN),
-                                new Point(32.000, 123.000, Point.CARTESIAN)
+                                new Point(84.000, 97.000, Point.CARTESIAN)
                         )
                 )
-                .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0));
+                .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(135));
         pathChain = builder.build();
         robot.followPath(pathChain);
     }

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

@@ -72,6 +72,9 @@ public class LiftActionsSubsystem {
     }
 
 
+   public Action toZeroPosition() {
+        return new MoveToPosition(0, LiftState.FLOOR);
+   }
 
 
 

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

@@ -25,12 +25,8 @@ public class SkyHookSubsystem {
 
 
 
-   public void raiseHook(double power){
+
-        this.hook.setPower(power);
+    public void moveSkyHook(double power){
-        this.hook.setDirection(DcMotorSimple.Direction.FORWARD);
-        //this.setState(SkyHookState.UP);
-   }
-    public void lowerHook(double power){
         this.hook.setPower(power);
         this.hook.setDirection(DcMotorSimple.Direction.REVERSE);
         //this.setState(SkyHookState.DOWN);