From 3aed4b86763527520384a44c33719ebf62bc6528 Mon Sep 17 00:00:00 2001 From: Carlos Rivas Date: Wed, 30 Oct 2024 22:44:19 -0700 Subject: [PATCH] Scaffolding of Autonomous class --- .../ftc/teamcode/CometBotAuto.java | 35 +++++ .../ftc/teamcode/runmodes/Auto.java | 131 ++++++------------ 2 files changed, 76 insertions(+), 90 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotAuto.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotAuto.java new file mode 100644 index 0000000..1ad67fd --- /dev/null +++ b/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(); + } + +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java index 494c69c..257c3d8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java +++ b/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 Path preload, element1, score1, element2, score2, element3, score3, park; - private Pose startPose, preloadPose, element1Pose, element1ControlPose, element2Pose, element2ControlPose, element3Pose, element3ControlPose, elementScorePose, parkControlPose, parkPose; - - public Auto(HardwareMap hardwareMap, Telemetry telemetry, Follower follower, boolean isBlue, boolean isBucket) { - claw = new ClawSubsystem(hardwareMap, clawState); + public Auto(HardwareMap hardwareMap, Telemetry telemetry, Follower follower) { + claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED); + arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK); + wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR); this.follower = follower; this.telemetry = telemetry; - startLocation = isBlue ? (isBucket ? RobotStart.BLUE_BUCKET : RobotStart.BLUE_OBSERVATION) : (isBucket ? RobotStart.RED_BUCKET : RobotStart.RED_OBSERVATION); - - createPoses(); - buildPaths(); - - transfer = new RunAction(this::transfer); + init(); } - public void init_loop() { - } - - public void createPoses() { - 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 init() { + claw.init(); + arm.init(); + wrist.init(); } public void start() { + clawTimer.resetTimer(); + armTimer.resetTimer(); + wristTimer.resetTimer(); + claw.start(); - //lift.start(); - //extend.start(); - //intake.start(); - //box.start(); + arm.start(); + wrist.start(); } public void update() { - follower.update(); - //lift.updatePIDF(); - //extend.updatePIDF(); + //follower.update(); + if (clawTimer.getElapsedTimeSeconds() > 2) { + 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) - ); - } }