Skip to content

Commit

Permalink
12/1/2017 Meeting Commit ftctechnh#1:
Browse files Browse the repository at this point in the history 
Contains OpenCV Code First Revision
  • Loading branch information
@tei06398
tei06398 committed Dec 1, 2017
1 parent 01bc412 commit 9b89a60
Show file tree
Hide file tree
Showing 4 changed files with 83 additions and 22 deletions.
20 changes: 15 additions & 5 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GunnerFunction.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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);
}
}
46 changes: 46 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RRAuton.java
View file
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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;
Expand Down Expand Up @@ -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);

Expand Down
36 changes: 19 additions & 17 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RRTeleOp.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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) {
Expand All @@ -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();
}

Expand All @@ -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);
Expand All @@ -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();
}
Expand Down
3 changes: 3 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestTeleOp.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -27,6 +28,7 @@ public void loop(){
this.glyphterServo.setPosition(position);

telemetry.addData("Position: ", position);
telemetry.addData("Ultrasonic: ", ultrasonic1.getUltrasonicLevel());
telemetry.update();
}

Expand All @@ -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);
Expand Down

0 comments on commit 9b89a60

Please sign in to comment.