Skip to content

Commit

Permalink
Added claw controls. reversed front and back. Added newest autonomous…
Browse files Browse the repository at this point in the history 
… route.
  • Loading branch information
@JimmyLi2020
JimmyLi2020 committed Jan 11, 2020
1 parent 81dff36 commit 6e23525
Show file tree
Hide file tree
Showing 3 changed files with 101 additions and 173 deletions.
119 changes: 56 additions & 63 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/TestAutonomous.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,17 +17,17 @@
import java.util.List;

@Autonomous(name = "TestAutonomous")
@Disabled
//@Disabled
public class TestAutonomous extends LinearOpMode {

private static final String TFOD_MODEL_ASSET = "Skystone.tflite";
private static final String LABEL_FIRST_ELEMENT = "Stone";
private static final String LABEL_SECOND_ELEMENT = "Skystone";
private static final String VUFORIA_KEY = "ATUNNu//////AAABmU6BPERoN0USgSQzxPQZ8JYg9RVnQhKO6YEHbNnOhkfL/iNrji3x9vzFkKsBgVzWgwH72G6eXpb3VCllKTrt1cD3gvQXZ48f+5EN43eYUQ3nuP3943NZB822XzV1djS3s6wDdaiS20PErO5K7lZUGyf9Z4Tb2TliOXv/ZoxUvwNQ/ndRjN344G0TAo8PUja0V3x2WKk+mCJavoZIgmOqgaitgmg5jim/aWBL2yk0a/QpqbP87KQfGn69zpisDBc98xdGPdSFj9ENkU9WTMem9UgnOFPgpdrHV5Zr5IpQH1jxLZIvwGuKOT97npm54kIvnJM0dzhBVA+s95JA3cxyac5ArHUYVtDePwlExuekZy9l";
// private static final String TFOD_MODEL_ASSET = "Skystone.tflite";
// private static final String LABEL_FIRST_ELEMENT = "Stone";
// private static final String LABEL_SECOND_ELEMENT = "Skystone";
// private static final String VUFORIA_KEY = "ATUNNu//////AAABmU6BPERoN0USgSQzxPQZ8JYg9RVnQhKO6YEHbNnOhkfL/iNrji3x9vzFkKsBgVzWgwH72G6eXpb3VCllKTrt1cD3gvQXZ48f+5EN43eYUQ3nuP3943NZB822XzV1djS3s6wDdaiS20PErO5K7lZUGyf9Z4Tb2TliOXv/ZoxUvwNQ/ndRjN344G0TAo8PUja0V3x2WKk+mCJavoZIgmOqgaitgmg5jim/aWBL2yk0a/QpqbP87KQfGn69zpisDBc98xdGPdSFj9ENkU9WTMem9UgnOFPgpdrHV5Zr5IpQH1jxLZIvwGuKOT97npm54kIvnJM0dzhBVA+s95JA3cxyac5ArHUYVtDePwlExuekZy9l";

private VuforiaLocalizer vuforia;
// private VuforiaLocalizer vuforia;

private TFObjectDetector tfod;
// private TFObjectDetector tfod;

DcMotor motorFL;
DcMotor motorFR;
Expand All @@ -53,35 +53,28 @@ public class TestAutonomous extends LinearOpMode {
@Override
public void runOpMode() {

initVuforia();
// initVuforia();

if (ClassFactory.getInstance().canCreateTFObjectDetector())
initTfod();
else {
telemetry.addData("Sorry!", "This device is not compatible with TFOD");
telemetry.update();
stop();
}
// if (ClassFactory.getInstance().canCreateTFObjectDetector())
// initTfod();
// else {
// telemetry.addData("Sorry!", "This device is not compatible with TFOD");
// telemetry.update();
// stop();
// }

tfod.activate();
// tfod.activate();

initMotors();
telemetry.addData("status", "Initialized");

waitForStart();

moveForward(-1);
straifLeft(5);

doAction("forward", 7.0);
doAction("left", 5.0);

// while(!())


// doAction("forward", 5.0);
// doAction("backward", 5.0);
// doAction("rotate", 10.0); // almost 360
// doAction("left", 5.0);
// doAction("right", 5.0);

sleep(1000);

Expand Down Expand Up @@ -232,42 +225,42 @@ public void encoderDrive(double speed,
}
}

private void initVuforia() {
/*
* Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
*/
VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();

parameters.vuforiaLicenseKey = VUFORIA_KEY;
parameters.cameraName = hardwareMap.get(WebcamName.class, "Webcam 1");

// Instantiate the Vuforia engine
vuforia = ClassFactory.getInstance().createVuforia(parameters);

// Loading trackables is not necessary for the TensorFlow Object Detection engine.
}

private void initTfod() {
int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
"tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
tfodParameters.minimumConfidence = 0.8;
tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_FIRST_ELEMENT, LABEL_SECOND_ELEMENT);
}

private void updateTensorflow() {
List<Recognition> updatedRecognitions = tfod.getUpdatedRecognitions();
if (updatedRecognitions != null) {
telemetry.addData("# Object Detected", updatedRecognitions.size());
int i = 0;
for (Recognition recognition : updatedRecognitions) {
telemetry.addData(String.format("label (%d)", i), recognition.getLabel());
telemetry.addData(String.format(" left,top (%d)", i), "%.03f , %.03f",
recognition.getLeft(), recognition.getTop());
telemetry.addData(String.format(" right,bottom (%d)", i), "%.03f , %.03f",
recognition.getRight(), recognition.getBottom());
}
}
}
// private void initVuforia() {
// /*
// * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
// */
// VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
//
// parameters.vuforiaLicenseKey = VUFORIA_KEY;
// parameters.cameraName = hardwareMap.get(WebcamName.class, "Webcam 1");
//
// // Instantiate the Vuforia engine
// vuforia = ClassFactory.getInstance().createVuforia(parameters);
//
// // Loading trackables is not necessary for the TensorFlow Object Detection engine.
// }
//
// private void initTfod() {
// int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
// "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
// TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
// tfodParameters.minimumConfidence = 0.8;
// tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
// tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_FIRST_ELEMENT, LABEL_SECOND_ELEMENT);
// }
//
// private void updateTensorflow() {
// List<Recognition> updatedRecognitions = tfod.getUpdatedRecognitions();
// if (updatedRecognitions != null) {
// telemetry.addData("# Object Detected", updatedRecognitions.size());
// int i = 0;
// for (Recognition recognition : updatedRecognitions) {
// telemetry.addData(String.format("label (%d)", i), recognition.getLabel());
// telemetry.addData(String.format(" left,top (%d)", i), "%.03f , %.03f",
// recognition.getLeft(), recognition.getTop());
// telemetry.addData(String.format(" right,bottom (%d)", i), "%.03f , %.03f",
// recognition.getRight(), recognition.getBottom());
// }
// }
// }
}
153 changes: 44 additions & 109 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/MechTeleOp.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,13 +27,15 @@ public class MechTeleOp extends OpMode {
final double calibBL = 1.00f;
final double calibBR = 1.00f;

// //Other motors
// DcMotor liftBottom;
// DcMotor liftTop;
// DcMotor claw;
//claw motors
DcMotor clawLeft;
DcMotor clawRight;

//servo for foundation clip
CRServo foundationClip;
// CRServo foundationClip;

//servo swinging the claw
// CRServo clawSwing;

//matrix corresponding to motors
double[][] motorPowers = {
Expand Down Expand Up @@ -153,19 +155,16 @@ public void setupMotors() {
motorBL.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
motorBR.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);

// liftBottom = hardwareMap.get(DcMotor.class, "liftBottom");
// liftTop = hardwareMap.get(DcMotor.class, "liftTop");
// claw = hardwareMap.get(DcMotor.class, "claw");
//
// liftBottom.setDirection(DcMotorSimple.Direction.FORWARD);
// liftTop.setDirection(DcMotorSimple.Direction.FORWARD);
// claw.setDirection(DcMotorSimple.Direction.FORWARD);
//
// liftBottom.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
// liftTop.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
// claw.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
clawLeft = hardwareMap.get(DcMotor.class, "clawLeft");
clawRight = hardwareMap.get(DcMotor.class, "clawRight");

clawLeft.setDirection(DcMotorSimple.Direction.FORWARD);
clawRight.setDirection(DcMotorSimple.Direction.REVERSE);

foundationClip = hardwareMap.crservo.get("foundationClip");
clawLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
clawRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);

// foundationClip = hardwareMap.crservo.get("foundationClip");

// leftServo = hardwareMap.get(CRServo.class, "leftServo");
// rightServo = hardwareMap.get(CRServo.class, "rightServo");
Expand Down Expand Up @@ -195,8 +194,8 @@ public void driveRobot() {
* to offset that effect.*/

leftXMat = new double[][]{
{-leftX, -leftX}, //Here the negative will probably have to on the bottom motors
{ leftX, leftX}
{leftX, -leftX}, //Here the negative will probably have to on the bottom motors
{-leftX, leftX}
};

leftYMat = new double[][] {
Expand All @@ -216,34 +215,6 @@ public void driveRobot() {

motorPowers = avgPowerMatrix(leftYMat, leftXMat, rightYMat, rightXMat);

/*New Implementation of old code*/
// switch (Math.max(abs(leftX), abs(leftY), abs(rightX), abs(rightY)) { // Picks the joystick vector with the most distance from the origin
// case leftX:
// motorPowers = new double[][]{
// {-leftX, -leftX},
// { leftX, leftX}
// }
// break;
// case leftY:
// motorPowers = new double[][]{
// { leftY, leftY},
// { leftY, leftY}
// }
// break;
// case rightX:
// motorPowers = new double[][]{
// {-rightX, rightX},
// {-rightX, rightX}
// }
// break;
// case rightY:
// motorPowers = new double[][]{
// { rightY, rightY},
// { rightY, rightY}
// }
// break;
// }

}else{
motorPowers = new double[][] {
{ 0.0, 0.0},
Expand All @@ -253,86 +224,50 @@ public void driveRobot() {

matrixToPowers(motorPowers);

/*Old Code for Joystick Movement*/
// if (Math.abs(getRX()) < Math.abs(getLX()) || Math.abs(getRX()) < Math.abs(getLY())) {
// if (Math.abs(getLX()) < 0.1 && Math.abs(getLY()) < 0.1)
// moveForward(0);
// else if (Math.abs(getLX()) > Math.abs(getLY()))
// straifLeft(Range.clip(getLX(), -1.0, 1.0));
// else if (Math.abs(getLY()) > Math.abs(getLX()))
// moveForward(Range.clip(getLY(), -1.0, 1.0));
// } else {
// if (Math.abs(getRX()) < 0.1)
// moveForward(0);
// else
// rotateLeft(getRX());
// }

// if (gamepad1.dpad_up) {
// leftServo.setPower(1.0);
// } else if (gamepad1.dpad_down) {
// leftServo.setPower(-1.0);
// } else {
// leftServo.setPower(0.0);
// }
//
// if (gamepad1.y) {
// rightServo.setPower(1.0);
// } else if (gamepad1.a) {
// rightServo.setPower(-1.0);
// } else {
// rightServo.setPower(0.0);
// }
//claw control
if (gamepad1.right_trigger >= 0.1) {
clawRight.setPower(0.5);
} else if (gamepad1.right_bumper) {
clawRight.setPower(-0.5);
} else {
clawRight.setPower(0.0);
}

// //arm base (closer to the base of the robot) up and down
// if (gamepad1.dpad_up)
// liftBottom.setPower(0.5);
// else if (gamepad1.dpad_down)
// liftBottom.setPower(-0.5);
// else
// liftBottom.setPower(0.0);
//
// //arm base (closer to the hand) up and down
// if (gamepad1.dpad_left)
// liftTop.setPower(0.5);
// else if (gamepad1.dpad_right)
// liftTop.setPower(-0.5);
// else
// liftTop.setPower(0.0);
//
// //hand open close
// if (gamepad1.a)
// claw.setPower(0.3);
// else if (gamepad1.y)
// claw.setPower(-0.3);
// else
// claw.setPower(0.0);

if (gamepad1.dpad_down) {
foundationClip.setPower(1.0);
} else if (gamepad1.dpad_up) {
foundationClip.setPower(-1.0);
if (gamepad1.left_trigger >= 0.1) {
clawLeft.setPower(0.5);
} else if (gamepad1.left_bumper) {
clawLeft.setPower(-0.5);
} else {
foundationClip.setPower(0.0);
clawLeft.setPower(0.0);
}


// //foundation clip control
// if (gamepad1.dpad_down) {
// foundationClip.setPower(1.0);
// } else if (gamepad1.dpad_up) {
// foundationClip.setPower(-1.0);
// } else {
// foundationClip.setPower(0.0);
// }


}

public float getLX() {
return gamepad1.left_stick_x;
}

public float getLY() {
return -gamepad1.left_stick_y;
return gamepad1.left_stick_y;
}

public float getRX() {
return -gamepad1.right_stick_x;
}

public float getRY() {
return -gamepad1.right_stick_y;
public float getRY() {
return gamepad1.right_stick_y;
}

}
2 changes: 1 addition & 1 deletion build.gradle
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@ buildscript {
jcenter()
}
dependencies {
classpath 'com.android.tools.build:gradle:3.5.0'
classpath 'com.android.tools.build:gradle:3.5.3'
classpath 'org.tensorflow:tensorflow-lite:+'
}
}
Expand Down

0 comments on commit 6e23525

Please sign in to comment.