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

Two Samples in bucket needs fine tuning

Browse Source
This commit is contained in:
robotics1
2025-01-14 17:28:22 -08:00
parent a39332954e
commit 11df322ec1
5 changed files with 44 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files

67
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComeBotDriveDevV2.java
View File

@ -47,18 +47,22 @@ public class ComeBotDriveDevV2 extends OpMode {
follower.setMaxPower(.75); follower.setMaxPower(.75);
path1 = new HighBasketAutoPath1(); path1 = new HighBasketAutoPath1();
path2 = new HighBasketAutoPath2(); path2 = new HighBasketAutoPath2();
path3 = new HighBasketAutoPath3();
pathPark = new AutoPark(); pathPark = new AutoPark();
comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2); comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
comp.initCloseClaw(); comp.initCloseClaw();
hook = new SkyHookSubsystem(hardwareMap); hook = new SkyHookSubsystem(hardwareMap);
state = 0; state = 0;
} }
public void loop() { public void loop() {
telemetry.addData("state", state); telemetry.addData("state", state);
telemetry.addData("followingPath", followingPath);
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
}
switch (state) { switch (state) {
case 0: case 0:
moveToPathOneAndHighBucket(); moveToPathOneAndHighBucket();
@ -73,22 +77,22 @@ public class ComeBotDriveDevV2 extends OpMode {
doPickUpThing(); doPickUpThing();
break; break;
case 4: case 4:
moveToBasketPath3(); moveToBasketPath3();
break; break;
case 5: case 5:
theArmThing(); theArmThing();
break; break;
case 6: case 6:
moveToPickupAgainPath4(); //moveToPickupAgainPath4();
break; break;
case 7: case 7:
doPickUpThingAgain(); //doPickUpThingAgain();
break; break;
case 8: case 8:
//line five //line five
break; break;
case 9: case 9:
theArmThingAgain(); //theArmThingAgain();
break; break;
case 10: case 10:
//line six //line six
@ -109,8 +113,8 @@ public class ComeBotDriveDevV2 extends OpMode {
} }
if (runtime != null) { if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds()); telemetry.addData("Runtime (seconds)", runtime.seconds());
if (follower.atParametricEnd() || runtime.seconds() > 6.0) { if (follower.atParametricEnd() || runtime.seconds() > 4) {
state = 3; state = 1;
followingPath = false; followingPath = false;
} }
} }
@ -140,12 +144,18 @@ public class ComeBotDriveDevV2 extends OpMode {
state = 2; state = 2;
} }
private void theArmThing(){ private void theArmThing() {
comp.highBucketDrop(); telemetry.addData("busy?", follower.isBusy());
state = 6; telemetry.addData("end?", follower.atParametricEnd());
if (follower.atParametricEnd()){
follower.breakFollowing();
comp.highBucketDrop();
state = 6;
}
// follower.breakFollowing();
} }
private void theArmThingAgain(){ private void theArmThingAgain() {
comp.highBucketDrop(); comp.highBucketDrop();
state = 10; state = 10;
} }
@ -163,6 +173,7 @@ public class ComeBotDriveDevV2 extends OpMode {
} }
} }
} }
private void moveToPickupAgainPath4() { private void moveToPickupAgainPath4() {
if (!followingPath) { if (!followingPath) {
path4.moveToPickupAgainPath4(follower); path4.moveToPickupAgainPath4(follower);
@ -176,6 +187,7 @@ public class ComeBotDriveDevV2 extends OpMode {
} }
} }
} }
private void moveToBasketPath3() { private void moveToBasketPath3() {
if (!followingPath) { if (!followingPath) {
path3.moveToBasketPath3(follower); path3.moveToBasketPath3(follower);
@ -190,28 +202,29 @@ public class ComeBotDriveDevV2 extends OpMode {
} }
} }
private void thePickUp(){ private void thePickUp() {
Actions.runBlocking(new SequentialAction( Actions.runBlocking(new SequentialAction(
comp.arm.toFloorPosition(), new SleepAction(.25),
new SleepAction(.5), comp.arm.toSubmarinePosition(),
comp.wrist.toPickupPosition(), new SleepAction(.5),
new SleepAction(1), comp.wrist.toPickupPosition(),
comp.claw.closeClaw(), new SleepAction(.75),
new SleepAction(1), comp.claw.closeClaw(),
comp.wrist.toFloorPosition(), new SleepAction(.75),
new SleepAction(0.5), comp.wrist.toFloorPosition(),
comp.arm.toParkPosition() new SleepAction(0.5),
)); comp.arm.toParkPosition(),
new SleepAction(.25)
));
} }
private void doPickUpThing(){ private void doPickUpThing() {
follower.breakFollowing();
thePickUp(); thePickUp();
if(!follower.isBusy()) { state = 4;
state = 4;
}
} }
private void doPickUpThingAgain(){ private void doPickUpThingAgain() {
thePickUp(); thePickUp();
state = 8; state = 8;
} }

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SpecimenTest.java
View File

@ -49,7 +49,6 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem; import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.AutoLine1; import org.firstinspires.ftc.teamcode.subsystem.AutoLine1;
import org.firstinspires.ftc.teamcode.subsystem.AutoLine2;
import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem; import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem; import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem; import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem;

40
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine2.java
View File

@ -1,40 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystem;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
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.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/*
AutoLine# - This file does something of a path......
*/
public class AutoLine2 {
private PathChain pathChain;
private PathChain goToStore;
private Pose startPose = new Pose(37.5, 72);
public AutoLine2(Follower robot) {
robot.setStartingPose(startPose);
pathChain = robot.pathBuilder().addPath(
new BezierLine(
new Point(37.500, 72.000, Point.CARTESIAN),
new Point(36.000, 72.000, Point.CARTESIAN)
)
).setConstantHeadingInterpolation(Math.toRadians(0)).build();
robot.followPath(pathChain);
}
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath1.java
View File

@ -29,7 +29,7 @@ public class HighBasketAutoPath1 {
new Point(18.000, 126.000, Point.CARTESIAN) new Point(18.000, 126.000, Point.CARTESIAN)
) )
) )
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)).build(); .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
pathChain = builder.build(); pathChain = builder.build();
robot.followPath(pathChain); robot.followPath(pathChain);
} }

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath3.java
View File

@ -2,7 +2,7 @@ package org.firstinspires.ftc.teamcode.subsystem;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; 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;
@ -14,10 +14,11 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
*/ */
public class HighBasketAutoPath3 { public class HighBasketAutoPath3 {
public void moveToBasketPath3(Follower robot) { public void moveToBasketPath3(Follower robot) {
PathChain pathChain; PathChain pathChain;
PathBuilder builder = new PathBuilder(); PathBuilder builder = new PathBuilder();
// robot.setStartingPose( new Pose(29, 120));
builder builder
.addPath( .addPath(
// Line 1 // Line 1
@ -27,7 +28,6 @@ public class HighBasketAutoPath3 {
) )
) )
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)); .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
pathChain = builder.build(); pathChain = builder.build();
robot.followPath(pathChain); robot.followPath(pathChain);
} }