SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Added new paths for autonomous

Browse Source
This commit is contained in:
robotics1
2025-01-23 17:47:00 -08:00
parent 7dcdde3b49
commit f03515a169
3 changed files with 86 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

102
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java
View File

@ -39,7 +39,7 @@ public class CometBotDriveV2 extends OpMode {
private HighBasketPath1 path1; private HighBasketPath1 path1;
private HighBasketPath2 path2; private HighBasketPath2 path2;
private HighBasketPath3 path3; private HighBasketPath3 path3;
private HighBasketPath4 path4;
//private CometBotTeleopCompetition comp; //private CometBotTeleopCompetition comp;
@ -59,61 +59,71 @@ public class CometBotDriveV2 extends OpMode {
path1 = new HighBasketPath1(); path1 = new HighBasketPath1();
path2 = new HighBasketPath2(); path2 = new HighBasketPath2();
path3 = new HighBasketPath3(); path3 = new HighBasketPath3();
path4 = new HighBasketPath4();
lift = new DualMotorSliderSubsystem(hardwareMap); lift = new DualMotorSliderSubsystem(hardwareMap);
arm = new ArmSubsystem(hardwareMap); arm = new ArmSubsystem(hardwareMap);
wrist = new WristSubsystem(hardwareMap); wrist = new WristSubsystem(hardwareMap);
claw = new ClawSubsystem(hardwareMap); claw = new ClawSubsystem(hardwareMap);
lift.init(); lift.init();
arm.initAuto(); arm.initAuto();
wrist.InitAuto(); wrist.InitAuto();
claw.init(); claw.init();
//comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2); //comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
// comp.initCloseClaw(); // comp.initCloseClaw();
runtime = new ElapsedTime();
} }
public void loop() { public void loop() {
telemetry.addData("state", state); 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("followingPath", followingPath);
telemetry.addData("busy", follower.isBusy()); telemetry.addData("busy", follower.isBusy());
if (runtime != null) { if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds()); telemetry.addData("Runtime (seconds)", runtime.seconds());
} }
switch (state) { switch (state) {
case 0: case 0:
// claw.thumbOutOfTheWay();
moveToPathOneAndHighBucket(); moveToPathOneAndHighBucket();
break; break;
case 1: case 1:
doArmThing(); //doArmThing();
//lift.update();
state = 2; state = 2;
break; break;
case 2: case 2:
moveToPathTwoAndPickSampleUp(); moveToPathTwoAndPickSampleUp();
//lift.update();
break; break;
case 3: case 3:
// doPickUpThing(); // doPickUpThing();
// runtime.reset();
state = 4; state = 4;
break; break;
case 4: case 4:
// moveToBasketPath3(); moveToBasketPath3();
break; break;
// case 5: case 5:
// theArmThing(); // theArmThing();
// break; state = 6;
// case 6: break;
// moveToPickupAgainPath4(); case 6:
// break; moveToPickupAgainPath4();
// case 7: break;
case 7:
// doPickUpThingAgain(); // doPickUpThingAgain();
// break; break;
// case 8: case 8:
// moveToPickupAgainPath5(); moveToPickupAgainPath5();
// break; break;
// case 9: // case 9:
// //theArmThingAgain(); // //theArmThingAgain();
// break; // break;
@ -121,7 +131,8 @@ public class CometBotDriveV2 extends OpMode {
// //moveToParkPath6(); // //moveToParkPath6();
// break; // break;
// case 11: // case 11:
default:
telemetry.addLine("default");
} }
telemetry.update(); telemetry.update();
@ -132,8 +143,8 @@ public class CometBotDriveV2 extends OpMode {
} }
private void moveToPathOneAndHighBucket() { private void moveToPathOneAndHighBucket() {
path1.moveToPath1(follower); path1.moveToPath1(follower);
state = 1; state = 1;
} }
public class SetStateAction implements Action { public class SetStateAction implements Action {
@ -143,6 +154,7 @@ public class CometBotDriveV2 extends OpMode {
public SetStateAction(int value) { public SetStateAction(int value) {
this.value = value; this.value = value;
} }
@Override @Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) { public boolean run(@NonNull TelemetryPacket telemetryPacket) {
state = value; state = value;
@ -157,8 +169,8 @@ public class CometBotDriveV2 extends OpMode {
private void doArmThing() { private void doArmThing() {
lift.toHighBucketReverseDrop(); lift.toHighBucketReverseDrop();
arm.toBucketPosition(); arm.toReverseBucket();
wrist.toBucketPosition(); wrist.toReverseBucket();
claw.openClaw(); claw.openClaw();
wrist.toFloorPosition(); wrist.toFloorPosition();
@ -184,46 +196,32 @@ public class CometBotDriveV2 extends OpMode {
// } // }
private void moveToPathTwoAndPickSampleUp() { private void moveToPathTwoAndPickSampleUp() {
if (!follower.isBusy()) { if (runtime.seconds() > 6) {
path2.moveToPath2(follower); path2.moveToPath2(follower);
state = 3; state = 3;
} }
} }
//
//
private void moveToBasketPath3() { private void moveToBasketPath3() {
if (!follower.isBusy()) { if (runtime.seconds() > 10)
{
path3.moveToBasketPath3(follower); path3.moveToBasketPath3(follower);
state = 5; state = 5;
} }
} }
// private void moveToPickupAgainPath4() { private void moveToPickupAgainPath4() {
// if (!followingPath) { if (runtime.seconds() > 14) {
// path4.moveToPickupAgainPath4(follower); path4.moveToPickupPath4(follower);
// followingPath = true; state = 7;
// } }
// if (runtime != null) { }
// telemetry.addData("Runtime (seconds)", runtime.seconds());
// if (follower.atParametricEnd() || runtime.seconds() > 27.0) { private void moveToPickupAgainPath5() {
// state = 7;
// followingPath = false; }
// }
// }
// }
//
// private void moveToPickupAgainPath5() {
// if (!followingPath) {
// path5.moveToPickupAgainPath5(follower);
// followingPath = true;
// }
// if (runtime != null) {
// telemetry.addData("Runtime (seconds)", runtime.seconds());
// if (follower.atParametricEnd() || runtime.seconds() > 36.0) {
// state = 9;
// followingPath = false;
// }
// }
// }
// private void moveToParkPath6() { // private void moveToParkPath6() {
// if (!followingPath) { // if (!followingPath) {
// path6.moveToParkPath6(follower); // path6.moveToParkPath6(follower);

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath3.java
View File

@ -23,7 +23,7 @@ public class HighBasketPath3 {
// Line 1 // Line 1
new BezierLine( new BezierLine(
new Point(26.000, 117.000, Point.CARTESIAN), new Point(26.000, 117.000, Point.CARTESIAN),
new Point(10.000, 89.00, Point.CARTESIAN) new Point(22.000, 132.000, Point.CARTESIAN)
) )
) )
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(320)); .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(320));

35
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath4.java Normal file
View File

@ -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 HighBasketPath4 {
public void moveToPickupPath4(Follower robot) {
PathChain pathChain;
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 1
new BezierLine(
new Point(22.000, 132.000, Point.CARTESIAN),
new Point(26.000, 127.500, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0));
pathChain = builder.build();
robot.followPath(pathChain);
}
}