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

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);
}
}