Merge pull request #67 from ftc16072/RoadRunnerTuning

VisionFixes
ftc16072 · Feb 17, 2024 · 1443ad9 · 1443ad9
2 parents 4d1ce6d + 9e488a6
commit 1443ad9
Show file tree

Hide file tree

Showing 17 changed files with 163 additions and 33 deletions.
diff --git a/...org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SpikeLocationTelemetry.java b/...org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SpikeLocationTelemetry.java
@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions;
+
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
+import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
+
+public class SpikeLocationTelemetry extends Node {
+    @Override
+    public State tick(QQOpMode opmode) {
+        opmode.telemetry.addData("Prop Location: ", opmode.robot.cameraBack.getTeamPropPosition());
+        return State.SUCCESS;
+    }
+}
diff --git a/...ires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Trajectories/SetLeftSpikeTrajectory.java b/...ires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Trajectories/SetLeftSpikeTrajectory.java
@@ -13,9 +13,9 @@ public class SetLeftSpikeTrajectory extends Node {
     @Override
     public State tick(QQOpMode opMode) {
         TrajectorySequenceBuilder builder = opMode.robot.nav.trajectorySequenceBuilder(opMode.robot.nav.getPoseEstimate());
-        TrajectorySequence sequence = builder.forward(FORWARD_DISTANCE_INCHES).
-                                            strafeLeft(LEFT_DISTANCE_INCHES).
-                                            strafeRight(LEFT_DISTANCE_INCHES).
+        TrajectorySequence sequence = builder.back(FORWARD_DISTANCE_INCHES).
+                                            //strafeLeft(LEFT_DISTANCE_INCHES).
+                                            //strafeRight(LEFT_DISTANCE_INCHES).
                                             forward(FORWARD_DISTANCE_INCHES).
                                             build();
         if (sequence == null){

diff --git a/...es/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Trajectories/SetMiddleSpikeTrajectory.java b/...es/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Trajectories/SetMiddleSpikeTrajectory.java
@@ -8,13 +8,15 @@
 
 public class SetMiddleSpikeTrajectory extends Node {
     public static final int FORWARD_DISTANCE_INCHES = 20;
+    public static final int BACKWARD_DISTANCE_INCHES = 20;
 
     @Override
     public State tick(QQOpMode opMode) {
         TrajectorySequenceBuilder builder = opMode.robot.nav.trajectorySequenceBuilder(opMode.robot.nav.getPoseEstimate());
-        TrajectorySequence sequence = builder.forward(FORWARD_DISTANCE_INCHES).
-                                            back(FORWARD_DISTANCE_INCHES).
-                                            build();
+        TrajectorySequence sequence = builder
+                .back(FORWARD_DISTANCE_INCHES)
+                .back(FORWARD_DISTANCE_INCHES).
+                build();
         if (sequence == null){
             return State.FAILURE;
         }

diff --git a/.../main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpikeAutoTree.java b/.../main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpikeAutoTree.java
@@ -1,8 +1,11 @@
 package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees;
 
+import org.checkerframework.checker.units.qual.A;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SpikeLocationTelemetry;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.PlacePurplePixel;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ReleaseAutoPixel;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SetBackboardFromSpikeTrajectory;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SpikeLocationTelemetry;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.FollowTrajectory;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.SetLeftSpikeTrajectory;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.SetMiddleSpikeTrajectory;
@@ -28,6 +31,7 @@ public class SpikeAutoTree {
  */
     public static Node root(){
         return new Sequence(
+                new SpikeLocationTelemetry(),
                 new Failover(
                         new Sequence(
                                 new IsLeftSpike(),
@@ -39,6 +43,7 @@ public static Node root(){
                         ),
                         new SetMiddleSpikeTrajectory()
                 ),
+
                 new FollowTrajectory(),
                 new ReleaseAutoPixel(),
                 new SetBackboardFromSpikeTrajectory(),

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Camera.java
@@ -120,19 +120,19 @@ public List<AprilTagDetection> getAprilTagDetections(){ // return robot position
     // TODO this program takes the first april tag reading. it needs to be able to sort through multiple ones
     // TODO getY, getX, and getHeading functions are all aprilTag relavtive, not field. this needs to be changed
 
-    public Pose2d getPose(){
+    public Pose2d getPose(){ //DO NOT USE THIS UNLESS ADDING APRILTAGS
         List<AprilTagDetection> detections = aprilTag.getDetections();
-        AprilTagDetection detection = selectTag(detections);
-        Pose2d aprilTagLocation = aprilTagPositions.get(detection.id);
-
-        return new Pose2d(aprilTagLocation.getX()+detection.ftcPose.y, aprilTagLocation.getY()+detection.ftcPose.x,aprilTagLocation.getHeading()+detection.ftcPose.yaw);
+        //AprilTagDetection detection = selectTag(detections);
+        //Pose2d aprilTagLocation = aprilTagPositions.get(detection.id);
 
+        //return new Pose2d(aprilTagLocation.getX()+detection.ftcPose.y, aprilTagLocation.getY()+detection.ftcPose.x,aprilTagLocation.getHeading()+detection.ftcPose.yaw);
+        return new Pose2d(1,2,2); //DONT USE THIS
     }
     public TeamPropLocation getTeamPropPosition(){
         return teamPropDetector.getPropLocation();
 
     }
-    public AprilTagDetection selectTag(List<AprilTagDetection> detections ){ // this method selects the april tag that is most straight on relative to the robot
+    /*public AprilTagDetection selectTag(List<AprilTagDetection> detections ){ // this method selects the april tag that is most straight on relative to the robot
         AprilTagDetection smallestDetection = detections.get(0);
         for (AprilTagDetection detection: detections){
             if (smallestDetection.ftcPose.yaw>detection.ftcPose.yaw){
@@ -143,11 +143,11 @@ public AprilTagDetection selectTag(List<AprilTagDetection> detections ){ // this
     }
     public boolean isTagDetected(){
         return (aprilTag.getDetections().size() != 0);
-    }
+    }*/
 
     @Override
     public List<QQtest> getTests() {
-        return Collections.singletonList(new TestCamera(cameraName,webcamName ));
+        return Collections.singletonList(new TestCamera(cameraName,webcamName, teamPropDetector));
     }
     public void stopStreaming(){
         visionPortal.stopStreaming();

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/MecanumDrive.java
@@ -31,8 +31,8 @@ public class MecanumDrive implements Mechanism {
     public static final double MM_PER_IN = 25.4;
 
     // depending on drive motor
-    public static final double TICKS_PER_REV = 537.7; // for gobilda 5203 312 rpm motors
-    public static final double MAX_RPM = 312;
+    public static final double TICKS_PER_REV = 384.5; // for gobilda 5203 312 rpm motors
+    public static final double MAX_RPM = 435;
     public static final double WHEEL_DIAM_IN = 96 / MM_PER_IN;
     public static double TRACK_WIDTH_IN = 16.65;  // from TrackWidthTuner
     public static final double MAX_MOTOR_VELOCITY = MAX_RPM * Math.PI * WHEEL_DIAM_IN / SECS_PER_MIN;

diff --git a/...de/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/BlueClose_SpikeAuto.java b/...de/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/BlueClose_SpikeAuto.java
@@ -2,14 +2,28 @@
 
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees.SpikeAutoTree;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees.TeleopTree;
 import org.firstinspires.ftc.teamcode.ftc16072.Util.Alliance;
 import org.firstinspires.ftc.teamcode.ftc16072.Util.StartPosition;
 @Autonomous
 public class BlueClose_SpikeAuto extends SpikeAutoBase {
+
+
+
     @Override
     public void init(){
+        super.init();
         board.add("Alliance", Alliance.BLUE);
         board.add("StartPosition", StartPosition.CLOSE);
-        super.init();
+
+
+    }
+
+    @Override
+    public void init_loop() {
+        super.init_loop();
+        telemetry.addData("Prop Location: ", robot.cameraBack.getTeamPropPosition());
     }
 }
diff --git a/...src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/MiddleSpikeToBackboard.java b/...src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/MiddleSpikeToBackboard.java
@@ -0,0 +1,78 @@
+package org.firstinspires.ftc.teamcode.ftc16072.OpModes;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.util.NanoClock;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.teamcode.ftc16072.Robot;
+
+@Autonomous
+public class MiddleSpikeToBackboard extends OpMode {
+    Robot robot = new Robot();
+    Trajectory trajectory;
+    private enum State {BEGIN, AWAY, PAUSE, RETURN, DONE}
+    State state = State.BEGIN;
+    NanoClock clock;
+    double startPause;
+    @Override
+    public void init() {
+        robot.makeDriveOnly();
+        robot.init(hardwareMap);
+        clock = NanoClock.system();
+    }
+
+    @Override
+    public void loop() {
+        robot.nav.updatePoseEstimate();
+        Pose2d currentPose = robot.nav.getPoseEstimate();
+
+        //Trajectory to middle tape
+        Trajectory trajectory1 = robot.nav.trajectoryBuilder(currentPose, false)
+                .back(25)
+                .strafeRight(25)
+                .build();
+
+        Trajectory trajectory2 = robot.nav.trajectoryBuilder(trajectory1.end(), false)
+                .strafeRight(25)
+                .build();
+
+        telemetry.addData("STATE", state);
+        telemetry.addData("POSE", "x = %.2f y = %.2f h = %.1f", currentPose.getX(), currentPose.getY(), Math.toDegrees(currentPose.getHeading()));
+        switch (state) {
+            case BEGIN:
+                state = State.AWAY;
+
+                robot.nav.setPoseEstimate(currentPose);
+                robot.nav.follower.followTrajectory(trajectory1);
+                break;
+            case AWAY:
+                if (robot.nav.isDoneFollowing(currentPose)) {
+                    state = State.PAUSE;
+                    startPause = clock.seconds();
+                }
+                break;
+            case PAUSE:
+                if ((clock.seconds() - startPause) > 2.0) {
+
+                    state = State.RETURN;
+
+                }
+                break;
+            case RETURN:
+                robot.nav.setPoseEstimate(trajectory1.end());
+                robot.nav.follower.followTrajectory(trajectory2);
+
+                break;
+            case DONE:
+                if (robot.nav.isDoneFollowing(currentPose)) {
+                    state = State.DONE;
+                }
+                break;
+        }
+
+
+    }
+}
diff --git a/...e/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/RRTests/RR_OurSpline.java b/...e/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/RRTests/RR_OurSpline.java
@@ -10,7 +10,7 @@
 
 import org.firstinspires.ftc.teamcode.ftc16072.Robot;
 
-@Disabled
+
 @Autonomous(group = "RR")
 public class RR_OurSpline extends OpMode {
 
@@ -39,6 +39,8 @@ public void loop() {
 
         telemetry.addData("STATE", state);
         telemetry.addData("POSE", "x = %.2f y = %.2f h = %.1f", currentPose.getX(), currentPose.getY(), Math.toDegrees(currentPose.getHeading()));
+        telemetry.addData("Seconds Elapsed", clock.seconds());
+        telemetry.addData("Pause", (clock.seconds() - startPause));
         switch (state) {
             case BEGIN:
                 state = State.AWAY;
@@ -61,12 +63,12 @@ public void loop() {
                 }
                 break;
             case PAUSE:
-                if ((clock.seconds() - startPause) > 2.0) {
+                if ((clock.seconds() - startPause) > 10.0) {
                     state = State.RETURN;
-                    trajectory = robot.nav.trajectoryBuilder(currentPose, true)
-                            .splineTo(new Vector2d(0, 0), Math.toRadians(180))
+                    Trajectory trajectory3 = robot.nav.trajectoryBuilder(currentPose, true)
+                            .back(24)
                             .build();
-                    robot.nav.follower.followTrajectory(trajectory);
+                    robot.nav.follower.followTrajectory(trajectory3);
                 }
                 break;
             case RETURN:

diff --git a/.../src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/RRTests/RR_StrafeTest.java b/.../src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/RRTests/RR_StrafeTest.java
@@ -11,7 +11,6 @@
 
 import org.firstinspires.ftc.teamcode.ftc16072.Robot;
 
-@Disabled
 @Autonomous(group = "RR")
 public class RR_StrafeTest extends OpMode {
 

diff --git a/...rc/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/RRTests/RR_StraightTest.java b/...rc/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/RRTests/RR_StraightTest.java
@@ -11,7 +11,6 @@
 
 import org.firstinspires.ftc.teamcode.ftc16072.Robot;
 
-@Disabled
 @Autonomous(group = "RR")
 public class RR_StraightTest extends OpMode {
     Robot robot = new Robot();

diff --git a/...main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/RRTests/RR_TrackWidthTuner.java b/...main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/RRTests/RR_TrackWidthTuner.java
@@ -13,7 +13,6 @@
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.MecanumDrive;
 import org.firstinspires.ftc.teamcode.ftc16072.Robot;
 
-@Disabled
 @Autonomous(group = "RR")
 public class RR_TrackWidthTuner extends OpMode {
     public static double TURN_ANGLE_DEGREES = 180;

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/SpikeAutoBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/SpikeAutoBase.java
@@ -4,14 +4,19 @@
 
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees.SpikeAutoTree;
+import org.firstinspires.ftc.teamcode.ftc16072.Util.Alliance;
 
 abstract public class SpikeAutoBase extends VisionBase{
     Node root = SpikeAutoTree.root();
+
     boolean done;
     @Override
     public void init(){
-        robot.makeDriveOnly();
         super.init();
+        board.add("proploocation", robot.cameraBack.getTeamPropPosition());
+        robot.makeDriveOnly();
+
+        telemetry.addData("prop Location", robot.cameraBack.getTeamPropPosition());
     }
     @Override
     public void loop() {

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/QQTest/TestCamera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/QQTest/TestCamera.java
@@ -2,16 +2,21 @@
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.teamcode.ftc16072.Util.TeamPropDetector;
 
 public class TestCamera extends QQtest{
+    TeamPropDetector teamPropDetector;
     WebcamName webcamName;
 
-    public TestCamera(String name, WebcamName webcamName){
+    public TestCamera(String name, WebcamName webcamName, TeamPropDetector teamPropDetector){
         super(name);
         this.webcamName = webcamName;
+        this.teamPropDetector = teamPropDetector;
     }
     @Override
     public void run(boolean on, Telemetry telemetry) {
         telemetry.addData("Is Camera Attached", webcamName.isAttached());
+        telemetry.addData("teamProplocation: ", teamPropDetector.getPropLocation());
+
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/Navigation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/Navigation.java
@@ -125,20 +125,26 @@ public void turnAsync(double angle) {
     public void startNewTrajectorySequence(TrajectorySequence trajectorySequence){
         trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence);
     }
-    public void update() {
+
+    public boolean update() {
         DriveSignal signal;
         updatePoseEstimate();
-
-        if (mecanumDrive.getAverageDrivetrainPower()<0.1 && cameraBack.isTagDetected()){
+        /*
+        if (mecanumDrive.getAverageDrivetrainPower()<0.1 ){
             Pose2d robotPos = cameraBack.getPose();
             signal = trajectorySequenceRunner.update(robotPos, getPoseVelocity());
 
 
         } else {
             signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity());
-        }
+        }*/
+        signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity());
 
-        if (signal != null) setDriveSignal(signal);
+
+        if (signal != null) {
+            setDriveSignal(signal);
+        }
+        return false;
     }
 
     public boolean isBusy() {

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/TeamPropDetector.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/TeamPropDetector.java
@@ -17,19 +17,22 @@
 public class TeamPropDetector implements VisionProcessor {
     public static final int SATURATION_THRESHOLD = 90;
     //makes "detection zones" for each tape zone
-    TeamPropLocation location = TeamPropLocation.NOT_DETECTED;
+
     public Rect leftTapeDetectionZone = new Rect(20,100,75,75);
     public Rect middleTapeDetectionZone = new Rect(200,180,75,75);
     public Rect rightTapeDetectionZone = new Rect(550,160,75,75);
     public double middleSaturation;
     public double rightSaturation;
+    TeamPropLocation location;
 
     //submats are smaller portions of the frame that you can get values from
     Mat submat = new Mat();
     Mat hsvMat = new Mat();
 
     @Override
     public void init(int width, int height, CameraCalibration calibration) {
+        location = TeamPropLocation.NOT_DETECTED;
+
 
     }