Scaffolding of Autonomous class

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

@@ -0,0 +1,35 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.runmodes.Auto;
+
+
+@Autonomous(name = "CometBot Auto", group = "Debug")
+public class CometBotAuto extends OpMode {
+    public Auto auto;
+
+    @Override
+    public void init() {
+        auto = new Auto(hardwareMap, telemetry, new Follower(hardwareMap));
+    }
+
+    @Override
+    public void start() {
+        auto.start();
+    }
+
+    @Override
+    public void loop() {
+        auto.update();
+        telemetry.addData("Arm State", auto.arm.getState());
+        telemetry.addData("Arm Position", auto.arm.getPosition());
+        telemetry.addData("Claw State", auto.claw.getState());
+        telemetry.addData("Wrist State", auto.wrist.getState());
+        telemetry.addData("Wrist Position", auto.wrist.getPosition());
+        telemetry.update();
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java

@@ -9,9 +9,12 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path;
 import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+import org.firstinspires.ftc.teamcode.pedroPathing.util.Timer;
+import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
 import static org.firstinspires.ftc.teamcode.util.action.FieldConstants.*;
 
+import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
 import org.firstinspires.ftc.teamcode.util.action.Action;
 import org.firstinspires.ftc.teamcode.util.action.RunAction;
 import org.firstinspires.ftc.teamcode.util.action.SequentialAction;
@@ -19,114 +22,62 @@ import org.firstinspires.ftc.teamcode.util.action.SleepAction;
 
 public class Auto {
 
-    private RobotStart startLocation;
-
     public ClawSubsystem claw;
-    public ClawSubsystem.ClawState clawState;
+    public ArmSubsystem arm;
+    public WristSubsystem wrist;
+
+    public Timer clawTimer = new Timer();
+    public Timer armTimer = new Timer();
+    public Timer wristTimer = new Timer();
 
     public Follower follower;
     public Telemetry telemetry;
 
-    public RunAction transfer;
+    public Auto(HardwareMap hardwareMap, Telemetry telemetry, Follower follower) {
-    public Path preload, element1, score1, element2, score2, element3, score3, park;
+        claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
-    private Pose startPose, preloadPose, element1Pose, element1ControlPose, element2Pose, element2ControlPose, element3Pose, element3ControlPose, elementScorePose, parkControlPose, parkPose;
+        arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
-
+        wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
-    public Auto(HardwareMap hardwareMap, Telemetry telemetry, Follower follower, boolean isBlue, boolean isBucket) {
-        claw = new ClawSubsystem(hardwareMap, clawState);
 
         this.follower = follower;
         this.telemetry = telemetry;
 
-        startLocation = isBlue ? (isBucket ? RobotStart.BLUE_BUCKET : RobotStart.BLUE_OBSERVATION) : (isBucket ? RobotStart.RED_BUCKET : RobotStart.RED_OBSERVATION);
+        init();
-
-        createPoses();
-        buildPaths();
-
-        transfer = new RunAction(this::transfer);
     }
 
-    public void init_loop() {
+    public void init() {
-    }
+        claw.init();
-
+        arm.init();
-    public void createPoses() {
+        wrist.init();
-        switch (startLocation) {
-            case BLUE_BUCKET:
-                startPose = blueBucketStartPose;
-                preloadPose = blueBucketPreloadPose;
-                element1ControlPose = blueBucketLeftSampleControlPose;
-                element1Pose = blueBucketLeftSamplePose;
-                element2ControlPose = blueBucketMidSampleControlPose;
-                element2Pose = blueBucketMidSamplePose;
-                element3ControlPose = blueBucketRightSampleControlPose;
-                element3Pose = blueBucketRightSamplePose;
-                elementScorePose = blueBucketScorePose;
-                parkControlPose = blueBucketParkControlPose;
-                parkPose = blueBucketParkPose;
-                break;
-
-            case BLUE_OBSERVATION:
-                startPose = blueObservationStartPose;
-                //parkPose = blueObservationPark;
-                break;
-
-            case RED_BUCKET:
-                startPose = redBucketStartPose;
-                //parkPose = redBucketPark;
-                break;
-
-            case RED_OBSERVATION:
-                startPose = redObservationStartPose;
-                //parkPose = redObservationPark;
-                break;
-        }
-
-    }
-
-    public void buildPaths() {
-        follower.setStartingPose(startPose);
-
-        preload = new Path(new BezierLine(new Point(startPose), new Point(preloadPose)));
-        preload.setLinearHeadingInterpolation(startPose.getHeading(), preloadPose.getHeading());
-
-        element1 = new Path(new BezierCurve(new Point(preloadPose), new Point(element1ControlPose), new Point(element1Pose)));
-        element1.setLinearHeadingInterpolation(preloadPose.getHeading(), element1Pose.getHeading());
-
-        score1 = new Path(new BezierLine(new Point(element1Pose), new Point(elementScorePose)));
-        score1.setLinearHeadingInterpolation(element1Pose.getHeading(), elementScorePose.getHeading());
-
-        element2 = new Path(new BezierCurve(new Point(element1Pose), new Point(element2ControlPose), new Point(element2Pose)));
-        element2.setLinearHeadingInterpolation(element1Pose.getHeading(), element2Pose.getHeading());
-
-        score2 = new Path(new BezierLine(new Point(element2Pose), new Point(elementScorePose)));
-        score2.setLinearHeadingInterpolation(element2Pose.getHeading(), elementScorePose.getHeading());
-
-        element3 = new Path(new BezierCurve(new Point(element2Pose), new Point(element3ControlPose), new Point(element3Pose)));
-        element3.setLinearHeadingInterpolation(element2Pose.getHeading(), element3Pose.getHeading());
-
-        score3 = new Path(new BezierLine(new Point(element3Pose), new Point(elementScorePose)));
-        score3.setLinearHeadingInterpolation(element3Pose.getHeading(), elementScorePose.getHeading());
-
-        park = new Path(new BezierCurve(new Point(elementScorePose), new Point(parkControlPose), new Point(parkPose)));
-        park.setLinearHeadingInterpolation(elementScorePose.getHeading(), parkPose.getHeading());
     }
 
     public void start() {
+        clawTimer.resetTimer();
+        armTimer.resetTimer();
+        wristTimer.resetTimer();
+
         claw.start();
-        //lift.start();
+        arm.start();
-        //extend.start();
+        wrist.start();
-        //intake.start();
-        //box.start();
     }
 
     public void update() {
-        follower.update();
+        //follower.update();
-        //lift.updatePIDF();
+        if (clawTimer.getElapsedTimeSeconds() > 2) {
-        //extend.updatePIDF();
+            claw.openClaw();
+        }
+        if (armTimer.getElapsedTimeSeconds() > 4) {
+            arm.engageArm();
+        }
+        if (clawTimer.getElapsedTimeSeconds() > 6) {
+            claw.closeClaw();
+        }
+        if (armTimer.getElapsedTimeSeconds() > 8) {
+            arm.bucketArm();
+            wrist.bucketWrist();
+        }
+        if (clawTimer.getElapsedTimeSeconds() > 10) {
+            claw.openClaw();
+        }
     }
 
-    private Action transfer() {
-        return new SequentialAction(
-            new SleepAction(1)
-        );
-    }
 }