From e471a27ac8df3b5e71994539c842652e4e91abc8 Mon Sep 17 00:00:00 2001 From: abirarun <146149304+abirarun@users.noreply.github.com> Date: Sat, 12 Oct 2024 17:49:09 -0700 Subject: [PATCH] LED lights turn on when buttons A and B on gamepad two are pressed(press x at the same time) --- .../ftc/teamcode/ColorSensorTeleOp.java | 33 +++++++- .../ftc/teamcode/FieldCentricBlue.java | 83 ++++++++++++++++--- .../ftc/teamcode/StraferHardware.java | 4 +- 3 files changed, 102 insertions(+), 18 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ColorSensorTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ColorSensorTeleOp.java index 5bd86401edc2..93d15859a5b3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ColorSensorTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ColorSensorTeleOp.java @@ -1,14 +1,17 @@ package org.firstinspires.ftc.teamcode; +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.hardwareMap; import static org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit.INCH; +import com.qualcomm.hardware.rev.RevBlinkinLedDriver; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.ColorSensor; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.DistanceSensor; -import com.qualcomm.robotcore.hardware.NormalizedRGBA; +import com.qualcomm.robotcore.hardware.HardwareMap; + @TeleOp public class ColorSensorTeleOp @@ -56,7 +59,8 @@ public void runOpMode() throws InterruptedException { double red = ribbit.red(); double blue = ribbit.blue(); double green = ribbit.green(); - + if(gamepad2.y) + display(1); if (blue - green > 100 && blue - red > 100) { telemetry.addLine("blue"); } @@ -84,6 +88,26 @@ public void runOpMode() throws InterruptedException { backRight.setPower(0); } + public void display (double color){ + StraferHardware hardware = new StraferHardware(hardwareMap); + RevBlinkinLedDriver lights = hardwareMap.get(RevBlinkinLedDriver.class,"lights"); + ColorSensor ribbit = hardwareMap.get(ColorSensor.class, "ribbit"); + double red = ribbit.red(); + double blue = ribbit.blue(); + double green = ribbit.green(); + boolean yellow = (green - blue > 100 && green - red > 100 && red >= 350); + + if (red<0){ + lights.setPattern(RevBlinkinLedDriver.BlinkinPattern.RED); + } + if(blue<0){ + lights.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE); + } + if(yellow){ + lights.setPattern(RevBlinkinLedDriver.BlinkinPattern.YELLOW); + } + } + public void Rumble() { StraferHardware hardware = new StraferHardware(hardwareMap); ColorSensor ribbit = hardwareMap.get(ColorSensor.class, "colorSensor"); @@ -99,8 +123,11 @@ public void Rumble() { gamepad1.rumbleBlips(1); } + + } -} + + } //end class. \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FieldCentricBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FieldCentricBlue.java index d8ab027b4c7d..740cab26638a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FieldCentricBlue.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FieldCentricBlue.java @@ -2,6 +2,7 @@ import static org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit.INCH; +import android.annotation.SuppressLint; import android.media.MediaPlayer; import android.util.Size; @@ -44,11 +45,12 @@ public class FieldCentricBlue extends LinearOpMode { private static final boolean USE_WEBCAM = true; - private Position cameraPosition = new Position(DistanceUnit.INCH, + private final Position cameraPosition = new Position(DistanceUnit.INCH, 5, 7, 0, 0); - private YawPitchRollAngles cameraOrientation = new YawPitchRollAngles(AngleUnit.DEGREES, + private final YawPitchRollAngles cameraOrientation = new YawPitchRollAngles(AngleUnit.DEGREES, 0, -90, 0, 0); //this is all copy-pasted from AprilTagYummy + private AprilTagProcessor aprilTag; private VisionPortal visionPortal; private CRServo con_servo; @@ -61,13 +63,13 @@ public void runOpMode() throws InterruptedException { initAprilTag(); StraferHardware hardware = new StraferHardware(hardwareMap); + ColorSensor ribbit = hardwareMap.get(ColorSensor.class, "ribbit"); navxMicro = hardwareMap.get(NavxMicroNavigationSensor.class, "gyro"); gyro = (IntegratingGyroscope) navxMicro; RevBlinkinLedDriver lights = hardwareMap.get(RevBlinkinLedDriver.class,"lights"); DistanceSensor sensor = hardwareMap.get(DistanceSensor.class, "distance"); - // ColorSensor ribbit = hardwareMap.get(ColorSensor.class, "colorSensor"); telemetry.log().add("Gyro Calibrating. Do Not Move!"); // Wait until the gyro calibration is complete @@ -96,16 +98,35 @@ public void runOpMode() throws InterruptedException { while (opModeIsActive()) { telemetryAprilTag(); telemetry.update(); - + double red = ribbit.red(); + double blue = ribbit.blue(); + double green = ribbit.green(); + telemetry.addData("blue",blue); + telemetry.addData("red",red); + telemetry.addData("green",green); + } // this is the gamepad controls for the driving. good luck double y = gamepad1.left_stick_y; // Remember, Y stick value is reversed double x = gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing double rx = gamepad1.right_stick_x; + double red = ribbit.red(); + double blue = ribbit.blue(); + double green = ribbit.green(); + telemetry.addData("blue",blue); + telemetry.addData("red",red); + telemetry.addData("green",green); // double MAX_POS = 1.0; Orientation angles = gyro.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.RADIANS); double botheading = angles.firstAngle; - + if (blue - green > 100 && blue - red > 100) { + telemetry.addLine("blue"); + } + if (red - blue > 100 && red - green > 100) { + telemetry.addLine("red"); + } + if (green - blue > 100 && green - red > 100 && red >= 350) { + telemetry.addLine("yellow"); //Rotate the movement direction counter to the bot's rotation. double rotX = x * Math.cos(-botheading) + y * Math.sin(-botheading); double rotY = x * Math.sin(-botheading) - y * Math.cos(-botheading); @@ -127,6 +148,7 @@ public void runOpMode() throws InterruptedException { hardware.frontRight.setPower(frontRightPower / 2); hardware.backRight.setPower(backRightPower / 2); + double color= 15; distance = sensor.getDistance(INCH); // telemetry.addData("distance", distance); @@ -165,6 +187,9 @@ public void runOpMode() throws InterruptedException { if (gamepad2.x){ blinkin(1); } + if (gamepad2.y){ + display(1); + } //Rumble(); //gamepad controls here. idle(); @@ -202,6 +227,7 @@ private void initAprilTag() { visionPortal = builder.build(); } + @SuppressLint("DefaultLocale") private void telemetryAprilTag() { List currentDetections = aprilTag.getDetections(); @@ -231,6 +257,7 @@ private void telemetryAprilTag() { } + @SuppressLint("DefaultLocale") String formatRate(float rate) { return String.format("%.3f", rate); } @@ -239,10 +266,11 @@ String formatAngle(AngleUnit angleUnit, double angle) { return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle)); } + @SuppressLint("DefaultLocale") String formatDegrees(double degrees) { return String.format("%.1f", AngleUnit.DEGREES.normalize(degrees)); } - ///////////////////////////////////////////////////////////////////////////////////////////// + ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// public void turn(double target) { StraferHardware hardware = new StraferHardware(hardwareMap); @@ -297,7 +325,7 @@ public void turn(double target) { } } - //////////////////////////////////////////////////////////////////////////////////// + ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// public void crservo(double deltaAngle){ con_servo=hardwareMap.crservo.get("intakeServo"); waitForStart(); @@ -315,7 +343,7 @@ public void crservo(double deltaAngle){ telemetry.update(); } } -////////////////////////////////////////////////////////////////////////////////////////// + ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// public void turn2(double deltaAngle) { //turn to an angle accurately. ElapsedTime timer = new ElapsedTime(); @@ -363,7 +391,7 @@ public void turn2(double deltaAngle) { } } - ///////////////////////////////////////////////////////////////////////////////// + ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// public void handservo(double deltaAngle) { //hand teleop controls :} StraferHardware hardware = new StraferHardware(hardwareMap); @@ -376,7 +404,7 @@ public void handservo(double deltaAngle) { } } - ////////////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// public void straightline(double power, double distance, double heading) { //drives in a straight line from robots current orientation ElapsedTime timer = new ElapsedTime(); @@ -421,10 +449,11 @@ public void straightline(double power, double distance, double heading) { } -//////////////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + public void Rumble() { StraferHardware hardware = new StraferHardware(hardwareMap); - ColorSensor ribbit = hardwareMap.get(ColorSensor.class, "colorSensor"); + ColorSensor ribbit = hardwareMap.get(ColorSensor.class, "ribbit"); double red = ribbit.red(); double blue = ribbit.blue(); double green = ribbit.green(); @@ -439,7 +468,7 @@ public void Rumble() { } - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// public void blinkin (double color){ StraferHardware hardware = new StraferHardware(hardwareMap); RevBlinkinLedDriver lights = hardwareMap.get(RevBlinkinLedDriver.class,"lights"); @@ -449,6 +478,34 @@ public void blinkin (double color){ lights.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE); } + ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + public void display (double color){ + StraferHardware hardware = new StraferHardware(hardwareMap); + RevBlinkinLedDriver lights = hardwareMap.get(RevBlinkinLedDriver.class,"lights"); + ColorSensor ribbit = hardwareMap.get(ColorSensor.class, "ribbit"); + double red = ribbit.red(); + double blue = ribbit.blue(); + double green = ribbit.green(); + boolean yellow = ((green - blue) > 100) && ((green - red) > 100) && (red >= 350); + if (blue - green > 100 && blue - red > 100) { + telemetry.addLine("blue"); + } + if (red - blue > 100 && red - green > 100) { + telemetry.addLine("red"); + } + if (green - blue > 100 && green - red > 100 && red >= 350) { + telemetry.addLine("yellow"); + } + if (red<0){ + lights.setPattern(RevBlinkinLedDriver.BlinkinPattern.RED); + } + if(blue<0){ + lights.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE); + } + if(yellow){ + lights.setPattern(RevBlinkinLedDriver.BlinkinPattern.YELLOW); + } + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StraferHardware.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StraferHardware.java index 0f5eccca8835..d5b746f218e4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StraferHardware.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StraferHardware.java @@ -61,8 +61,8 @@ public StraferHardware(HardwareMap map) { @HardwareName("Webcam 1") public CameraName Webcam1; - //@HardwareName("ribbit") - //public ColorSensor colorSensor; + @HardwareName("ribbit") + public ColorSensor colorSensor; // public Servo hand; }