This repository has been archived by the owner on Aug 31, 2024. It is now read-only.
generated from rh-robotics/Basecode
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* [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 <milobanks@rowlandhall.org>
1 parent
2ec9fd2
commit 1c13880
Showing
14 changed files
with
1,106 additions
and
186 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
391 changes: 391 additions & 0 deletions
391
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/AutonomousV1.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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); | ||
} | ||
} | ||
} |
575 changes: 575 additions & 0 deletions
575
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/AutonomousV2.java
Large diffs are not rendered by default.
Oops, something went wrong.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
25 changes: 0 additions & 25 deletions
25
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/SlidesUpDown.java
This file was deleted.
Oops, something went wrong.
47 changes: 0 additions & 47 deletions
47
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/VisionTest.java
This file was deleted.
Oops, something went wrong.
6 changes: 6 additions & 0 deletions
6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/enums/AllianceColor.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,6 @@ | ||
package org.firstinspires.ftc.teamcode.auton.enums; | ||
|
||
// ------ Alliance Color Enum ------ // | ||
public enum AllianceColor { | ||
RED, BLUE | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters