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.telemetry.MultipleTelemetry;
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.util.ElapsedTime;
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.localization.Pose;
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.PathChain;
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")
public class BlueBasketAuto extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private int state;
private PathChain path;
private final Pose startPose = new Pose(8, 65);
private HighBasketAutoPath1 path1;
private HighBasketAutoPath2 path2;
private CometBotTeleopCompetition comp;
private ElapsedTime runtime;
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.75);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.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();
path1 = new HighBasketAutoPath1();
path2 = new HighBasketAutoPath2();
comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
comp.initCloseClaw();
}
@Override
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.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.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
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.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
@Autonomous(name = "Auto Test", group = "Dev")
@Disabled
@Autonomous(name = "DKJBFWOFHBOUW EHBF", group = "Dev")
public class BlueBasketAutoWithDrop3 extends OpMode {
private Telemetry telemetryA;
@ -20,7 +21,6 @@ public class BlueBasketAutoWithDrop3 extends OpMode {
private PathChain path;
private final Pose startPose = new Pose(8, 65);
@Override
public void init() {
@ -28,8 +28,6 @@ public class BlueBasketAutoWithDrop3 extends OpMode {
follower.setMaxPower(.75);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0))
.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.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
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.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
@Autonomous(name = "Auto Test", group = "Dev")
@Disabled
@Autonomous(name = "rndnohsdnhngph", group = "Dev")
public class BlueBasketAutoWithDrop4 extends OpMode {
private Telemetry telemetryA;
@ -21,7 +22,6 @@ public class BlueBasketAutoWithDrop4 extends OpMode {
private PathChain path;
private final Pose startPose = new Pose(8, 65);
@Override
public void init() {
@ -29,8 +29,6 @@ public class BlueBasketAutoWithDrop4 extends OpMode {
follower.setMaxPower(.75);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0))
.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.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
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.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
@Autonomous(name = "Auto Test", group = "Dev")
@Disabled
@Autonomous(name = "rboeruhbgoeruhbg", group = "Dev")
public class BlueBasketAutoWithDrop5 extends OpMode {
private Telemetry telemetryA;
@ -21,7 +22,6 @@ public class BlueBasketAutoWithDrop5 extends OpMode {
private PathChain path;
private final Pose startPose = new Pose(8, 65);
@Override
public void init() {
@ -29,8 +29,6 @@ public class BlueBasketAutoWithDrop5 extends OpMode {
follower.setMaxPower(.75);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0))
.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.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
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.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
@Autonomous(name = "Auto Test", group = "Dev")
@Disabled
@Autonomous(name = "piqeripgqreipbripbipb", group = "Dev")
public class BlueBasketAutoWithDrop6 extends OpMode {
private Telemetry telemetryA;
@ -20,16 +21,12 @@ public class BlueBasketAutoWithDrop6 extends OpMode {
private PathChain path;
private final Pose startPose = new Pose(8, 65);
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.75);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.addPath(
// Line 6

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

@ -88,6 +88,18 @@ public class CometBotTeleopCompetition {
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() {
this.previousGP1.copy(currentGP1);
this.currentGP1.copy(this.GP1);
@ -162,28 +174,32 @@ public class CometBotTeleopCompetition {
if (this.arm.getState() == ArmActionsSubsystem.ArmState.PARK) {
this.follower.breakFollowing();
fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
Actions.runBlocking(new SequentialAction(
new SleepAction(.5),
this.lift.toHighBucketPosition(),
new SleepAction(.5),
this.arm.toBucketPosition(),
new SleepAction(.5),
this.wrist.toBucketPosition(),
new SleepAction(.5),
this.claw.openClaw(),
new SleepAction(.5),
this.wrist.toFloorPosition(),
new SleepAction(.5),
this.arm.toParkPosition(),
this.lift.toFloorPosition(),
new SleepAction(.5)
));
highBucketDrop();
fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
this.follower.startTeleopDrive();
}
}
}
public void highBucketDrop() {
Actions.runBlocking(new SequentialAction(
new SleepAction(.5),
this.lift.toHighBucketPosition(),
new SleepAction(.5),
this.arm.toBucketPosition(),
new SleepAction(.5),
this.wrist.toBucketPosition(),
new SleepAction(.5),
this.claw.openClaw(),
new SleepAction(.5),
this.wrist.toFloorPosition(),
new SleepAction(.5),
this.arm.toParkPosition(),
this.lift.toFloorPosition(),
new SleepAction(.5)
));
}
/*
Type: PS4 / Logitech
Controller: 2

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

@ -68,7 +68,7 @@ public class ClawActionsSubsystem {
}
public void init() {
Actions.runBlocking(openClaw());
Actions.runBlocking(closeClaw());
}
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);
}
}