From 9b89a606d407b4421779aa0be7b79e7103208a2e Mon Sep 17 00:00:00 2001 From: Seth Teichman Date: Fri, 1 Dec 2017 17:58:57 -0500 Subject: [PATCH] 12/1/2017 Meeting Commit #1: Contains OpenCV Code First Revision --- .../ftc/teamcode/GunnerFunction.java | 20 ++++++-- .../firstinspires/ftc/teamcode/RRAuton.java | 46 +++++++++++++++++++ .../firstinspires/ftc/teamcode/RRTeleOp.java | 36 ++++++++------- .../ftc/teamcode/TestTeleOp.java | 3 ++ 4 files changed, 83 insertions(+), 22 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GunnerFunction.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GunnerFunction.java index 91ace4b6158..6358a79c1ee 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GunnerFunction.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GunnerFunction.java @@ -5,17 +5,19 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; public class GunnerFunction { - private DcMotor winchMotor; - private Servo glyphterServo; - private Servo jewelServo; - private Telemetry telemetry; + private final DcMotor motorRelicSlide; + private final DcMotor winchMotor; + private final Servo glyphterServo; + private final Servo jewelServo; + private final Telemetry telemetry; // put in actual values later private static final int GLYPHTER_SERVO_CLOSE_POS = 0; private static final int GLYPHTER_SERVO_OPEN_POS = 100; - GunnerFunction(DcMotor winch, Servo glyphter, Servo jewel, Telemetry t) { + GunnerFunction(DcMotor winch, DcMotor motorRelicSlide, Servo glyphter, Servo jewel, Telemetry t) { winchMotor = winch; + this.motorRelicSlide = motorRelicSlide; glyphterServo = glyphter; jewelServo = jewel; telemetry = t; @@ -40,4 +42,12 @@ public void openGlyphter() { public void closeGlyphter() { glyphterServo.setPosition(GLYPHTER_SERVO_CLOSE_POS); } + + public void expandRelicSlide() { + motorRelicSlide.setPower(0.5); + } + + public void retractRelicSlide() { + motorRelicSlide.setPower(-0.5); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RRAuton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RRAuton.java index a716ea12931..24880f378d1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RRAuton.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RRAuton.java @@ -1,10 +1,15 @@ package org.firstinspires.ftc.teamcode; +import android.hardware.camera2.CameraAccessException; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.UltrasonicSensor; import org.firstinspires.ftc.robotcore.external.ClassFactory; import org.firstinspires.ftc.robotcore.external.navigation.*; +import org.opencv.core.Core; +import org.opencv.core.Mat; +import org.opencv.videoio.VideoCapture; /** * Created 11/13/2017 @@ -16,6 +21,7 @@ public class RRAuton extends LinearOpMode { protected DcMotor motorRF = null; protected DcMotor motorLB = null; protected DcMotor motorRB = null; + protected UltrasonicSensor ultrasonic1 = null; private VuforiaLocalizer vuforia; @@ -56,11 +62,51 @@ public void runOpMode() throws InterruptedException { this.motorRF = this.hardwareMap.dcMotor.get("rfMotor"); this.motorLB = this.hardwareMap.dcMotor.get("lbMotor"); this.motorRB = this.hardwareMap.dcMotor.get("rbMotor"); + this.ultrasonic1 = this.hardwareMap.ultrasonicSensor.get("ultrasonic1"); + this.motorLF.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); this.motorRF.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); this.motorLB.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); this.motorRB.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + System.loadLibrary(Core.NATIVE_LIBRARY_NAME); + VideoCapture camera = new VideoCapture(0); + Mat frame = new Mat(); + if(!camera.isOpened()) { + telemetry.addData("Camera working", "false"); + } else { + telemetry.addData("Camera working", "true"); + while (!gamepad2.x) { + try { + Thread.sleep(1); + } catch (InterruptedException e) { + // Shouldn't happen + } + + if (camera.read(frame)) { + final int HEIGHT = frame.rows(); + final int WIDTH = frame.cols(); + + int ballCol = (int) (WIDTH * 0.67); + int ballRow = (int) (HEIGHT * 0.75); + + double blueWeight = 0; + double redWeight = 0; + for (int row = (int) (HEIGHT * 0.5); row < HEIGHT; row += 10) { + for (int col = (int) (WIDTH * 0.33); col < WIDTH; col += 10) { + double distWeight = Math.abs(ballRow - row) + Math.abs(ballCol - col); + + blueWeight += frame.get(row, col)[0] / (distWeight + 20); + redWeight += frame.get(row, col)[2] / (distWeight + 20); + } + } + telemetry.addData("Red: ", redWeight); + telemetry.addData("Blue: ", blueWeight); + telemetry.addData("Color: ", redWeight > blueWeight ? "RED" : "BLUE"); + } + } + camera.release(); + } //Makes RobotDriving Object RobotDriving robotDriving = new RobotDriving(motorLF, motorLB, motorRF, motorRB, telemetry); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RRTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RRTeleOp.java index 81636818e4b..870e212e2ec 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RRTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RRTeleOp.java @@ -20,6 +20,7 @@ public class RRTeleOp extends OpMode { protected DcMotor motorLB = null; protected DcMotor motorRB = null; protected DcMotor motorWinch = null; + protected DcMotor motorRelicSlide = null; protected Servo servoGlyphter = null; protected Servo servoJewelTipper = null; protected RobotDriving robotDriving; @@ -29,13 +30,26 @@ public class RRTeleOp extends OpMode { public void loop() { steering.setSpeedRatio((this.gamepad1.right_trigger > 0.5) ? MIN_SPEED_RATIO : MAX_SPEED_RATIO); - //Controls orientation of robot + // GAMEPAD 1 (DRIVER) + // Right stick: turn if (this.gamepad1.right_stick_x > 0.1) { steering.turnClockwise(); } else if (this.gamepad1.right_stick_x < -0.1) { steering.turnCounterclockwise(); } + // Left stick: driving + if (Math.abs(this.gamepad1.left_stick_x) > 0.1 || Math.abs(this.gamepad1.left_stick_y) > 0.1) { + double angle = Math.atan2(-gamepad1.left_stick_y, gamepad1.left_stick_x); + telemetry.addData("angle: ", angle); + + steering.moveRadians(angle); + } else { + telemetry.addData("angle: ", 0); + } + + // GAMEPAD 2 (GUNNER) + // Up/down keys: winch if (this.gamepad2.dpad_up) { gunnerFunction.upWinch(); } else if (this.gamepad2.dpad_down) { @@ -44,29 +58,16 @@ public void loop() { gunnerFunction.stopWinch(); } + // Left bumper: close glyphter + // Right bumper: open glyphter if (this.gamepad2.left_bumper) gunnerFunction.closeGlyphter(); if (this.gamepad2.right_bumper) gunnerFunction.openGlyphter(); - //Controls linear movement of robot - // Only actually move if the joystick is offset. - if (Math.abs(this.gamepad1.left_stick_x) > 0.1 || Math.abs(this.gamepad1.left_stick_y) > 0.1) { - double angle = Math.atan2(-gamepad1.left_stick_y, gamepad1.left_stick_x); - telemetry.addData("angle: ", angle); - - steering.moveRadians(angle); - } else { - telemetry.addData("angle: ", 0); - } - steering.finishSteering(); telemetry.addData("Right stick x: ", this.gamepad1.right_stick_x); telemetry.addData("Left stick x: ", this.gamepad1.left_stick_x); telemetry.addData("Left stick y: ", this.gamepad1.left_stick_y); - /*telemetry.addData("powerLF: ", powerLF); - telemetry.addData("powerRB: ", powerRB); - telemetry.addData("powerLB: ", powerLB); - telemetry.addData("powerRF: ", powerRF);*/ telemetry.update(); } @@ -78,6 +79,7 @@ public void init(){ this.motorLB = this.hardwareMap.dcMotor.get("lbMotor"); this.motorRB = this.hardwareMap.dcMotor.get("rbMotor"); this.motorWinch = this.hardwareMap.dcMotor.get("winchMotor"); + this.motorRelicSlide = this.hardwareMap.dcMotor.get("relicSlide"); this.servoGlyphter = this.hardwareMap.servo.get("glyphterServo"); this.servoJewelTipper = this.hardwareMap.servo.get("jewelTipperServo"); this.motorLF.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); @@ -86,7 +88,7 @@ public void init(){ this.motorRB.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); robotDriving = new RobotDriving(motorLF,motorLB,motorRF,motorRB,telemetry); - gunnerFunction = new GunnerFunction(motorWinch, servoGlyphter, servoJewelTipper, telemetry); + gunnerFunction = new GunnerFunction(motorWinch, motorRelicSlide, servoGlyphter, servoJewelTipper, telemetry); steering = robotDriving.getSteering(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestTeleOp.java index 19687dbbcf0..13c9465f804 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestTeleOp.java @@ -17,6 +17,7 @@ public class TestTeleOp extends OpMode { protected DcMotor motorLB = null; protected DcMotor motorRB = null; protected Servo glyphterServo = null; + protected UltrasonicSensor ultrasonic1 = null; public void loop(){ double position = 0; @@ -27,6 +28,7 @@ public void loop(){ this.glyphterServo.setPosition(position); telemetry.addData("Position: ", position); + telemetry.addData("Ultrasonic: ", ultrasonic1.getUltrasonicLevel()); telemetry.update(); } @@ -37,6 +39,7 @@ public void init(){ this.motorLB = this.hardwareMap.dcMotor.get("lbMotor"); this.motorRB = this.hardwareMap.dcMotor.get("rbMotor"); this.glyphterServo = this.hardwareMap.servo.get("glyphterServo"); + this.ultrasonic1 = this.hardwareMap.ultrasonicSensor.get("ultrasonic1"); this.motorLF.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); this.motorRF.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); this.motorLB.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);