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);
path1 = new HighBasketAutoPath1();
path2 = new HighBasketAutoPath2();
path3 = new HighBasketAutoPath3();
pathPark = new AutoPark();
comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
comp.initCloseClaw();
hook = new SkyHookSubsystem(hardwareMap);
state = 0;
}
public void loop() {
telemetry.addData("state", state);
telemetry.addData("followingPath", followingPath);
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
}
switch (state) {
case 0:
moveToPathOneAndHighBucket();
@ -73,22 +77,22 @@ public class ComeBotDriveDevV2 extends OpMode {
doPickUpThing();
break;
case 4:
moveToBasketPath3();
moveToBasketPath3();
break;
case 5:
theArmThing();
break;
case 6:
moveToPickupAgainPath4();
//moveToPickupAgainPath4();
break;
case 7:
doPickUpThingAgain();
//doPickUpThingAgain();
break;
case 8:
//line five
break;
case 9:
theArmThingAgain();
//theArmThingAgain();
break;
case 10:
//line six
@ -109,8 +113,8 @@ public class ComeBotDriveDevV2 extends OpMode {
}
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
if (follower.atParametricEnd() || runtime.seconds() > 6.0) {
state = 3;
if (follower.atParametricEnd() || runtime.seconds() > 4) {
state = 1;
followingPath = false;
}
}
@ -140,12 +144,18 @@ public class ComeBotDriveDevV2 extends OpMode {
state = 2;
}
private void theArmThing(){
comp.highBucketDrop();
state = 6;
private void theArmThing() {
telemetry.addData("busy?", follower.isBusy());
telemetry.addData("end?", follower.atParametricEnd());
if (follower.atParametricEnd()){
follower.breakFollowing();
comp.highBucketDrop();
state = 6;
}
// follower.breakFollowing();
}
private void theArmThingAgain(){
private void theArmThingAgain() {
comp.highBucketDrop();
state = 10;
}
@ -163,6 +173,7 @@ public class ComeBotDriveDevV2 extends OpMode {
}
}
}
private void moveToPickupAgainPath4() {
if (!followingPath) {
path4.moveToPickupAgainPath4(follower);
@ -176,6 +187,7 @@ public class ComeBotDriveDevV2 extends OpMode {
}
}
}
private void moveToBasketPath3() {
if (!followingPath) {
path3.moveToBasketPath3(follower);
@ -190,28 +202,29 @@ public class ComeBotDriveDevV2 extends OpMode {
}
}
private void thePickUp(){
private void thePickUp() {
Actions.runBlocking(new SequentialAction(
comp.arm.toFloorPosition(),
new SleepAction(.5),
comp.wrist.toPickupPosition(),
new SleepAction(1),
comp.claw.closeClaw(),
new SleepAction(1),
comp.wrist.toFloorPosition(),
new SleepAction(0.5),
comp.arm.toParkPosition()
));
new SleepAction(.25),
comp.arm.toSubmarinePosition(),
new SleepAction(.5),
comp.wrist.toPickupPosition(),
new SleepAction(.75),
comp.claw.closeClaw(),
new SleepAction(.75),
comp.wrist.toFloorPosition(),
new SleepAction(0.5),
comp.arm.toParkPosition(),
new SleepAction(.25)
));
}
private void doPickUpThing(){
private void doPickUpThing() {
follower.breakFollowing();
thePickUp();
if(!follower.isBusy()) {
state = 4;
}
state = 4;
}
private void doPickUpThingAgain(){
private void doPickUpThingAgain() {
thePickUp();
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.subsystem.ArmActionsSubsystem;
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.LiftActionsSubsystem;
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)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)).build();
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
pathChain = builder.build();
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.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.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
@ -14,10 +14,11 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
*/
public class HighBasketAutoPath3 {
public void moveToBasketPath3(Follower robot) {
PathChain pathChain;
PathBuilder builder = new PathBuilder();
// robot.setStartingPose( new Pose(29, 120));
builder
.addPath(
// Line 1
@ -27,7 +28,6 @@ public class HighBasketAutoPath3 {
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
pathChain = builder.build();
robot.followPath(pathChain);
}