diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CopyofCometBotDriveV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CopyofCometBotDriveV2.java new file mode 100644 index 0000000..5470c54 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CopyofCometBotDriveV2.java @@ -0,0 +1,333 @@ +package org.firstinspires.ftc.teamcode.cometbots.paths; + + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem; + +@Autonomous(name = "Copy of Auto Competition V2", group = "A") +public class CopyofCometBotDriveV2 extends OpMode { + private Follower follower; + private int state; + + private HighBasketPath1 path1; + private HighBasketPath2 path2; + private HighBasketPath3 path3; + private HighBasketPath4 path4; + private HighBasketPath5 path5; + //private HighBasketPath6 path6; + private HighBasketPath6b path6; + + + //private CometBotTeleopCompetition comp; + private static ElapsedTime runtime; + private static boolean initalized = false; + private static boolean followingPath = false; + private DualMotorSliderSubsystem lift; + private ClawSubsystem claw; + private WristSubsystem wrist; + private ArmSubsystem arm; + + @Override + public void init() { + follower = new Follower(hardwareMap); + follower.setMaxPower(.75); + + path1 = new HighBasketPath1(); + path2 = new HighBasketPath2(); + path3 = new HighBasketPath3(); + path4 = new HighBasketPath4(); + path5 = new HighBasketPath5(); + path6 = new HighBasketPath6b(); + //path6 = new HighBasketPath6(); + lift = new DualMotorSliderSubsystem(hardwareMap); + arm = new ArmSubsystem(hardwareMap); + wrist = new WristSubsystem(hardwareMap); + claw = new ClawSubsystem(hardwareMap); + + + lift.init(); + arm.initAuto(); + wrist.InitAuto(); + claw.init(); + + //comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2); + // comp.initCloseClaw(); + runtime = new ElapsedTime(); + } + + + public void loop() { + telemetry.addData("state", state); +// telemetry.addData("arm", arm); +// telemetry.addData("lift", lift); +// telemetry.addData("wrist", wrist); +// telemetry.addData("claw", claw); + telemetry.addData("followingPath", followingPath); + telemetry.addData("busy", follower.isBusy()); + + if (runtime != null) { + telemetry.addData("Runtime (seconds)", runtime.seconds()); + } + switch (state) { + case 0: + + runtime.reset(); + moveToPathOneAndHighBucket(); + break; + case 1: + doArmThing(); + state = 2; + break; + 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: + moveToBasketPath5(); + break; + case 9: + theArmThingAgain(); + break; + case 10: + moveToParkPath6(); + break; + case 11: + moveToPark(); + break; + case 99: + wrist.toFloorPosition(); + break; + default: + telemetry.addLine("default"); + + } + telemetry.update(); + lift.update(); + follower.update(); + + + } + + public void moveToPark() { + if (runtime.seconds() > 26) { + path6.moveToBasketPath6(follower); + state = 99; + } + } + + private void moveToPathOneAndHighBucket() { + path1.moveToPath1(follower); + state = 1; + } + private void moveToBasketPath5() { + if(runtime.seconds() > 19){ + path5.moveToBasketPath5(follower); + lift.toHighBucketReverseDrop(); + wrist.toTravelPosition(); + state = 9; + } + + } + + + public class SetStateAction implements Action { + + private int value; + + public SetStateAction(int value) { + this.value = value; + } + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + state = value; + return false; + } + } + + private Action setStateValue(int value) { + return new SetStateAction(value); + } + + private void doArmThing() { + + claw.thumbOutOfTheWay(); + wrist.toTravelPosition(); + lift.toHighBucketReverseDrop(); + //arm.toReverseBucket(); + //wrist.toReverseBucket(); + //claw.openClaw(); + //wrist.toFloorPosition(); + + } + + + private void theArmThing() { + if(lift.getFixedPosition() >= 4280){ + arm.toReverseBucket(); + if(runtime.seconds() > 12) { + wrist.toReverseBucket(); + } + if(runtime.seconds() > 12.75) { + claw.autoOpenClaw(); + state = 6; + } + } + } + + private void theArmThingAgain() { + if(lift.getFixedPosition() >= 4280){ + arm.toReverseBucket(); + if(runtime.seconds() > 22) { + wrist.toReverseBucket(); + } + if(runtime.seconds() > 23.00) { + claw.autoOpenClaw(); + state = 10; + } + } + } + + + private void moveToPathTwoAndPickSampleUp() { + if (runtime.seconds() > 3.25 && runtime.seconds() < 4) { + wrist.toReverseBucket(); + } + if (runtime.seconds() > 3.5) { + claw.autoOpenClaw(); + } + if (runtime.seconds() > 4) { + wrist.toTravelPosition(); + lift.toFloorPosition(); + path2.moveToPath2(follower); + state = 3; + } + } + + // + private void moveToBasketPath3() { + if (runtime.seconds() > 7.25) { + lift.toHighBucketReverseDrop(); + path3.moveToBasketPath3(follower); + state = 5; + } + } + + private void moveToPickupAgainPath4() { + if (runtime.seconds() > 13) { + wrist.toTravelPosition(); + } + if (runtime.seconds() > 14) { + lift.toFloorPosition(); + path4.moveToPickupPath4(follower); + state = 7; + } + } + + + + private void moveToParkPath6() { + if (runtime.seconds() > 24.5) { + arm.toBucketPosition(); + wrist.toTravelPosition(); + } + if (runtime.seconds() > 25.) { + lift.toFloorPosition(); + claw.closeClaw(); + state = 11; + } + } +// +// 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() { + wrist.toPickupPosition(); + arm.toFloorPosition(); + if (runtime.seconds() > 7){ + claw.closeClaw(); + state = 4; + } + } + + private void doPickUpThingAgain() { + wrist.toPickupPosition(); + arm.toFloorPosition(); + if (runtime.seconds() > 18){ + claw.closeClaw(); + state = 8; + } + } + + +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath6b.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath6b.java new file mode 100644 index 0000000..be11653 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath6b.java @@ -0,0 +1,44 @@ +package org.firstinspires.ftc.teamcode.cometbots.paths; + + +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +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 HighBasketPath6b { + + + public void moveToBasketPath6(Follower robot) { + PathChain pathChain; + PathBuilder builder = new PathBuilder(); + builder + .addPath( + // Line 1 + new BezierLine( + new Point(27.000, 128.000, Point.CARTESIAN), + new Point(92.448, 125.671, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(315), Math.toRadians(270)) + .addPath( + // Line 2 + new BezierLine( + new Point(92.448, 125.671, Point.CARTESIAN), + new Point(93.000, 94.000, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation(); + pathChain = builder.build(); + robot.followPath(pathChain); + } + +} +