diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java index f6cea25..0c1650c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java @@ -1,35 +1,19 @@ package org.firstinspires.ftc.teamcode.cometbots.paths; -import static com.qualcomm.hardware.lynx.commands.core.LynxSetMotorPIDFControlLoopCoefficientsCommand.InternalMotorControlAlgorithm.Max; 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; 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.cometbots.CometBotTeleopCompetition; -//import org.firstinspires.ftc.teamcode.cometbots.paths.HighBasketPath1; import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; -//import org.firstinspires.ftc.teamcode.subsystem.AutoPark; -import org.firstinspires.ftc.teamcode.cometbots.paths.HighBasketPath1; import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem; import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem; import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem; -//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath2; -//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath3; -//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath4; -//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath5; -//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath6; -//import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem; -//import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem; - @Autonomous(name = "Auto Test Competition V2", group = "Dev") public class CometBotDriveV2 extends OpMode { @@ -40,6 +24,8 @@ public class CometBotDriveV2 extends OpMode { private HighBasketPath2 path2; private HighBasketPath3 path3; private HighBasketPath4 path4; + private HighBasketPath5 path5; + private HighBasketPath6 path6; //private CometBotTeleopCompetition comp; @@ -60,6 +46,8 @@ public class CometBotDriveV2 extends OpMode { path2 = new HighBasketPath2(); path3 = new HighBasketPath3(); path4 = new HighBasketPath4(); + path5 = new HighBasketPath5(); + path6 = new HighBasketPath6(); lift = new DualMotorSliderSubsystem(hardwareMap); arm = new ArmSubsystem(hardwareMap); wrist = new WristSubsystem(hardwareMap); @@ -91,61 +79,77 @@ public class CometBotDriveV2 extends OpMode { } switch (state) { case 0: -// claw.thumbOutOfTheWay(); + + runtime.reset(); moveToPathOneAndHighBucket(); break; case 1: - //doArmThing(); - //lift.update(); + doArmThing(); state = 2; break; case 2: moveToPathTwoAndPickSampleUp(); - //lift.update(); break; case 3: -// doPickUpThing(); -// runtime.reset(); - state = 4; + doPickUpThing(); break; case 4: moveToBasketPath3(); break; case 5: - // theArmThing(); - state = 6; + theArmThing(); break; case 6: moveToPickupAgainPath4(); break; - case 7: -// doPickUpThingAgain(); + case 7: + doPickUpThingAgain(); break; case 8: - moveToPickupAgainPath5(); + moveToBasketPath5(); + break; + case 9: + theArmThingAgain(); + break; + case 10: + moveToParkPath6(); + break; + case 11: + moveToPark(); break; -// case 9: -// //theArmThingAgain(); -// break; -// case 10: -// //moveToParkPath6(); -// break; -// case 11: default: telemetry.addLine("default"); } telemetry.update(); - + lift.update(); follower.update(); } + public void moveToPark() { + if (runtime.seconds() > 26) { + path6.moveToBasketPath6(follower); + wrist.toTravelPosition(); + 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 { @@ -168,35 +172,54 @@ public class CometBotDriveV2 extends OpMode { private void doArmThing() { + claw.thumbOutOfTheWay(); + wrist.toTravelPosition(); lift.toHighBucketReverseDrop(); - arm.toReverseBucket(); - wrist.toReverseBucket(); - claw.openClaw(); - wrist.toFloorPosition(); + //arm.toReverseBucket(); + //wrist.toReverseBucket(); + //claw.openClaw(); + //wrist.toFloorPosition(); - state = 2; } -// private void theArmThing() { -// telemetry.addData("busy?", follower.isBusy()); -// telemetry.addData("end?", follower.atParametricEnd()); -// if (follower.atParametricEnd()){ -// follower.breakFollowing(); -// comp.highBucketDropAuto(); -// state = 6; -// } -// follower.breakFollowing(); -// } + private void theArmThing() { + if(lift.getFixedPosition() >= 4280){ + arm.toReverseBucket(); + if(runtime.seconds() > 12) { + wrist.toReverseBucket(); + } + if(runtime.seconds() > 12.75) { + claw.openClaw(); + state = 6; + } + } + } + + private void theArmThingAgain() { + if(lift.getFixedPosition() >= 4280){ + arm.toReverseBucket(); + if(runtime.seconds() > 22) { + wrist.toReverseBucket(); + } + if(runtime.seconds() > 23.00) { + claw.openClaw(); + state = 10; + } + } + } -// private void theArmThingAgain() { -// follower.breakFollowing(); -// comp.highBucketDropAuto(); -// state = 10; -// } private void moveToPathTwoAndPickSampleUp() { - if (runtime.seconds() > 6) { + if (runtime.seconds() > 3.25 && runtime.seconds() < 4) { + wrist.toReverseBucket(); + } + if (runtime.seconds() > 3.5) { + claw.openClaw(); + } + if (runtime.seconds() > 4) { + wrist.toTravelPosition(); + lift.toFloorPosition(); path2.moveToPath2(follower); state = 3; } @@ -204,15 +227,19 @@ public class CometBotDriveV2 extends OpMode { // private void moveToBasketPath3() { - if (runtime.seconds() > 10) - { + 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; } @@ -222,19 +249,17 @@ public class CometBotDriveV2 extends OpMode { } -// private void moveToParkPath6() { -// if (!followingPath) { -// path6.moveToParkPath6(follower); -// followingPath = true; -// } -// if (runtime != null) { -// telemetry.addData("Runtime (seconds)", runtime.seconds()); -// if (follower.atParametricEnd() || runtime.seconds() > 48.0) { -// state = 11; -// followingPath = false; -// } -// } -// } + 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) { @@ -283,18 +308,24 @@ public class CometBotDriveV2 extends OpMode { // )); // } // -// private void doPickUpThing() { -// follower.breakFollowing(); -// thePickUpAuto(); -// state = 4; -// } -// -// private void doPickUpThingAgain() { -// follower.breakFollowing(); -// thePickUpAuto(); -// state = 8; -// } -// -// + 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/HighBasketPath2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath2.java index ad5c2a4..def967c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath2.java @@ -24,7 +24,7 @@ public class HighBasketPath2 { // Line 1 new BezierLine( new Point(20.000, 130.000, Point.CARTESIAN), - new Point(26.000, 117.000, Point.CARTESIAN) + new Point(27.000, 117.000, Point.CARTESIAN) ) ) .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath4.java index b699774..feb7f5a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath4.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath4.java @@ -23,7 +23,7 @@ public class HighBasketPath4 { // Line 1 new BezierLine( new Point(22.000, 132.000, Point.CARTESIAN), - new Point(26.000, 127.500, Point.CARTESIAN) + new Point(27.000, 128.500, Point.CARTESIAN) ) ) .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath5.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath5.java new file mode 100644 index 0000000..442684a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath5.java @@ -0,0 +1,35 @@ +package org.firstinspires.ftc.teamcode.cometbots.paths; + + +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +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 HighBasketPath5 { + + + public void moveToBasketPath5(Follower robot) { + PathChain pathChain; + PathBuilder builder = new PathBuilder(); + builder + .addPath( + // Line 1 + new BezierLine( + new Point(27.000, 127.000, Point.CARTESIAN), + new Point(22.000, 132.000, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(320)); + pathChain = builder.build(); + robot.followPath(pathChain); + } + +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath6.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath6.java new file mode 100644 index 0000000..ebe176d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath6.java @@ -0,0 +1,36 @@ +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 HighBasketPath6 { + + + public void moveToBasketPath6(Follower robot) { + PathChain pathChain; + PathBuilder builder = new PathBuilder(); + builder + .addPath( + // Line 1 + new BezierLine( + new Point(27.000, 128.500, Point.CARTESIAN), + new Point(84.00, 100.00, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)); + pathChain = builder.build(); + robot.followPath(pathChain); + } + +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java index 089bccf..3f2cdfa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java @@ -21,18 +21,20 @@ public class RobotConstants { public final static int slideBelowFloor = -150; public final static int backwardBucketDrop = 4670; public final static double armBucket = 0.45; - public final static double armInit = 0.135; - public final static double wristInit = 0.25; + public final static double armInit = 0.13; + public final static double wristInit = 0.125; public final static double wristPickup = 0.475; public final static double wristBucket = 0.56; public final static double wristFloor = 0.75; - public final static double wristBackwardBucket = 0.775; + public final static double wristBackwardBucket = 0.725; public final static double wristRung = 0.55; public final static double wristSpeciemen = 0.1; public final static int toBar = 500; + public final static double wristtravel = 0.525; + public final static int toFloor = 0; public final static int liftToFloorPos = 350; public final static int liftToSubmarinePos = 350; @@ -43,6 +45,6 @@ public class RobotConstants { public final static int liftToHighRungAttach = 1050; - public final static int liftToHighBucketPos = 4000; + public final static int liftToHighBucketPos = 4500; public final static double liftPower = 1; } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java index 3bf3826..5ba32a0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java @@ -9,6 +9,7 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristHangBlu import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristInit; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristPickup; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpeciemen; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristtravel; import androidx.annotation.NonNull; @@ -21,7 +22,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx; public class WristSubsystem { public enum WristState { - FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT, REVERSE_BUCKET + FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT, REVERSE_BUCKET, TRAVEL } public ServoImplEx wrist; @@ -97,6 +98,11 @@ public class WristSubsystem { wrist.setPosition(wristBackwardBucket); setState(WristState.REVERSE_BUCKET); } + public void toTravelPosition(){ + wrist.setPosition(wristtravel); + setState(WristState.TRAVEL); + } + public WristState getState() { return this.state; }