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

Path one, backwards, works as expected

Browse Source
This commit is contained in:
robotics1
2025-01-21 16:32:18 -08:00
parent e1be70c23f
commit 93ff65ee53
3 changed files with 12 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -65,7 +65,7 @@ public class CometBotDriveV2 extends OpMode {
lift.init();
arm.initAuto();
wrist.initTeleOp();
wrist.InitAuto();
claw.init();
//comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
@ -77,6 +77,7 @@ public class CometBotDriveV2 extends OpMode {
public void loop() {
telemetry.addData("state", state);
telemetry.addData("followingPath", followingPath);
telemetry.addData("busy", follower.isBusy());
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
}
@ -127,18 +128,8 @@ public class CometBotDriveV2 extends OpMode {
}
private void moveToPathOneAndHighBucket() {
if (!followingPath) {
runtime = new ElapsedTime();
path1.moveToPath1(follower);
followingPath = true;
}
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
if (runtime.seconds() > 4) {
state = 1;
followingPath = false;
}
}
state = 1;
}
public class SetStateAction implements Action {
@ -148,7 +139,6 @@ public class CometBotDriveV2 extends OpMode {
public SetStateAction(int value) {
this.value = value;
}
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
state = value;

13
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath1.java
View File

@ -4,6 +4,7 @@ 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.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;
@ -15,7 +16,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
*/
public class HighBasketPath1 {
private final Pose startPose = new Pose(8, 89);
private final Pose startPose = new Pose(10, 89);
public void moveToPath1(Follower robot) {
PathChain pathChain;
@ -23,13 +24,13 @@ public class HighBasketPath1 {
PathBuilder builder = new PathBuilder();
builder
.addPath(
new BezierCurve(
new Point(8.000, 89.000, Point.CARTESIAN),
new Point(24.000, 96.000, Point.CARTESIAN),
new Point(18.000, 126.000, Point.CARTESIAN)
// Line 1
new BezierLine(
new Point(10.000, 89.000, Point.CARTESIAN),
new Point(20.000, 130.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(320));
pathChain = builder.build();
robot.followPath(pathChain);
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java
View File

@ -17,8 +17,8 @@ public class RobotConstants {
public final static double armHangBlueberrySkyhook = 0.6;
public final static double wristHangBlueberrySkyhook = 0.3;
public final static double armBucket = 0.45;
public final static double armInit = 0.125;
public final static double wristInit = 0.0;
public final static double armInit = 0.135;
public final static double wristInit = 0.25;
public final static double wristPickup = 0.475;
public final static double wristBucket = 0.56;
public final static double wristFloor = 0.75;