Camera, Autos, Autoalign etc

TeamCode/src/main/java/opmodes/AutoBase.java

@@ -35,13 +35,24 @@ public abstract class AutoBase extends LinearOpMode {
     protected CenterStageCommon.PropLocation propLocation;
     protected final Pose2d initialPosition;
     protected final CenterStageCommon.Alliance alliance;
-    protected final Pose2d park;
+    protected final Pose2d parkLeft;
+    protected final Pose2d parkRight;
+
+    protected Pose2d park;
+    protected int delay = 0;
+    boolean leftWasPressed = false;
+    boolean rightWasPressed = false;
+    boolean upWasPressed = false;
+    boolean downWasPressed = false;
 
 
-    protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Pose2d park) {
+
+
+    protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Pose2d parkLeft, Pose2d parkRight) {
         this.alliance = alliance;
         this.initialPosition = initialPosition;
-        this.park = park;
+        this.parkLeft = parkLeft;
+        this.parkRight = parkRight;
     }
 
     @Override
@@ -50,11 +61,33 @@ public abstract class AutoBase extends LinearOpMode {
         this.robot = new Robot(hardwareMap, telemetry, initialPosition, alliance);
         this.dashboard = FtcDashboard.getInstance();
         this.dashboardTelemetry = dashboard.getTelemetry();
-
         // Wait for match to start
         while(!isStarted() && !isStopRequested()) {
             this.robot.update();
             this.sleep(20);
+
+            boolean leftPressed = gamepad1.dpad_left;
+            boolean rightPressed = gamepad1.dpad_right;
+            boolean upPressed = gamepad1.dpad_up;
+            boolean downPressed = gamepad1.dpad_down;
+            this.telemetry.addData("To select parking location, use the dpad right or left. To add delay, use the dpad up to increase delay, and dpad down to decrease delay", "");
+            if(leftPressed && !leftWasPressed) {
+                this.park = parkLeft;
+                this.telemetry.addData("Park set to", "Left");
+            } else if(rightPressed && !rightWasPressed) {
+                this.park = parkRight;
+                this.telemetry.addData("Park set to", "Right");
+            } else if(upPressed && !upWasPressed) {
+                this.delay += 1000;
+                this.telemetry.addData("Delay increased by", "1000");
+                this.sleep(200);
+            } else if(downPressed && !downWasPressed) {
+                this.delay -= 1000;
+                this.telemetry.addData("Delay decreased by", "1000");
+                this.sleep(200);
+            }
+            this.telemetry.addData("Delay", this.delay);
+            this.telemetry.addData("Park set to", this.park);
         }
         if (isStopRequested()) {  // remove later if nessacary as recent addition might be interefering
             return;
@@ -75,6 +108,7 @@ public abstract class AutoBase extends LinearOpMode {
                 propRight();
                 break;
         }
+        this.sleep(delay);
         moveToEasel();
         prepareToScore();
         scorePreloadedPixel();
@@ -130,6 +164,7 @@ public abstract class AutoBase extends LinearOpMode {
         }
 
         Pose2d inferredPos = this.robot.getCamera().estimatePoseFromAprilTag();
+//        inferredPos = inferredPos != null ? inferredPos : robot.getDrive().getPoseEstimate(); // new code (maybe get rid of)
         this.robot.getDrive().setPoseEstimate(inferredPos);
         Pose2d target = new Pose2d(
                 60 - SCORING_DISTANCE_FROM_APRIL_TAG - CAMERA_FORWARD_OFFSET_IN, // 60 is the X position of the april tag
@@ -164,7 +199,7 @@ public abstract class AutoBase extends LinearOpMode {
     }
 
     protected void determinePropLocation() {
-        this.robot.getClaw().setArmPositionAsync(PICKUP_ARM_MIN);
+        this.robot.getClaw().setArmPositionAsync(PICKUP_ARM_MIN); // changed from setArmPositionAsync
 
         while (!this.robot.getClaw().isArmAtPosition()) {
             this.robot.update();
@@ -173,7 +208,7 @@ public abstract class AutoBase extends LinearOpMode {
 
         sleep(250);
 
-        setPropLocationIfVisible(Center, Unknown);
+        setPropLocationIfVisible(Center, Unknown); //only works if arm is async
         if (this.propLocation != Center) {
             peekRight();
         }

TeamCode/src/main/java/opmodes/AutoSandbox.java

@@ -1,73 +1,73 @@
-package opmodes;
+//package opmodes;
-
+//
-import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN;
+//import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN;
-
+//
-import com.acmerobotics.roadrunner.geometry.Pose2d;
+//import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.geometry.Vector2d;
+//import com.acmerobotics.roadrunner.geometry.Vector2d;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+//import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-
+//
-import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+//import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
-import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
+//import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
-
+//
-@Autonomous(name = "AutoSandbox", preselectTeleOp = "MainTeleOp")
+//@Autonomous(name = "AutoSandbox", preselectTeleOp = "MainTeleOp")
-public class AutoSandbox extends AutoBase {
+//public class AutoSandbox extends AutoBase {
-    private final Pose2d rendezvous = new Pose2d(12, 48, Math.toRadians(-89));
+//    private final Pose2d rendezvous = new Pose2d(12, 48, Math.toRadians(-89));
-
+//
-    public AutoSandbox() {
+//    public AutoSandbox() {
-        super(
+//        super(
-                CenterStageCommon.Alliance.Blue,
+//                CenterStageCommon.Alliance.Blue,
-                new Pose2d(12, 63, Math.toRadians(-90)),
+//                new Pose2d(12, 63, Math.toRadians(-90)),
-                new Pose2d(62, 62));
+//                new Pose2d(62, 62));
-    }
+//    }
-
+//
-    protected void propLeft() {
+//    protected void propLeft() {
-        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
-        builder.lineToLinearHeading(new Pose2d(36, 25, Math.toRadians(-33)));
-        builder.addDisplacementMarker(10, () -> {
-            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
-        });
-        this.robot.getDrive().followTrajectorySequence(builder.build());
-
-        openAndLiftClaw();
-    }
-
-    protected void propCenter() {
-
-        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
-        builder.lineToLinearHeading(new Pose2d(12,48, Math.toRadians(90)));
-        builder.addDisplacementMarker(10, () -> {
-            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
-        });
-        builder.lineToConstantHeading(new Vector2d(12,12));
-
-        this.robot.getDrive().followTrajectorySequence(builder.build());
-
-        openAndLiftClaw();
-
-//        builder = this.robot.getTrajectorySequenceBuilder();
-//        builder.turn((Math.toRadians(90)));
-//        this.robot.getDrive().followTrajectorySequence(builder.build());
-
-
 //        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
-//        builder.lineToConstantHeading(rendezvous.vec());
+//        builder.lineToLinearHeading(new Pose2d(36, 25, Math.toRadians(-33)));
 //        builder.addDisplacementMarker(10, () -> {
 //            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
 //        });
 //        this.robot.getDrive().followTrajectorySequence(builder.build());
 //
 //        openAndLiftClaw();
-    }
+//    }
-
+//
-    protected void propRight() {
+//    protected void propCenter() {
-        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+//
-        builder.lineToLinearHeading(new Pose2d(32, 34, Math.toRadians(0)));
+//        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
-        builder.lineToConstantHeading(new Vector2d(19, 34));
+//        builder.lineToLinearHeading(new Pose2d(12,48, Math.toRadians(90)));
-        builder.addTemporalMarker(0.5, () -> {
+//        builder.addDisplacementMarker(10, () -> {
-            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
+//            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
-        });
+//        });
-        this.robot.getDrive().followTrajectorySequence(builder.build());
+//        builder.lineToConstantHeading(new Vector2d(12,12));
-
+//
-        openAndLiftClaw();
+//        this.robot.getDrive().followTrajectorySequence(builder.build());
-    }
+//
-}
+//        openAndLiftClaw();
+//
+////        builder = this.robot.getTrajectorySequenceBuilder();
+////        builder.turn((Math.toRadians(90)));
+////        this.robot.getDrive().followTrajectorySequence(builder.build());
+//
+//
+////        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+////        builder.lineToConstantHeading(rendezvous.vec());
+////        builder.addDisplacementMarker(10, () -> {
+////            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
+////        });
+////        this.robot.getDrive().followTrajectorySequence(builder.build());
+////
+////        openAndLiftClaw();
+//    }
+//
+//    protected void propRight() {
+//        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+//        builder.lineToLinearHeading(new Pose2d(32, 34, Math.toRadians(0)));
+//        builder.lineToConstantHeading(new Vector2d(19, 34));
+//        builder.addTemporalMarker(0.5, () -> {
+//            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
+//        });
+//        this.robot.getDrive().followTrajectorySequence(builder.build());
+//
+//        openAndLiftClaw();
+//    }
+//}

TeamCode/src/main/java/opmodes/BlueBackStage.java

@@ -11,17 +11,18 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
 
 @Autonomous(name = "BlueBackStage", preselectTeleOp = "MainTeleOp")
 public class BlueBackStage extends AutoBase {
-    private static final int delay = 0;
 
     public BlueBackStage() {
         super(
                 CenterStageCommon.Alliance.Blue,
                 new Pose2d(12, 63, Math.toRadians(90)),
-                new Pose2d(62, 62));
+                new Pose2d(62, 62),
+                new Pose2d(62, 12, Math.toRadians(0)));
+
+
     }
 
     protected void propLeft() {
-        this.sleep(delay);
 
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
         builder.lineToLinearHeading(new Pose2d(17.3, 41.6, Math.toRadians(108.25)));
@@ -35,7 +36,6 @@ public class BlueBackStage extends AutoBase {
     }
 
     protected void propCenter() {
-        this.sleep(delay);
 
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
         builder.lineToLinearHeading(new Pose2d(12, 42, initialPosition.getHeading()));
@@ -45,7 +45,6 @@ public class BlueBackStage extends AutoBase {
     }
 
     protected void propRight() {
-        this.sleep(delay);
 
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
         builder.lineToLinearHeading(new Pose2d(18.25, 32, Math.toRadians(0)));

TeamCode/src/main/java/opmodes/BlueFrontStage.java

@@ -11,17 +11,17 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
 
 @Autonomous(name = "BlueFrontStage", preselectTeleOp = "MainTeleOp")
 public class BlueFrontStage extends AutoBase {
-    private static final int delay = 0;
 
     public BlueFrontStage() {
         super(
                 CenterStageCommon.Alliance.Blue,
                 new Pose2d(-36, 63, Math.toRadians(90)),
+                new Pose2d(62, 62),
                 new Pose2d(62, 12, Math.toRadians(0)));
+
     }
 
     protected void propLeft() {
-        this.sleep(delay);
 
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
         builder.lineToLinearHeading(new Pose2d(-43.5, 31.5, Math.toRadians(180)));
@@ -31,7 +31,6 @@ public class BlueFrontStage extends AutoBase {
     }
 
     protected void propCenter() {
-        this.sleep(delay);
 
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
         builder.lineToLinearHeading(new Pose2d(-36, 42, initialPosition.getHeading()));
@@ -41,7 +40,6 @@ public class BlueFrontStage extends AutoBase {
     }
 
     protected void propRight() {
-        this.sleep(delay);
 
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
         builder.lineToLinearHeading(new Pose2d(-39.34, 40.45, Math.toRadians(64.3)));

TeamCode/src/main/java/opmodes/MainTeleOp.java

@@ -3,6 +3,7 @@ package opmodes;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_DELTA;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CAMERA_FORWARD_OFFSET_IN;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DRONE_LAUNCH_PAUSE_S;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_ARM_MIN;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MAX;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SCORING_DISTANCE_FROM_APRIL_TAG;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CAMERA_SIDE_OFFSET_IN;
@@ -39,6 +40,7 @@ public class MainTeleOp extends OpMode {
     private int targetTagId;
     private double droneLaunchStart;
     private boolean wasRobotDroneLaunch;
+    private double gantryArmPosition = 0;
 
     @Override
     public void init() {

TeamCode/src/main/java/opmodes/RedBackStage.java

@@ -11,17 +11,16 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
 
 @Autonomous(name = "RedBackStage", preselectTeleOp = "MainTeleOp")
 public class RedBackStage extends AutoBase {
-    private static final int delay = 0;
 
     public RedBackStage() {
         super(
                 CenterStageCommon.Alliance.Red,
                 new Pose2d(12, -63, Math.toRadians(-90)),
+                new Pose2d(61, -12),
                 new Pose2d(62, -62));
     }
 
     protected void propLeft() {
-        this.sleep(delay);
 
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
         builder.lineToLinearHeading(new Pose2d(18.25, -33.5, Math.toRadians(0)));
@@ -31,7 +30,6 @@ public class RedBackStage extends AutoBase {
     }
 
     protected void propCenter() {
-        this.sleep(delay);
 
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
         builder.lineToLinearHeading(new Pose2d(12, -42, initialPosition.getHeading()));
@@ -41,7 +39,6 @@ public class RedBackStage extends AutoBase {
     }
 
     protected void propRight() {
-        this.sleep(delay);
 
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
         builder.lineToLinearHeading(new Pose2d(15.9, -41.35, Math.toRadians(244.15)));

TeamCode/src/main/java/opmodes/RedFrontStage.java

@@ -11,17 +11,17 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
 
 @Autonomous(name = "RedFrontStage", preselectTeleOp = "MainTeleOp")
 public class RedFrontStage extends AutoBase {
-    private static final int delay = 0;
 
     public RedFrontStage() {
         super(
                 CenterStageCommon.Alliance.Red,
                 new Pose2d(-36, -63, Math.toRadians(-90)),
-                new Pose2d(61, -12));
+                new Pose2d(61, -12),
+                new Pose2d(62, -62));
+
     }
 
     protected void propLeft() {
-        this.sleep(delay);
 
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
         builder.lineToLinearHeading(new Pose2d(-40.46, -41.93, Math.toRadians(291.8)));
@@ -31,7 +31,6 @@ public class RedFrontStage extends AutoBase {
     }
 
     protected void propCenter() {
-        this.sleep(delay);
 
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
         builder.lineToLinearHeading(new Pose2d(-36, -42, initialPosition.getHeading()));
@@ -41,7 +40,6 @@ public class RedFrontStage extends AutoBase {
     }
 
     protected void propRight() {
-        this.sleep(delay);
 
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
         builder.lineToLinearHeading(new Pose2d(-41.82, -33.68, Math.toRadians(180)));

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java

@@ -32,16 +32,16 @@ public class RobotConfig {
     public static double AUTO_STRAFE_SLOWDOWN = 4;
 
     // Arm
-    public static double PICKUP_ARM_MIN = 0.175;
+    public static double PICKUP_ARM_MIN = .19;
-    public static double PICKUP_ARM_MAX = 0.760;
+    public static double PICKUP_ARM_MAX = 0.76; // Changed .760 --> .74 Claw arm go up
-    public static double CLAW_MIN = 0.89;
+    public static double CLAW_MIN = 0.91; // Changed .89 --> .88 Probably the Claw Clamp
-    public static double CLAW_MAX = 0.65;
+    public static double CLAW_MAX = 0.65; // Claw release
     public static double CLAW_ARM_DELTA = 0.03;
     public static double CLAW_KP = 0.3;
     public static double CLAW_ARM_KP = 0.15;
 
     // Gantry
-    public static double GANTRY_ARM_MIN = 0.42;
+    public static double GANTRY_ARM_MIN = 0.43; // Changed 0.42 --> 0.45 pickup position
     public static double GANTRY_ARM_MAX = 0.96;
     public static int GANTRY_LIFT_DELTA = 50;
     public static double GANTRY_ARM_KP = 0.1;
@@ -49,7 +49,7 @@ public class RobotConfig {
     public static double X_MAX = 0.86;
     public static double X_MIN = 0.16;
     public static double X_CENTER = 0.54;
-    public static double GANTRY_ARM_DELTA_MAX = 0.013;
+    public static double GANTRY_ARM_DELTA_MAX = 0.015;
     public static int SLIDE_UP = 820;
     public static double SLIDE_POWER = 0.25;