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

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

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.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; 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.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
@ -15,7 +16,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
*/ */
public class HighBasketPath1 { public class HighBasketPath1 {
private final Pose startPose = new Pose(8, 89); private final Pose startPose = new Pose(10, 89);
public void moveToPath1(Follower robot) { public void moveToPath1(Follower robot) {
PathChain pathChain; PathChain pathChain;
@ -23,13 +24,13 @@ public class HighBasketPath1 {
PathBuilder builder = new PathBuilder(); PathBuilder builder = new PathBuilder();
builder builder
.addPath( .addPath(
new BezierCurve( // Line 1
new Point(8.000, 89.000, Point.CARTESIAN), new BezierLine(
new Point(24.000, 96.000, Point.CARTESIAN), new Point(10.000, 89.000, Point.CARTESIAN),
new Point(18.000, 126.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(); pathChain = builder.build();
robot.followPath(pathChain); 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 armHangBlueberrySkyhook = 0.6;
public final static double wristHangBlueberrySkyhook = 0.3; public final static double wristHangBlueberrySkyhook = 0.3;
public final static double armBucket = 0.45; public final static double armBucket = 0.45;
public final static double armInit = 0.125; public final static double armInit = 0.135;
public final static double wristInit = 0.0; public final static double wristInit = 0.25;
public final static double wristPickup = 0.475; public final static double wristPickup = 0.475;
public final static double wristBucket = 0.56; public final static double wristBucket = 0.56;
public final static double wristFloor = 0.75; public final static double wristFloor = 0.75;