From 1c13880159010823a24b47545ab883db63908095 Mon Sep 17 00:00:00 2001 From: "A. Teo Welton" <76081718+DragonDev07@users.noreply.github.com> Date: Sun, 25 Feb 2024 23:01:39 -0700 Subject: [PATCH] AutonomousV1 & AutonomousV2 (#101) * [fix] motor coefficients * [av1] autonv1 basics * code * Complete Auton Rewrite & Simplification * Manual Starting State Selection & TestingMode * Trajectory fixes * bad solution, but hopefully fixes * boo * im sorry * [roadrunner] DriveConstants update * Updates to roadrunner tuning * auton fixes, hopefully * Fix Auton Trajectories * Add Moving after Yellow Pixel Delivery * FTC Dashboard Telemetry * Blue Side Trajectories * Telemetry Fix * Slow trajectory fixes * things * Merge Master * Auton mostly works, we are conisdering a different method of detecting * AutonV1 Rewrite + AutonV2 Red Only * AutonV1 Edits + AutonV2 Massive Changes * Fix blue side trajectories for autov1 * Intake Detection While Driving + New Experimental Sweeping Idea * Pixel Stack -> Backboard Trajectory WORKS * woops * HWC Changes * things * Comp Changes! --------- Co-authored-by: Milo Banks --- TeamCode/build.gradle | 4 +- .../ftc/teamcode/auton/AutonomousV1.java | 391 ++++++++++++ .../ftc/teamcode/auton/AutonomousV2.java | 575 ++++++++++++++++++ .../ftc/teamcode/auton/SimpleParkAuton.java | 8 +- .../ftc/teamcode/auton/SlidesUpDown.java | 25 - .../ftc/teamcode/auton/VisionTest.java | 47 -- .../teamcode/auton/enums/AllianceColor.java | 6 + .../ftc/teamcode/subsystems/HWC.java | 157 ++--- .../subsystems/pid/MotorPIDTuning.java | 2 +- .../roadrunner/drive/DriveConstants.java | 30 +- .../roadrunner/drive/SampleMecanumDrive.java | 10 +- .../drive/StandardTrackingWheelLocalizer.java | 33 +- .../ftc/teamcode/teleop/ColorTest.java | 2 +- .../teamcode/teleop/SingleDriverTeleOp.java | 2 +- 14 files changed, 1106 insertions(+), 186 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/AutonomousV1.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/AutonomousV2.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/SlidesUpDown.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/VisionTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/enums/AllianceColor.java diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 01b9de2..66f2c25 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -65,10 +65,10 @@ dependencies { umlDoclet "nl.talsmasoftware:umldoclet:2.1.0" implementation 'com.acmerobotics.roadrunner:core:0.5.5' - implementation 'com.acmerobotics.dashboard:dashboard:0.4.12' + implementation 'com.acmerobotics.dashboard:dashboard:0.4.15' implementation 'org.openftc:easyopencv:1.7.0' implementation 'org.openftc:apriltag:2.0.0' implementation 'org.apache.commons:commons-math3:3.6.1' implementation 'org.ftclib.ftclib:core:2.0.1' implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7' -} +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/AutonomousV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/AutonomousV1.java new file mode 100644 index 0000000..629bce5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/AutonomousV1.java @@ -0,0 +1,391 @@ +package org.firstinspires.ftc.teamcode.auton; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +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.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.auton.enums.AllianceColor; +import org.firstinspires.ftc.teamcode.subsystems.HWC; +import org.firstinspires.ftc.teamcode.subsystems.roadrunner.trajectorysequence.TrajectorySequence; + +@Autonomous(name = "AutonomousV1") +public class AutonomousV1 extends OpMode { + // ------ State Enum ------ // + private enum State { + DRIVING_TO_DEPOSIT_PURPLE_PIXEL, DEPOSITING_PURPLE_PIXEL, DRIVING_TO_BACKBOARD, DEPOSITING_YELLOW_PIXEL, PARK, STOP + } + + // ------ Declare Others ------ // + private HWC robot; + private State state = State.DRIVING_TO_DEPOSIT_PURPLE_PIXEL; + private AllianceColor allianceColor = AllianceColor.RED; + private HWC.Location elementLocation; + private String activeTrajectory = ""; + private boolean firstRun = true; + + // ------ Trajectories ------ // + private Trajectory toDepositCenter; + private Trajectory toDepositRight; + private Trajectory toDepositLeft; + private Trajectory toBackboardFromCenter; + private Trajectory toBackboardFromRight; + private Trajectory toBackboardFromLeft; + private Trajectory toParkFromBackboardLeft; + private Trajectory toParkFromBackboardRight; + private Trajectory toParkFromBackboardCenter; + + // ------ Starting Positions ------ // + private final Pose2d START_POSE_RED = new Pose2d(12, -60, Math.toRadians(90.00)); + private final Pose2d START_POSE_BLUE = new Pose2d(12, 60, Math.toRadians(270.00)); + + @Override + public void init() { + // ------ Telemetry ------ // + telemetry.addData("Status", "Initializing"); + telemetry.update(); + + // ------ Initialize Robot Hardware ------ // + robot = new HWC(hardwareMap, telemetry, true); + + // ------ Start FTC Dashboard Telemetry ------ // + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + // ------ Start Tensorflow ------ // + robot.initTFOD("fpaVision.tflite"); + + // ------ Reset Servos ------ // + robot.clawL.setPosition(1); + robot.clawR.setPosition(0); + + // ------ Close Claw for Yellow Pixel ------ // + robot.toggleClaw('L'); + + // ------ Telemetry ------ // + telemetry.addData("Status", "Initialized"); + telemetry.addData(">", "Yellow Pixel on Left Side"); + telemetry.addData(">", "Purple Pixel in intake"); + telemetry.update(); + } + + @Override + public void init_loop() { + // ------ GamePad Updates ------ // + robot.previousGamepad1.copy(robot.currentGamepad1); + robot.previousGamepad2.copy(robot.currentGamepad2); + robot.currentGamepad1.copy(gamepad1); + robot.currentGamepad2.copy(gamepad2); + + // ------ Save Element Position ------ // + elementLocation = robot.detectElement(allianceColor); + + // ------ Alliance Color Selection ------ // + if (robot.currentGamepad1.a && !robot.previousGamepad1.a) { + allianceColor = allianceColor.equals(AllianceColor.RED) ? AllianceColor.BLUE : AllianceColor.RED; + } + + + // ------ Set Trajectories based on Alliance Color ------ // + switch(allianceColor) { + case RED: + // ------ Set Robot Start Pose ------ // + robot.drive.setPoseEstimate(START_POSE_RED); + + // ------ Declare Trajectories ------ // + // Drive to Center Line + toDepositCenter = robot.drive.trajectoryBuilder(START_POSE_RED) + .lineTo(new Vector2d(12.0, -34)) + .build(); + + // Drive to Right Line + toDepositRight = robot.drive.trajectoryBuilder(START_POSE_RED) + .strafeTo(new Vector2d(22, -43)) + .build(); + + // Drive to Left Line + toDepositLeft = robot.drive.trajectoryBuilder(START_POSE_RED) + .lineToLinearHeading(new Pose2d(9, -40, Math.toRadians(135))) + .build(); + + // Drive to Backboard from Center + toBackboardFromCenter = robot.drive.trajectoryBuilder(toDepositCenter.end()) + .lineToLinearHeading(new Pose2d(49, -35, Math.toRadians(180))) + .build(); + + // Drive to Backboard from Right + toBackboardFromRight = robot.drive.trajectoryBuilder(toDepositRight.end()) + .lineToLinearHeading(new Pose2d(49, -37, Math.toRadians(180))) + .build(); + + // Drive to Backboard from Left + toBackboardFromLeft = robot.drive.trajectoryBuilder(toDepositLeft.end()) + .lineToLinearHeading(new Pose2d(49, -30, Math.toRadians(180))) + .build(); + + // Park from Backboard Center + toParkFromBackboardCenter = robot.drive.trajectoryBuilder(toBackboardFromCenter.end()) + .strafeTo(new Vector2d(48, -60)) + .build(); + + // Park from Backboard Right + toParkFromBackboardRight = robot.drive.trajectoryBuilder(toBackboardFromRight.end()) + .strafeTo(new Vector2d(48, -60)) + .build(); + + // Drive to Park from Backboard 2 + toParkFromBackboardLeft = robot.drive.trajectoryBuilder(toBackboardFromLeft.end()) + .strafeTo(new Vector2d(48, -60)) + .build(); + + break; + case BLUE: + // ------ Set Robot Start Pose ------ // + robot.drive.setPoseEstimate(START_POSE_BLUE); + + // ------ Declare Trajectories ------ // + // Drive to Center Line + toDepositCenter = robot.drive.trajectoryBuilder(START_POSE_BLUE) + .lineTo(new Vector2d(12.0, 34)) + .build(); + + // Drive to Right Line + toDepositLeft = robot.drive.trajectoryBuilder(START_POSE_BLUE) + .strafeTo(new Vector2d(23, 45)) + .build(); + + // Drive to Left Line + toDepositRight = robot.drive.trajectoryBuilder(START_POSE_BLUE) + .splineToLinearHeading(new Pose2d(7, 39, Math.toRadians(225)), Math.toRadians(225)) + .build(); + + // Drive to Backboard from Center + toBackboardFromCenter = robot.drive.trajectoryBuilder(toDepositCenter.end()) + .lineToLinearHeading(new Pose2d(49, 35, Math.toRadians(180))) + .build(); + + // Drive to Backboard from Right + toBackboardFromLeft = robot.drive.trajectoryBuilder(toDepositLeft.end()) + .lineToLinearHeading(new Pose2d(49, 41, Math.toRadians(180))) + .build(); + + // Drive to Backboard from Left + toBackboardFromRight = robot.drive.trajectoryBuilder(toDepositRight.end()) + .lineToLinearHeading(new Pose2d(49, 30, Math.toRadians(180))) + .build(); + + // Park from Backboard Center + toParkFromBackboardCenter = robot.drive.trajectoryBuilder(toBackboardFromCenter.end()) + .strafeTo(new Vector2d(48, 60)) + .build(); + + // Park from Backboard Right + toParkFromBackboardRight = robot.drive.trajectoryBuilder(toBackboardFromRight.end()) + .strafeTo(new Vector2d(48, 60)) + .build(); + + // Drive to Park from Backboard 2 + toParkFromBackboardLeft = robot.drive.trajectoryBuilder(toBackboardFromLeft.end()) + .strafeTo(new Vector2d(48, 60)) + .build(); + break; + } + + // ------ Telemetry ------ // + telemetry.addData(">", "Yellow Pixel on Left Side"); + telemetry.addData(">", "Purple Pixel in intake"); + telemetry.addData("Element Location", elementLocation); + telemetry.addData("Selected Alliance Color", allianceColor); + telemetry.addData("Status", "Init Loop"); + telemetry.update(); + } + + @Override + public void loop() { + // ------ GamePad Updates ------ // + robot.previousGamepad1.copy(robot.currentGamepad1); + robot.previousGamepad2.copy(robot.currentGamepad2); + robot.currentGamepad1.copy(gamepad1); + robot.currentGamepad2.copy(gamepad2); + + // ------ State Machine ------ // + switch (state) { + case DRIVING_TO_DEPOSIT_PURPLE_PIXEL: + drivingToDepositPurplePixel(); + break; + case DEPOSITING_PURPLE_PIXEL: + depositingPurplePixel(); + break; + case DRIVING_TO_BACKBOARD: + drivingToBackboard(); + break; + case DEPOSITING_YELLOW_PIXEL: + depositingYellowPixel(); + break; + case PARK: + drivingToPark(); + case STOP: + stop(); + break; + } + + // ------ Move Slides Using PID ------ // + robot.pulleyLComponent.moveUsingPID(); + robot.pulleyRComponent.moveUsingPID(); + + // ------ Move Robot ------ // + robot.drive.update(); + + // ------ Telemetry ------ // + telemetry.addData("Status", state); + telemetry.addData("Element Location", elementLocation); + telemetry.addData("Pose", robot.drive.getPoseEstimate()); + telemetry.addData("Active Trajectory", activeTrajectory); + telemetry.addLine(); + telemetry.addData("Hardware", robot); + telemetry.update(); + } + + // ------ State Methods ------ // + private void drivingToDepositPurplePixel() { + // ------ Select Trajectory ------ // + if(firstRun) { + firstRun = false; + if (elementLocation == HWC.Location.CENTER) { + activeTrajectory = "toDepositCenter"; + robot.drive.followTrajectoryAsync(toDepositCenter); + } else if (elementLocation == HWC.Location.RIGHT) { + activeTrajectory = "toDepositRight"; + robot.drive.followTrajectoryAsync(toDepositRight); + } else { + activeTrajectory = "toDepositLeft"; + robot.drive.followTrajectoryAsync(toDepositLeft); + } + } + + // ------ Set Next State ------ // + if (!robot.drive.isBusy()) { + state = State.DEPOSITING_PURPLE_PIXEL; + firstRun = true; + } + } + + // Deposit Purple Pixel + private void depositingPurplePixel() { + // ------ Run Intake Motor Backwards for 1.5 Seconds ------ // + robot.intakeMotor.setPower(0.8); + robot.elapsedTimeSleep(1500); + robot.intakeMotor.setPower(0); + + // ------ Set Next State ------ // + state = State.DRIVING_TO_BACKBOARD; + } + + // Drive to Backboard + private void drivingToBackboard() { + // ------ Select Trajectory ------ // + if(firstRun) { + firstRun = false; + if (elementLocation == HWC.Location.CENTER) { + activeTrajectory = "toBackboardFromInitial"; + robot.drive.followTrajectoryAsync(toBackboardFromCenter); + } else if (elementLocation == HWC.Location.RIGHT) { + activeTrajectory = "toBackboardFromSecond"; + robot.drive.followTrajectoryAsync(toBackboardFromRight); + } else { + activeTrajectory = "toBackBoardFromLeft"; + robot.drive.followTrajectoryAsync(toBackboardFromLeft); + } + } + + // ------ Set Next State ------ // + if (!robot.drive.isBusy()) { + state = State.DEPOSITING_YELLOW_PIXEL; + firstRun = true; + } + } + + // Deposit Yellow Pixel + private void depositingYellowPixel() { + // ------ Move Slides, Passover & Wrist ------ // + deliver(); + + // ------ Wait for Passover to Move ------ // + robot.elapsedTimeSleep(1000); + + // ------ Open Claw ------ // + robot.toggleClaw('L'); + + // ------ Wait for Claw to Open ------ // + robot.elapsedTimeSleep(1000); + + // ------ Move Slides, Passover & Wrist ------ // + intake(); + + // ------ Set Next State ------ // + state = State.PARK; + } + + // Drive to Park + private void drivingToPark() { + // ------ Select Trajectory ------ // + if(firstRun) { + firstRun = false; + activeTrajectory = "toParkFromBackboard2"; + if (elementLocation == HWC.Location.CENTER) { + activeTrajectory = "toParkFromBackboardCenter"; + robot.drive.followTrajectoryAsync(toParkFromBackboardCenter); + } else if (elementLocation == HWC.Location.RIGHT) { + activeTrajectory = "toParkFromBackboardRight"; + robot.drive.followTrajectoryAsync(toParkFromBackboardRight); + } else { + activeTrajectory = "toParkFromBackboardLeft"; + robot.drive.followTrajectoryAsync(toParkFromBackboardLeft); + } + } + + // ------ Set Next State ------ // + if (!robot.drive.isBusy()) { + state = State.STOP; + firstRun = true; + } + } + + // Method to move to Delivery Position + private void deliver() { + robot.passoverArmLeft.setPosition(HWC.passoverDeliveryPos); + robot.passoverArmRight.setPosition(HWC.passoverDeliveryPos); + robot.wrist.setPosition(HWC.wristDeliveryPos); + robot.pulleyLComponent.setTarget(HWC.slidePositions[1] / 4.0); + robot.pulleyRComponent.setTarget(HWC.slidePositions[1] / 4.0); + } + + // Method to move to Intake Position + private void intake() { + robot.passoverArmLeft.setPosition(HWC.passoverIntakePos); + robot.passoverArmRight.setPosition(HWC.passoverIntakePos); + robot.wrist.setPosition(HWC.wristIntakePos); + robot.pulleyLComponent.setTarget(HWC.slidePositions[0]); + robot.pulleyRComponent.setTarget(HWC.slidePositions[0]); + } + + // Method to Check Claws & Close + private void checkClaws() { + // Close Claws when Pixel Detected + if (robot.colorLeft.getDistance(DistanceUnit.CM) <= 2) { + robot.clawL.setPosition(0.5); + } + + if (robot.colorRight.getDistance(DistanceUnit.CM) <= 2) { + robot.clawR.setPosition(0.5); + } + + // If both Pixels are Detected, Stop Intake + if (robot.colorLeft.getDistance(DistanceUnit.CM) <= 1 && robot.colorRight.getDistance(DistanceUnit.CM) <= 1) { + robot.intakeMotor.setPower(0); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/AutonomousV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/AutonomousV2.java new file mode 100644 index 0000000..213f6b0 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/AutonomousV2.java @@ -0,0 +1,575 @@ +package org.firstinspires.ftc.teamcode.auton; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +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.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.auton.enums.AllianceColor; +import org.firstinspires.ftc.teamcode.subsystems.HWC; +import org.firstinspires.ftc.teamcode.subsystems.roadrunner.trajectorysequence.TrajectorySequence; + +@Autonomous(name = "AutonomousV2") +public class AutonomousV2 extends OpMode { + // ------ State Enum ------ // + private enum State { + DRIVING_TO_DEPOSIT_PURPLE_PIXEL, DEPOSITING_PURPLE_PIXEL, DRIVING_TO_BACKBOARD, DEPOSITING_YELLOW_PIXEL, DRIVING_TO_PIXEL_STACK, KNOCKING_PIXEL_STACK, INTAKING_PIXELS, DRIVING_TO_BACKBOARD_2, DELIVERING_BACKBOARD, PARK, STOP + } + + // ------ Declare Others ------ // + private HWC robot; + private State state = State.DRIVING_TO_DEPOSIT_PURPLE_PIXEL; + private AllianceColor allianceColor = AllianceColor.RED; + private HWC.Location elementLocation; + private String activeTrajectory = ""; + private boolean firstRun = true; + + // ------ Trajectories ------ // + private Trajectory toDepositCenter; + private Trajectory toDepositRight; + private Trajectory toDepositLeft; + private Trajectory toBackboardFromCenter; + private Trajectory toBackboardFromRight; + private Trajectory toBackboardFromLeft; + private Trajectory toPixelStackFromCenter; + private Trajectory toPixelStackFromRight; + private Trajectory toPixelStackFromLeft; + private Trajectory knockingPixelStack; + private Trajectory intakingPixels; + private Trajectory intakingPixelsSweep; + private TrajectorySequence toBackboardFromPixelStack; + private Trajectory toParkFromBackboard2; + + // ------ Starting Positions ------ // + private final Pose2d START_POSE_RED = new Pose2d(12, -60, Math.toRadians(90.00)); + private final Pose2d START_POSE_BLUE = new Pose2d(12, 60, Math.toRadians(270.00)); + + @Override + public void init() { + // ------ Telemetry ------ // + telemetry.addData("Status", "Initializing"); + telemetry.update(); + + // ------ Initialize Robot Hardware ------ // + robot = new HWC(hardwareMap, telemetry, true); + + // ------ Start FTC Dashboard Telemetry ------ // + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + // ------ Start Tensorflow ------ // + robot.initTFOD("fpaVision.tflite"); + + // ------ Reset Servos ------ // + robot.clawL.setPosition(1); + robot.clawR.setPosition(0); + + // ------ Close Claw for Yellow Pixel ------ // + robot.toggleClaw('L'); + + // ------ Telemetry ------ // + telemetry.addData("Status", "Initialized"); + telemetry.addData(">", "Yellow Pixel on Left Side"); + telemetry.addData(">", "Purple Pixel in intake"); + telemetry.update(); + } + + @Override + public void init_loop() { + // ------ GamePad Updates ------ // + robot.previousGamepad1.copy(robot.currentGamepad1); + robot.previousGamepad2.copy(robot.currentGamepad2); + robot.currentGamepad1.copy(gamepad1); + robot.currentGamepad2.copy(gamepad2); + + // ------ Save Element Position ------ // + elementLocation = robot.detectElement(allianceColor); + + // ------ Alliance Color Selection ------ // + if (robot.currentGamepad1.a && !robot.previousGamepad1.a) { + allianceColor = allianceColor.equals(AllianceColor.RED) ? AllianceColor.BLUE : AllianceColor.RED; + } + + + // ------ Set Trajectories based on Alliance Color ------ // + switch(allianceColor) { + case RED: + // ------ Set Robot Start Pose ------ // + robot.drive.setPoseEstimate(START_POSE_RED); + + // ------ Declare Trajectories ------ // + // Drive to Center Line + toDepositCenter = robot.drive.trajectoryBuilder(START_POSE_RED) + .lineTo(new Vector2d(12.0, -34)) + .build(); + + // Drive to Right Line + toDepositRight = robot.drive.trajectoryBuilder(START_POSE_RED) + .strafeTo(new Vector2d(22, -43)) + .build(); + + // Drive to Left Line + toDepositLeft = robot.drive.trajectoryBuilder(START_POSE_RED) + .splineToLinearHeading(new Pose2d(7, -39, Math.toRadians(135)), Math.toRadians(135)) + .build(); + + // Drive to Backboard from Center + toBackboardFromCenter = robot.drive.trajectoryBuilder(toDepositCenter.end()) + .lineToLinearHeading(new Pose2d(49, -35, Math.toRadians(180))) + .build(); + + // Drive to Backboard from Right + toBackboardFromRight = robot.drive.trajectoryBuilder(toDepositRight.end()) + .lineToLinearHeading(new Pose2d(49, -37, Math.toRadians(180))) + .build(); + + // Drive to Backboard from Left + toBackboardFromLeft = robot.drive.trajectoryBuilder(toDepositLeft.end()) + .lineToLinearHeading(new Pose2d(49, -30, Math.toRadians(180))) + .build(); + + // Drive to Pixel Stack from Center Backboard + toPixelStackFromCenter = robot.drive.trajectoryBuilder(toBackboardFromCenter.end()) + .splineTo(new Vector2d(7, -9), Math.toRadians(180)) + .splineTo(new Vector2d(-60, -24), Math.toRadians(180)) + .build(); + + // Drive to Pixel Stack from Right Backboard + toPixelStackFromRight = robot.drive.trajectoryBuilder(toBackboardFromRight.end()) + .splineTo(new Vector2d(7, -9), Math.toRadians(180)) + .splineTo(new Vector2d(-60, -24), Math.toRadians(180)) + .build(); + + // Drive to Pixel Stack from Left Backboard + toPixelStackFromLeft = robot.drive.trajectoryBuilder(toBackboardFromLeft.end()) + .splineTo(new Vector2d(7, -9), Math.toRadians(180)) + .splineTo(new Vector2d(-60, -24), Math.toRadians(180)) + .build(); + + // Knock Pixel Stack + knockingPixelStack = robot.drive.trajectoryBuilder(toPixelStackFromCenter.end()) + .splineToLinearHeading(new Pose2d(-60, -25, Math.toRadians(270)), Math.toRadians(270)) + .build(); + + // Intake Pixels + intakingPixels = robot.drive.trajectoryBuilder(knockingPixelStack.end()) + .splineToLinearHeading(new Pose2d(-60, -24, Math.toRadians(180)), Math.toRadians(180)) + .addDisplacementMarker(() -> robot.drive.followTrajectoryAsync(intakingPixelsSweep)) + .build(); + + // Intaking Pixels Sweep + intakingPixelsSweep = robot.drive.trajectoryBuilder(intakingPixels.end()) + .splineToLinearHeading(new Pose2d(-60, -25, Math.toRadians(270)), Math.toRadians(270)) + .build(); + + // Drive to Backboard 2 from Pixel Stack + toBackboardFromPixelStack = robot.drive.trajectorySequenceBuilder(knockingPixelStack.end()) + .setReversed(true) + .splineTo(new Vector2d(7, -9), Math.toRadians(0)) + .splineTo(new Vector2d(48, -35), Math.toRadians(0)) + .build(); + + // Drive to Park from Backboard 2 + toParkFromBackboard2 = robot.drive.trajectoryBuilder(toBackboardFromPixelStack.end()) + .strafeTo(new Vector2d(48, -60)) + .build(); + + break; + case BLUE: + // ------ Set Robot Start Pose ------ // + robot.drive.setPoseEstimate(START_POSE_BLUE); + + // ------ Declare Trajectories ------ // + // Drive to Center Line + toDepositCenter = robot.drive.trajectoryBuilder(START_POSE_BLUE) + .lineTo(new Vector2d(12.0, 34)) + .build(); + + // Drive to Right Line + toDepositLeft = robot.drive.trajectoryBuilder(START_POSE_BLUE) + .strafeTo(new Vector2d(23, 45)) + .build(); + + // Drive to Left Line + toDepositRight = robot.drive.trajectoryBuilder(START_POSE_BLUE) + .splineToLinearHeading(new Pose2d(7, -39, Math.toRadians(135)), Math.toRadians(135)) + .build(); + + // Drive to Backboard from Center + toBackboardFromCenter = robot.drive.trajectoryBuilder(toDepositCenter.end()) + .lineToLinearHeading(new Pose2d(49, 35, Math.toRadians(180))) + .build(); + + // Drive to Backboard from Right + toBackboardFromLeft = robot.drive.trajectoryBuilder(toDepositLeft.end()) + .lineToLinearHeading(new Pose2d(49, 41, Math.toRadians(180))) + .build(); + + // Drive to Backboard from Left + toBackboardFromRight = robot.drive.trajectoryBuilder(toDepositRight.end()) + .lineToLinearHeading(new Pose2d(49, 30, Math.toRadians(180))) + .build(); + + // Drive to Backboard from Left + toBackboardFromRight = robot.drive.trajectoryBuilder(toDepositRight.end()) + .lineToLinearHeading(new Pose2d(47, 30, Math.toRadians(180))) + .build(); + + // Drive to Pixel Stack from Center Backboard + toPixelStackFromCenter = robot.drive.trajectoryBuilder(toBackboardFromCenter.end()) + .splineTo(new Vector2d(7, 9), Math.toRadians(180)) + .splineTo(new Vector2d(-60, 24), Math.toRadians(180)) + .build(); + + // Drive to Pixel Stack from Right Backboard + toPixelStackFromRight = robot.drive.trajectoryBuilder(toBackboardFromRight.end()) + .splineTo(new Vector2d(7, 9), Math.toRadians(180)) + .splineTo(new Vector2d(-60, 24), Math.toRadians(180)) + .build(); + + // Drive to Pixel Stack from Left Backboard + toPixelStackFromLeft = robot.drive.trajectoryBuilder(toBackboardFromLeft.end()) + .splineTo(new Vector2d(7, 9), Math.toRadians(180)) + .splineTo(new Vector2d(-60, 24), Math.toRadians(180)) + .build(); + + // Knock Pixel Stack + knockingPixelStack = robot.drive.trajectoryBuilder(toPixelStackFromCenter.end()) + .splineToLinearHeading(new Pose2d(-60, 25, Math.toRadians(270)), Math.toRadians(270)) + .build(); + + // Intake Pixels + intakingPixels = robot.drive.trajectoryBuilder(knockingPixelStack.end()) + .splineToLinearHeading(new Pose2d(-60, 24, Math.toRadians(180)), Math.toRadians(180)) + .addDisplacementMarker(() -> robot.drive.followTrajectoryAsync(intakingPixelsSweep)) + .build(); + + // Intaking Pixels Sweep + intakingPixelsSweep = robot.drive.trajectoryBuilder(intakingPixels.end()) + .splineToLinearHeading(new Pose2d(-60, 25, Math.toRadians(270)), Math.toRadians(270)) + .build(); + + // Drive to Backboard 2 from Pixel Stack + toBackboardFromPixelStack = robot.drive.trajectorySequenceBuilder(knockingPixelStack.end()) + .setReversed(true) + .splineTo(new Vector2d(7, 9), Math.toRadians(0)) + .splineTo(new Vector2d(48, 35), Math.toRadians(0)) + .build(); + + // Drive to Park from Backboard 2 + toParkFromBackboard2 = robot.drive.trajectoryBuilder(toBackboardFromPixelStack.end()) + .strafeTo(new Vector2d(48, 60)) + .build(); + break; + } + + // ------ Telemetry ------ // + telemetry.addData(">", "Yellow Pixel on Left Side"); + telemetry.addData(">", "Purple Pixel in intake"); + telemetry.addData("Element Location", elementLocation); + telemetry.addData("Selected Alliance Color", allianceColor); + telemetry.addData("Status", "Init Loop"); + telemetry.update(); + } + + @Override + public void loop() { + // ------ GamePad Updates ------ // + robot.previousGamepad1.copy(robot.currentGamepad1); + robot.previousGamepad2.copy(robot.currentGamepad2); + robot.currentGamepad1.copy(gamepad1); + robot.currentGamepad2.copy(gamepad2); + + // ------ State Machine ------ // + switch (state) { + case DRIVING_TO_DEPOSIT_PURPLE_PIXEL: + drivingToDepositPurplePixel(); + break; + case DEPOSITING_PURPLE_PIXEL: + depositingPurplePixel(); + break; + case DRIVING_TO_BACKBOARD: + drivingToBackboard(); + break; + case DEPOSITING_YELLOW_PIXEL: + depositingYellowPixel(); + break; + case DRIVING_TO_PIXEL_STACK: + drivingToPixelStack(); + break; + case KNOCKING_PIXEL_STACK: + knockingPixelStack(); + break; + case INTAKING_PIXELS: + intakingPixels(); + break; + case DRIVING_TO_BACKBOARD_2: + drivingToBackboard2(); + break; + case DELIVERING_BACKBOARD: + deliveringBackboard(); + break; + case PARK: + drivingToPark(); + case STOP: + stop(); + break; + } + + // ------ Move Slides Using PID ------ // + robot.pulleyLComponent.moveUsingPID(); + robot.pulleyRComponent.moveUsingPID(); + + // ------ Move Robot ------ // + robot.drive.update(); + + // ------ Telemetry ------ // + telemetry.addData("Status", state); + telemetry.addData("Element Location", elementLocation); + telemetry.addData("Pose", robot.drive.getPoseEstimate()); + telemetry.addData("Active Trajectory", activeTrajectory); + telemetry.addLine(); + telemetry.addData("Hardware", robot); + telemetry.update(); + } + + // ------ State Methods ------ // + private void drivingToDepositPurplePixel() { + // ------ Select Trajectory ------ // + if(firstRun) { + firstRun = false; + if (elementLocation == HWC.Location.CENTER) { + activeTrajectory = "toDepositCenter"; + robot.drive.followTrajectoryAsync(toDepositCenter); + } else if (elementLocation == HWC.Location.RIGHT) { + activeTrajectory = "toDepositRight"; + robot.drive.followTrajectoryAsync(toDepositRight); + } else { + activeTrajectory = "toDepositLeft"; + robot.drive.followTrajectoryAsync(toDepositLeft); + } + } + + // ------ Set Next State ------ // + if (!robot.drive.isBusy()) { + state = State.DEPOSITING_PURPLE_PIXEL; + firstRun = true; + } + } + + // Deposit Purple Pixel + private void depositingPurplePixel() { + // ------ Run Intake Motor Backwards for 1.5 Seconds ------ // + robot.intakeMotor.setPower(0.8); + robot.elapsedTimeSleep(1500); + robot.intakeMotor.setPower(0); + + // ------ Set Next State ------ // + state = State.DRIVING_TO_BACKBOARD; + } + + // Drive to Backboard + private void drivingToBackboard() { + // ------ Select Trajectory ------ // + if(firstRun) { + firstRun = false; + if (elementLocation == HWC.Location.CENTER) { + activeTrajectory = "toBackboardFromInitial"; + robot.drive.followTrajectoryAsync(toBackboardFromCenter); + } else if (elementLocation == HWC.Location.RIGHT) { + activeTrajectory = "toBackboardFromSecond"; + robot.drive.followTrajectoryAsync(toBackboardFromRight); + } else { + activeTrajectory = "toBackBoardFromLeft"; + robot.drive.followTrajectoryAsync(toBackboardFromLeft); + } + } + + // ------ Set Next State ------ // + if (!robot.drive.isBusy()) { + state = State.DEPOSITING_YELLOW_PIXEL; + firstRun = true; + } + } + + // Deposit Yellow Pixel + private void depositingYellowPixel() { + // ------ Move Slides, Passover & Wrist ------ // + deliver(); + + // ------ Wait for Passover to Move ------ // + robot.elapsedTimeSleep(1000); + + // ------ Open Claw ------ // + robot.toggleClaw('L'); + + // ------ Wait for Claw to Open ------ // + robot.elapsedTimeSleep(1000); + + // ------ Move Slides, Passover & Wrist ------ // + intake(); + + // ------ Set Next State ------ // + state = State.DRIVING_TO_PIXEL_STACK; + } + + // Drive to Pixel Stack + private void drivingToPixelStack() { + // ------ Select Trajectory ------ // + if (firstRun) { + firstRun = false; + if (elementLocation == HWC.Location.RIGHT) { + activeTrajectory = "toPixelStackFromRight"; + robot.drive.followTrajectoryAsync(toPixelStackFromRight); + } else if (elementLocation == HWC.Location.LEFT) { + activeTrajectory = "toPixelStackFromLeft"; + robot.drive.followTrajectoryAsync(toPixelStackFromLeft); + } else { + activeTrajectory = "toPixelStackFromCenter"; + robot.drive.followTrajectoryAsync(toPixelStackFromCenter); + } + } + + // ------ Set Next State ------ // + if (!robot.drive.isBusy()) { + state = State.KNOCKING_PIXEL_STACK; + firstRun = true; + } + } + + // Knock Pixel Stack + private void knockingPixelStack() { + // ------ Start Trajectory ------ // + if (firstRun) { + firstRun = false; + activeTrajectory = "knockingPixelStack"; + robot.drive.followTrajectoryAsync(knockingPixelStack); + } + + // ------ Set Next State ------ // + if (!robot.drive.isBusy()) { + state = State.INTAKING_PIXELS; + firstRun = true; + } + } + + // Intake Pixels + private void intakingPixels() { + // ----- Start Trajectory ------ // + if (firstRun) { + firstRun = false; + robot.time.reset(); + activeTrajectory = "intakingPixels"; + robot.drive.followTrajectoryAsync(intakingPixels); + } + + // ------ Run Intake Motor Forward------ // + robot.intakeMotor.setPower(-1); + + // ------ Close Claw on Color Sensor ------ // + checkClaws(); + + // ------ Set Next State ------ // + if(!robot.drive.isBusy() && robot.time.seconds() >= 5) { + state = State.DRIVING_TO_BACKBOARD_2; + firstRun = true; + } + } + + // Drive to Backboard 2 + private void drivingToBackboard2() { + // ------ Start Trajectory ------ // + if (firstRun) { + firstRun = false; + activeTrajectory = "toBackboard2FromPixelStack"; + robot.drive.followTrajectorySequenceAsync(toBackboardFromPixelStack); + } + + // ------ Check Claws While Driving ------ // + checkClaws(); + + // ------ Set Next State ------ // + if (!robot.drive.isBusy()) { + state = State.DELIVERING_BACKBOARD; + firstRun = true; + } + } + + // Deliver Backboard + private void deliveringBackboard() { + // ------ If Intake is on Turn it Off ------ // + robot.intakeMotor.setPower(0); + + // ------ Move Slides, Passover & Wrist ------ // + deliver(); + + // ------ Wait for Passover to Move ------ // + robot.elapsedTimeSleep(1000); + + // ------ Open Claw ------ // + robot.toggleClaw('L'); + + // ------ Wait for Claw to Open ------ // + robot.elapsedTimeSleep(1000); + + // ------ Move Slides, Passover & Wrist ------ // + intake(); + + // ------ Set Next State ------ // + state = State.PARK; + } + + // Drive to Park + private void drivingToPark() { + // ------ Select Trajectory ------ // + if(firstRun) { + firstRun = false; + activeTrajectory = "toParkFromBackboard2"; + robot.drive.followTrajectoryAsync(toParkFromBackboard2); + } + + // ------ Set Next State ------ // + if (!robot.drive.isBusy()) { + state = State.STOP; + firstRun = true; + } + } + + // Method to move to Delivery Position + private void deliver() { + robot.passoverArmLeft.setPosition(HWC.passoverDeliveryPos); + robot.passoverArmRight.setPosition(HWC.passoverDeliveryPos); + robot.wrist.setPosition(HWC.wristDeliveryPos); + robot.pulleyLComponent.setTarget(HWC.slidePositions[1] / 4.0); + robot.pulleyRComponent.setTarget(HWC.slidePositions[1] / 4.0); + } + + // Method to move to Intake Position + private void intake() { + robot.passoverArmLeft.setPosition(HWC.passoverIntakePos); + robot.passoverArmRight.setPosition(HWC.passoverIntakePos); + robot.wrist.setPosition(HWC.wristIntakePos); + robot.pulleyLComponent.setTarget(HWC.slidePositions[0]); + robot.pulleyRComponent.setTarget(HWC.slidePositions[0]); + } + + // Method to Check Claws & Close + private void checkClaws() { + // Close Claws when Pixel Detected + if (robot.colorLeft.getDistance(DistanceUnit.CM) <= 2) { + robot.clawL.setPosition(0.5); + } + + if (robot.colorRight.getDistance(DistanceUnit.CM) <= 2) { + robot.clawR.setPosition(0.5); + } + + // If both Pixels are Detected, Stop Intake + if (robot.colorLeft.getDistance(DistanceUnit.CM) <= 1 && robot.colorRight.getDistance(DistanceUnit.CM) <= 1) { + robot.intakeMotor.setPower(0); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/SimpleParkAuton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/SimpleParkAuton.java index 11bc79b..b241200 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/SimpleParkAuton.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/SimpleParkAuton.java @@ -20,7 +20,7 @@ public void init() { telemetry.update(); // ------ Initialize Robot Hardware ------ // - robot = new HWC(hardwareMap, telemetry); + robot = new HWC(hardwareMap, telemetry, false); // ------ Reset Servos ------ // robot.clawL.setPosition(1); @@ -48,14 +48,14 @@ public void init_loop() { @Override public void start() { - robot.betterSleep(10000); + robot.elapsedTimeSleep(10000); if (color.equals("red")) { robot.leftFront.setPower(-2); robot.leftRear.setPower(2); robot.rightFront.setPower(-2); robot.rightRear.setPower(2); - robot.betterSleep(1500); + robot.elapsedTimeSleep(1500); robot.leftFront.setPower(0); robot.leftRear.setPower(0); robot.rightFront.setPower(0); @@ -65,7 +65,7 @@ public void start() { robot.leftRear.setPower(-2); robot.rightFront.setPower(2); robot.rightRear.setPower(-2); - robot.betterSleep(1500); + robot.elapsedTimeSleep(1500); robot.leftFront.setPower(0); robot.leftRear.setPower(0); robot.rightFront.setPower(0); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/SlidesUpDown.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/SlidesUpDown.java deleted file mode 100644 index 7f84750..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/SlidesUpDown.java +++ /dev/null @@ -1,25 +0,0 @@ -package org.firstinspires.ftc.teamcode.auton; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.teamcode.subsystems.HWC; - -@Autonomous -public class SlidesUpDown extends LinearOpMode { - HWC robot; - - @Override - public void runOpMode() throws InterruptedException { - robot = new HWC(hardwareMap, telemetry); - - waitForStart(); - while (opModeIsActive()) { - - robot.powerSlides(1); - robot.betterSleep(1750); - robot.powerSlides(-1); - robot.betterSleep(1750); - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/VisionTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/VisionTest.java deleted file mode 100644 index 44e553d..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/VisionTest.java +++ /dev/null @@ -1,47 +0,0 @@ -package org.firstinspires.ftc.teamcode.auton; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.teamcode.subsystems.vision.SleeveDetection; -import org.openftc.easyopencv.OpenCvCamera; -import org.openftc.easyopencv.OpenCvCameraFactory; -import org.openftc.easyopencv.OpenCvCameraRotation; - -/** - * Autonomous OpMode to test scanning signal sleeves - */ -@Autonomous(name = "Signal Sleeve Test", group = "Testing") -public class VisionTest extends LinearOpMode { - - SleeveDetection sleeveDetection = new SleeveDetection(145, 168, 30, 50); - OpenCvCamera camera; - String webcamName = "webcam"; - - @Override - public void runOpMode() throws InterruptedException { - int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); - camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, webcamName), cameraMonitorViewId); - sleeveDetection = new SleeveDetection(20, 20, 20, 20); - camera.setPipeline(sleeveDetection); - - camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { - @Override - public void onOpened() { - camera.startStreaming(320, 240, OpenCvCameraRotation.SIDEWAYS_LEFT); - } - - @Override - public void onError(int errorCode) { - } - }); - - while (!isStarted()) { - telemetry.addData("ROTATION: ", sleeveDetection.getPosition()); - telemetry.update(); - } - - waitForStart(); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/enums/AllianceColor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/enums/AllianceColor.java new file mode 100644 index 0000000..8ecd0e0 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/enums/AllianceColor.java @@ -0,0 +1,6 @@ +package org.firstinspires.ftc.teamcode.auton.enums; + +// ------ Alliance Color Enum ------ // +public enum AllianceColor { + RED, BLUE +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HWC.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HWC.java index 1611127..b0dd15c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HWC.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HWC.java @@ -18,6 +18,7 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.external.tfod.Recognition; +import org.firstinspires.ftc.teamcode.auton.enums.AllianceColor; import org.firstinspires.ftc.teamcode.subsystems.pid.RobotComponents; import org.firstinspires.ftc.teamcode.subsystems.roadrunner.drive.SampleMecanumDrive; import org.firstinspires.ftc.vision.VisionPortal; @@ -34,6 +35,11 @@ public class HWC { "blue", "red" }; + // ------ Computer Vision Location Enum ------ // + public enum Location { + LEFT, CENTER, RIGHT + } + // ------ Declare Slide Positions ------ // public static int[] slidePositions = { 0, -964, -2110, -3460}; @@ -80,6 +86,7 @@ public class HWC { // ------ Computer Vision VisionPortal ------ // private VisionPortal visionPortal; + private boolean roadrunner; /** * Constructor for HWC, declares all hardware components @@ -87,18 +94,20 @@ public class HWC { * @param hardwareMap HardwareMap - Used to retrieve hardware devices * @param telemetry Telemetry - Used to add telemetry to driver hub */ - public HWC(@NonNull HardwareMap hardwareMap, Telemetry telemetry) { - // ------ Telemetry ------ // + public HWC(@NonNull HardwareMap hardwareMap, Telemetry telemetry, boolean roadrunner) { this.telemetry = telemetry; + this.roadrunner = roadrunner; - // ------ Declare RR Drivetrain ------ // - drive = new SampleMecanumDrive(hardwareMap); - - // ------ Retrieve Drive Motors ------ // - leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); - rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); - leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); - rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); + if (roadrunner) { + // ------ Declare RR Drivetrain ------ // + drive = new SampleMecanumDrive(hardwareMap); + } else { + // ------ Retrieve Drive Motors ------ // + leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); + rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); + rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); + } // ------ Retrieve Other Motors ------ // rightPulley = hardwareMap.get(DcMotorEx.class, "pulleyR"); @@ -119,24 +128,33 @@ public HWC(@NonNull HardwareMap hardwareMap, Telemetry telemetry) { colorRight = hardwareMap.get(ColorRangeSensor.class, "colorR"); // ------ Set Motor Directions ------ // - leftFront.setDirection(DcMotorEx.Direction.FORWARD); - rightFront.setDirection(DcMotorEx.Direction.FORWARD); - leftRear.setDirection(DcMotorEx.Direction.FORWARD); - rightRear.setDirection(DcMotorEx.Direction.FORWARD); + if (!roadrunner) { + leftFront.setDirection(DcMotorEx.Direction.FORWARD); + rightFront.setDirection(DcMotorEx.Direction.FORWARD); + leftRear.setDirection(DcMotorEx.Direction.FORWARD); + rightRear.setDirection(DcMotorEx.Direction.FORWARD); + } + leftPulley.setDirection(DcMotorEx.Direction.REVERSE); // ------ Set Motor Brake Modes ------ // - leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - leftRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + if (!roadrunner) { + leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } + intakeMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); // ------ Set Motor Modes ------ // - leftFront.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER); - rightFront.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER); - leftRear.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER); - rightRear.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER); + if (!roadrunner) { + leftFront.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER); + rightFront.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER); + leftRear.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER); + rightRear.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER); + } + intakeMotor.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER); leftPulley.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER); rightPulley.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER); @@ -162,49 +180,24 @@ public void toggleClaw(char servo) { } } - public void betterSleep(int sleep) { + // ------ Function that Allows For Sleeping in OpModes ------ // + public void elapsedTimeSleep(int milliseconds) { sleepTime.reset(); - while (sleepTime.milliseconds() < sleep) { + while (sleepTime.milliseconds() < milliseconds) { + pulleyLComponent.moveUsingPID(); + pulleyRComponent.moveUsingPID(); + telemetry.addData("sleeping", "true"); telemetry.update(); } telemetry.addData("sleeping", "slept"); telemetry.update(); - - } - - public void sleepDeliver(int time) { - intakeMotor.setPower(-0.3); - betterSleep(time); - intakeMotor.setPower(0); - } - - // ------ Function to Power Slides ------ // - public void powerSlides(double target) { - pulleyLComponent.incrementTarget(target); - pulleyRComponent.incrementTarget(target); - } - - // ------ Function to Reset Motor Encoder Positions [EMERGENCY ONLY] ------ // - public void resetEncoders() { - leftFront.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER); - rightFront.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER); - leftRear.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER); - rightRear.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER); - - leftFront.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER); - rightFront.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER); - leftRear.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER); - rightRear.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER); } + // ------ Function to Initialize TensorFlow Object Detection ------ // public void initTFOD(String TFOD_MODEL_ASSET) { - // Create the TensorFlow processor by using a builder. - tfod = new TfodProcessor.Builder() - .setModelAssetName(TFOD_MODEL_ASSET) - .setModelLabels(LABELS) - .build(); + tfod = new TfodProcessor.Builder().setModelAssetName(TFOD_MODEL_ASSET).setModelLabels(LABELS).build(); // Create the vision portal by using a builder. VisionPortal.Builder builder = new VisionPortal.Builder(); @@ -220,36 +213,56 @@ public void initTFOD(String TFOD_MODEL_ASSET) { tfod.setMinResultConfidence(0.75f); } - private double telemetryTFOD() { + // ------ Function to add Telemetry for TensorFlow Object Detection ------ // + public Location detectElement(AllianceColor allianceColor) { List currentRecognitions = tfod.getRecognitions(); telemetry.addData("# Objects Detected", currentRecognitions.size()); - double x = 800; - // Step through the list of recognitions and display info for each one. + double x = 1000, y; + for (Recognition recognition : currentRecognitions) { x = (recognition.getLeft() + recognition.getRight()) / 2; - double y = (recognition.getTop() + recognition.getBottom()) / 2; - + y = (recognition.getTop() + recognition.getBottom()) / 2; telemetry.addData("", " "); telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100); telemetry.addData("- Position", "%.0f / %.0f", x, y); telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight()); } - return x; + + switch (allianceColor) { + default: + case RED: + if (x < 400) { + return Location.CENTER; + } else if (x > 400 && x < 800) { + return Location.RIGHT; + } else { + return Location.LEFT; + } + case BLUE: + if (x < 400) { + return Location.LEFT; + } else if (x > 400 && x < 800) { + return Location.CENTER; + } else { + return Location.RIGHT; + } + } } - public int cv() { - double pos = telemetryTFOD(); //set it to a value depending on locaiton of obkect - if (pos > 800) { - return 0; - } else if (pos < 100 && pos > 0) { - return 1; - } else if (pos < 400) { - return 2; - } else if (pos < 800) { - return 3; + @NonNull + @Override + public String toString() { + StringBuilder builder = new StringBuilder("HWC {"); + + if (roadrunner) { + builder.append("\n\tbusy = ").append(drive.isBusy()); } else { - return 0; + builder.append("\n\tleftFront = ").append(leftFront.getPower()).append("\n\trightFront = ").append(rightFront.getPower()).append("\n\tleftRear = ").append(leftRear.getPower()).append("\n\trightRear = ").append(rightRear.getPower()); } + + builder.append("\n\trightPulley = ").append(rightPulley.getPower()).append("\n\tleftPulley = ").append(leftPulley.getPower()).append("\n\tintakeMotor = ").append(intakeMotor.getPower()).append("\n\twrist = ").append(wrist.getPosition()).append("\n\tclawR = ").append(clawR.getPosition()).append("\n\tclawL = ").append(clawL.getPosition()).append("\n\tpassoverArmLeft = ").append(passoverArmLeft.getPosition()).append("\n\tpassoverArmRight = ").append(passoverArmRight.getPosition()).append("\n}"); + + return builder.toString(); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/pid/MotorPIDTuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/pid/MotorPIDTuning.java index bbfcf72..bb08f43 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/pid/MotorPIDTuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/pid/MotorPIDTuning.java @@ -32,7 +32,7 @@ public class MotorPIDTuning extends OpMode { @Override public void init() { // ------ Initialize Hardware ------ // - kit = new HWC(hardwareMap, telemetry); + kit = new HWC(hardwareMap, telemetry, false); slideLeft = kit.leftPulley; slideRight = kit.rightPulley; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/roadrunner/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/roadrunner/drive/DriveConstants.java index 182d199..48807e4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/roadrunner/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/roadrunner/drive/DriveConstants.java @@ -21,8 +21,8 @@ public class DriveConstants { /* * These are motor constants that should be listed online for your motors. */ - public static final double TICKS_PER_REV = 3895.9; - public static final double MAX_RPM = 312.0; + public static final double TICKS_PER_REV = 384.5; + public static final double MAX_RPM = 435; /* * Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders. @@ -32,8 +32,8 @@ public class DriveConstants { * If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients * from DriveVelocityPIDTuner. */ - public static final boolean RUN_USING_ENCODER = true; - public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0.061, 0.00473, 0.0, + public static final boolean RUN_USING_ENCODER = false; + public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0, getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV)); /* @@ -44,9 +44,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 = 3.77953; // in - public static double GEAR_RATIO = 1.0 / 10.71; // output (wheel) speed / input (motor) speed - public static double TRACK_WIDTH = 10.6299; // in + public static double WHEEL_RADIUS = 2; // in + public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed + public static double TRACK_WIDTH = 13.39; // in /* * These are the feedforward parameters used to model the drive motor behavior. If you are using @@ -54,9 +54,9 @@ public class DriveConstants { * motor encoders or have elected not to use them for velocity control, these values should be * empirically tuned. */ - public static double kV = 1.0 / rpmToVelocity(MAX_RPM); - public static double kA = 1.95; - public static double kStatic = 0.02; + public static double kV = 0.0155; // 1.0 / rpmToVelocity(MAX_RPM); + public static double kA = 0.003; + public static double kStatic = 0; /* * These values are used to generate the trajectories for you robot. To ensure proper operation, @@ -65,10 +65,10 @@ public class DriveConstants { * small and gradually increase them later after everything is working. All distance units are * inches. */ - public static double MAX_VEL = 25; - public static double MAX_ACCEL = 25; - public static double MAX_ANG_VEL = Math.toRadians(50); - public static double MAX_ANG_ACCEL = Math.toRadians(50); + public static double MAX_VEL = 30; + public static double MAX_ACCEL = 30; + public static double MAX_ANG_VEL = 6.217; + public static double MAX_ANG_ACCEL = Math.toRadians(180); /* * Adjust the orientations here to match your robot. See the FTC SDK documentation for details. @@ -91,4 +91,4 @@ public static double getMotorVelocityF(double ticksPerSecond) { // see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx return 32767 / ticksPerSecond; } -} +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/roadrunner/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/roadrunner/drive/SampleMecanumDrive.java index e721488..6fba9ff 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/roadrunner/drive/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/roadrunner/drive/SampleMecanumDrive.java @@ -57,8 +57,8 @@ public class SampleMecanumDrive extends MecanumDrive { private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH); private static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(MAX_ACCEL); - public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0); - public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 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; public static double VX_WEIGHT = 1; public static double VY_WEIGHT = 1; @@ -126,12 +126,16 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { } // TODO: reverse any motors using DcMotor.setDirection() + leftFront.setDirection(DcMotorEx.Direction.REVERSE); + rightFront.setDirection(DcMotorEx.Direction.FORWARD); + leftRear.setDirection(DcMotorEx.Direction.REVERSE); + rightRear.setDirection(DcMotorEx.Direction.FORWARD); List lastTrackingEncPositions = new ArrayList<>(); List lastTrackingEncVels = new ArrayList<>(); // TODO: if desired, use setLocalizer() to change the localization method - // setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels)); + setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels)); trajectorySequenceRunner = new TrajectorySequenceRunner( follower, HEADING_PID, batteryVoltageSensor, diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/roadrunner/drive/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/roadrunner/drive/StandardTrackingWheelLocalizer.java index 2ef342d..0aa0c0e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/roadrunner/drive/StandardTrackingWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/roadrunner/drive/StandardTrackingWheelLocalizer.java @@ -28,13 +28,14 @@ */ @Config public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer { - public static double TICKS_PER_REV = 0; - public static double WHEEL_RADIUS = 2; // in + public static double TICKS_PER_REV = 2000; + public static double WHEEL_RADIUS = 0.3; // in public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed - public static double LATERAL_DISTANCE = 10; // in; distance between the left and right wheels - public static double FORWARD_OFFSET = 4; // in; offset of the lateral wheel - + public static double LATERAL_DISTANCE = 12.57; // in; distance between the left and right wheels + public static double FORWARD_OFFSET = -6.5; // in; offset of the lateral wheel + public static double X_MULTIPLIER = 3.12; + public static double Y_MULTIPLIER = 2.89; private final Encoder leftEncoder; private final Encoder rightEncoder; private final Encoder frontEncoder; @@ -52,11 +53,13 @@ public StandardTrackingWheelLocalizer(HardwareMap hardwareMap, List las lastEncPositions = lastTrackingEncPositions; lastEncVels = lastTrackingEncVels; - leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftEncoder")); - rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightEncoder")); - frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontEncoder")); + leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightFront")); + rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightRear")); + frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftFront")); // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) + rightEncoder.setDirection(Encoder.Direction.REVERSE); + frontEncoder.setDirection(Encoder.Direction.REVERSE); } public static double encoderTicksToInches(double ticks) { @@ -76,9 +79,9 @@ public List getWheelPositions() { lastEncPositions.add(frontPos); return Arrays.asList( - encoderTicksToInches(leftPos), - encoderTicksToInches(rightPos), - encoderTicksToInches(frontPos) + encoderTicksToInches(leftPos) * X_MULTIPLIER, + encoderTicksToInches(rightPos) * X_MULTIPLIER, + encoderTicksToInches(frontPos) * Y_MULTIPLIER ); } @@ -95,9 +98,9 @@ public List getWheelVelocities() { lastEncVels.add(frontVel); return Arrays.asList( - encoderTicksToInches(leftVel), - encoderTicksToInches(rightVel), - encoderTicksToInches(frontVel) + encoderTicksToInches(leftVel) * X_MULTIPLIER, + encoderTicksToInches(rightVel) * X_MULTIPLIER, + encoderTicksToInches(frontVel) * Y_MULTIPLIER ); } -} +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ColorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ColorTest.java index f1623fe..87f7362 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ColorTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ColorTest.java @@ -19,7 +19,7 @@ public void init() { telemetry.update(); // ------ Initialize Robot Hardware ------ // - robot = new HWC(hardwareMap, telemetry); + robot = new HWC(hardwareMap, telemetry, false); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SingleDriverTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SingleDriverTeleOp.java index 079cb51..8569ba2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SingleDriverTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SingleDriverTeleOp.java @@ -42,7 +42,7 @@ public void init() { telemetry.update(); // ------ Initialize Robot Hardware ------ // - robot = new HWC(hardwareMap, telemetry); + robot = new HWC(hardwareMap, telemetry, false); // ------ Reset Pulley Encoders ------ // robot.leftPulley.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);