From 970045d1aa9271bd170a086f05b9eb8edcd876e1 Mon Sep 17 00:00:00 2001 From: JJTech0130 Date: Sat, 6 Jan 2024 11:05:14 -0500 Subject: [PATCH] add purple pixel, start adapting OpMode for Psi --- .../ftc/teamcode/drive/PsiBot.java | 57 ++++++++++++------- .../ftc/teamcode/drive/Robot.java | 18 +++--- .../ftc/teamcode/drive/RoboticaBot.java | 12 ++++ .../ftc/teamcode/opmode/PsiAuto.java | 39 ++++++------- 4 files changed, 75 insertions(+), 51 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/PsiBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/PsiBot.java index 79eaf982..24639b4d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/PsiBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/PsiBot.java @@ -1,10 +1,21 @@ package org.firstinspires.ftc.teamcode.drive; +import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.control.PIDCoefficients; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.PIDFCoefficients; - -public class PsiBot extends Robot{ +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.teamcode.vision.AprilTagCamera; + +@Config +public class PsiBot extends Robot { + public static double LEFTFORTYFIVE = -45; + public static double LEFTSEVENTY = 70; + public static double RIGHTFORTYFIVE = 45; + public static double RIGHTSEVENTY = -70; + private final AprilTagCamera[] cameras; public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(8, 0, 0); public static PIDCoefficients HEADING_PID = new PIDCoefficients(8, 0, 0); private final TrajectoryDrive drive; @@ -29,8 +40,18 @@ public static double getMotorVelocityF(double ticksPerSecond) { public static double kV = 0.014129716300132542; public static double kA = 0.0032; public static double kStatic = 0; - protected PsiBot(HardwareMap hardwareMap) { + + private final Servo purpleServo; + + public PsiBot(HardwareMap hardwareMap) { super(hardwareMap); + cameras = new AprilTagCamera[3]; + cameras[0] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Left"), 8, Math.toRadians(LEFTSEVENTY), Math.toRadians(LEFTFORTYFIVE)); + cameras[1] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Center"), 7, Math.toRadians(90), Math.toRadians(0)); + cameras[2] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Right"), 8, Math.toRadians(RIGHTSEVENTY), Math.toRadians(RIGHTFORTYFIVE)); + + purpleServo = hardwareMap.get(Servo.class, "purple"); + // TODO: Reverse Motors, encoders & such drive = new TrajectoryDrive( hardwareMap, @@ -52,31 +73,25 @@ protected PsiBot(HardwareMap hardwareMap) { kA, kV, kStatic - - - - - - - - - - - ); - - } + @Override + public AprilTagCamera[] getCameras() { + return cameras; + } - - + @Override + public void dropPurplePixel(boolean state) { + if (state) { + purpleServo.setPosition(0.3); + } else { + purpleServo.setPosition(0.5); + } + } @Override public TrajectoryDrive getDrive() { return drive; } - - - } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/Robot.java index e1df73ac..e23f1224 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/Robot.java @@ -1,21 +1,21 @@ package org.firstinspires.ftc.teamcode.drive; -import com.acmerobotics.roadrunner.drive.MecanumDrive; -import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.HardwareMap; -import org.firstinspires.ftc.teamcode.drive.psi.PsiDrive; -import org.firstinspires.ftc.teamcode.drive.testbot.TestBotDrive; +import org.firstinspires.ftc.teamcode.vision.AprilTagCamera; public abstract class Robot { - public abstract TrajectoryDrive getDrive(); protected Robot(HardwareMap hardwareMap) { - - - + // Any shared initialization goes here } + public abstract AprilTagCamera[] getCameras(); - + /** + * Sets the purple pixel on the robot; + * true: dropped; + * false: closed + */ + public abstract void dropPurplePixel(boolean state); } 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 ceff8a95..dca03b77 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 @@ -2,12 +2,24 @@ import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.teamcode.vision.AprilTagCamera; + public class RoboticaBot extends Robot { protected RoboticaBot(HardwareMap hardwareMap) { super(hardwareMap); } + @Override + public AprilTagCamera[] getCameras() { + return new AprilTagCamera[0]; + } + + @Override + public void dropPurplePixel(boolean state) { + // TODO + } + @Override public TrajectoryDrive getDrive() { return null; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/PsiAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/PsiAuto.java index 1051bb01..5394ab8e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/PsiAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/PsiAuto.java @@ -9,8 +9,8 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.Servo; -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.teamcode.drive.psi.PsiDrive; +import org.firstinspires.ftc.teamcode.drive.PsiBot; +import org.firstinspires.ftc.teamcode.drive.TrajectoryDrive; import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; import org.firstinspires.ftc.teamcode.util.GlobalOpMode; import org.firstinspires.ftc.teamcode.util.Timeout; @@ -18,8 +18,6 @@ import org.firstinspires.ftc.teamcode.vision.AprilTagLocalizer; import org.firstinspires.ftc.teamcode.vision.TensorFlowDetection; -import java.util.Timer; - @Autonomous(name = "Psi Auto", group = "A") @Config public class PsiAuto extends LinearOpMode { @@ -39,25 +37,18 @@ private Pose2d estimateWithAllCameras(AprilTagCamera[] cameras, AprilTagLocalize return pose; } - - public static double LEFTFORTYFIVE = -45; - public static double LEFTSEVENTY = 70; - public static double RIGHTFORTYFIVE = 45; - public static double RIGHTSEVENTY = -70; Servo droneServo; @Override public void runOpMode() throws InterruptedException { GlobalOpMode.opMode = this; - PsiDrive drive = new PsiDrive(hardwareMap); + PsiBot bot = new PsiBot(hardwareMap); + TrajectoryDrive drive = bot.getDrive(); + //PsiDrive drive = new PsiDrive(hardwareMap); - AprilTagCamera[] cameras = new AprilTagCamera[3]; - cameras[0] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Left"), 8, Math.toRadians(LEFTSEVENTY), Math.toRadians(LEFTFORTYFIVE)); - cameras[1] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Center"), 7, Math.toRadians(90), Math.toRadians(0)); - cameras[2] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Right"), 8, Math.toRadians(RIGHTSEVENTY), Math.toRadians(RIGHTFORTYFIVE)); + AprilTagCamera[] cameras = bot.getCameras(); - Servo purpleServo = hardwareMap.get(Servo.class, "purple"); // Search for AprilTags across the three cameras, until we find one or init ends AprilTagLocalizer aprilTag = new AprilTagLocalizer(cameras); @@ -126,7 +117,8 @@ public void runOpMode() throws InterruptedException { .turn(Math.toRadians(90)) .addTemporalMarker(() ->{ android.util.Log.i("DROP", "Drop"); - purpleServo.setPosition(.3); + bot.dropPurplePixel(true); + //purpleServo.setPosition(.3); }) @@ -154,7 +146,8 @@ public void runOpMode() throws InterruptedException { .addTemporalMarker(()->{ //PIXEL DROP Log.i("DROP", "dropping purple"); - purpleServo.setPosition(.3); + bot.dropPurplePixel(true); + //purpleServo.setPosition(.3); }) .waitSeconds(1.0) // Back up to the BLUE PARK Y coord @@ -213,7 +206,8 @@ public void runOpMode() throws InterruptedException { .lineTo(new Vector2d(startPose.getX(), -2*12 - 3)) .addTemporalMarker(() -> { Log.i("DROP", "dropping purple"); - purpleServo.setPosition(.3); + bot.dropPurplePixel(true); + //purpleServo.setPosition(.3); }) .waitSeconds(1) .lineTo(new Vector2d(startPose.getX(), -5 * 12+3)) @@ -286,7 +280,8 @@ public void runOpMode() throws InterruptedException { .addTemporalMarker(()->{ //PIXEL DROP Log.i("DROP", "dropping purple"); - purpleServo.setPosition(.3); + bot.dropPurplePixel(true); + //purpleServo.setPosition(.3); }) .waitSeconds(1.0) // Back up to the BLUE PARK Y coord @@ -306,7 +301,8 @@ public void runOpMode() throws InterruptedException { .addTemporalMarker(()->{ //PIXEL DROP Log.i("DROP", "dropping purple"); - purpleServo.setPosition(.3); + bot.dropPurplePixel(true); + //purpleServo.setPosition(.3); }) .lineTo(new Vector2d(3*12+7, 3*12-2)) .build(); @@ -319,7 +315,8 @@ public void runOpMode() throws InterruptedException { .addTemporalMarker(()->{ //PIXEL DROP Log.i("DROP", "dropping purple"); - purpleServo.setPosition(.3); + bot.dropPurplePixel(true); + //purpleServo.setPosition(.3); }) .waitSeconds(1.0) // Back up to the BLUE PARK Y coord