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

Blue basket auto comp 2 updates

Browse Source
This commit is contained in:
carlos
2024-12-13 16:42:29 -08:00
parent 2efe4229e9
commit 2720d596b5
9 changed files with 149 additions and 201 deletions
Download Patch File Download Diff File Expand all files Collapse all files

192
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java
View File

@ -3,9 +3,12 @@ package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleopCompetition;
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;
@ -13,184 +16,47 @@ 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;
import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath1;
import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath2;
@Disabled
@Autonomous(name = "Auto Test", group = "Dev") @Autonomous(name = "Auto Test", group = "Dev")
public class BlueBasketAuto extends OpMode { public class BlueBasketAuto extends OpMode {
private Telemetry telemetryA;
private Follower follower; private Follower follower;
private int state;
private PathChain path; private HighBasketAutoPath1 path1;
private HighBasketAutoPath2 path2;
private final Pose startPose = new Pose(8, 65); private CometBotTeleopCompetition comp;
private ElapsedTime runtime;
@Override @Override
public void init() { public void init() {
follower = new Follower(hardwareMap); follower = new Follower(hardwareMap);
follower.setMaxPower(.75); follower.setMaxPower(.75);
path1 = new HighBasketAutoPath1();
follower.setStartingPose(startPose); path2 = new HighBasketAutoPath2();
comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
path = follower.pathBuilder() comp.initCloseClaw();
.addPath(
// Line 1
new BezierLine(
new Point(8.000, 65.000, Point.CARTESIAN),
new Point(33.000, 65.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 2
new BezierLine(
new Point(33.000, 65.000, Point.CARTESIAN),
new Point(31.000, 65.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 3
new BezierCurve(
new Point(31.000, 65.000, Point.CARTESIAN),
new Point(26.000, 32.000, Point.CARTESIAN),
new Point(60.000, 34.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
.addPath(
// Line 4
new BezierLine(
new Point(60.000, 34.000, Point.CARTESIAN),
new Point(60.000, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 5
new BezierLine(
new Point(60.000, 24.000, Point.CARTESIAN),
new Point(14.500, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 6
new BezierLine(
new Point(14.500, 24.000, Point.CARTESIAN),
new Point(18.000, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 1
new BezierCurve(
new Point(18.000, 24.000, Point.CARTESIAN),
new Point(18.000, 67.000, Point.CARTESIAN),
new Point(31.000, 67.000, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 8
new BezierLine(
new Point(31.000, 67.500, Point.CARTESIAN),
new Point(33.000, 67.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 9
new BezierLine(
new Point(33.000, 67.500, Point.CARTESIAN),
new Point(31.000, 67.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 10
new BezierCurve(
new Point(31.000, 67.500, Point.CARTESIAN),
new Point(26.000, 32.000, Point.CARTESIAN),
new Point(60.000, 34.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
.addPath(
// Line 11
new BezierLine(
new Point(60.000, 34.000, Point.CARTESIAN),
new Point(60.000, 12.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 12
new BezierLine(
new Point(60.000, 12.000, Point.CARTESIAN),
new Point(12.000, 12.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 13
new BezierLine(
new Point(12.000, 12.000, Point.CARTESIAN),
new Point(20.000, 12.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 14
new BezierLine(
new Point(20.000, 12.000, Point.CARTESIAN),
new Point(20.000, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 15
new BezierLine(
new Point(20.000, 24.000, Point.CARTESIAN),
new Point(14.000, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 16
new BezierCurve(
new Point(14.000, 24.000, Point.CARTESIAN),
new Point(14.000, 70.000, Point.CARTESIAN),
new Point(31.000, 70.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(0))
.addPath(
// Line 17
new BezierLine(
new Point(31.000, 70.000, Point.CARTESIAN),
new Point(33.000, 70.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 18
new BezierLine(
new Point(33.000, 70.000, Point.CARTESIAN),
new Point(31.000, 70.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180)).build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
} }
@Override @Override
public void loop() { public void loop() {
switch(state) {
case 0:
path1.moveToPath1(follower);
state = 1;
runtime.reset();
case 1:
if (runtime.seconds() > 5) {
path2.moveToPath1(follower);
state = 2;
}
default:
System.out.println("default");
}
follower.update(); follower.update();
follower.telemetryDebug(telemetryA); follower.telemetryDebug(telemetry);
} }
} }

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop3.java
View File

@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.Telemetry;
@ -11,8 +12,8 @@ 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.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;
@Disabled
@Autonomous(name = "Auto Test", group = "Dev") @Autonomous(name = "DKJBFWOFHBOUW EHBF", group = "Dev")
public class BlueBasketAutoWithDrop3 extends OpMode { public class BlueBasketAutoWithDrop3 extends OpMode {
private Telemetry telemetryA; private Telemetry telemetryA;
@ -20,7 +21,6 @@ public class BlueBasketAutoWithDrop3 extends OpMode {
private PathChain path; private PathChain path;
private final Pose startPose = new Pose(8, 65);
@Override @Override
public void init() { public void init() {
@ -28,8 +28,6 @@ public class BlueBasketAutoWithDrop3 extends OpMode {
follower.setMaxPower(.75); follower.setMaxPower(.75);
follower.setStartingPose(startPose);
path = follower.pathBuilder() path = follower.pathBuilder()
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0)) .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0))
.addPath( .addPath(

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop4.java
View File

@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.Telemetry;
@ -12,8 +13,8 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
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;
@Disabled
@Autonomous(name = "Auto Test", group = "Dev") @Autonomous(name = "rndnohsdnhngph", group = "Dev")
public class BlueBasketAutoWithDrop4 extends OpMode { public class BlueBasketAutoWithDrop4 extends OpMode {
private Telemetry telemetryA; private Telemetry telemetryA;
@ -21,7 +22,6 @@ public class BlueBasketAutoWithDrop4 extends OpMode {
private PathChain path; private PathChain path;
private final Pose startPose = new Pose(8, 65);
@Override @Override
public void init() { public void init() {
@ -29,8 +29,6 @@ public class BlueBasketAutoWithDrop4 extends OpMode {
follower.setMaxPower(.75); follower.setMaxPower(.75);
follower.setStartingPose(startPose);
path = follower.pathBuilder() path = follower.pathBuilder()
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0)) .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0))
.addPath( .addPath(

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop5.java
View File

@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.Telemetry;
@ -12,8 +13,8 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
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;
@Disabled
@Autonomous(name = "Auto Test", group = "Dev") @Autonomous(name = "rboeruhbgoeruhbg", group = "Dev")
public class BlueBasketAutoWithDrop5 extends OpMode { public class BlueBasketAutoWithDrop5 extends OpMode {
private Telemetry telemetryA; private Telemetry telemetryA;
@ -21,7 +22,6 @@ public class BlueBasketAutoWithDrop5 extends OpMode {
private PathChain path; private PathChain path;
private final Pose startPose = new Pose(8, 65);
@Override @Override
public void init() { public void init() {
@ -29,8 +29,6 @@ public class BlueBasketAutoWithDrop5 extends OpMode {
follower.setMaxPower(.75); follower.setMaxPower(.75);
follower.setStartingPose(startPose);
path = follower.pathBuilder() path = follower.pathBuilder()
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0)) .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0))
.addPath( .addPath(

9
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop6.java
View File

@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.Telemetry;
@ -11,8 +12,8 @@ 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.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;
@Disabled
@Autonomous(name = "Auto Test", group = "Dev") @Autonomous(name = "piqeripgqreipbripbipb", group = "Dev")
public class BlueBasketAutoWithDrop6 extends OpMode { public class BlueBasketAutoWithDrop6 extends OpMode {
private Telemetry telemetryA; private Telemetry telemetryA;
@ -20,16 +21,12 @@ public class BlueBasketAutoWithDrop6 extends OpMode {
private PathChain path; private PathChain path;
private final Pose startPose = new Pose(8, 65);
@Override @Override
public void init() { public void init() {
follower = new Follower(hardwareMap); follower = new Follower(hardwareMap);
follower.setMaxPower(.75); follower.setMaxPower(.75);
follower.setStartingPose(startPose);
path = follower.pathBuilder() path = follower.pathBuilder()
.addPath( .addPath(
// Line 6 // Line 6

24
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleopCompetition.java
View File

@ -88,6 +88,18 @@ public class CometBotTeleopCompetition {
follower.startTeleopDrive(); follower.startTeleopDrive();
} }
public void initCloseClaw(){
this.motors.init();
this.hook.init();
this.claw.init();
this.arm.init();
this.wrist.init();
this.lift.init();
this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
follower.setMaxPower(MAX_POWER);
follower.startTeleopDrive();
}
public void update() { public void update() {
this.previousGP1.copy(currentGP1); this.previousGP1.copy(currentGP1);
this.currentGP1.copy(this.GP1); this.currentGP1.copy(this.GP1);
@ -162,6 +174,14 @@ public class CometBotTeleopCompetition {
if (this.arm.getState() == ArmActionsSubsystem.ArmState.PARK) { if (this.arm.getState() == ArmActionsSubsystem.ArmState.PARK) {
this.follower.breakFollowing(); this.follower.breakFollowing();
fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET); fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
highBucketDrop();
fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
this.follower.startTeleopDrive();
}
}
}
public void highBucketDrop() {
Actions.runBlocking(new SequentialAction( Actions.runBlocking(new SequentialAction(
new SleepAction(.5), new SleepAction(.5),
this.lift.toHighBucketPosition(), this.lift.toHighBucketPosition(),
@ -178,10 +198,6 @@ public class CometBotTeleopCompetition {
this.lift.toFloorPosition(), this.lift.toFloorPosition(),
new SleepAction(.5) new SleepAction(.5)
)); ));
fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
this.follower.startTeleopDrive();
}
}
} }
/* /*

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

@ -68,7 +68,7 @@ public class ClawActionsSubsystem {
} }
public void init() { public void init() {
Actions.runBlocking(openClaw()); Actions.runBlocking(closeClaw());
} }
public void start() { public void start() {

39
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath1.java Normal file
View File

@ -0,0 +1,39 @@
package org.firstinspires.ftc.teamcode.subsystem;
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.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 HighBasketAutoPath1 {
private final Pose startPose = new Pose(8, 89);
public void moveToPath1(Follower robot) {
PathChain pathChain;
robot.setStartingPose(startPose);
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 1
new BezierCurve(
new Point(8.000, 89.000, Point.CARTESIAN),
new Point(24.000, 96.000, Point.CARTESIAN),
new Point(16.000, 128.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
pathChain = builder.build();
robot.followPath(pathChain);
}
}

36
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath2.java Normal file
View File

@ -0,0 +1,36 @@
package org.firstinspires.ftc.teamcode.subsystem;
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.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 HighBasketAutoPath2 {
public void moveToPath1(Follower robot) {
PathChain pathChain;
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 2
new BezierCurve(
new Point(16.000, 128.000, Point.CARTESIAN),
new Point(16.000, 120.000, Point.CARTESIAN),
new Point(32.000, 123.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0));
pathChain = builder.build();
robot.followPath(pathChain);
}
}