diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java deleted file mode 100644 index 9261fa1..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java +++ /dev/null @@ -1,55 +0,0 @@ -package org.firstinspires.ftc.teamcode.hardware; - -import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_DETECTION; -import static org.firstinspires.ftc.teamcode.util.Constants.TARGETING_WEBCAM; -import static org.firstinspires.ftc.teamcode.util.Constants.WEBCAM_HEIGHT; -import static org.firstinspires.ftc.teamcode.util.Constants.WEBCAM_ROTATION; -import static org.firstinspires.ftc.teamcode.util.Constants.WEBCAM_WIDTH; - -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.teamcode.vision.Detection; -import org.firstinspires.ftc.teamcode.vision.TargetingPipeline; -import org.openftc.easyopencv.OpenCvCamera; -import org.openftc.easyopencv.OpenCvCameraFactory; - -public class Camera { - private final HardwareMap hardwareMap; - private OpenCvCamera targetingCamera; - private TargetingPipeline targetingPipeline; - private boolean targetingCameraInitialized; - - public Camera(HardwareMap hardwareMap) { - this.hardwareMap = hardwareMap; - } - - public void initTargetingCamera() { - int targetingCameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); - this.targetingCamera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, TARGETING_WEBCAM), targetingCameraMonitorViewId); - this.targetingPipeline = new TargetingPipeline(); - targetingCamera.setPipeline(targetingPipeline); - targetingCamera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { - @Override - public void onOpened() { - targetingCamera.startStreaming(WEBCAM_WIDTH, WEBCAM_HEIGHT, WEBCAM_ROTATION); - targetingCameraInitialized = true; - } - - @Override - public void onError(int errorCode) { - - } - }); - } - - public void stopTargetingCamera() { - if (targetingCameraInitialized) { - targetingCamera.closeCameraDeviceAsync(() -> targetingCameraInitialized = false); - } - } - - public Detection getRed() { - return (targetingCameraInitialized ? targetingPipeline.getRed() : INVALID_DETECTION); - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java index d673664..89cbb8c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java @@ -6,7 +6,6 @@ import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_FRONT_LEFT; import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_FRONT_RIGHT; import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareMap; public class Drive { @@ -54,12 +53,4 @@ public class Drive { backLeft.setPower(blPower); backRight.setPower(brPower); } - - public void setInput(Gamepad gamepad1, Gamepad gamepad2) { - double x = gamepad1.left_stick_x; - double y = -gamepad1.left_stick_y; - double z = gamepad1.right_stick_x; - - setInput(x, y, z); - } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java index d96a838..8ab6567 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java @@ -1,11 +1,23 @@ package org.firstinspires.ftc.teamcode.hardware; +import static org.firstinspires.ftc.teamcode.util.Configurables.ARMDOWN; +import static org.firstinspires.ftc.teamcode.util.Configurables.ARMUP; +import static org.firstinspires.ftc.teamcode.util.Configurables.BIGOPEN; +import static org.firstinspires.ftc.teamcode.util.Configurables.CLOSE; import static org.firstinspires.ftc.teamcode.util.Configurables.LOCK; import static org.firstinspires.ftc.teamcode.util.Configurables.LOCKSPEED; +import static org.firstinspires.ftc.teamcode.util.Configurables.OPEN; +import static org.firstinspires.ftc.teamcode.util.Configurables.PICKUP; import static org.firstinspires.ftc.teamcode.util.Configurables.UNLOCK; import static org.firstinspires.ftc.teamcode.util.Configurables.UNLOCKSPEED; -import static org.firstinspires.ftc.teamcode.util.Constants.LEFT; -import static org.firstinspires.ftc.teamcode.util.Constants.RIGHT; +import static org.firstinspires.ftc.teamcode.util.Configurables.WRISTPICKUP; +import static org.firstinspires.ftc.teamcode.util.Configurables.WRISTSCORE; +import static org.firstinspires.ftc.teamcode.util.Constants.CLAW; +import static org.firstinspires.ftc.teamcode.util.Constants.LEFTARM; +import static org.firstinspires.ftc.teamcode.util.Constants.LEFTHANG; +import static org.firstinspires.ftc.teamcode.util.Constants.RIGHTARM; +import static org.firstinspires.ftc.teamcode.util.Constants.RIGHTHANG; +import static org.firstinspires.ftc.teamcode.util.Constants.WRIST; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; @@ -14,25 +26,29 @@ import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; +import lombok.Getter; + public class Robot { + @Getter private MecanumDrive drive; - private Hang hang; - private Intake intake; +// private Hang hang; + @Getter private Intake intake; + @Getter private Arm arm; + @Getter private Wrist wrist; + @Getter private Claw claw; + @Getter private Hang hang; public Robot init(HardwareMap hardwareMap) { this.drive = new MecanumDrive(hardwareMap); this.hang = new Hang().init(hardwareMap); - + this.intake = new Intake().init(hardwareMap); + this.arm = new Arm().init(hardwareMap); + this.wrist = new Wrist().init(hardwareMap); + this.claw = new Claw().init(hardwareMap); return this; } - public Intake getIntake() {return this.intake;} - - public Hang getHang() {return this.hang;} - - public MecanumDrive getDrive() {return this.drive;} - - public static class Intake { + public static class Intake{ private DcMotor intake = null; public Intake init(HardwareMap hardwareMap) { @@ -45,6 +61,9 @@ public class Robot { public void spinIn() {this.intake.setPower(0.7);} public void spinOut() {this.intake.setPower(-0.7);} + + public void stop() {this.intake.setPower(0);} + } public static class Hang { @@ -52,8 +71,10 @@ public class Robot { private Servo hangRight; public Hang init(HardwareMap hardwareMap) { - this.hangLeft = hardwareMap.get(Servo.class, LEFT); - this.hangRight = hardwareMap.get(Servo.class, RIGHT); + this.hangLeft = hardwareMap.get(Servo.class, LEFTHANG); + this.hangRight = hardwareMap.get(Servo.class, RIGHTHANG); + this.hangLeft.setPosition(LOCKSPEED); + this.hangRight.setPosition(LOCK); return this; } @@ -68,4 +89,74 @@ public class Robot { } } + public static class Arm { + private Servo leftArm; + private Servo rightArm; + + + public Arm init(HardwareMap hardwareMap){ + this.leftArm = hardwareMap.get(Servo.class, LEFTARM); + this.rightArm = hardwareMap.get(Servo.class, RIGHTARM); + this.rightArm.setDirection(Servo.Direction.REVERSE); + this.rightArm.setPosition(ARMUP); + this.leftArm.setPosition(ARMUP); + return this; + } + + public void pickup() { + this.leftArm.setPosition(PICKUP); + this.rightArm.setPosition(PICKUP); + } + + public void rest() { + this.leftArm.setPosition(ARMDOWN); + this.rightArm.setPosition(ARMDOWN); + } + + public void scoreArm() { + this.leftArm.setPosition(ARMUP); + this.rightArm.setPosition(ARMUP); + } + } + + public static class Wrist { + private Servo wrist; + + public Wrist init(HardwareMap hardwareMap){ + this.wrist = hardwareMap.get(Servo.class, WRIST); + this.wrist.setPosition(WRISTPICKUP); + return this; + } + + public void wristPickup() { + this.wrist.setPosition(WRISTPICKUP); + } + + public void wristScore() { + this.wrist.setPosition(WRISTSCORE); + } + } + + public static class Claw { + private Servo claw; + + public Claw init (HardwareMap hardwareMap) { + this.claw = hardwareMap.get(Servo.class, CLAW); + return this; + } + + public void close() { + this.claw.setPosition(CLOSE); + } + + public void open() { + this.claw.setPosition(OPEN); + } + + public void openScore() { + this.claw.setPosition(BIGOPEN); + } + + } + } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java index f029065..b1d495e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java @@ -21,8 +21,8 @@ public class DriveConstants { /* * These are motor constants that should be listed online for your motors. */ - public static final double TICKS_PER_REV = 1; - public static final double MAX_RPM = 1; + public static final double TICKS_PER_REV = 384.5; + public static final double MAX_RPM = 435; /* * Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders. @@ -44,10 +44,13 @@ public class DriveConstants { * angular distances although most angular parameters are wrapped in Math.toRadians() for * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO. */ - public static double WHEEL_RADIUS = 2; // in + public static double WHEEL_RADIUS = 1.88976; // in public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed - public static double TRACK_WIDTH = 1; // in + public static double TRACK_WIDTH = 12.244; // in + // public static double WHEEL_RADIUS = 1.88976; // in +// public static double GEAR_RATIO = 652.5/435; // output (wheel) speed / input (motor) speed +// public static double TRACK_WIDTH = 13; // in /* * These are the feedforward parameters used to model the drive motor behavior. If you are using * the built-in velocity PID, *these values are fine as is*. However, if you do not have drive @@ -65,10 +68,10 @@ public class DriveConstants { * small and gradually increase them later after everything is working. All distance units are * inches. */ - public static double MAX_VEL = 30; - public static double MAX_ACCEL = 30; - public static double MAX_ANG_VEL = Math.toRadians(60); - public static double MAX_ANG_ACCEL = Math.toRadians(60); + public static double MAX_VEL = 60; + public static double MAX_ACCEL = 60; + public static double MAX_ANG_VEL = Math.toRadians(90); + public static double MAX_ANG_ACCEL = Math.toRadians(90); /* * Adjust the orientations here to match your robot. See the FTC SDK documentation for details. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java index cce0932..944229c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java @@ -11,6 +11,8 @@ import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveCons import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kA; import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kStatic; import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kV; +import static org.firstinspires.ftc.teamcode.util.Configurables.SPEED; +import static org.firstinspires.ftc.teamcode.util.Configurables.TURN; import androidx.annotation.NonNull; @@ -56,8 +58,8 @@ import java.util.List; */ @Config public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive { - public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0); - public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0); + public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(25, 0, 2.5); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(15, 0, 1); public static double LATERAL_MULTIPLIER = 1; @@ -105,6 +107,14 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + this.leftFront.setDirection(DcMotor.Direction.REVERSE); + this.rightFront.setDirection(DcMotor.Direction.FORWARD); + this.leftRear.setDirection(DcMotor.Direction.REVERSE); + this.rightRear.setDirection(DcMotor.Direction.FORWARD); + this.leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + this.rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + this.leftRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + this.rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront); @@ -315,9 +325,9 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); this.setWeightedDrivePower( new Pose2d( - gamepad1.left_stick_y* -1, - gamepad1.left_stick_x*-1, - -gamepad1.right_stick_x + gamepad1.left_stick_y* -1 * SPEED, + gamepad1.left_stick_x*-1 * SPEED, + -gamepad1.right_stick_x * TURN )); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/StandardTrackingWheelLocalizer.java index b08629b..9702f5e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/StandardTrackingWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/StandardTrackingWheelLocalizer.java @@ -8,7 +8,7 @@ import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.HardwareMap; -import org.firstinspires.ftc.teamcode.util.Encoder; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.Encoder; import java.util.Arrays; import java.util.List; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/LogFiles.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/LogFiles.java index f0ddbbe..c538d7d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/LogFiles.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/LogFiles.java @@ -74,16 +74,6 @@ public final class LogFiles { public double mecHeadingD = MecanumDrive.HEADING_PID.kD; public double mecLateralMultiplier = MecanumDrive.LATERAL_MULTIPLIER; - public double tankAxialP = SampleTankDrive.AXIAL_PID.kP; - public double tankAxialI = SampleTankDrive.AXIAL_PID.kI; - public double tankAxialD = SampleTankDrive.AXIAL_PID.kD; - public double tankCrossTrackP = SampleTankDrive.CROSS_TRACK_PID.kP; - public double tankCrossTrackI = SampleTankDrive.CROSS_TRACK_PID.kI; - public double tankCrossTrackD = SampleTankDrive.CROSS_TRACK_PID.kD; - public double tankHeadingP = SampleTankDrive.HEADING_PID.kP; - public double tankHeadingI = SampleTankDrive.HEADING_PID.kI; - public double tankHeadingD = SampleTankDrive.HEADING_PID.kD; - public double trackingTicksPerRev = StandardTrackingWheelLocalizer.TICKS_PER_REV; public double trackingWheelRadius = StandardTrackingWheelLocalizer.WHEEL_RADIUS; public double trackingGearRatio = StandardTrackingWheelLocalizer.GEAR_RATIO; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java index 678c49b..69a8a46 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java @@ -4,17 +4,16 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.teamcode.hardware.Robot; import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; +import org.firstinspires.ftc.teamcode.util.Configurables; -@com.qualcomm.robotcore.eventloop.opmode.TeleOp(name = "CenterStage", group = "Development") +@com.qualcomm.robotcore.eventloop.opmode.TeleOp(name = "Main TeleOp", group = "Development") public class MainTeleOp extends OpMode { private Robot robot; - private double speedMultiplier = 1.0; // Start at 100% speed private MecanumDrive drive; @Override public void init() { this.robot = new Robot().init(hardwareMap); - this.robot.getHang().lock(); } @Override @@ -22,19 +21,48 @@ public class MainTeleOp extends OpMode { //Drive robot.getDrive().setInput(gamepad1, gamepad2); //Hang - if (gamepad1.b) { + if (gamepad2.dpad_up) { this.robot.getHang().release(); } else { this.robot.getHang().lock(); } //Intake - if (gamepad1.x) { - this.robot.getIntake().spinIn(); - } else if (gamepad1.y) { - this.robot.getIntake().spinOut(); +// if (gamepad1.x) { +// this.robot.getIntake().spinIn(); +// } else if (gamepad1.y) { +// this.robot.getIntake().spinOut(); +// } else { +// this.robot.getIntake().stop(); +// } +//Arm + if (gamepad2.dpad_down) { + this.robot.getArm().pickup(); + } else if (gamepad2.dpad_right ||gamepad2.x) { + this.robot.getArm().scoreArm(); + } else if (gamepad2.dpad_left || gamepad2.a) { + this.robot.getArm().rest(); + } +//Claw + if (gamepad2.b) { + this.robot.getClaw().open(); + } else if (gamepad2.y) { + this.robot.getClaw().openScore(); + } else { + this.robot.getClaw().close(); + } +//Wrist + if (gamepad2.left_bumper || gamepad2.x) { + this.robot.getWrist().wristPickup(); + } else if (gamepad2.right_bumper || gamepad2.a) { + this.robot.getWrist().wristScore(); + } +//SLOWMO + if (gamepad1.y) { + Configurables.SPEED = .5; + Configurables.TURN = .75; + } else { + Configurables.SPEED = 1; + Configurables.TURN = 1; } - - - } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java index 350c5a3..01a9b2f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java @@ -1,25 +1,9 @@ package org.firstinspires.ftc.teamcode.util; +import com.acmerobotics.dashboard.config.Config; + +@Config public class Configurables { - // Robot Constants - public static double R_ARM_POWER = 0.2; - public static double R_ARM_SPEED = 20; - public static int R_ARM_DEFAULT_POS = 0; - public static int R_ARM_UP_POS = 221; - public static int R_ARM_ALMOST_DOWN_POS = 650; - public static int R_ARM_DOWN_POS = 750; - public static double R_CLAW_CLOSED = 0.13; - public static double R_CLAW_OPEN = 0.7; - public static double R_INTAKE_SPEED = 0.9; - public static double R_INTAKE_SHIELD_UP = 0.17;//0.05 - public static double R_INTAKE_SHIELD_DOWN = 0.68;//0.95 - public static double R_INTAKE_SHIELD_SPEED = 0.04; - public static double R_SHOOTER_GOAL_POWER = 0.66; - public static double R_SHOOTER_MID_GOAL_POWER = 0.54; - public static double R_SHOOTER_POWERSHOT_POWER = 0.57; - public static double R_PUSHER_CLOSED = 0.35; - public static double R_PUSHER_OPEN = 0.55; - public static double R_PUSHER_DELAY = 0.15; // CV Color Threshold Constants public static Color FTC_RED_LOWER = new Color(165, 80, 80); @@ -29,17 +13,22 @@ public class Configurables { public static Color FTC_WHITE_LOWER = new Color(0, 0, 40); public static Color FTC_WHITE_UPPER = new Color(180, 30, 255); - // CV Detection Constants - public static double CV_MIN_STARTERSTACK_AREA = 0; - public static double CV_MIN_STARTERSTACK_SINGLE_AREA = 0.08; - public static double CV_MIN_STARTERSTACK_QUAD_AREA = 1.3; - public static double CV_MIN_GOAL_AREA = 0; - public static double CV_MAX_GOAL_AREA = 0.3; - //Servo Positions - public static double UNLOCKSPEED = .5; - public static double LOCKSPEED = .1; - public static double UNLOCK = .5; - public static double LOCK = .1; + public static double UNLOCKSPEED = .1; + public static double LOCKSPEED = .25; + public static double UNLOCK = .6; + public static double LOCK = 0.4; + public static double ARMUP = .88; + public static double ARMDOWN = 0.15; + public static double PICKUP = .935; + public static double WRISTPICKUP = 0.28; + public static double WRISTSCORE = .96; + public static double OPEN = 0.53; + public static double BIGOPEN = 0.45; + public static double CLOSE = 0.6; + + //Drive Speed + public static double SPEED = 1; + public static double TURN = 1; } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java index 199cdf2..a2cb636 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java @@ -51,6 +51,9 @@ public class Constants { public static final String STACK_WEBCAM = "Stack Webcam"; public static final String TARGETING_WEBCAM = "Targeting Webcam"; public static final String IMU_SENSOR = "imu"; - public static final String LEFT = "leftHang"; - public static final String RIGHT = "rightHang"; + public static final String LEFTHANG = "leftHang"; + public static final String RIGHTHANG = "rightHang"; + public static final String LEFTARM = "leftArm"; + public static final String RIGHTARM = "rightArm"; + public static final String WRIST = "wrist"; } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TargetingPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TargetingPipeline.java deleted file mode 100644 index 6a7fd95..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TargetingPipeline.java +++ /dev/null @@ -1,82 +0,0 @@ -package org.firstinspires.ftc.teamcode.vision; - -import static org.firstinspires.ftc.teamcode.util.Configurables.FTC_RED_LOWER; -import static org.firstinspires.ftc.teamcode.util.Configurables.FTC_RED_UPPER; -import static org.firstinspires.ftc.teamcode.util.Configurables.CV_MAX_GOAL_AREA; -import static org.firstinspires.ftc.teamcode.util.Configurables.CV_MIN_GOAL_AREA; -import static org.firstinspires.ftc.teamcode.util.Constants.ANCHOR; -import static org.firstinspires.ftc.teamcode.util.Constants.BLUR_SIZE; -import static org.firstinspires.ftc.teamcode.util.Constants.ERODE_DILATE_ITERATIONS; -import static org.firstinspires.ftc.teamcode.util.Constants.RED; -import static org.firstinspires.ftc.teamcode.util.Constants.STRUCTURING_ELEMENT; -import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getLargestContour; - -import org.opencv.core.Core; -import org.opencv.core.Mat; -import org.opencv.core.MatOfPoint; -import org.opencv.core.Scalar; -import org.opencv.imgproc.Imgproc; -import org.openftc.easyopencv.OpenCvPipeline; - -import java.util.ArrayList; - -public class TargetingPipeline extends OpenCvPipeline { - Mat blurred = new Mat(); - Mat hsv = new Mat(); - Mat redMask1 = new Mat(); - Mat redMask2 = new Mat(); - Mat redMask = new Mat(); - Mat whiteMask = new Mat(); - Scalar redGoalLower1; - Scalar redGoalUpper1; - Scalar redGoalLower2; - Scalar redGoalUpper2; - - private Detection red; - - // Init - @Override - public void init(Mat input) { - red = new Detection(input.size(), CV_MIN_GOAL_AREA); - } - - // Process each frame that is received from the webcam - @Override - public Mat processFrame(Mat input) { - Imgproc.GaussianBlur(input, blurred, BLUR_SIZE, 0); - Imgproc.cvtColor(blurred, hsv, Imgproc.COLOR_RGB2HSV); - - updateRed(input); - - return input; - } - - // Update the Red Goal Detection - private void updateRed(Mat input) { - // take pixels that are in the color range and put them into a mask, eroding and dilating them to remove white noise - redGoalLower1 = new Scalar(FTC_RED_LOWER.getH(), FTC_RED_LOWER.getS(), FTC_RED_LOWER.getV()); - redGoalUpper1 = new Scalar(180, FTC_RED_UPPER.getS(), FTC_RED_UPPER.getV()); - redGoalLower2 = new Scalar(0, FTC_RED_LOWER.getS(), FTC_RED_LOWER.getV()); - redGoalUpper2 = new Scalar(FTC_RED_UPPER.getH(), FTC_RED_UPPER.getS(), FTC_RED_UPPER.getV()); - Core.inRange(hsv, redGoalLower1, redGoalUpper1, redMask1); - Core.inRange(hsv, redGoalLower2, redGoalUpper2, redMask2); - Core.add(redMask1, redMask2, redMask); - Imgproc.erode(redMask, redMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS); - Imgproc.dilate(redMask, redMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS); - - ArrayList contours = new ArrayList<>(); - Imgproc.findContours(redMask, contours, new Mat(), Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE); - for (int i = 0; i < contours.size(); i++) { - Detection newDetection = new Detection(input.size(), CV_MIN_GOAL_AREA, CV_MAX_GOAL_AREA); - newDetection.setContour(contours.get(i)); - newDetection.draw(input, RED); - } - - red.setContour(getLargestContour(contours)); - red.fill(input, RED); - } - - public Detection getRed() { - return this.red; - } -} \ No newline at end of file