eaglesfe
/
robotcode
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Auto Tunes + Simulator changes

This commit is contained in:
Justin 2024-02-17 10:45:49 -06:00
parent cd33603cfe
commit 28c4f0432b
5 changed files with 297 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java
View File

@ -23,10 +23,80 @@ public class MeepMeepTesting {
.build()
);
RoadRunnerBotEntity BlueFrontStage = new DefaultBotBuilder(meepMeep)
.setConstraints(60,60,Math.toRadians(180),Math.toRadians(180),15)
.followTrajectorySequence(drive ->
drive.trajectorySequenceBuilder(new Pose2d(-36, 61.5, Math.toRadians(-90)))
//Score then pick up 1 white
.lineToLinearHeading(new Pose2d(-39.7, 33.5, Math.toRadians(-90)))
.lineToLinearHeading(new Pose2d(-51.5, 33.6, Math.toRadians(180)).plus(new Pose2d(-5.4,-1.5))).waitSeconds(.01)
//Spline to board
.setTangent(Math.toRadians(90))
.splineToConstantHeading(new Pose2d(-33, 59.5, Math.toRadians(180)).vec(),Math.toRadians(0))
.lineToConstantHeading(new Pose2d(33, 59.5, Math.toRadians(180)).vec())
.splineToConstantHeading(new Pose2d(52, 34, Math.toRadians(8)).vec(),Math.toRadians(0))
//Go back to white stack
.splineToConstantHeading(new Pose2d(33, 59.5, Math.toRadians(180)).plus(new Pose2d(0,-2)).vec(), Math.toRadians(180))
.lineToConstantHeading(new Pose2d(-45, 59.5, Math.toRadians(180)).plus(new Pose2d(0,-2)).vec())
.splineToConstantHeading(new Pose2d(-51.5, 33.6, Math.toRadians(180)).plus(new Pose2d(-3.9, -3.7)).vec(), Math.toRadians(180))
//Go back to board
.setTangent(Math.toRadians(90))
.splineToConstantHeading(new Pose2d(-33, 59.5, Math.toRadians(180)).vec(),Math.toRadians(0))
.lineToConstantHeading(new Pose2d(33, 59.5, Math.toRadians(180)).vec())
.splineToConstantHeading(new Pose2d(52, 34, Math.toRadians(8)).vec(),Math.toRadians(0))
.build()
);
RoadRunnerBotEntity BlueFrontStage3 = new DefaultBotBuilder(meepMeep)
.setConstraints(60,60,Math.toRadians(180),Math.toRadians(180),15)
.followTrajectorySequence(drive ->
drive.trajectorySequenceBuilder(new Pose2d(-36, 61.5, Math.toRadians(-90)))
//Score then pick up 1 white
.lineToLinearHeading(new Pose2d(-46.7, 39.5, Math.toRadians(-90)))
.lineToLinearHeading(new Pose2d(-46.7,50.5,Math.toRadians(-90)))
.lineToLinearHeading(new Pose2d(-51.5, 33.6, Math.toRadians(180)).plus(new Pose2d(-5.4,-1.5))).waitSeconds(.01)
//Spline to board
.setTangent(Math.toRadians(90))
.splineToConstantHeading(new Pose2d(-33, 59.5, Math.toRadians(180)).vec(),Math.toRadians(0))
.lineToConstantHeading(new Pose2d(33, 59.5, Math.toRadians(180)).vec())
.splineToConstantHeading(new Pose2d(53.6, 32, Math.toRadians(8)).vec(),Math.toRadians(0))
//Park
.lineToLinearHeading(new Pose2d(45,58,Math.toRadians(-180)))
.build()
);
RoadRunnerBotEntity BlueFrontStage1 = new DefaultBotBuilder(meepMeep)
.setConstraints(60,60,Math.toRadians(180),Math.toRadians(180),15)
.followTrajectorySequence(drive ->
drive.trajectorySequenceBuilder(new Pose2d(-36, 61.5, Math.toRadians(-90)))
//Score then pick up 1 white
.lineToLinearHeading(new Pose2d(-36, 45.5, Math.toRadians(-90)))
.lineToLinearHeading(new Pose2d(-36,33.5,Math.toRadians(0)))
.lineToLinearHeading(new Pose2d(-32,33.5,Math.toRadians(0)))
.lineToLinearHeading(new Pose2d(-51.5, 33.6, Math.toRadians(180)).plus(new Pose2d(-5.4,-1.5))).waitSeconds(.01)
//Spline to board
.setTangent(Math.toRadians(90))
.splineToConstantHeading(new Pose2d(-33, 59.5, Math.toRadians(180)).vec(),Math.toRadians(0))
.lineToConstantHeading(new Pose2d(33, 59.5, Math.toRadians(180)).vec())
.splineToConstantHeading(new Pose2d(53.6, 32, Math.toRadians(8)).vec(),Math.toRadians(0))
//Park
.lineToLinearHeading(new Pose2d(45,58,Math.toRadians(-180)))
.build()
);
meepMeep.setBackground(MeepMeep.Background.FIELD_CENTERSTAGE_JUICE_DARK)
.setDarkMode(true)
.setBackgroundAlpha(0.95f)
.addEntity(myBot)
//.addEntity(myBot)
//.addEntity(BlueFrontStage)
//.addEntity(BlueFrontStage3)
.addEntity(BlueFrontStage1)
.start();
}
}

217
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontPreload.java Normal file
View File

@ -0,0 +1,217 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK1;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.Slides;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config
@Autonomous(name = "Blue FrontPreload (2+1)", group = "Competition", preselectTeleOp = "Main TeleOp")
public class BlueFrontPreload extends AutoBase {
public static final Pose2d DROP_1 = new Pose2d(-35, 32.5, Math.toRadians(0)); //THIS ANGLE NEEDS TO BE CHANGED
public static final Pose2d DROP_2 = new Pose2d(-39.7, 33.5, Math.toRadians(-90));
public static final Pose2d DROP_2_PART_2 = new Pose2d(-39.7,36.5, Math.toRadians(-90));
public static final Pose2d DROP_3 = new Pose2d(-49, 33.5, Math.toRadians(-90));
public static final Pose2d DROP_1M = new Pose2d(-48.5, 30, Math.toRadians(-90));
public static final Pose2d DROP_2M = new Pose2d(-48.5, 30, Math.toRadians(-90));
public static final Pose2d DROP_3M = new Pose2d(-48.5, 30, Math.toRadians(-90));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, 38.3, Math.toRadians(8));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, 34, Math.toRadians(8));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.6, 32, Math.toRadians(8));//817
public static final Pose2d STACK_LOCATION = new Pose2d(-51.5, 33.6, Math.toRadians(180));
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, 12.4, Math.toRadians(10));//-36
public static final Pose2d POST_DROP_POS = new Pose2d(-45, 59.5, Math.toRadians(180));
public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, 59.5, Math.toRadians(180));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, 59.5, Math.toRadians(180));
public static final Pose2d PARK_1 = new Pose2d(45,58,Math.toRadians(-180));
public static final Pose2d PARK_2 = new Pose2d(58,58,Math.toRadians(-180));
@Override
public void createTrajectories() {
// set start position
Pose2d start = new Pose2d(-36, 61.5, Math.toRadians(-90));
robot.drive.setPoseEstimate(start);
robot.camera.setAlliance(CenterStageCommon.Alliance.Blue);
}
@Override
public void followTrajectories() {
TrajectorySequenceBuilder builder = null;
switch (macroState) {
case 0:
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1M);
builder.lineToLinearHeading(DROP_1);
break;
case 2:
builder.lineToLinearHeading(DROP_2);
break;
case 3:
builder.lineToLinearHeading(DROP_3);
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
break;
// DRIVE TO TAPE
case 1:
// if drive is done move on
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
macroState++;
}
break;
// RUN INTAKE
case 2:
// intake
if (getRuntime() < macroTime + 0.32) {
robot.intake.setDcMotor(-0.18);
}
else{
builder = this.robot.getTrajectorySequenceBuilder();
robot.intake.setDcMotor(0);
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_2_PART_2);
break;
case 2:
builder.lineToLinearHeading(DROP_2_PART_2);
break;
case 3:
builder.lineToLinearHeading(DROP_2_PART_2);
break;
}
robot.arm.setDoor(OPEN);
builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.4,2.5))).waitSeconds(.01);
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK6);
macroTime = getRuntime();
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
// if intake is done move on
break;
case 3:
// if drive is done move on
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
macroState++;
}
break;
case 4:
if (getRuntime() > macroTime + 0.06) {
robot.arm.setDoor(CLOSE);
robot.intake.setDcMotor(-0.2);
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
case 1:
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break;
case 2:
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
break;
case 3:
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
macroTime = getRuntime();
}
break;
case 5:
// if drive is done move on
if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1-70, getRuntime());
macroState++;
}
break;
//extending the macro and about to score
case 6:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
//Park
case 7:
robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(PARK_1);
builder.lineToLinearHeading(PARK_2);
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
case 8:
// if drive is done move on
if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
macroState++;
}
break;
case 9:
robot.resetMacro(0, getRuntime());
if (robot.macroState == 0) {
macroState = -1;
}
case 19:
robot.resetMacro(0, getRuntime());
if (robot.macroState == 0) {
macroState = -1;
}
}
}
}

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontStageAuto.java
View File

@ -62,6 +62,7 @@ public class BlueFrontStageAuto extends AutoBase {
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1M);
builder.lineToLinearHeading(DROP_1);
break;
case 2:

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java
View File

@ -29,7 +29,7 @@ public class RedBackStageAuto extends AutoBase {
public static final Pose2d ALINE = new Pose2d(51,-32.5, Math.toRadians(180));
public static final Pose2d DROP_3 = new Pose2d(25, -45.5, Math.toRadians(90));
public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52, -28.3, Math.toRadians(180));
public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52, -26.3, Math.toRadians(180));
public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(52.3, -34.5, Math.toRadians(190));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(51, -40.7, Math.toRadians(190));
@ -41,13 +41,13 @@ public class RedBackStageAuto extends AutoBase {
//public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, -8.5);//-36
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(27, -11.2, Math.toRadians(185));//-36
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(27, -10.2, Math.toRadians(190));//-36
public static final Pose2d STACK_LOCATION_1 = new Pose2d(-56, -11.2, Math.toRadians(185));
public static final Pose2d STACK_LOCATION_1 = new Pose2d(-56, -10.2, Math.toRadians(190));
public static final Pose2d STACK_LOCATION_2 = new Pose2d(-56.2, -11.2, Math.toRadians(185));
public static final Pose2d STACK_LOCATION_2 = new Pose2d(-56.2, -10.2, Math.toRadians(190));
public static final Pose2d STACK_LOCATION_3 = new Pose2d(-56.2, -11.2, Math.toRadians(185));
public static final Pose2d STACK_LOCATION_3 = new Pose2d(-56.2, -10.2, Math.toRadians(185));
@Override
public void createTrajectories() {
@ -101,7 +101,7 @@ public class RedBackStageAuto extends AutoBase {
//Scores yellow pixel
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DEPOSIT_PRELOAD_1 );
builder.lineToLinearHeading(DEPOSIT_PRELOAD_1);
break;
case 2:
robot.extendMacro(Slides.mini_tier1 -20, getRuntime());
@ -140,7 +140,7 @@ public class RedBackStageAuto extends AutoBase {
builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-1.85)));
break;
case 2:
builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-5)));
builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-3)));
break;
case 3:
builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-3)));

1
settings.gradle
View File

@ -1,3 +1,4 @@
include ':FtcRobotController'
include ':TeamCode'
include ':MeepMeepTesting'
include ':MeepMeepTesting'