From a036de64abc216f71f8f75f73fbc4b4d52709353 Mon Sep 17 00:00:00 2001 From: Thomas <@TOMMY> Date: Thu, 8 Feb 2024 16:46:12 -0600 Subject: [PATCH] Camera, Autos, Autoalign etc --- TeamCode/src/main/java/opmodes/AutoBase.java | 47 ++++++- .../src/main/java/opmodes/AutoSandbox.java | 132 +++++++++--------- .../src/main/java/opmodes/BlueBackStage.java | 9 +- .../src/main/java/opmodes/BlueFrontStage.java | 6 +- .../src/main/java/opmodes/MainTeleOp.java | 2 + .../src/main/java/opmodes/RedBackStage.java | 5 +- .../src/main/java/opmodes/RedFrontStage.java | 8 +- .../ftc/teamcode/hardware/RobotConfig.java | 12 +- 8 files changed, 125 insertions(+), 96 deletions(-) diff --git a/TeamCode/src/main/java/opmodes/AutoBase.java b/TeamCode/src/main/java/opmodes/AutoBase.java index a2d28dd..61c1688 100644 --- a/TeamCode/src/main/java/opmodes/AutoBase.java +++ b/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(); } diff --git a/TeamCode/src/main/java/opmodes/AutoSandbox.java b/TeamCode/src/main/java/opmodes/AutoSandbox.java index d134e24..5aa892a 100644 --- a/TeamCode/src/main/java/opmodes/AutoSandbox.java +++ b/TeamCode/src/main/java/opmodes/AutoSandbox.java @@ -1,73 +1,73 @@ -package opmodes; - -import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN; - -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.geometry.Vector2d; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; - -import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; -import org.firstinspires.ftc.teamcode.util.CenterStageCommon; - -@Autonomous(name = "AutoSandbox", preselectTeleOp = "MainTeleOp") -public class AutoSandbox extends AutoBase { - private final Pose2d rendezvous = new Pose2d(12, 48, Math.toRadians(-89)); - - public AutoSandbox() { - super( - CenterStageCommon.Alliance.Blue, - new Pose2d(12, 63, Math.toRadians(-90)), - new Pose2d(62, 62)); - } - - 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()); - - +//package opmodes; +// +//import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN; +// +//import com.acmerobotics.roadrunner.geometry.Pose2d; +//import com.acmerobotics.roadrunner.geometry.Vector2d; +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +// +//import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; +//import org.firstinspires.ftc.teamcode.util.CenterStageCommon; +// +//@Autonomous(name = "AutoSandbox", preselectTeleOp = "MainTeleOp") +//public class AutoSandbox extends AutoBase { +// private final Pose2d rendezvous = new Pose2d(12, 48, Math.toRadians(-89)); +// +// public AutoSandbox() { +// super( +// CenterStageCommon.Alliance.Blue, +// new Pose2d(12, 63, Math.toRadians(-90)), +// new Pose2d(62, 62)); +// } +// +// protected void propLeft() { // 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() { - 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(); - } -} \ No newline at end of file +// } +// +// 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.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(); +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/java/opmodes/BlueBackStage.java b/TeamCode/src/main/java/opmodes/BlueBackStage.java index 8470e63..64900ba 100644 --- a/TeamCode/src/main/java/opmodes/BlueBackStage.java +++ b/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))); diff --git a/TeamCode/src/main/java/opmodes/BlueFrontStage.java b/TeamCode/src/main/java/opmodes/BlueFrontStage.java index 9f5cf70..7a4bc9f 100644 --- a/TeamCode/src/main/java/opmodes/BlueFrontStage.java +++ b/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))); diff --git a/TeamCode/src/main/java/opmodes/MainTeleOp.java b/TeamCode/src/main/java/opmodes/MainTeleOp.java index 96ca9e1..296267c 100644 --- a/TeamCode/src/main/java/opmodes/MainTeleOp.java +++ b/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() { diff --git a/TeamCode/src/main/java/opmodes/RedBackStage.java b/TeamCode/src/main/java/opmodes/RedBackStage.java index 12a3f20..eafe38a 100644 --- a/TeamCode/src/main/java/opmodes/RedBackStage.java +++ b/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))); diff --git a/TeamCode/src/main/java/opmodes/RedFrontStage.java b/TeamCode/src/main/java/opmodes/RedFrontStage.java index 6584acb..5579422 100644 --- a/TeamCode/src/main/java/opmodes/RedFrontStage.java +++ b/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))); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java index 4c85a12..acc967b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java +++ b/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_MAX = 0.760; - public static double CLAW_MIN = 0.89; - public static double CLAW_MAX = 0.65; + public static double PICKUP_ARM_MIN = .19; + public static double PICKUP_ARM_MAX = 0.76; // Changed .760 --> .74 Claw arm go up + public static double CLAW_MIN = 0.91; // Changed .89 --> .88 Probably the Claw Clamp + 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;