diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java index fec2c7b..2c68684 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java @@ -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; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java index e03bf16..5ce0598 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java @@ -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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AbstractAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AbstractAuto.java deleted file mode 100644 index c4a4b8c..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AbstractAuto.java +++ /dev/null @@ -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 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; - } - }); - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoBase.java index fbcb826..3a73f2e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoBase.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoBase.java @@ -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(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueLeftAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueLeftAuto.java deleted file mode 100644 index 5640391..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueLeftAuto.java +++ /dev/null @@ -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); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueRightAuto_Drivepathonly_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueRightAuto_Drivepathonly_.java deleted file mode 100644 index 9df0943..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueRightAuto_Drivepathonly_.java +++ /dev/null @@ -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); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java index 072df29..a454644 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java @@ -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; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedRightAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedRightAuto.java deleted file mode 100644 index 537f6c5..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedRightAuto.java +++ /dev/null @@ -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; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/SimpleBlueLeftAuto_ignore.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/SimpleBlueLeftAuto_ignore.java deleted file mode 100644 index 9881292..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/SimpleBlueLeftAuto_ignore.java +++ /dev/null @@ -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); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/Step.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/Step.java deleted file mode 100644 index 2deaa24..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/Step.java +++ /dev/null @@ -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; - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java index 51b7991..24e8cdf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java @@ -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; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/FollowerPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/FollowerPIDTuner.java index e4fbfae..128f22e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/FollowerPIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/FollowerPIDTuner.java @@ -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 {