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

Path two working to pick up 1st specimen

Browse Source
This commit is contained in:
robotics1
2025-01-21 17:10:30 -08:00
parent 93ff65ee53
commit 2cb8ce41dd
2 changed files with 57 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -37,6 +37,8 @@ public class CometBotDriveV2 extends OpMode {
private int state;
private HighBasketPath1 path1;
private HighBasketPath2 path2;
private HighBasketPath3 path3;
@ -55,7 +57,8 @@ public class CometBotDriveV2 extends OpMode {
follower.setMaxPower(.75);
path1 = new HighBasketPath1();
path2 = new HighBasketPath2();
path3 = new HighBasketPath3();
lift = new DualMotorSliderSubsystem(hardwareMap);
arm = new ArmSubsystem(hardwareMap);
wrist = new WristSubsystem(hardwareMap);
@ -88,10 +91,11 @@ public class CometBotDriveV2 extends OpMode {
break;
case 1:
// doArmThing2();
state = 2;
break;
case 2:
moveToPathTwoAndPickSampleUp();
break;
// case 2:
// moveToPathTwoAndPickSampleUp();
// break;
// case 3:
// doPickUpThing();
// break;
@ -181,20 +185,20 @@ public class CometBotDriveV2 extends OpMode {
// state = 10;
// }
// private void moveToPathTwoAndPickSampleUp() {
// if (!followingPath) {
// path2.moveToPath2(follower);
// followingPath = true;
// }
// if (runtime != null) {
// telemetry.addData("Runtime (seconds)", runtime.seconds());
// if (follower.atParametricEnd() || runtime.seconds() > 22.0) {
// state = 3;
// followingPath = false;
// }
// }
// }
private void moveToPathTwoAndPickSampleUp() {
if (!follower.isBusy()) {
path2.moveToPath2(follower);
state = 3;
}
}
//
private void moveToPathBasketPath3() {
if (!follower.isBusy()) {
path3.moveToBasketPath3(follower);
state = 3;
} }
// private void moveToPickupAgainPath4() {
// if (!followingPath) {
// path4.moveToPickupAgainPath4(follower);

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

@ -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.localization.Pose;
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 HighBasketPath2 {
public void moveToPath2(Follower robot) {
PathChain pathChain;
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 1
new BezierLine(
new Point(20.000, 130.000, Point.CARTESIAN),
new Point(26.000, 117.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0));
pathChain = builder.build();
robot.followPath(pathChain);
}
}