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

Robby is dumb and stupid

This commit is contained in:
Robert Teal 2023-12-22 17:28:13 -06:00
parent 2fcc757bbb
commit 4b23276e62
12 changed files with 63 additions and 741 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -42,7 +42,7 @@ public class Robot {
camEnabled = true;
}
public void extendMacro(Slides.Position slidePos, double runTime) {
public void extendMacro(int slidePos, double runTime) {
switch(macroState) {
case(0):
macroStartTime = runTime;
@ -58,14 +58,19 @@ public class Robot {
case(2):
arm.setArmPos(Arm.Position.SCORE);
arm.setWristPos(Arm.Position.SCORE);
macroState = 0;
lastMacro = runningMacro;
runningMacro = 0;
macroState++;
break;
case (3):
if(arm.armAtPosition()){
macroState = 0;
lastMacro = runningMacro;
runningMacro = 0;
}
break;
}
}
public void resetMacro(Slides.Position pos, double runTime) {
public void resetMacro(int slidePos, double runTime) {
switch(macroState) {
case(0):
macroStartTime = runTime;
@ -75,7 +80,7 @@ public class Robot {
break;
case(1):
if (runTime > macroStartTime + scoreWait) {
macroState ++;
macroState ++;
}
break;
case(2):
@ -90,10 +95,16 @@ public class Robot {
}
break;
case(4):
slides.setTarget(pos);
macroState = 0;
lastMacro = runningMacro;
runningMacro = 0;
slides.setTarget(slidePos);
macroState++;
break;
case (5):
if (slides.atTarget()){
macroState = 0;
lastMacro = runningMacro;
runningMacro = 0;
}
break;
}
}

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

@ -22,6 +22,7 @@ public class Slides {
public static int targetMax = 830;
public static int down = 0;
public static int mini_tier1 = 155;
public static int tier1 = 350;
public static int tier2 = 500;
public static int tier3 = 760;

338
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AbstractAuto.java
View File

@ -1,338 +0,0 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.hardware.Arm;
import org.firstinspires.ftc.teamcode.hardware.Intake;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.Slides;
import org.firstinspires.ftc.teamcode.util.CameraPosition;
import java.util.ArrayList;
import java.util.Locale;
public abstract class AbstractAuto extends LinearOpMode {
public Robot robot;
private int teamElementLocation = 2;
private ArrayList<Step> steps;
private double currentRuntime;
public CameraPosition cameraPosition;
// Main method to run all the steps for autonomous
@Override
public void runOpMode() {
// telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
telemetry.addLine("Initializing...");
telemetry.update();
setCameraPosition();
robot = new Robot(hardwareMap);
makeTrajectories();
while (robot.camera.getFrameCount() < 1) {
idle();
}
// wait for start
while (!(isStarted() || isStopRequested())) {
currentRuntime = getRuntime();
robot.update(currentRuntime);
// int newLocation = robot.camera.getMarkerId();
// if (newLocation != -1) {
// teamElementLocation = newLocation;
// }
telemetry.addLine("Initialized");
telemetry.addLine("Randomization: "+teamElementLocation);
telemetry.addLine(robot.getTelemetry());
telemetry.update();
}
resetRuntime();
// build the first step
steps = new ArrayList<>();
// stopCamera();
initializeSteps(teamElementLocation);
int stepNumber = 0;
double stepTimeout;
Step step = steps.get(stepNumber);
stepTimeout = step.getTimeout() != -1 ? currentRuntime + step.getTimeout() : Double.MAX_VALUE;
step.start();
// run the remaining steps
while (opModeIsActive()) {
currentRuntime = getRuntime();
// once a step finishes
if (step.isFinished() || currentRuntime >= stepTimeout) {
// do the finishing move
step.end();
stepNumber++;
// if it was the last step break out of the while loop
if (stepNumber > steps.size() - 1) {
break;
}
// else continue to the next step
step = steps.get(stepNumber);
stepTimeout = step.getTimeout() != -1 ? currentRuntime + step.getTimeout() : Double.MAX_VALUE;
step.start();
}
// while the step is running display telemetry
step.whileRunning();
robot.update(currentRuntime);
telemetry.addLine(String.format(Locale.US, "Runtime: %.0f", currentRuntime));
telemetry.addLine("Step " + (stepNumber + 1) + " of " + steps.size() + ", " + step.getTelemetry() + "\n");
telemetry.addLine(robot.getTelemetry());
telemetry.update();
}
}
// Load up all of the steps for the autonomous: to be overridden with the specific steps in the specific auto
public abstract void initializeSteps(int location);
//methods to be implemented in the specific autos
public abstract void setAlliance();
public abstract void setCameraPosition();
public abstract boolean useCamera();
public abstract void makeTrajectories();
// public abstract void setArm(Arm.Position armPos, Claw.Position clawPos);
//other methods that do certain tasks
public void turn(double degrees) {
steps.add(new Step("Following a trajectory") {
@Override
public void start() {
robot.drive.turn(degrees);
}
@Override
public void whileRunning() {
robot.drive.update();
}
@Override
public void end() {
}
@Override
public boolean isFinished() {
return !robot.drive.isBusy();
}
});
}
public void followTrajectory(Trajectory trajectory) {
steps.add(new Step("Following a trajectory") {
@Override
public void start() {
robot.drive.followTrajectoryAsync(trajectory);
}
@Override
public void whileRunning() {
robot.drive.update();
}
@Override
public void end() {
}
@Override
public boolean isFinished() {
return !robot.drive.isBusy();
}
});
}
public void followAndExtend(Trajectory trajectory, Slides.Position slidePos) {
steps.add(new Step("Following a trajectory") {
@Override
public void start() {
robot.drive.followTrajectoryAsync(trajectory);
if (slidePos == Slides.Position.TIER1) {
robot.runningMacro = 1;
} else if (slidePos == Slides.Position.TIER2) {
robot.runningMacro = 2;
} else if (slidePos == Slides.Position.TIER3) {
robot.runningMacro = 3;
}
robot.extendMacro(slidePos, currentRuntime);
}
@Override
public void whileRunning() {
if (robot.runningMacro != 0) {
robot.extendMacro(slidePos, currentRuntime);
}
robot.drive.update();
}
@Override
public void end() {
}
@Override
public boolean isFinished() {
return !robot.drive.isBusy() && robot.runningMacro == 0;
}
});
}
public void followAndRetract(Trajectory trajectory, Slides.Position slidePos) {
steps.add(new Step("Following a trajectory") {
@Override
public void start() {
robot.drive.followTrajectoryAsync(trajectory);
robot.runningMacro = 4;
robot.resetMacro(slidePos, currentRuntime);
}
@Override
public void whileRunning() {
if (robot.runningMacro != 0) {
robot.resetMacro(slidePos, currentRuntime);
}
robot.drive.update();
}
@Override
public void end() {
}
@Override
public boolean isFinished() {
return !robot.drive.isBusy() && robot.runningMacro == 0;
}
});
}
public void intakeStack(Intake.Position stackHeight1, Intake.Position stackHeight2) {
steps.add(new Step("Intaking Stack") {
@Override
public void start() {
robot.intake.setDcMotor(0.5);
robot.arm.setDoor(Arm.DoorPosition.OPEN);
}
@Override
public void whileRunning() {
switch(macroState) {
case 0:
macroStart = currentRuntime;
robot.intake.setpos(stackHeight1);
macroState++;
break;
case 1:
if (currentRuntime > macroStart + 1.0) {
macroState++;
}
break;
case 2:
robot.intake.setpos(stackHeight2);
macroState++;
break;
case 3:
if (currentRuntime > macroStart + 1.0) {
macroState++;
}
break;
case 4:
macroState = 0;
}
}
@Override
public void end() {
robot.intake.setDcMotor(0);
robot.intake.setpos(Intake.Position.UP);
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
}
@Override
public boolean isFinished() {
return macroState == 0;
}
});
}
public void runIntake(double power, double timeout) {
steps.add(new Step("Running Intake" + timeout + " seconds", timeout) {
@Override
public void start() {
robot.intake.setDcMotor(power);
}
@Override
public void whileRunning() {
}
@Override
public void end() {
robot.intake.setDcMotor(0);
}
@Override
public boolean isFinished() {
return false;
}
});
}
// Functions to add steps
public void addDelay(double timeout) {
steps.add(new Step("Waiting for " + timeout + " seconds", timeout) {
@Override
public void start() {
}
@Override
public void whileRunning() {
}
@Override
public void end() {
}
@Override
public boolean isFinished() {
return false;
}
});
}
public void stopCamera() {
steps.add(new Step("Stopping Signal Camera") {
@Override
public void start() {
robot.camera.stopTargetingCamera();
}
@Override
public void whileRunning() {
}
@Override
public void end() {
}
@Override
public boolean isFinished() {
return true;
}
});
}
}

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoBase.java
View File

@ -31,12 +31,12 @@ public abstract class AutoBase extends LinearOpMode {
createTrajectories();
// wait for start
while (!isStarted() || !isStopRequested()) {
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
//teamPropLocation = robot.camera.getMarkerId(); // or whatever method you end up using
telemetry.addData("Team Prop Location", teamPropLocation);
}
telemetry.update();
@ -51,6 +51,7 @@ public abstract class AutoBase extends LinearOpMode {
robot.update(getRuntime());
// update telemetry
telemetry.addData("Macro State", macroState);
telemetry.addLine(robot.getTelemetry());
telemetry.update();
}

59
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueLeftAuto.java
View File

@ -1,59 +0,0 @@
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 = "Blue Left Auto")
public class BlueLeftAuto extends AbstractAuto {
public Trajectory scorePurple;
public Trajectory scoreYellow;
public Trajectory parkRobot;
@Override
public void initializeSteps(int location) {
followTrajectory(scorePurple);
runIntake(-0.3, 1);
followAndExtend(scoreYellow, Slides.Position.TIER1);
followAndRetract(parkRobot, Slides.Position.DOWN);
}
@Override
public void setAlliance() {
}
@Override
public void setCameraPosition() {
}
@Override
public boolean useCamera() {
return false;
}
@Override
public void makeTrajectories() {
Pose2d start = new Pose2d(24, 61.5, Math.toRadians(-90));
// Pose2d dropLeft = new Pose2d(24, 60, Math.toRadians(-90));
Pose2d dropMiddle = new Pose2d(24, 40.5, Math.toRadians(-90));
// Pose2d dropRight = new Pose2d(24, 60, Math.toRadians(-90));
Pose2d score = new Pose2d(62, 36, Math.toRadians(180));
Pose2d park = new Pose2d(62, 60, Math.toRadians(180));
scorePurple = robot.drive.trajectoryBuilder(start)
.lineToLinearHeading(dropMiddle)
.build();
scoreYellow = robot.drive.trajectoryBuilder(scorePurple.end())
.lineToLinearHeading(score)
.build();
parkRobot = robot.drive.trajectoryBuilder(scoreYellow.end())
.lineToLinearHeading(park)
.build();
robot.drive.setPoseEstimate(start);
}
}

79
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueRightAuto_Drivepathonly_.java
View File

@ -1,79 +0,0 @@
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 com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
@Autonomous(name = "BlueRightAuto(Drive only)")
public class BlueRightAuto_Drivepathonly_ extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
// add more trajectories here
Pose2d start = new Pose2d(-36, 61.5, Math.toRadians(-90));
// Pose2d dropLeft = new Pose2d(24, 60, Math.toRadians(-90));
Pose2d dropMiddle = new Pose2d(-36, 40.5, Math.toRadians(-90));
//
// Pose2d nine = new Pose2d(-36, 40.5, Math.toRadians(-180));
Pose2d stack1 = new Pose2d(-50, 40.5, Math.toRadians(-180));
Pose2d dropMiddle2 = new Pose2d(-36, 40.5, Math.toRadians(-180));
Pose2d bmid = new Pose2d(-36, 5, Math.toRadians(-180));
Pose2d bmid2 = new Pose2d(36, 5, Math.toRadians(-180));
Pose2d alimb = new Pose2d(54, 40.5, Math.toRadians(-180));
Pose2d score = new Pose2d(54, 36, Math.toRadians(180));
drive.setPoseEstimate(start);
// add this per trajectories
Trajectory scorePurple = drive.trajectoryBuilder(start)
.lineToLinearHeading(dropMiddle)
.build();
Trajectory mid_drop = drive.trajectoryBuilder(dropMiddle)
.lineToLinearHeading(stack1)
.build();
Trajectory back_to_mid = drive.trajectoryBuilder(stack1)
.lineToLinearHeading(dropMiddle2)
.build();
Trajectory front_gate = drive.trajectoryBuilder(dropMiddle2)
.lineToLinearHeading(bmid)
.build();
Trajectory front_gate_pt_2 = drive.trajectoryBuilder(bmid)
.lineToLinearHeading(bmid2)
.build();
Trajectory front_gate_pt_3 = drive.trajectoryBuilder(bmid2)
.lineToLinearHeading(alimb)
.build();
Trajectory about_to_score = drive.trajectoryBuilder(alimb)
.lineToLinearHeading(score)
.build();
// the scorePurple. should be whatever the start pose2d thing was
Trajectory scoreYellow = drive.trajectoryBuilder(scorePurple.end())
.lineToLinearHeading(score)
.build();
waitForStart();
if(isStopRequested()) return;
// add this per trajectories
drive.followTrajectory(scorePurple);
drive.turn(-90);
drive.followTrajectory(mid_drop);
// drive.followTrajectory(stack);
drive.followTrajectory(back_to_mid);
drive.followTrajectory(front_gate);
drive.followTrajectory(front_gate_pt_2);
drive.followTrajectory(front_gate_pt_3);
drive.followTrajectory(about_to_score);
drive.followTrajectory(scoreYellow);
}
}

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

@ -29,13 +29,13 @@ public class RedBackStageAuto extends AutoBase {
// 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 drop1 = new Pose2d(12, -39.5, Math.toRadians(90));
Pose2d drop2 = new Pose2d(12, -39.5, Math.toRadians(90));
Pose2d drop3 = new Pose2d(12, -39.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 depositPreload1 = new Pose2d(50.5, -32, Math.toRadians(187));
Pose2d depositPreload2 = new Pose2d(50.5, -32, Math.toRadians(187));
Pose2d depositPreload3 = new Pose2d(50.5, -32, Math.toRadians(187));
Pose2d park1 = new Pose2d(48, -12, Math.toRadians(180));
Pose2d park2 = new Pose2d(48, -12, Math.toRadians(180));
@ -77,49 +77,59 @@ public class RedBackStageAuto extends AutoBase {
public void followTrajectories() {
switch (macroState) {
case 0:
// start drive to tape
robot.drive.followTrajectoryAsync(teamPropLocation==1?scorePurple1:(teamPropLocation==2?scorePurple2:scorePurple3));
macroState++;
break;
// DRIVE TO TAPE
case 1:
// wait until robot is at tape
// if drive is done move on
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
macroState++;
}
break;
// RUN INTAKE
case 2:
// run intake
// intake
if (getRuntime() < macroTime + 0.5) {
robot.intake.setDcMotor(-0.3);
robot.intake.setDcMotor(-0.26);
}
// start drive to backdrop
// if intake is done move on
else {
robot.extendMacro(Slides.Position.TIER1, getRuntime());
robot.intake.setDcMotor(0);
robot.extendMacro(Slides.mini_tier1, getRuntime());
robot.drive.followTrajectoryAsync(teamPropLocation==1?scoreYellow1:(teamPropLocation==2?scoreYellow2:scoreYellow3));
macroState++;
}
break;
// EXTEND AND MOVE TO BACKBOARD
case 3:
// extend macro
if (robot.macroState != 0) {
robot.extendMacro(Slides.Position.TIER1, getRuntime());
robot.extendMacro(Slides.mini_tier1, getRuntime());
}
// if macro and drive are done, start park
else if (!robot.drive.isBusy()) {
robot.resetMacro(Slides.Position.DOWN, getRuntime());
// if macro and drive are done, move on
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
case 4:
robot.resetMacro(0, getRuntime());
if (robot.macroState >= 2){
robot.drive.followTrajectoryAsync(teamPropLocation==1?parkRobot1:(teamPropLocation==2?parkRobot2:parkRobot3));
macroState++;
}
break;
// park robot
case 4:
// reset macro
// PARK ROBOT
case 5:
// reset macro'
if (robot.macroState != 0) {
robot.resetMacro(Slides.Position.DOWN, getRuntime());
robot.resetMacro(0, getRuntime());
}
// if macro and drive are done, end auto
if (!robot.drive.isBusy()) {
macroState = -1;
if (robot.macroState == 0 && !robot.drive.isBusy()) {
macroState=-1;
}
break;
}

135
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedRightAuto.java
View File

@ -1,135 +0,0 @@
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.Intake;
import org.firstinspires.ftc.teamcode.hardware.Slides;
import org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
@Autonomous(name = "Red Right Auto")
public class RedRightAuto extends AbstractAuto {
public Trajectory scorePurple;
public Trajectory scoreYellow;
public Trajectory load1;
public Trajectory score1;
public Trajectory load2;
public Trajectory score2;
public Trajectory parkRobot;
@Override
public void makeTrajectories() {
Pose2d start = new Pose2d(12, -61.5, Math.toRadians(90));
Pose2d dropMiddle = new Pose2d(12, -40.5, Math.toRadians(90));
Pose2d depositPreload = new Pose2d(46, -36, Math.toRadians(180));
Pose2d lineup1 = new Pose2d(12, -14, Math.toRadians(180));
Pose2d pickup1 = new Pose2d(-58, -14, Math.toRadians(180));
Pose2d driveup1 = new Pose2d(12-6, -14, Math.toRadians(180));
Pose2d deposit1 = new Pose2d(48-6, -36-4, Math.toRadians(180));
Pose2d lineup2 = new Pose2d(12, -14, Math.toRadians(180));
Pose2d pickup2 = new Pose2d(-58, -14, Math.toRadians(180));
Pose2d driveup2 = new Pose2d(12-6, -14, Math.toRadians(180));
Pose2d deposit2 = new Pose2d(48-6, -36-4, Math.toRadians(180));
Pose2d park = new Pose2d(48, -12, Math.toRadians(180));
scorePurple = robot.drive.trajectoryBuilder(start)
.lineToLinearHeading(dropMiddle)
.build();
scoreYellow = robot.drive.trajectoryBuilder(scorePurple.end())
.lineToLinearHeading(depositPreload)
.build();
int driveSpeed = 45;
load1 = robot.drive.trajectoryBuilder(scoreYellow.end())
.splineToConstantHeading(lineup1.vec(), lineup1.getHeading(),
SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
)
.lineToLinearHeading(pickup1,
SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
)
.build();
score1 = robot.drive.trajectoryBuilder(load1.end(), true)
.lineToLinearHeading(driveup1,
SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
)
.splineToConstantHeading(deposit1.vec(), deposit1.getHeading(),
SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
)
.build();
load2 = robot.drive.trajectoryBuilder(score1.end())
.splineToConstantHeading(lineup1.vec(), lineup1.getHeading(),
SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
)
.lineToLinearHeading(pickup1,
SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
)
.build();
score2 = robot.drive.trajectoryBuilder(load2.end(), true)
.lineToLinearHeading(driveup1,
SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
)
.splineToConstantHeading(deposit1.vec(), deposit1.getHeading(),
SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
)
.build();
parkRobot = robot.drive.trajectoryBuilder(score2.end())
.lineToLinearHeading(park)
.build();
robot.drive.setPoseEstimate(start);
}
@Override
public void initializeSteps(int location) {
// score preloads
followTrajectory(scorePurple);
runIntake(-0.4, 0.5);
followAndExtend(scoreYellow, Slides.Position.TIER1);
// cycle
followAndRetract(load1, Slides.Position.DOWN);
intakeStack(Intake.Position.STACK5, Intake.Position.STACK4);
followAndExtend(score1, Slides.Position.TIER1);
followAndRetract(load2, Slides.Position.DOWN);
intakeStack(Intake.Position.STACK3, Intake.Position.STACK2);
followAndExtend(score2, Slides.Position.TIER1);
followAndRetract(parkRobot, Slides.Position.DOWN);
}
@Override
public void setAlliance() {
}
@Override
public void setCameraPosition() {
}
@Override
public boolean useCamera() {
return false;
}
}

42
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/SimpleBlueLeftAuto_ignore.java
View File

@ -1,42 +0,0 @@
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 com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
@Autonomous(name = "Simple Blue Left Auto")
public class SimpleBlueLeftAuto_ignore extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
// add more trajectories here
Pose2d start = new Pose2d(24, 61.5, Math.toRadians(-90));
// Pose2d dropLeft = new Pose2d(24, 60, Math.toRadians(-90));
Pose2d dropMiddle = new Pose2d(24, 40.5, Math.toRadians(-90));
// Pose2d dropRight = new Pose2d(24, 60, Math.toRadians(-90));
Pose2d score = new Pose2d(60, 36, Math.toRadians(180));
drive.setPoseEstimate(start);
// add this per trajectories
Trajectory scorePurple = drive.trajectoryBuilder(start)
.lineToLinearHeading(dropMiddle)
.build();
// the scorePurple. should be whatever the start pose@d thing was
Trajectory scoreYellow = drive.trajectoryBuilder(scorePurple.end())
.lineToLinearHeading(score)
.build();
waitForStart();
if(isStopRequested()) return;
// add this per trajectories
drive.followTrajectory(scorePurple);
// drive.wait(1);
drive.followTrajectory(scoreYellow);
}
}

48
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/Step.java
View File

@ -1,48 +0,0 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import org.firstinspires.ftc.teamcode.vision.Detection;
// Class for every step that the autonomous program will take
public abstract class Step {
private final double timeout;
private String telemetry;
// macro variables
public double macroStart;
public int macroState;
// Constructors
public Step(String telemetry) {
this.telemetry = telemetry;
this.timeout = -1;
}
public Step(String telemetry, double timeout) {
this.telemetry = telemetry;
this.timeout = timeout;
}
// Abstract methods to be overrode
public abstract void start();
public abstract void whileRunning();
public abstract void end();
public abstract boolean isFinished();
// Return the timeout for the step
public double getTimeout() {
return timeout;
}
public void setTelemetry(String telemetry) {
this.telemetry = telemetry;
}
// Return the Telemetry for the step
public String getTelemetry() {
return telemetry;
}
}

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java
View File

@ -119,16 +119,16 @@ public class MainTeleOp extends OpMode {
}
break;
case (1):
robot.extendMacro(Slides.Position.TIER1, getRuntime());
robot.extendMacro(Slides.tier1, getRuntime());
break;
case (2):
robot.extendMacro(Slides.Position.TIER2, getRuntime());
robot.extendMacro(Slides.tier2, getRuntime());
break;
case (3):
robot.extendMacro(Slides.Position.TIER3, getRuntime());
robot.extendMacro(Slides.tier3, getRuntime());
break;
case (4):
robot.resetMacro(Slides.Position.DOWN, getRuntime());
robot.resetMacro(0, getRuntime());
break;
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/FollowerPIDTuner.java
View File

@ -22,10 +22,10 @@ import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySe
* These coefficients can be tuned live in dashboard.
*/
//@Config
@Disabled
@Autonomous(group = "drive")
public class FollowerPIDTuner extends LinearOpMode {
public static double DISTANCE = 48; // in
public static double DISTANCE = 36; // in
@Override
public void runOpMode() throws InterruptedException {