Skip to content

Commit

Permalink
16750 Auto Commands and auto testing in meepmeep - Maggie 10-11
Browse files Browse the repository at this point in the history
  • Loading branch information
@TechnototesLaptop @kevinfrei
TechnototesLaptop authored and kevinfrei committed Oct 13, 2024
1 parent c11dafc commit a5399c4
Show file tree
Hide file tree
Showing 3 changed files with 62 additions and 38 deletions.
62 changes: 35 additions & 27 deletions MeepMeepTesting/src/main/java/com/example/meepmeeptesting/Sixteen750Testing.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ public static void main(String[] args) {
new TrajectoryBuilder(pose, Math.PI + pose.getHeading(), min_vel, prof_accel);
RoadRunnerBotEntity myBot = new DefaultBotBuilder(meepMeep)
.setDimensions(14, 17)
.followTrajectorySequence(Sixteen750Testing::getParkingTrajectory);
.followTrajectorySequence(Sixteen750Testing::getRedTrajectory);
try {
// Try to load the field image from the repo:
meepMeep.setBackground(ImageIO.read(new File("Field.jpg")));
Expand All @@ -61,17 +61,17 @@ public static void main(String[] args) {

private static TrajectorySequence getRedTrajectory(DriveShim drive) {
return drive
.trajectorySequenceBuilder(AutoConstants.START)
.addTrajectory(AutoConstants.START_TO_NETSCORING.get())
/*
.turn(Math.toRadians(-135))
.addTrajectory(AutoConstants.NETSCORING_TO_INTAKE1.get())
.addTrajectory(AutoConstants.INTAKE1_TO_NETSCORING.get())
.addTrajectory(AutoConstants.NETSCORING_TO_INTAKE2.get())
.addTrajectory(AutoConstants.INTAKE2_TO_NETSCORING.get())
.addTrajectory(AutoConstants.NETSCORING_TO_INTAKE3.get())
.addTrajectory(AutoConstants.INTAKE3_TO_NETSCORING.get()
.trajectorySequenceBuilder(AutoConstants.START)
.addTrajectory(AutoConstants.START_TO_NETSCORING.get())

//.turn(Math.toRadians(-135))
.addTrajectory(AutoConstants.NETSCORING_TO_INTAKE1.get())
.addTrajectory(AutoConstants.INTAKE1_TO_NETSCORING.get())
.addTrajectory(AutoConstants.NETSCORING_TO_INTAKE2.get())
.addTrajectory(AutoConstants.INTAKE2_TO_NETSCORING.get())
.addTrajectory(AutoConstants.NETSCORING_TO_INTAKE3.get())
.addTrajectory(AutoConstants.INTAKE3_TO_NETSCORING.get())
/*
splines
.addTrajectory(AutoConstants.START_TO_RIGHT_SPIKE.get())
.addTrajectory(AutoConstants.RIGHT_SPIKE_TO_STAGE.get())
Expand All @@ -88,26 +88,34 @@ private static TrajectorySequence getRedTrajectory(DriveShim drive) {

private static TrajectorySequence getParkingTrajectory(DriveShim drive) {
return drive
.trajectorySequenceBuilder(AutoConstants.NETSCORING)
// .turn(Math.toRadians(135))
.addTrajectory(AutoConstants.NETSCORING_TO_ASCENT.get())
.addTrajectory(AutoConstants.ASCENT_TO_NETSCORING.get())
.build();
.trajectorySequenceBuilder(AutoConstants.NETSCORING)
//.turn(Math.toRadians(135))
// .setReversed(true)
//.addTrajectory(AutoConstants.NETSCORING_TO_NETSCORING_CLEAR.get())
//.addTrajectory(AutoConstants.NETSCORING_CLEAR_TO_ASCENT.get())
.addTrajectory(AutoConstants.NETSCORING_TO_ASCENT.get())
.build();
}

private static TrajectorySequence getBlueTrajectory(DriveShim drive) {
return drive
.trajectorySequenceBuilder(AutoConstants.START)
/*
.addTrajectory(AutoConstantsRed.Stage.START_TO_LEFT_LOW.get())
.addTrajectory(AutoConstants.START_TO_MID_CLEAR.get())
.addTrajectory(WingRed.MIDSPIKE_TO_RIGHT_SPIKE.get())
.addTrajectory(WingRed.RIGHT_SPIKE_TO_MIDSPIKE.get())
.addTrajectory(WingRed.MIDSPIKE_TO_BACK.get())
.addTrajectory(AutoConstantsRed.Wing.BACK_TO_PARK_CORNER.get())
.addTrajectory(AutoConstantsRed.Stage.LEFT_SPIKE_TO_CENTER_SPIKE.get())
.addTrajectory(AutoConstantsRed.Stage.CENTER_SPIKE_TO_RIGHT_SPIKE.get())
*/
.trajectorySequenceBuilder(AutoConstants.START)
.addTrajectory(AutoConstants.START_TO_NETSCORING.get())

//.turn(Math.toRadians(-135))
.addTrajectory(AutoConstants.NETSCORING_TO_INTAKE1.get())
.addTrajectory(AutoConstants.INTAKE1_TO_NETSCORING.get())
.addTrajectory(AutoConstants.NETSCORING_TO_INTAKE2.get())
.addTrajectory(AutoConstants.INTAKE2_TO_NETSCORING.get())
.addTrajectory(AutoConstants.NETSCORING_TO_INTAKE3.get())
.addTrajectory(AutoConstants.INTAKE3_TO_NETSCORING.get())
//.addTrajectory(AutoConstants.START_TO_MID_CLEAR.get())
//.addTrajectory(WingRed.MIDSPIKE_TO_RIGHT_SPIKE.get())
//.addTrajectory(WingRed.RIGHT_SPIKE_TO_MIDSPIKE.get())
//.addTrajectory(WingRed.MIDSPIKE_TO_BACK.get())
//.addTrajectory(AutoConstantsRed.Wing.BACK_TO_PARK_CORNER.get())
//.addTrajectory(AutoConstantsRed.Stage.LEFT_SPIKE_TO_CENTER_SPIKE.get())
//.addTrajectory(AutoConstantsRed.Stage.CENTER_SPIKE_TO_RIGHT_SPIKE.get())
.build();
}
/*
Expand Down
28 changes: 17 additions & 11 deletions Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/AutoConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@ public class AutoConstants {
public static ConfigurablePoseD START = new ConfigurablePoseD(35, 63, 0);
public static ConfigurablePoseD NETSCORING = new ConfigurablePoseD(55, 55, 45);
public static ConfigurablePoseD NETSCORING_TEST = new ConfigurablePoseD(55, 55, 45);
public static ConfigurablePoseD NETSCORING_CLEAR = new ConfigurablePoseD(45, 35, -45);
public static ConfigurablePoseD SPLINETEST1 = new ConfigurablePoseD(0, -55, 0);
public static ConfigurablePoseD SPLINETEST2 = new ConfigurablePoseD(55, 0, 0);

Expand All @@ -29,7 +30,7 @@ public class AutoConstants {
public static ConfigurablePoseD INTAKE1 = new ConfigurablePoseD(47, 40, 90);
public static ConfigurablePoseD INTAKE2 = new ConfigurablePoseD(60, 35, 90);
public static ConfigurablePoseD INTAKE3 = new ConfigurablePoseD(64, 37, 90);
public static ConfigurablePoseD ASCENT = new ConfigurablePoseD(23, 10, 180);
public static ConfigurablePoseD ASCENT = new ConfigurablePoseD(23, 12, -0);


//These are testing constants for last year's game
Expand Down Expand Up @@ -72,16 +73,21 @@ public class AutoConstants {
public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence>
NETSCORING_TO_ASCENT = b ->
b.apply(NETSCORING_TEST.toPose())
// .splineToLinearHeading(NETCLEAR.toPose(), NETCLEAR.getHeading())
.setReversed(true)
.splineToLinearHeading(ASCENT.toPose(), ASCENT.getHeading())
.build();
public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence>
ASCENT_TO_NETSCORING = b ->
b.apply(ASCENT.toPose())
.setReversed(true)
.splineToLinearHeading(NETSCORING_TEST.toPose(), NETSCORING_TEST.getHeading())
.build();
// .splineToLinearHeading(NETCLEAR.toPose(), NETCLEAR.getHeading())
// .splineToSplineHeading(NETSCORING_CLEAR.toPose(), NETSCORING_CLEAR.getHeading())
.splineToLinearHeading(ASCENT.toPose(), ASCENT.getHeading())
//.setReversed(true)
.build();
public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence>
NETSCORING_TO_NETSCORING_CLEAR = b ->
b.apply(NETSCORING_TEST.toPose())
.splineToLinearHeading(NETSCORING_CLEAR.toPose(), NETSCORING_CLEAR.getHeading())
.build();
public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence>
NETSCORING_CLEAR_TO_ASCENT = b ->
b.apply(NETSCORING_CLEAR.toPose())
.splineToLinearHeading(ASCENT.toPose(), ASCENT.getHeading())
.build();

public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence>
SPLINETEST1_TO_SPLINETEST2 = b ->
Expand Down
10 changes: 10 additions & 0 deletions Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/auto/Paths.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,4 +10,14 @@ public class Paths {
public static Command splineTestCommand(Robot r) {
return new TrajectorySequenceCommand(r.drivebase, AutoConstants.SPLINETEST1_TO_SPLINETEST2);
}

public static Command SampleScoring(Robot r) {
return new TrajectorySequenceCommand(r.drivebase, AutoConstants.START_TO_NETSCORING).andThen(
new TrajectorySequenceCommand(r.drivebase, AutoConstants.NETSCORING_TO_INTAKE1)).andThen(
new TrajectorySequenceCommand(r.drivebase, AutoConstants.INTAKE1_TO_NETSCORING)).andThen(
new TrajectorySequenceCommand(r.drivebase, AutoConstants.NETSCORING_TO_INTAKE2)).andThen(
new TrajectorySequenceCommand(r.drivebase, AutoConstants.INTAKE2_TO_NETSCORING)).andThen(
new TrajectorySequenceCommand(r.drivebase, AutoConstants.NETSCORING_TO_INTAKE3)).andThen(
new TrajectorySequenceCommand(r.drivebase, AutoConstants.INTAKE3_TO_NETSCORING));
}
}

0 comments on commit a5399c4

Please sign in to comment.