Working actions files sky hook and more

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

@@ -0,0 +1,195 @@
+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.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;
+
+@Autonomous(name = "Auto Test", group = "Dev")
+public class BlueBasketAutoWithDrop extends OpMode {
+    private Telemetry telemetryA;
+
+    private Follower follower;
+
+    private PathChain path;
+
+    private final Pose startPose = new Pose(8, 65);
+
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+
+        follower.setMaxPower(.75);
+
+        follower.setStartingPose(startPose);
+
+        path = follower.pathBuilder()
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(8.000, 65.000, Point.CARTESIAN),
+                                new Point(33.000, 65.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 2
+                        new BezierLine(
+                                new Point(33.000, 65.000, Point.CARTESIAN),
+                                new Point(31.000, 65.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 3
+                        new BezierCurve(
+                                new Point(31.000, 65.000, Point.CARTESIAN),
+                                new Point(26.000, 32.000, Point.CARTESIAN),
+                                new Point(60.000, 34.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
+                .addPath(
+                        // Line 4
+                        new BezierLine(
+                                new Point(60.000, 34.000, Point.CARTESIAN),
+                                new Point(60.000, 24.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 5
+                        new BezierLine(
+                                new Point(60.000, 24.000, Point.CARTESIAN),
+                                new Point(14.500, 24.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 6
+                        new BezierLine(
+                                new Point(14.500, 24.000, Point.CARTESIAN),
+                                new Point(18.000, 24.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 1
+                        new BezierCurve(
+                                new Point(18.000, 24.000, Point.CARTESIAN),
+                                new Point(18.000, 67.000, Point.CARTESIAN),
+                                new Point(31.000, 67.000, Point.CARTESIAN)
+                        )
+                )
+                .setTangentHeadingInterpolation()
+                .addPath(
+                        // Line 8
+                        new BezierLine(
+                                new Point(31.000, 67.500, Point.CARTESIAN),
+                                new Point(33.000, 67.500, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 9
+                        new BezierLine(
+                                new Point(33.000, 67.500, Point.CARTESIAN),
+                                new Point(31.000, 67.500, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 10
+                        new BezierCurve(
+                                new Point(31.000, 67.500, Point.CARTESIAN),
+                                new Point(26.000, 32.000, Point.CARTESIAN),
+                                new Point(60.000, 34.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
+                .addPath(
+                        // Line 11
+                        new BezierLine(
+                                new Point(60.000, 34.000, Point.CARTESIAN),
+                                new Point(60.000, 12.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 12
+                        new BezierLine(
+                                new Point(60.000, 12.000, Point.CARTESIAN),
+                                new Point(12.000, 12.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 13
+                        new BezierLine(
+                                new Point(12.000, 12.000, Point.CARTESIAN),
+                                new Point(20.000, 12.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 14
+                        new BezierLine(
+                                new Point(20.000, 12.000, Point.CARTESIAN),
+                                new Point(20.000, 24.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 15
+                        new BezierLine(
+                                new Point(20.000, 24.000, Point.CARTESIAN),
+                                new Point(14.000, 24.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 16
+                        new BezierCurve(
+                                new Point(14.000, 24.000, Point.CARTESIAN),
+                                new Point(14.000, 70.000, Point.CARTESIAN),
+                                new Point(31.000, 70.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(0))
+                .addPath(
+                        // Line 17
+                        new BezierLine(
+                                new Point(31.000, 70.000, Point.CARTESIAN),
+                                new Point(33.000, 70.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 18
+                        new BezierLine(
+                                new Point(33.000, 70.000, Point.CARTESIAN),
+                                new Point(31.000, 70.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180)).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/NetAuto.java

@@ -18,14 +18,10 @@ public class NetAuto extends OpMode {
         follower = new Follower(hardwareMap);
     }
 
-    public Action autoLine1() {return new AutoLine1(follower);}
-
     @Override
     public void loop() {
         follower.update();
-        Actions.runBlocking(
+
-                autoLine1()
-        );
         follower.telemetryDebug(telemetry);
     }
 }

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

@@ -10,10 +10,10 @@ public class PedroConstants {
     /*
         Motor configuration names
     */
-    public static final String FRONT_LEFT_MOTOR = "Drive front lt";
+    public static final String FRONT_LEFT_MOTOR = "front-left";
-    public static final String BACK_LEFT_MOTOR = "Drive back lt";
+    public static final String BACK_LEFT_MOTOR = "back-left";
-    public static final String FRONT_RIGHT_MOTOR = "Drive front rt";
+    public static final String FRONT_RIGHT_MOTOR = "front-right";
-    public static final String BACK_RIGHT_MOTOR = "Drive back rt";
+    public static final String BACK_RIGHT_MOTOR = "back-right";
 
     /*
         Motor directions
@@ -31,7 +31,7 @@ public class PedroConstants {
     /*
         Motor Max Power
      */
-    public static final double MAX_POWER = .50;
+    public static final double MAX_POWER = .675;
 
     /*
         IMU
@@ -45,9 +45,9 @@ public class PedroConstants {
     /*
         Dead wheels
      */
-    public static final String LEFT_ENCODER = "encoder left";
+    public static final String RIGHT_ENCODER = "back-right";
-    public static final String RIGHT_ENCODER = "encoder right";
+    public static final String BACK_ENCODER = "front-right";
-    public static final String BACK_ENCODER = "encoder back";
+    public static final String LEFT_ENCODER = "front-left";
 
     /*
         Dead wheel directions

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

@@ -99,9 +99,6 @@ public class CometBotAutoDevelopment {
         this.telemetry.addData("Lift Position", this.lift.getPosition());
     }
 
-    public Action autoLine1() {return new AutoLine1(follower);}
-
-
 
     /*
     Controller: 1

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

@@ -294,15 +294,15 @@ public class CometBotTeleopCompetition {
         }
     }
 
-    public void raiseSkyHook() {
+    public void lowerSkyHook() {
         hook.raiseHook(currentGP2.left_trigger);
 
 
     }
 
 
-    public void lowerSkyHook() {
+    public void raiseSkyHook() {
-        hook.lowerHook(currentGP2.right_trigger);
+        hook.lowerHook(currentGP2.right_trigger * 2.0);
 
 
     }

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

@@ -29,6 +29,10 @@
 
 package org.firstinspires.ftc.teamcode.cometbots.tests;
 
+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;
@@ -38,7 +42,14 @@ import com.qualcomm.robotcore.hardware.Gamepad;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
 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.Path;
+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;
@@ -49,9 +60,21 @@ public class SpecimenTest extends LinearOpMode {
 
     private final ElapsedTime runtime = new ElapsedTime();
 
+    private PathChain path;
+
+    private Follower robot;
+
+    private final Pose startPose = new Pose(36, 72);
+
+
+
+
+    public AutoLine1 firstPath = new AutoLine1();
+
     @Override
     public void runOpMode() {
 
+
         /*
          * Instantiate Lift
          */
@@ -60,19 +83,35 @@ public class SpecimenTest extends LinearOpMode {
         ArmActionsSubsystem arm = new ArmActionsSubsystem(hardwareMap);
         ClawActionsSubsystem claw = new ClawActionsSubsystem(hardwareMap);
         Follower robot = new Follower(hardwareMap);
+        firstPath.moveToPath1(robot);
+
+        /*
+          Robot stuff
+         */
+        robot.setStartingPose(startPose);
+        path = robot.pathBuilder().addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(37.500, 72.000, Point.CARTESIAN),
+                                new Point(36.000, 72.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0)).build();
 
         /*
          * Instantiate gamepad state holders
          */
         Gamepad currentGamepad1 = new Gamepad();
         Gamepad previousGamepad1 = new Gamepad();
+        Gamepad currentGamepad2 = new Gamepad();
+        Gamepad previousGamepad2 = new Gamepad();
 
         lift.init();
         wrist.init();
         arm.init();
         claw.init();
-        robot.setMaxPower(.75);
+        robot.setMaxPower(.40);
-        robot.startTeleopDrive();
+        robot.followPath(path);
 
         waitForStart();
         runtime.reset();
@@ -84,6 +123,10 @@ public class SpecimenTest extends LinearOpMode {
             previousGamepad1.copy(currentGamepad1);
             currentGamepad1.copy(gamepad1);
 
+            previousGamepad2.copy(currentGamepad2);
+            currentGamepad2.copy(gamepad2);
+
+
             robot.setTeleOpMovementVectors(-currentGamepad1.left_stick_y, -currentGamepad1.left_stick_x, -currentGamepad1.right_stick_x);
             //robot.update();
 
@@ -98,25 +141,19 @@ public class SpecimenTest extends LinearOpMode {
                 claw.switchState();
             }
             if (currentGamepad1.dpad_right && !previousGamepad1.dpad_right) {
+                robot.update();
                 Actions.runBlocking(
                         new SequentialAction(
                                 wrist.toFloorPosition(),
                                 lift.toHighRung(),
                                 wrist.toSpeciemenBar(),
                                 lift.dropToHighRung()
+               //                 reverseMoveToPath(robot),
+                 //               claw.openClaw(),
+                   //             wrist.toFloorPosition(),
+                     //           lift.toFloorPosition()
                         )
                 );
-
-            }
-            if (currentGamepad1.dpad_left && !previousGamepad1.dpad_left) {
-                Actions.runBlocking(
-                        new SequentialAction(
-                                claw.openClaw(),
-                                wrist.toFloorPosition(),
-                                lift.toFloorPosition()
-                        )
-                );
-
             }
 
 
@@ -136,6 +173,17 @@ public class SpecimenTest extends LinearOpMode {
                 lift.setPosition(lift.getPosition() - 25);
             }
 
+            if (currentGamepad2.x && !previousGamepad2.x) {
+                while (true) {
+                    robot.update();
+                    if (!robot.isBusy()) {
+                        robot.breakFollowing();
+                        robot.startTeleopDrive();
+                        break;
+                    }
+                }
+            }
+
 
             // Show the elapsed game time and wheel power.
             telemetry.addData("Status", "Run Time: " + runtime.toString());
@@ -146,4 +194,4 @@ public class SpecimenTest extends LinearOpMode {
             telemetry.update();
         }
     }
-}
+}

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

@@ -22,13 +22,13 @@ public class RobotConstants {
 
     public final static int liftToFloorPos = 350;
     public final static int liftToSubmarinePos = 350;
-    public final static int liftToLowBucketPos = 2250;
+    public final static int liftToLowBucketPos = 2650;
     public final static int liftToHighRung = 2100;
     public final static int dropToHighRung = 1675;
 
     public final static int liftToHighRungAttach = 1050;
 
 
-    public final static int liftToHighBucketPos = 3850;
+    public final static int liftToHighBucketPos = 4900;
-    public final static double liftPower = .625;
+    public final static double liftPower = 1;
 }

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

@@ -1,81 +1,36 @@
 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.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 AutoLine1 implements Action {
+public class AutoLine1 {
 
-    private Follower actionRobot;
+    private Pose startPose = new Pose(36, 72);
-    private PathChain pathChain;
-
-    private Pose startPose = new Pose(8,65);
-
-    public AutoLine1(Follower robot) {
-        this.actionRobot = robot;
-        this.actionRobot.setStartingPose(startPose);
 
+    public void moveToPath1(Follower robot) {
+        PathChain pathChain;
+        robot.setStartingPose(startPose);
         PathBuilder builder = new PathBuilder();
         builder
                 .addPath(
-                        // Line 1
-                        new BezierLine(
-                                new Point(8.000, 65.000, Point.CARTESIAN),
-                                new Point(36.000, 72.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 2
                         new BezierCurve(
                                 new Point(36.000, 72.000, Point.CARTESIAN),
-                                new Point(36.000, 36.000, Point.CARTESIAN),
+                                new Point(37.500, 72.000, Point.CARTESIAN)
-                                new Point(48.000, 36.000, Point.CARTESIAN)
                         )
-                )
+                );
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 3
-                        new BezierCurve(
-                                new Point(48.000, 36.000, Point.CARTESIAN),
-                                new Point(72.000, 30.000, Point.CARTESIAN),
-                                new Point(57.000, 24.000, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 4
-                        new BezierLine(
-                                new Point(57.000, 24.000, Point.CARTESIAN),
-                                new Point(10.000, 24.000, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation();
-
         pathChain = builder.build();
-
+        robot.followPath(pathChain);
-        this.actionRobot.followPath(this.pathChain);
     }
 
-    @Override
-    public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-        this.actionRobot.update();
-        return this.actionRobot.isBusy();
-    }
 }
 

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

@@ -0,0 +1,40 @@
+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/SkyHookSubsystem.java

@@ -19,7 +19,7 @@ public class SkyHookSubsystem {
     private SkyHookState skyHookState;
 
     public SkyHookSubsystem(HardwareMap hardwareMap) {
-        hook = hardwareMap.get(DcMotor.class, LEFT_ENCODER);
+        hook = hardwareMap.get(DcMotor.class, "skyhook");
     }
 
 
@@ -34,6 +34,7 @@ public class SkyHookSubsystem {
         this.hook.setDirection(DcMotorSimple.Direction.REVERSE);
         this.setState(SkyHookState.DOWN);
     }
+
     public void init() {
         hook.setPower(0);
         hook.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);