From 5329732517c8f918406e59af05a3c27c2fe7c12c Mon Sep 17 00:00:00 2001 From: Robert Teal Date: Mon, 20 Nov 2023 23:36:55 -0600 Subject: [PATCH] Tuned roadrunner, refactored a bunch of things, added macros, added a basic auto, added a new teleop --- TeamCode/build.gradle | 16 - .../ftc/teamcode/hardware/Arm.java | 75 +-- .../ftc/teamcode/hardware/Intake.java | 9 + .../ftc/teamcode/hardware/Robot.java | 131 ++--- .../ftc/teamcode/hardware/robby/Slides.java | 41 +- .../opmode/autonomous/AbstractAuto.java | 291 ++++++++++ .../opmode/autonomous/BlueLeftAuto.java | 59 ++ .../teamcode/opmode/autonomous/BotGoMove.java | 27 - .../opmode/autonomous/SimpleBlueLeftAuto.java | 42 ++ .../autonomous/StartFromLeftCenterSpike.java | 59 -- .../ftc/teamcode/opmode/autonomous/Step.java | 106 ++-- .../teamcode/opmode/teleop/Meet1TeleOp.java | 504 +++++++++--------- .../ftc/teamcode/opmode/teleop/NewTeleop.java | 99 ++++ .../roadrunner/drive/DriveConstants.java | 16 +- .../roadrunner/drive/SampleMecanumDrive.java | 6 +- .../drive/TwoWheelTrackingLocalizer.java | 20 +- 16 files changed, 925 insertions(+), 576 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AbstractAuto.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueLeftAuto.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BotGoMove.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/SimpleBlueLeftAuto.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/StartFromLeftCenterSpike.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/NewTeleop.java diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index af94d2b..32553f2 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -10,27 +10,11 @@ // Custom definitions may go here - -// Include common definitions from above. apply from: '../build.common.gradle' apply from: '../build.dependencies.gradle' android { - - defaultConfig { - externalNativeBuild { - cmake { - cppFlags '' - } - } - } namespace = 'org.firstinspires.ftc.teamcode' - externalNativeBuild { - cmake { - path file('src/main/cpp/CMakeLists.txt') - version '3.18.1' - } - } packagingOptions { jniLibs.useLegacyPackaging true diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java index 9382cde..d3ad605 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java @@ -21,8 +21,8 @@ public class Arm { private double ARM_KP = 0.001; - public enum APosition { - SDOWN, SCORE, SAFTEYDOWN, SAFTEYUP; + public enum Position { + INTAKE, SCORE; } public Arm(HardwareMap hardwareMap) { @@ -34,17 +34,36 @@ public class Arm { rAServo.setDirection(Servo.Direction.REVERSE); dServo.setDirection(Servo.Direction.REVERSE); // wristServo.setDirection(Servo.Direction.REVERSE); + + setArmPos(Position.INTAKE); + setWristPos(Position.INTAKE); } - public static double armScorePos = 1; public static double doorOpenpos = 0.5; public static double doorClosePos = 0.85; - public static double startingWristPos = 0.735; - public static double startingArmPos = 0.325; - public static double safteyDownPos = 0.4; - public static double safteyUpPos = 0.9; - public static double wristScorePos = 0.98; - public static double safeWrist = 0.8; + + public static double armStart = 0.325; + public static double armScore = 1; + public static double wristStart = 0.735; + public static double wristScore = 0.98; + + public void setArmPos(Position pos) { + if (pos == Position.INTAKE) { + rAServo.setPosition(armStart); + lAServo.setPosition(armStart); + } else if (pos == Position.SCORE) { + rAServo.setPosition(armScore); + lAServo.setPosition(armScore); + } + } + + public void setWristPos(Position pos) { + if (pos == Position.INTAKE) { + wristServo.setPosition(wristStart); + } else if (pos == Position.SCORE) { + wristServo.setPosition(wristScore); + } + } public void openDoor(DoorPos.DoorPosition pos) { if (pos == DoorPos.DoorPosition.OPEN) { @@ -54,48 +73,10 @@ public class Arm { } } - public void setPos(APosition tape) { - if (tape == APosition.SDOWN) { - this.armControllerTarget = startingArmPos; - lAServo.setPosition(startingArmPos); - rAServo.setPosition(startingArmPos); - wristServo.setPosition(startingWristPos); - } else if (tape == APosition.SCORE) { - this.armControllerTarget = armScorePos; - lAServo.setPosition(armScorePos); - rAServo.setPosition(armScorePos); - wristServo.setPosition(wristScorePos); - } else if (tape == APosition.SAFTEYDOWN) { - lAServo.setPosition(safteyDownPos); - rAServo.setPosition(safteyDownPos); - wristServo.setPosition(wristScorePos); - } else if (tape == APosition.SAFTEYUP) { - lAServo.setPosition(safteyUpPos); - rAServo.setPosition(safteyUpPos); - wristServo.setPosition(wristScorePos); - } - } - - -// public void setHopper(HopperPos.hopperPos hopper) { -// if (hopper == HopperPos.hopperPos.UP) { -// wristServo.setPosition(wristScorePos); -// } else if (hopper == HopperPos.hopperPos.DOWN) { -// wristServo.setPosition(startingWristPos); -// } -// } - public String getTelemetry() { return "Wrist Servo: "+wristServo.getPosition()+"Left Arm Servo: "+lAServo.getPosition()+"Right Arm Servo: "+rAServo.getPosition()+"Door Servo: "+dServo.getPosition(); } public void update() { - armController.setP(0.01); - this.armController.setSetPoint(this.armControllerTarget); - - double output = this.armController.calculate(this.lAServo.getPosition()); - - this.lAServo.setPosition(this.lAServo.getPosition() + output); - this.rAServo.setPosition(this.rAServo.getPosition() + output); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java index 215d325..40019ec 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java @@ -11,6 +11,7 @@ public class Intake { private Servo lServo; private DcMotor dcMotor; private Position pos = Position.UP; + public enum Position { STACK1, STACK2, STACK3, STACK4, STACK5, UP; @@ -63,6 +64,14 @@ public class Intake { } } + public void incrementPos() { + pos = pos.nextPosition(); + } + + public void decrementPos() { + pos = pos.previousPosition(); + } + public void setDcMotor(double pwr) { dcMotor.setPower(pwr); } 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 61e0a75..a9db0b0 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 @@ -5,6 +5,7 @@ import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.UP; import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.teamcode.hardware.robby.Slides; import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; import org.firstinspires.ftc.teamcode.roadrunner.util.Encoder; @@ -15,102 +16,78 @@ public class Robot { public SampleMecanumDrive drive; public Camera camera; public Intake intake; - public KhangSlides slides; + public Slides slides; public Arm arm; public double macroStartTime = 0; public int macroState = 0; - public int runningMacro = 0; // 0 = no macro | 1 = low macro | 2 = mid macro | 3 = high macro | 4 = pickup macro + public int runningMacro = 0; // 0 = no macro | 1 = tier 1 | 2 = tier 2 | 3 = tier 3 | 4 = return public int lastMacro = 0; private boolean camEnabled = true; //Name the objects public Robot(HardwareMap hardwareMap) { - arm = new Arm(hardwareMap); drive = new SampleMecanumDrive(hardwareMap); - slides = new KhangSlides(hardwareMap); camera = new Camera(hardwareMap); camera.initTargetingCamera(); intake = new Intake(hardwareMap, UP); + slides = new Slides(hardwareMap); + arm = new Arm(hardwareMap); camEnabled = true; - } + } + + public void extendMacro(Slides.Position slidePos, double runTime) { + switch(macroState) { + case(0): + macroStartTime = runTime; + slides.setTarget(slidePos); + macroState ++; + break; + case(1): + if (runTime > macroStartTime + 1) { + macroState ++; + } + break; + case(2): + macroStartTime = runTime; + arm.setArmPos(Arm.Position.SCORE); + arm.setWristPos(Arm.Position.SCORE); + macroState = 0; + lastMacro = runningMacro; + runningMacro = 0; + break; + } + } + + public void resetMacro(Slides.Position pos, double runTime) { + switch(macroState) { + case(0): + macroStartTime = runTime; + arm.setArmPos(Arm.Position.INTAKE); + arm.setWristPos(Arm.Position.INTAKE); + macroState++; + break; + case(1): + if (runTime > macroStartTime + 1) { + macroState ++; + } + break; + case(2): + macroStartTime = runTime; + slides.setTarget(pos); + macroState = 0; + lastMacro = runningMacro; + runningMacro = 0; + } + } - //Update on runtime and dive public void update(double runTime) { drive.update(); + slides.update(runTime); + arm.update(); } - //Return position to control hub public String getTelemetry() { - Encoder slide = null; - return String.format("position: %s", slide.getCurrentPosition()); - } - -// public void resetMacroNoDunk(int pos, double runTime) { -// switch(macroState) { -// case(0): -// macroStartTime = runTime; -// macroState++; -// break; -// case(1): -// if (runTime > macroStartTime) { -// macroState ++; -// } -// break; -// case(2): -// macroStartTime = runTime; -// slides.setTarget(pos); -// macroState = 0; -// runningMacro = 0; -// lastMacro = 0; -// } -// } -// -// public void resetMacro(int pos, double runTime) { -// switch(macroState) { -// case(0): -// macroStartTime = runTime; -// macroState++; -// break; -// case(1): -// if (runTime > macroStartTime) { -// macroState ++; -// } -// break; -// case(2): -// macroStartTime = runTime; -// slides.setTarget(pos); -// macroState = 0; -// runningMacro = 0; -// lastMacro = 0; -// } -// } -// -// public void resetMacroEnd(int pos, double runTime) { -// switch(macroState) { -// case(0): -// macroStartTime = runTime; -// macroState++; -// break; -// case(1): -// if (runTime > macroStartTime) { -// macroState ++; -// } -// break; -// case(2): -// macroStartTime = runTime; -// slides.setTarget(pos); -// macroState = 0; -// runningMacro = 0; -// lastMacro = 0; -// } -// } - - public Arm getArm() { - return this.arm; - } - //return drive - public SampleMecanumDrive getDrive() { - return this.drive; + return "Telemetry?"; } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/robby/Slides.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/robby/Slides.java index 2b17e37..5eebc85 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/robby/Slides.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/robby/Slides.java @@ -18,33 +18,28 @@ public class Slides { public static double pTolerance = 20; public static PIDController controller = new PIDController(p, i, d); - public static int[] heights = {0, (int)(190/4.0), 2*(int)(190/4.0), 3*(int)(190/4.0), 4*(int)(180/4)}; - - public static int heightOffset = 0; public static int targetMin = -10; - public static int targetMax = 770; + public static int targetMax = 1000; - public static int highPos = 720 + heightOffset; // ALSO DEFINED IN UPDATE SLIDES - public static int midPos = 350 + heightOffset; // ALSO DEFINED IN UPDATE SLIDES - public static int lowPos = heightOffset; // ALSO DEFINED IN UPDATE SLIDES - public static int pickupPos = 220 + heightOffset; // ALSO DEFINED IN UPDATE SLIDES - public static int downPos = heightOffset; + public static int down = 0; + public static int tier1 = 200; + public static int tier2 = 350; + public static int tier3 = 500; private int target = 0; public static int manualSpeed = 20; - public static int zeroPower = 5; - public enum Position { HIGH, MEDIUM, LOW, PICKUP, DOWN } + public enum Position { DOWN, TIER1, TIER2, TIER3 } public Slides(HardwareMap hardwareMap) { - slide = hardwareMap.get(DcMotor.class, "slide"); + slide = hardwareMap.get(DcMotor.class, "Right Slide Motor"); slide.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); slide.setMode(DcMotor.RunMode.RUN_USING_ENCODER); // slide.setDirection(DcMotorSimple.Direction.REVERSE); slide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - slide2 = hardwareMap.get(DcMotor.class, "slide2"); + slide2 = hardwareMap.get(DcMotor.class, "Left Slide Motor"); slide2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); slide2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); slide2.setDirection(DcMotorSimple.Direction.REVERSE); @@ -56,16 +51,14 @@ public class Slides { } public void setTarget(Position pos) { - if (pos == Position.HIGH) { - target = Math.min(Math.max(highPos, targetMin), targetMax); - } else if (pos == Position.MEDIUM) { - target = Math.min(Math.max(midPos, targetMin), targetMax); - } else if (pos == Position.LOW) { - target = Math.min(Math.max(lowPos, targetMin), targetMax); - } else if (pos == Position.PICKUP) { - target = Math.min(Math.max(pickupPos, targetMin), targetMax); - } else if (pos == Position.DOWN) { - target = Math.min(Math.max(downPos, targetMin), targetMax); + if (pos == Position.DOWN) { + target = Math.min(Math.max(down, targetMin), targetMax); + } else if (pos == Position.TIER1) { + target = Math.min(Math.max(tier1, targetMin), targetMax); + } else if (pos == Position.TIER2) { + target = Math.min(Math.max(tier2, targetMin), targetMax); + } else if (pos == Position.TIER3) { + target = Math.min(Math.max(tier3, targetMin), targetMax); } } @@ -121,6 +114,6 @@ public class Slides { } public String getTelemetry() { - return String.format("Position: %s %s\nTarget: %s %s\nPower: %s %s\nHeightOffset: %s", slide.getCurrentPosition(), slide2.getCurrentPosition(), target, target, slide.getPower(), slide2.getPower(), heightOffset); + return String.format("Position: %s %s\nTarget: %s %s\nPower: %s %s\nHeightOffset: %s", slide.getCurrentPosition(), slide2.getCurrentPosition(), target, target, slide.getPower(), slide2.getPower()); } } \ No newline at end of file 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 new file mode 100644 index 0000000..49b31cd --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AbstractAuto.java @@ -0,0 +1,291 @@ +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.Robot; +import org.firstinspires.ftc.teamcode.hardware.robby.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 Robot..."); + 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(); + } + +// // update turret and slides position +// PoseStorage.slidesPosition = robot.actuators.getSlides(); +// PoseStorage.turretPosition = robot.actuators.getTurret(); + + // 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 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/BlueLeftAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueLeftAuto.java new file mode 100644 index 0000000..b97e512 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueLeftAuto.java @@ -0,0 +1,59 @@ +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.robby.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/BotGoMove.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BotGoMove.java deleted file mode 100644 index 468e66c..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BotGoMove.java +++ /dev/null @@ -1,27 +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 com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; - -@Autonomous -public class BotGoMove extends LinearOpMode { - @Override - public void runOpMode() throws InterruptedException { - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); - - Trajectory myTrajectory = drive.trajectoryBuilder(new Pose2d()) - .forward(3.7) - .build(); - - waitForStart(); - - if(isStopRequested()) return; - - drive.followTrajectory(myTrajectory); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/SimpleBlueLeftAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/SimpleBlueLeftAuto.java new file mode 100644 index 0000000..0a7d9da --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/SimpleBlueLeftAuto.java @@ -0,0 +1,42 @@ +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 extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + 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); + + Trajectory scorePurple = drive.trajectoryBuilder(start) + .lineToLinearHeading(dropMiddle) + .build(); + + Trajectory scoreYellow = drive.trajectoryBuilder(scorePurple.end()) + .lineToLinearHeading(score) + .build(); + + waitForStart(); + + if(isStopRequested()) return; + + drive.followTrajectory(scorePurple); +// drive.wait(1); + drive.followTrajectory(scoreYellow); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/StartFromLeftCenterSpike.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/StartFromLeftCenterSpike.java deleted file mode 100644 index deb5f9f..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/StartFromLeftCenterSpike.java +++ /dev/null @@ -1,59 +0,0 @@ -//package org.firstinspires.ftc.teamcode.opmode.autonomous; -//import com.acmerobotics.roadrunner.geometry.Pose2d; -//import com.acmerobotics.roadrunner.geometry.Vector2d; -//import com.acmerobotics.roadrunner.trajectory.Trajectory; -//import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -//import com.qualcomm.robotcore.hardware.HardwareMap; -// -//import org.firstinspires.ftc.teamcode.hardware.Camera; -//import org.firstinspires.ftc.teamcode.hardware.Intake; -//import org.firstinspires.ftc.teamcode.hardware.Robot; -//import org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants; -//import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; -//import org.firstinspires.ftc.teamcode.util.CameraPosition; -// -//@Autonomous(name = "Start From Left Wall", group = "Left Start", preselectTeleOp = "Khang Main") -//public class StartFromLeftCenterSpike extends AbstractAuto { -// -// public Robot robot; -// public CameraPosition cameraPosition; -// //Steps -// public Trajectory start; -// -// @Override -// public void initializeSteps(int location) { -// followTrajectory(start); -// } -// -// @Override -// public void setAlliance() {} -// -// @Override -// public void setCameraPosition() { -// cameraPosition = CameraPosition.FRONT; -// } -// -// @Override -// public boolean useCamera() { -// return true; -// } -// -// @Override -// public void waitForStart() { -// super.waitForStart(); -// } -// -// @Override -// public void makeTrajectories() { -// -// // positions -// Pose2d start = new Pose2d(-65.125,-48,Math.toRadians(90)); -// Vector2d step1 = new Vector2d(-24,-48); -// Pose2d step2 = new Pose2d(-24,-48,Math.toRadians(90)); -// -//// this.start = robot.drive.trajectoryBuilder(start) -// this.start = robot.drive.trajectoryBuilder(new Pose2d()) -// .forward(3) -// .build(); -// } -//} 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 index ae44d63..b6ea1f9 100644 --- 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 @@ -1,53 +1,53 @@ -//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; -// -// // variables when moving -// public double x; -// public double y; -// public double stepTime; -// double tempTime = stepTime; -// -// // variables when shooting -// public Detection teamProp; -// -// // 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 +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; + + // variables when moving + public double x; + public double y; + public double stepTime; + double tempTime = stepTime; + + // variables when shooting + public Detection teamProp; + + // 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/Meet1TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Meet1TeleOp.java index f4f9162..b6f67f5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Meet1TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Meet1TeleOp.java @@ -1,260 +1,260 @@ -package org.firstinspires.ftc.teamcode.opmode.teleop; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.qualcomm.robotcore.eventloop.opmode.OpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.Servo; - -import org.firstinspires.ftc.teamcode.controller.Controller; -import org.firstinspires.ftc.teamcode.hardware.Arm; -import org.firstinspires.ftc.teamcode.hardware.DoorPos; -import org.firstinspires.ftc.teamcode.hardware.Down; -import org.firstinspires.ftc.teamcode.hardware.Hieght; -import org.firstinspires.ftc.teamcode.hardware.HopperPos; -import org.firstinspires.ftc.teamcode.hardware.Intake; -import org.firstinspires.ftc.teamcode.hardware.Robot; -import org.firstinspires.ftc.teamcode.hardware.SlidePos; -import com.qualcomm.robotcore.util.ElapsedTime; - -@Config -@TeleOp(name = "Meet 1 TeleOp", group = "OpModes") -public class Meet1TeleOp extends OpMode { - - //turbo mode - public static double normal = 0.5; - public static double turbo = 1; - - //keep track of runtime for advanced macros - private ElapsedTime runTime = new ElapsedTime(); - - //create and set start position of intake - private Intake.Position Currentpos = Intake.Position.UP; - //inform if slides are in lowest position or not - private Down.DownArm DownQuestion = Down.DownArm.YES; - //create and set start position of slides - private SlidePos.SPosition CurrentSpos = SlidePos.SPosition.DOWN; - //create and set start position of arm - private Arm.APosition CurrentApos = Arm.APosition.SDOWN; - //create and set start position of door, default close - private DoorPos.DoorPosition CurrentDpos = DoorPos.DoorPosition.CLOSE; - //create and set start position of hopper - private HopperPos.hopperPos hopperpos = HopperPos.hopperPos.DOWN; - //create manual slide height varable and set it to hold position as start - private Hieght.height CurrentHeight = Hieght.height.HOLD; - //create robot instance - private Robot robot; - //create servo for plane - private Servo planeServo; - //create controller 1 (driver) - private Controller controller1; - //create controller 2 (macro user and one with hard life) - private Controller controller2; - //set starting slide height to 0 - private double sHeight = 0; - - @Override - public void init() { - - //reset runtime when opmode is initialized - runTime.reset(); - - //make each servo, motor, and controller and configure them - planeServo = hardwareMap.get(Servo.class, "Plane Servo"); - this.robot = new Robot(hardwareMap); - controller1 = new Controller(gamepad1); - controller2 = new Controller(gamepad2); - //feedback to driver hub to know if init was successful - telemetry.addLine("Started"); - //update to make sure we receive last line of code - telemetry.update(); - } - - @Override - public void loop() { - - // Calculate the runtime in seconds - double currentTime = runTime.seconds(); - - //set start of macro runtime - double macroStartTime = 0; - - //create and set X, Y, and Z for drive inputs - double x = -gamepad1.left_stick_y; - double y = -gamepad1.left_stick_x; - double z = -gamepad1.right_stick_x; - robot.drive.setWeightedDrivePower(new Pose2d(x, y, z)); - - //turbo activation - if (controller1.getA().isJustPressed()){ - x *= turbo; - y *= turbo; - z *= turbo; - } else if (controller1.getA().isJustReleased()){ - x *= normal; - y *= normal; - z *= normal; - } - - //set intake to be pressure reactant to right trigger - robot.intake.setDcMotor(gamepad2.right_trigger); - //manual slide height control - double sHeight = gamepad2.right_stick_y; - //activate intake or not - double intakeON = gamepad2.right_trigger; - //graph input of Y joystick to make macros for slides - double sY = -gamepad2.left_stick_y; - //graph of X - double sX = gamepad2.left_stick_x; - double manualSY = gamepad2.right_stick_y; - - //reset value to set slides to starting value - double reset = gamepad2.left_trigger; - - //variable to determine shoot drone or not - double shoot = gamepad1.right_trigger; - - //update variables to constantly feed position each component should be in - //intake - robot.intake.setpos(Currentpos); - //slide macro - robot.slides.setSPos(CurrentSpos); - //slide manual -// robot.slides.setHeight(CurrentHeight); - //arm position - robot.arm.setPos(CurrentApos); - //door open or not - robot.arm.openDoor(CurrentDpos); - //update - controller2.update(); - - //manual slide input reader -// if (sHeight >= 0.2) { -// CurrentHeight = Hieght.height.ASCEND; -// } else if (sHeight <= -0.2) { -// CurrentHeight = Hieght.height.DESCEND; +//package org.firstinspires.ftc.teamcode.opmode.teleop; +//import com.acmerobotics.dashboard.config.Config; +//import com.acmerobotics.roadrunner.geometry.Pose2d; +//import com.qualcomm.robotcore.eventloop.opmode.OpMode; +//import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +//import com.qualcomm.robotcore.hardware.Servo; +// +//import org.firstinspires.ftc.teamcode.controller.Controller; +//import org.firstinspires.ftc.teamcode.hardware.Arm; +//import org.firstinspires.ftc.teamcode.hardware.DoorPos; +//import org.firstinspires.ftc.teamcode.hardware.Down; +//import org.firstinspires.ftc.teamcode.hardware.Hieght; +//import org.firstinspires.ftc.teamcode.hardware.HopperPos; +//import org.firstinspires.ftc.teamcode.hardware.Intake; +//import org.firstinspires.ftc.teamcode.hardware.Robot; +//import org.firstinspires.ftc.teamcode.hardware.SlidePos; +//import com.qualcomm.robotcore.util.ElapsedTime; +// +//@Config +//@TeleOp(name = "Meet 1 TeleOp", group = "OpModes") +//public class Meet1TeleOp extends OpMode { +// +// //turbo mode +// public static double normal = 0.5; +// public static double turbo = 1; +// +// //keep track of runtime for advanced macros +// private ElapsedTime runTime = new ElapsedTime(); +// +// //create and set start position of intake +// private Intake.Position Currentpos = Intake.Position.UP; +// //inform if slides are in lowest position or not +// private Down.DownArm DownQuestion = Down.DownArm.YES; +// //create and set start position of slides +// private SlidePos.SPosition CurrentSpos = SlidePos.SPosition.DOWN; +// //create and set start position of arm +// private Arm.Position CurrentApos = Arm.Position.SDOWN; +// //create and set start position of door, default close +// private DoorPos.DoorPosition CurrentDpos = DoorPos.DoorPosition.CLOSE; +// //create and set start position of hopper +// private HopperPos.hopperPos hopperpos = HopperPos.hopperPos.DOWN; +// //create manual slide height varable and set it to hold position as start +// private Hieght.height CurrentHeight = Hieght.height.HOLD; +// //create robot instance +// private Robot robot; +// //create servo for plane +// private Servo planeServo; +// //create controller 1 (driver) +// private Controller controller1; +// //create controller 2 (macro user and one with hard life) +// private Controller controller2; +// //set starting slide height to 0 +// private double sHeight = 0; +// +// @Override +// public void init() { +// +// //reset runtime when opmode is initialized +// runTime.reset(); +// +// //make each servo, motor, and controller and configure them +// planeServo = hardwareMap.get(Servo.class, "Plane Servo"); +// this.robot = new Robot(hardwareMap); +// controller1 = new Controller(gamepad1); +// controller2 = new Controller(gamepad2); +// //feedback to driver hub to know if init was successful +// telemetry.addLine("Started"); +// //update to make sure we receive last line of code +// telemetry.update(); +// } +// +// @Override +// public void loop() { +// +// // Calculate the runtime in seconds +// double currentTime = runTime.seconds(); +// +// //set start of macro runtime +// double macroStartTime = 0; +// +// //create and set X, Y, and Z for drive inputs +// double x = -gamepad1.left_stick_y; +// double y = -gamepad1.left_stick_x; +// double z = -gamepad1.right_stick_x; +// robot.drive.setWeightedDrivePower(new Pose2d(x, y, z)); +// +// //turbo activation +// if (controller1.getA().isJustPressed()){ +// x *= turbo; +// y *= turbo; +// z *= turbo; +// } else if (controller1.getA().isJustReleased()){ +// x *= normal; +// y *= normal; +// z *= normal; +// } +// +// //set intake to be pressure reactant to right trigger +// robot.intake.setDcMotor(gamepad2.right_trigger); +// //manual slide height control +// double sHeight = gamepad2.right_stick_y; +// //activate intake or not +// double intakeON = gamepad2.right_trigger; +// //graph input of Y joystick to make macros for slides +// double sY = -gamepad2.left_stick_y; +// //graph of X +// double sX = gamepad2.left_stick_x; +// double manualSY = gamepad2.right_stick_y; +// +// //reset value to set slides to starting value +// double reset = gamepad2.left_trigger; +// +// //variable to determine shoot drone or not +// double shoot = gamepad1.right_trigger; +// +// //update variables to constantly feed position each component should be in +// //intake +// robot.intake.setpos(Currentpos); +// //slide macro +// robot.slides.setSPos(CurrentSpos); +// //slide manual +//// robot.slides.setHeight(CurrentHeight); +// //arm position +// robot.arm.setArmPos(CurrentApos); +// //door open or not +// robot.arm.openDoor(CurrentDpos); +// //update +// controller2.update(); +// +// //manual slide input reader +//// if (sHeight >= 0.2) { +//// CurrentHeight = Hieght.height.ASCEND; +//// } else if (sHeight <= -0.2) { +//// CurrentHeight = Hieght.height.DESCEND; +//// } else { +//// CurrentHeight = Hieght.height.HOLD; +//// } +// +// //read values to determine if plane should shoot or not +// if (shoot >= 0.9) { +// planeServo.setPosition(0.2); // } else { -// CurrentHeight = Hieght.height.HOLD; +// planeServo.setPosition(0); // } - - //read values to determine if plane should shoot or not - if (shoot >= 0.9) { - planeServo.setPosition(0.2); - } else { - planeServo.setPosition(0); - } - - //make door rise as intake goes on - if (intakeON >= 0.01) { - CurrentDpos = DoorPos.DoorPosition.OPEN; - } else { - CurrentDpos = DoorPos.DoorPosition.CLOSE; - } - - //make intake go to stack 1 when pressed down - if (intakeON >= 0.25) { - Currentpos = Intake.Position.STACK1; - } else { - Currentpos = Intake.Position.UP; - } - - //reset slide position - if (reset >= 0.2) { - CurrentSpos = CurrentSpos.DOWN; - } - -// //control position of hopper -// if (controller2.getLeftBumper().isJustPressed()) { -// hopperpos = HopperPos.hopperPos.UP; -// } else if (controller2.getLeftBumper().isJustReleased()) { -// hopperpos = HopperPos.hopperPos.DOWN; +// +// //make door rise as intake goes on +// if (intakeON >= 0.01) { +// CurrentDpos = DoorPos.DoorPosition.OPEN; +// } else { +// CurrentDpos = DoorPos.DoorPosition.CLOSE; // } - - //shift intake up one pixel - if (controller2.getDLeft().isJustPressed()) { - //prevent from going higher than highest legal value - if (Currentpos != Intake.Position.UP) { - Currentpos = Currentpos.nextPosition(); - } - } - - //shift intake down one pixel - if (controller2.getDRight().isJustPressed()) { - //prevent from going lower than lowest value - if (Currentpos != Intake.Position.STACK1) { - Currentpos = Currentpos.previousPosition(); - } - } - - //set intake to max position - if (controller2.getDUp().isJustPressed()) { - Currentpos = Currentpos.UP; - } - - //set intake to lowest position - if (controller2.getDDown().isJustPressed()) { - Currentpos = Currentpos.STACK1; - } - - //arm safety pause going up - if (controller2.getA().isJustPressed()){ - CurrentApos = CurrentApos.SAFTEYUP; - DownQuestion = DownQuestion.NO; - } - - //arm safety pause going down - if (controller2.getX().isJustPressed()) { - CurrentApos = CurrentApos.SAFTEYDOWN; - DownQuestion = DownQuestion.NO; - } - - //arm all the way up - if (controller2.getB().isJustPressed()) { - CurrentApos = CurrentApos.SCORE; - DownQuestion = DownQuestion.NO; - } - - //arm all the way down - if (controller2.getY().isJustPressed()) { - CurrentApos = CurrentApos.SDOWN; - DownQuestion = DownQuestion.YES; - } - - //set slides to max when left joystick is up - if (sY >= 0.5) { - CurrentSpos = CurrentSpos.MAX; - } - - //set slides to tape 1 level when left joystick is down - if (sY <= -0.5) { - CurrentSpos = CurrentSpos.TAPE1; - } - - //set slides to tape 3 when left joystick is right - if (sX >= 0.2) { - CurrentSpos = CurrentSpos.TAPE3; - } - - //set slides to tape 2 when left joystick is left - if (sX <= -0.2) { - CurrentSpos = CurrentSpos.TAPE2; - } - - if (manualSY >=0.1) { - CurrentSpos = CurrentSpos.MANUAL; - } - -// //set slides all the way down when left bumper gets pressed -// if (controller2.getLeftBumper().isJustPressed()) { +// +// //make intake go to stack 1 when pressed down +// if (intakeON >= 0.25) { +// Currentpos = Intake.Position.STACK1; +// } else { +// Currentpos = Intake.Position.UP; +// } +// +// //reset slide position +// if (reset >= 0.2) { // CurrentSpos = CurrentSpos.DOWN; // } // -// //set intake all teh way up when right bumper is pressed -// if (controller2.getRightBumper().isJustPressed()) { +//// //control position of hopper +//// if (controller2.getLeftBumper().isJustPressed()) { +//// hopperpos = HopperPos.hopperPos.UP; +//// } else if (controller2.getLeftBumper().isJustReleased()) { +//// hopperpos = HopperPos.hopperPos.DOWN; +//// } +// +// //shift intake up one pixel +// if (controller2.getDLeft().isJustPressed()) { +// //prevent from going higher than highest legal value +// if (Currentpos != Intake.Position.UP) { +// Currentpos = Currentpos.nextPosition(); +// } +// } +// +// //shift intake down one pixel +// if (controller2.getDRight().isJustPressed()) { +// //prevent from going lower than lowest value +// if (Currentpos != Intake.Position.STACK1) { +// Currentpos = Currentpos.previousPosition(); +// } +// } +// +// //set intake to max position +// if (controller2.getDUp().isJustPressed()) { // Currentpos = Currentpos.UP; // } - - // update the runtime on the telemetry - telemetry.addData("Runtime", currentTime); - telemetry.update(); - } -} +// +// //set intake to lowest position +// if (controller2.getDDown().isJustPressed()) { +// Currentpos = Currentpos.STACK1; +// } +// +// //arm safety pause going up +// if (controller2.getA().isJustPressed()){ +// CurrentApos = CurrentApos.SAFTEYUP; +// DownQuestion = DownQuestion.NO; +// } +// +// //arm safety pause going down +// if (controller2.getX().isJustPressed()) { +// CurrentApos = CurrentApos.SAFTEYDOWN; +// DownQuestion = DownQuestion.NO; +// } +// +// //arm all the way up +// if (controller2.getB().isJustPressed()) { +// CurrentApos = CurrentApos.SCORE; +// DownQuestion = DownQuestion.NO; +// } +// +// //arm all the way down +// if (controller2.getY().isJustPressed()) { +// CurrentApos = CurrentApos.SDOWN; +// DownQuestion = DownQuestion.YES; +// } +// +// //set slides to max when left joystick is up +// if (sY >= 0.5) { +// CurrentSpos = CurrentSpos.MAX; +// } +// +// //set slides to tape 1 level when left joystick is down +// if (sY <= -0.5) { +// CurrentSpos = CurrentSpos.TAPE1; +// } +// +// //set slides to tape 3 when left joystick is right +// if (sX >= 0.2) { +// CurrentSpos = CurrentSpos.TAPE3; +// } +// +// //set slides to tape 2 when left joystick is left +// if (sX <= -0.2) { +// CurrentSpos = CurrentSpos.TAPE2; +// } +// +// if (manualSY >=0.1) { +// CurrentSpos = CurrentSpos.MANUAL; +// } +// +//// //set slides all the way down when left bumper gets pressed +//// if (controller2.getLeftBumper().isJustPressed()) { +//// CurrentSpos = CurrentSpos.DOWN; +//// } +//// +//// //set intake all teh way up when right bumper is pressed +//// if (controller2.getRightBumper().isJustPressed()) { +//// Currentpos = Currentpos.UP; +//// } +// +// // update the runtime on the telemetry +// telemetry.addData("Runtime", currentTime); +// telemetry.update(); +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/NewTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/NewTeleop.java new file mode 100644 index 0000000..e9b3f04 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/NewTeleop.java @@ -0,0 +1,99 @@ +package org.firstinspires.ftc.teamcode.opmode.teleop; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.teamcode.controller.Controller; +import org.firstinspires.ftc.teamcode.hardware.Intake; +import org.firstinspires.ftc.teamcode.hardware.Robot; +import org.firstinspires.ftc.teamcode.hardware.robby.Slides; + +@Config +@TeleOp(name = "Main TeleOp", group = "OpModes") +public class NewTeleop extends OpMode { + + public static double normal = 0.5; + public static double turbo = 1; + + private Robot robot; + private Controller controller1; + private Controller controller2; + + @Override + public void init() { + controller1 = new Controller(gamepad1); + controller2 = new Controller(gamepad2); + + this.robot = new Robot(hardwareMap); + robot.intake.setpos(Intake.Position.STACK1); + + telemetry.addLine("Started"); + telemetry.update(); + } + + @Override + public void loop() { + // Driver 1 + double x = -gamepad1.left_stick_y; + double y = -gamepad1.left_stick_x; + double z = -gamepad1.right_stick_x; + + if (controller1.getA().isPressed()) { + x *= turbo; + y *= turbo; + z *= turbo; + } else { + x *= normal; + y *= normal; + z *= normal; + } + robot.drive.setWeightedDrivePower(new Pose2d(x, y, z)); + + // Driver 2 + robot.intake.setDcMotor(gamepad2.right_trigger); + if (controller2.getRightBumper().isJustPressed()) { + robot.intake.incrementPos(); + } + if (controller2.getLeftBumper().isJustPressed()) { + robot.intake.decrementPos(); + } + + // macros + switch (robot.runningMacro) { + case(0): // manual mode + robot.slides.increaseTarget(controller2.getLeftStick().getY()); + if (controller2.getX().isJustPressed()) { + robot.runningMacro = 1; + } else if (controller2.getY().isJustPressed()) { + robot.runningMacro = 2; + } else if (controller2.getB().isJustPressed()) { + robot.runningMacro = 3; + } else if (controller2.getA().isJustPressed()) { + robot.runningMacro = 4; + } + break; + case(1): + robot.extendMacro(Slides.Position.TIER1, getRuntime()); + break; + case(2): + robot.extendMacro(Slides.Position.TIER2, getRuntime()); + break; + case(3): + robot.extendMacro(Slides.Position.TIER3, getRuntime()); + break; + case(4): + robot.resetMacro(Slides.Position.DOWN, getRuntime()); + break; + } + + // update and telemetry + robot.update(getRuntime()); + controller1.update(); + controller2.update(); + telemetry.addLine(robot.getTelemetry()); + telemetry.update(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java index 05f0c73..3e384fa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java @@ -43,9 +43,9 @@ public class DriveConstants { * angular distances although most angular parameters are wrapped in Math.toRadians() for * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO. */ - public static double WHEEL_RADIUS = 1.8045; // in + public static double WHEEL_RADIUS = 1.889765; // in public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed - public static double TRACK_WIDTH = 15.75; // in + public static double TRACK_WIDTH = 13.5; // in /* * These are the feedforward parameters used to model the drive motor behavior. If you are using @@ -55,8 +55,8 @@ public class DriveConstants { */ // public static double kV = 1.0 / rpmToVelocity(MAX_RPM); - public static double kV = 0.013; - public static double kA = 0.003; + public static double kV = 0.012; + public static double kA = 0.004; public static double kStatic = 0; /* @@ -68,10 +68,10 @@ public class DriveConstants { */ // old is 35 35 120 120 // new is 70 45 90 90 - public static double MAX_VEL = 35; - public static double MAX_ACCEL = 35; - public static double MAX_ANG_VEL = Math.toRadians(90); - public static double MAX_ANG_ACCEL = Math.toRadians(90); + public static double MAX_VEL = 60; + public static double MAX_ACCEL = 60; + public static double MAX_ANG_VEL = Math.toRadians(120); + public static double MAX_ANG_ACCEL = Math.toRadians(120); public static double encoderTicksToInches(double ticks) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java index 5e107ee..43e5950 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java @@ -39,7 +39,7 @@ import java.util.List; @Config public class SampleMecanumDrive extends MecanumDrive { - public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(10, 0, 0); + public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(8, 0, 0); public static PIDCoefficients HEADING_PID = new PIDCoefficients(8, 0, 0); public static double LATERAL_MULTIPLIER = 1; @@ -65,7 +65,7 @@ public class SampleMecanumDrive extends MecanumDrive { super(DriveConstants.kV, DriveConstants.kA, DriveConstants.kStatic, DriveConstants.TRACK_WIDTH, DriveConstants.TRACK_WIDTH, LATERAL_MULTIPLIER); follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID, - new Pose2d(0.5, 0.5, Math.toRadians(5)), 0.1); + new Pose2d(0.5, 0.5, Math.toRadians(5)), 0.5); LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap); @@ -136,7 +136,7 @@ public class SampleMecanumDrive extends MecanumDrive { // TODO: if desired, use setLocalizer() to change the localization method // for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...)); -// setLocalizer(new TwoWheelTrackingLocalizer(hardwareMap, this)); + setLocalizer(new TwoWheelTrackingLocalizer(hardwareMap, this)); // setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap)); trajectorySequenceRunner = new TrajectorySequenceRunner(follower, HEADING_PID); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/TwoWheelTrackingLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/TwoWheelTrackingLocalizer.java index 5faa500..d519761 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/TwoWheelTrackingLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/TwoWheelTrackingLocalizer.java @@ -33,18 +33,18 @@ import java.util.List; * */ public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer { - public static double TICKS_PER_REV = 8192; - public static double WHEEL_RADIUS = 0.6889764; // in + public static double TICKS_PER_REV = 2000; + public static double WHEEL_RADIUS = 0.944882; // in public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed - public static double PARALLEL_X = -4.48818897638; // X is the up and down direction - public static double PARALLEL_Y = 3.10679133858; // Y is the strafe direction + public static double PARALLEL_X = -1.5; // X is the up and down direction + public static double PARALLEL_Y = 7.25; // Y is the strafe direction - public static double PERPENDICULAR_X = 2.21801181102; + public static double PERPENDICULAR_X = -6.1; public static double PERPENDICULAR_Y = 0; - public static double MULTIPLIER_X = 1.4; - public static double MULTIPLIER_Y = 1.4; + public static double MULTIPLIER_X = 1.0; + public static double MULTIPLIER_Y = 1.0; // Parallel/Perpendicular to the forward axis // Parallel wheel is parallel to the forward axis @@ -61,12 +61,12 @@ public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer { this.drive = drive; -// parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "parallel")); + parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "backLeft")); // parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "backRight")); -// perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "perpendicular")); + perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "Intake Motor")); // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) -// perpendicularEncoder.setDirection(Encoder.Direction.REVERSE); + perpendicularEncoder.setDirection(Encoder.Direction.REVERSE); // parallelEncoder.setDirection(Encoder.Direction.REVERSE); }