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

created a new abstract auto and example red backstage auto that are simplified versions of the current autos utilizing only state machines instead of the step class and builder functions

This commit is contained in:
Robert Teal 2023-12-21 13:05:44 -06:00
parent 5eb278042b
commit 2fcc757bbb
3 changed files with 188 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
View File

@ -26,7 +26,6 @@ public class Robot {
public int lastMacro = 0;
private boolean camEnabled = true;
public static double slideWait = 1.5;
public static double scoreWait = 0.65;
public static double armWait = 2.0;

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

@ -0,0 +1,61 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot;
public abstract class AutoBase extends LinearOpMode {
public Robot robot;
protected int macroState;
protected double macroTime;
protected int teamPropLocation = 2; // 1 is left, 2 is middle, 3 is right, perhaps these could be enums instead
// abstract methods for each auto to implement
public abstract void createTrajectories();
public abstract void followTrajectories();
@Override
public void runOpMode() {
// create telemetry object for both driver hub and dashboard
// check SampleMecanumDrive to make sure it doesn't override the dashboard telemetry
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
// initialize robot
robot = new Robot(hardwareMap);
// create trajectories
createTrajectories();
// wait for start
while (!isStarted() || !isStopRequested()) {
if (robot.camera.getFrameCount() < 1) {
telemetry.addLine("Initializing...");
} else {
telemetry.addLine("Initialized");
teamPropLocation = robot.camera.getMarkerId(); // or whatever method you end up using
telemetry.addData("Team Prop Location", teamPropLocation);
}
telemetry.update();
}
// run auto
while (macroState != -1 && !isStopRequested()) {
// state machine to follow trajectories
followTrajectories();
// update robot
robot.update(getRuntime());
// update telemetry
telemetry.addLine(robot.getTelemetry());
telemetry.update();
}
// stop camera
robot.camera.stopTargetingCamera();
}
}

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

@ -0,0 +1,127 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.Slides;
@Autonomous(name = "Red Backstage Auto", group = "Competition", preselectTeleOp = "Main TeleOp")
public class RedBackStageAuto extends AutoBase {
public Trajectory scorePurple1;
public Trajectory scorePurple2;
public Trajectory scorePurple3;
public Trajectory scoreYellow1;
public Trajectory scoreYellow2;
public Trajectory scoreYellow3;
public Trajectory parkRobot1;
public Trajectory parkRobot2;
public Trajectory parkRobot3;
@Override
public void createTrajectories() {
// set start position
Pose2d start = new Pose2d(12, -61.5, Math.toRadians(90));
robot.drive.setPoseEstimate(start);
// create pose2d variables
// you might not need 3 instances of the deposit position, for example, however based on localization accuracy
// you might need them for each one to be slightly different
Pose2d drop1 = new Pose2d(12, -40.5, Math.toRadians(90));
Pose2d drop2 = new Pose2d(12, -40.5, Math.toRadians(90));
Pose2d drop3 = new Pose2d(12, -40.5, Math.toRadians(90));
Pose2d depositPreload1 = new Pose2d(46, -36, Math.toRadians(180));
Pose2d depositPreload2 = new Pose2d(46, -36, Math.toRadians(180));
Pose2d depositPreload3 = new Pose2d(46, -36, Math.toRadians(180));
Pose2d park1 = new Pose2d(48, -12, Math.toRadians(180));
Pose2d park2 = new Pose2d(48, -12, Math.toRadians(180));
Pose2d park3 = new Pose2d(48, -12, Math.toRadians(180));
// create trajectories
scorePurple1 = robot.drive.trajectoryBuilder(start)
.lineToLinearHeading(drop1)
.build();
scorePurple2 = robot.drive.trajectoryBuilder(start)
.lineToLinearHeading(drop2)
.build();
scorePurple3 = robot.drive.trajectoryBuilder(start)
.lineToLinearHeading(drop3)
.build();
scoreYellow1 = robot.drive.trajectoryBuilder(scorePurple1.end())
.lineToLinearHeading(depositPreload1)
.build();
scoreYellow2 = robot.drive.trajectoryBuilder(scorePurple2.end())
.lineToLinearHeading(depositPreload2)
.build();
scoreYellow3 = robot.drive.trajectoryBuilder(scorePurple3.end())
.lineToLinearHeading(depositPreload3)
.build();
parkRobot1 = robot.drive.trajectoryBuilder(scoreYellow1.end())
.lineToLinearHeading(park1)
.build();
parkRobot2 = robot.drive.trajectoryBuilder(scoreYellow2.end())
.lineToLinearHeading(park2)
.build();
parkRobot3 = robot.drive.trajectoryBuilder(scoreYellow3.end())
.lineToLinearHeading(park3)
.build();
}
@Override
public void followTrajectories() {
switch (macroState) {
case 0:
// start drive to tape
robot.drive.followTrajectoryAsync(teamPropLocation==1?scorePurple1:(teamPropLocation==2?scorePurple2:scorePurple3));
macroState++;
break;
case 1:
// wait until robot is at tape
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
macroState++;
}
case 2:
// run intake
if (getRuntime() < macroTime + 0.5) {
robot.intake.setDcMotor(-0.3);
}
// start drive to backdrop
else {
robot.extendMacro(Slides.Position.TIER1, getRuntime());
robot.drive.followTrajectoryAsync(teamPropLocation==1?scoreYellow1:(teamPropLocation==2?scoreYellow2:scoreYellow3));
macroState++;
}
break;
case 3:
// extend macro
if (robot.macroState != 0) {
robot.extendMacro(Slides.Position.TIER1, getRuntime());
}
// if macro and drive are done, start park
else if (!robot.drive.isBusy()) {
robot.resetMacro(Slides.Position.DOWN, getRuntime());
robot.drive.followTrajectoryAsync(teamPropLocation==1?parkRobot1:(teamPropLocation==2?parkRobot2:parkRobot3));
macroState++;
}
break;
// park robot
case 4:
// reset macro
if (robot.macroState != 0) {
robot.resetMacro(Slides.Position.DOWN, getRuntime());
}
// if macro and drive are done, end auto
if (!robot.drive.isBusy()) {
macroState = -1;
}
break;
}
}
}