From 04e44aeb867c7c8b473ef97bbd7b5cb3f0fa61f2 Mon Sep 17 00:00:00 2001 From: PowerfulHermes Date: Mon, 4 Mar 2024 17:43:41 -0500 Subject: [PATCH] refining auto --- .../ftc/teamcode/drive/RoboticaBot.java | 4 +-- .../ftc/teamcode/opmode/TestAuto.java | 6 ++-- .../ftc/teamcode/opmode/TestTeleOp.java | 2 +- .../teamcode/opmode/components/GoToBoard.java | 12 ++++---- .../teamcode/opmode/components/ParkingIn.java | 3 +- .../components/PurplePixelComponent.java | 30 ++++++++++--------- .../TrajectorySequenceBuilder.java | 7 +++-- .../teamcode/vision/AprilTagLocalizer.java | 2 +- 8 files changed, 38 insertions(+), 28 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/RoboticaBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/RoboticaBot.java index fb59e6db..decd73e5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/RoboticaBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/RoboticaBot.java @@ -215,7 +215,7 @@ public AprilTagCamera getPrimaryCamera() { @Override public void dropPurplePixel(boolean state) { if (state) { - purpleServo.setPosition(1); + purpleServo.setPosition(.8); } else { purpleServo.setPosition(0.5); } @@ -245,7 +245,7 @@ public TrajectoryDrive getDrive() { public void recalibrateShoulder() { // Raise the arm up - shoulderMotor.setTargetPosition(700); + shoulderMotor.setTargetPosition(1200); shoulderMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); shoulderMotor.setPower(1); while (shoulderMotor.isBusy() && !GlobalOpMode.opMode.isStopRequested()) {} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestAuto.java index bdf7a8fc..ee4023ff 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestAuto.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.opmode; +import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -24,6 +25,7 @@ import java.util.Objects; @Autonomous +@Config public class TestAuto extends LinearOpMode { private Pose2d estimateWithAllCameras(AprilTagCamera[] cameras, AprilTagLocalizer aprilTag) { Pose2d pose = null; @@ -35,7 +37,7 @@ private Pose2d estimateWithAllCameras(AprilTagCamera[] cameras, AprilTagLocalize } return pose; } - + public static TensorFlowDetection.PropPosition TENSORFLOWDEF = TensorFlowDetection.PropPosition.RIGHT; private TensorFlowDetection.PropPosition detectProp(Robot robot, int timeout) { if (robot.getClass() == PsiBot.class) { ((PsiBot) robot).armMotor.setTargetPosition(-109); @@ -46,7 +48,7 @@ private TensorFlowDetection.PropPosition detectProp(Robot robot, int timeout) { TensorFlowDetection.PropPosition position = tensor.getPropPosition(new Timeout(timeout)); if (position == null) { telemetry.log().add("Unable to detect prop, using CENTER"); - position = TensorFlowDetection.PropPosition.CENTER; + position = TENSORFLOWDEF; } return position; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestTeleOp.java index c9265392..ca04cde9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestTeleOp.java @@ -217,7 +217,7 @@ public void runOpMode() throws InterruptedException { public static int RAISED_ARM = 1855; public static double RAISED_ELBOW = 0.272; - public static double RAISED_WRIST = 0.456; + public static double RAISED_WRIST = 0.544; public static int NEUTRAL_ARM = 113; public static double NEUTRAL_ELBOW = 0.248; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/GoToBoard.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/GoToBoard.java index 261fcd6b..bc7e32c9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/GoToBoard.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/GoToBoard.java @@ -32,7 +32,7 @@ public void drive() { trajB.lineTo(new Vector2d(startPose.getX(), y)) .lineTo(new Vector2d(fi(3, 10), y)) - .turnTo(Math.toRadians(0)) + .turnTo(Math.toRadians(180)) //.turn(Math.toRadians(90)) .lineTo(new Vector2d(fi(3,8), fi(-2,11))); @@ -40,10 +40,12 @@ public void drive() { getRobot().getDrive().followTrajectorySequence(traj); - if (getRobot().getClass() == RoboticaBot.class) { - ((RoboticaBot) getRobot()).recalibrateShoulder(); - ((RoboticaBot) getRobot()).setShoulderTargetPosition(TestTeleOp.RAISED_ARM); - } +// if (getRobot().getClass() == RoboticaBot.class) { +// ((RoboticaBot) getRobot()).recalibrateShoulder(); +// ((RoboticaBot) getRobot()).setShoulderTargetPosition(TestTeleOp.RAISED_ARM); +// ((RoboticaBot) getRobot()).elbowServo.setPosition(TestTeleOp.RAISED_ELBOW); +// ((RoboticaBot) getRobot()).wristLiftServo.setPosition(TestTeleOp.RAISED_WRIST); +// } //GlobalOpMode.opMode.sleep(2000); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/ParkingIn.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/ParkingIn.java index abf20eb0..d7d71a73 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/ParkingIn.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/ParkingIn.java @@ -24,7 +24,8 @@ public void drive() { int y = (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_BOARD) || (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_AUDIENCE) ? fi(0,10) : fi(0,-10); trajB.lineTo(new Vector2d(currentPose.getX(), y)); - trajB.lineTo(new Vector2d(fi(4,6), y)); + trajB.lineTo(new Vector2d(fi(4,0), y)); + trajB.turnTo(Math.toRadians(180)); TrajectorySequence traj = trajB.build(); getRobot().getDrive().followTrajectorySequence(traj); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/PurplePixelComponent.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/PurplePixelComponent.java index 7c8971cd..7aba0dfa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/PurplePixelComponent.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/PurplePixelComponent.java @@ -54,20 +54,21 @@ public void drive() { if (propPosition == TensorFlowDetection.PropPosition.LEFT) { // Special hack since our dropper is off-center - if (getRobot().getClass() == RoboticaBot.class && (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_BOARD || getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_AUDIENCE)) { - trajB.turn(Math.toRadians(45)) - .forward(12 * driveBackwards); - } if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_AUDIENCE) { - trajB.forward(9 * driveBackwards) - .turn(Math.toRadians(90)) - .forward(8 * driveBackwards); - } else { - trajB.turn(Math.toRadians(90)) - .forward(8 * driveBackwards); // orig 13 - } + //if (getRobot().getClass() == RoboticaBot.class && (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_BOARD || getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_AUDIENCE)) { + trajB.back(6 * driveBackwards) + .turn(Math.toRadians(45)) + .forward(14 * driveBackwards); +// } if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_AUDIENCE) { +// trajB.forward(9 * driveBackwards) +// .turn(Math.toRadians(90)) +// .forward(8 * driveBackwards); +// } else { +// trajB.turn(Math.toRadians(90)) +// .forward(8 * driveBackwards); // orig 13 +// } } else if (propPosition == TensorFlowDetection.PropPosition.RIGHT) { trajB.turn(Math.toRadians(-90)) - .forward(10 * driveBackwards); + .forward(7 * driveBackwards); } else { if (moveTowardsCenter) { trajB.turn(Math.toRadians(180)) @@ -83,11 +84,12 @@ public void drive() { .waitSeconds(0.5); //if (propPosition == TensorFlowDetection.PropPosition.CENTER && (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_AUDIENCE || getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_AUDIENCE )) { - if (propPosition == TensorFlowDetection.PropPosition.LEFT && getRobot().getClass() == RoboticaBot.class && getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_BOARD) { - trajB.forward(-14 * driveBackwards) + if (propPosition == TensorFlowDetection.PropPosition.LEFT/* && getRobot().getClass() == RoboticaBot.class && getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_BOARD*/) { + trajB.forward(-16 * driveBackwards) .turn(Math.toRadians(-45)); } if (moveTowardsCenter && propPosition == TensorFlowDetection.PropPosition.CENTER) { + // be careful with relative forwards, this wouldn't work for RIGHT etc. since we never un-turn trajB.forward(-14 * driveBackwards); } else { trajB.lineTo(currentTapeMarks()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceBuilder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceBuilder.java index 9f3c4d01..5a82e269 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceBuilder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceBuilder.java @@ -462,9 +462,12 @@ public TrajectorySequenceBuilder turnTo(double angle) { } public TrajectorySequenceBuilder turnTo(double angle, double maxAngVel, double maxAngAccel) { - return turn(angle - lastPose.getHeading(), maxAngVel, maxAngAccel); + if (lastPose.getHeading() > Math.toRadians(-180) && lastPose.getHeading() < Math.toRadians(180)) { + return turn(angle - lastPose.getHeading(), maxAngVel, maxAngAccel); + } else { + return turn(angle + (2*Math.PI) - lastPose.getHeading(), maxAngVel, maxAngAccel); + } } - public TrajectorySequenceBuilder waitSeconds(double seconds) { pushPath(); sequenceSegments.add(new WaitSegment(lastPose, seconds, Collections.emptyList())); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/AprilTagLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/AprilTagLocalizer.java index c30b2ef2..2f001adc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/AprilTagLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/AprilTagLocalizer.java @@ -81,7 +81,7 @@ private Pose2d estimateCameraPoseFromAprilTags(AprilTagCamera camera) { List detections = aprilTag.getFreshDetections(); // Wait for detections to not be null // TODO: Timeout - while (detections == null) { + while (detections == null && !GlobalOpMode.opMode.isStopRequested()) { //android.util.Log.i("APRILTAG", "Waiting for non null return"); detections = aprilTag.getFreshDetections(); }