Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Getting Skystone Blue Side #3

Open
wants to merge 5 commits into
base: master
Choose a base branch
from
Open

Getting Skystone Blue Side #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
1c0f1a9
Merge pull request #1 from ishgar686/master
kriagg007 Jan 12, 2020
7a6b489
Red Side Blocks
kriagg007 Jan 12, 2020
5397fa7
Skystone Detection Code 1/5/2020
kriagg007 Jan 15, 2020
68d5764
Skystone Detection Code 1/5/2020
kriagg007 Jan 20, 2020
1ab7081
Refactored the code:
kriagg007 Feb 2, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 0 additions & 22 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousConstructionRed.java
View file
Open in desktop

This file was deleted.

162 changes: 94 additions & 68 deletions .../ftc/teamcode/DetectingSkystonesFast.java → .../ftc/teamcode/DetectingSkystonesBlue.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -80,9 +80,9 @@
*/


@Autonomous(name="DetectingSkystonesFast", group ="Concept")
@Autonomous(name="DetectingSkystonesBlue", group ="Concept")
//@Disabled
public class DetectingSkystonesFast extends Movement {
public class DetectingSkystonesBlue extends Movement {

// IMPORTANT: For Phone Camera, set 1) the camera source and 2) the orientation, based on how your phone is mounted:
// 1) Camera Source. Valid choices are: BACK (behind screen) or FRONT (selfie side)
Expand Down Expand Up @@ -302,7 +302,7 @@ public class DetectingSkystonesFast extends Movement {
// AFTER you hit Init on the Driver Station, use the "options menu" to select "Camera Stream"
// Tap the preview window to receive a fresh image.

goLeft(1,1000);
strafeRight(1,1100);

targetsSkyStone.activate();
while (skystone1Detected) {
Expand All @@ -312,6 +312,7 @@ public class DetectingSkystonesFast extends Movement {
for (VuforiaTrackable trackable : allTrackables) {
if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) {
telemetry.addData("Visible Target", trackable.getName());
telemetry.update();
targetVisible = true;

// getUpdatedRobotLocation() will return null if no new information is available since
Expand All @@ -331,95 +332,120 @@ public class DetectingSkystonesFast extends Movement {
telemetry.addData("Pos (in)", "{X, Y, Z} = %.1f, %.1f, %.1f",
translation.get(0) / mmPerInch, translation.get(1) / mmPerInch, translation.get(2) / mmPerInch);

telemetry.update();
// express the rotation of the robot in degrees.
Orientation rotation = Orientation.getOrientation(lastLocation, EXTRINSIC, XYZ, DEGREES);
telemetry.addData("Rot (deg)", "{Roll, Pitch, Heading} = %.0f, %.0f, %.0f", rotation.firstAngle, rotation.secondAngle, rotation.thirdAngle);
telemetry.update();
stopWithSleep("Wahoo", 10);
goForward(0.5, 50);
Turnleft(1, 1,575);
goBackward(1, 20);
frontServo.setPosition(0.4);
goForward(0.4, 475);
targetsSkyStone.deactivate();
turnLeft(1, 1,650);
goForward(1, 300);
stopWithSleep("stop", 100);
armclamp();
stopWithSleep("stopafterarmclamp", 200);
goBackward(0.6, 525);
stopWithSleep("stop", 200);
Turnleft(1,1, 525);
stopWithSleep("stop", 200);
goForward(0.4, (int) (0.25 *blockdistance));
goForward(1, 1250);
armrelease();
armClamp();
stopWithSleep("stopafterarmclamp", 100);
goBackward(0.9, 600);
stopWithSleep("stop", 100);
turnRight(1,1, 550);
stopWithSleep(50);
goForward(0.8, (int) (0.125 *blockdistance));
stopWithSleep(50);
goForward(1, 1300);
stopWithSleep(50);
armRelease();


// Turning back for 2nd skystone
targetsSkyStone.activate();
stopWithSleep(50);
goBackward(1, 1250);
goBackward(1, 1300);
stopWithSleep(50);
turnRight(1, 1, 1175);
goBackward(0.8, (int) (0.125 *blockdistance));
goBackward(1, 150);
stopWithSleep(50);
goForward(0.4, (int) (0.25 *blockdistance));

// Detect second skystone
while(skystone2Detected) {

targetVisible = false;
for (VuforiaTrackable trackable : allTrackables) {
if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) {
telemetry.addData("Visible Target", trackable.getName());
targetVisible = true;

// getUpdatedRobotLocation() will return null if no new information is available since
// the last time that call was made, or if the trackable is not currently visible.
OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation();
if (robotLocationTransform != null) {
lastLocation = robotLocationTransform;
}
break;
}
}
skystone1Detected = false;
targetVisible = false;

if (targetVisible) {

telemetry.addData("Pos (in)", "{X, Y, Z} = %.1f, %.1f, %.1f",
translation.get(0) / mmPerInch, translation.get(1) / mmPerInch, translation.get(2) / mmPerInch);
telemetry.addData("Rot (deg)", "{Roll, Pitch, Heading} = %.0f, %.0f, %.0f", rotation.firstAngle, rotation.secondAngle, rotation.thirdAngle);
stopWithSleep("Wahoo", 10);
goForward(0.5, 50);
Turnleft(1, 1,575);
frontServo.setPosition(0.4);
goForward(0.4, 475);
stopWithSleep("stop", 100);
armclamp();
stopWithSleep("stopafterarmclamp", 200);
goBackward(0.6, 525);
stopWithSleep("stop", 200);
Turnleft(1,1, 525);
stopWithSleep("stop", 200);
goForward(0.4, (int) (0.25 *blockdistance2));
goForward(1, 1250);
armrelease();
}

else {
telemetry.addData("Visible Target", "none");
goForward(0.1, 1);
blockdistance2 += 1;
telemetry.addData("blockdistance", String.valueOf(blockdistance2));
}

}
}
else {
telemetry.addData("Visible Target", "none");
goForward(0.1, 1);
goBackward(0.1, 1);
blockdistance += 1;
telemetry.addData("blockdistance", String.valueOf(blockdistance));
telemetry.update();
}
telemetry.update();

}

// Scan for 2nd skystone
// Detect second skystone

while(skystone2Detected) {

targetVisible = false;
for (VuforiaTrackable trackable : allTrackables) {
if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) {
telemetry.addData("Visible 2 Target", trackable.getName());
telemetry.update();
targetVisible = true;

// getUpdatedRobotLocation() will return null if no new information is available since
// the last time that call was made, or if the trackable is not currently visible.
OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation();
if (robotLocationTransform != null) {
lastLocation = robotLocationTransform;
}
break;
}
}

if (targetVisible) {

VectorF translation = lastLocation.getTranslation();
telemetry.addData("2 Pos (in)", "{X, Y, Z} = %.1f, %.1f, %.1f",
translation.get(0) / mmPerInch, translation.get(1) / mmPerInch, translation.get(2) / mmPerInch);
Orientation rotation = Orientation.getOrientation(lastLocation, EXTRINSIC, XYZ, DEGREES);
telemetry.addData("2 Rot (deg)", "{Roll, Pitch, Heading} = %.0f, %.0f, %.0f", rotation.firstAngle, rotation.secondAngle, rotation.thirdAngle);
telemetry.update();
stopWithSleep("Wahoo", 10);
frontServo.setPosition(0.4);
targetsSkyStone.deactivate();
turnLeft(1, 1,650);
goForward(1, 300);
stopWithSleep("stop", 100);
armClamp();
stopWithSleep("stopafterarmclamp", 100);
goBackward(0.9, 700);
/*
stopWithSleep("stopafterarmclamp", 200);
goBackward(0.6, 575);
stopWithSleep("stop", 100);
turnLeft(1,1, 600);
stopWithSleep("stop", 100);
goForward(0.8, (int) (0.3 *blockdistance2));
goForward(1, 2000);
armRelease();
stopWithSleep("stop", 100);
goBackward(1, 300);

*/

skystone2Detected = false;
}


else {
telemetry.addData("Visible Target", "none");
goBackward(0.1, 1);
blockdistance2 += 1;
telemetry.addData("blockdistance 2", String.valueOf(blockdistance2));
telemetry.update();
}

}

// Disable Tracking when we are done;
targetsSkyStone.deactivate();
Expand Down
32 changes: 16 additions & 16 deletions ...ires/ftc/teamcode/DetectingSkystones.java → ...s/ftc/teamcode/DetectingSkystonesRed.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -81,9 +81,9 @@
*/


@Autonomous(name="DetectingSkystones", group ="Concept")
@Autonomous(name="DetectingSkystonesRed", group ="Concept")
//@Disabled
public class DetectingSkystones extends Movement {
public class DetectingSkystonesRed extends Movement {

// IMPORTANT: For Phone Camera, set 1) the camera source and 2) the orientation, based on how your phone is mounted:
// 1) Camera Source. Valid choices are: BACK (behind screen) or FRONT (selfie side)
Expand Down Expand Up @@ -303,7 +303,7 @@ public class DetectingSkystones extends Movement {
// AFTER you hit Init on the Driver Station, use the "options menu" to select "Camera Stream"
// Tap the preview window to receive a fresh image.

goLeft(1,1000);
strafeRight(1,1000);

targetsSkyStone.activate();
while (skystone1Detected) {
Expand Down Expand Up @@ -342,18 +342,18 @@ public class DetectingSkystones extends Movement {
frontServo.setPosition(0.4);
targetsSkyStone.deactivate();
goForward(0.5, 50);
Turnleft(1, 1,575);
turnLeft(1, 1,575);
goForward(0.4, 475);
stopWithSleep("stop", 100);
armclamp();
armClamp();
stopWithSleep("stopafterarmclamp", 200);
goBackward(0.6, 525);
stopWithSleep("stop", 200);
Turnleft(1,1, 525);
turnLeft(1,1, 525);
stopWithSleep("stop", 200);
goForward(0.4, (int) (0.25 *blockdistance));
goForward(0.4, (int) (0.8 *blockdistance));
goForward(1, 1250);
armrelease();
armRelease();


// Turning back for 2nd skystone
Expand All @@ -363,8 +363,8 @@ public class DetectingSkystones extends Movement {
stopWithSleep(50);
turnRight(1, 1, 1225);
stopWithSleep(50);
goForward(1, 500);
goForward(0.4, (int) (0.25 *blockdistance));
goForward(1, 460);
goForward(0.4, (int) (0.45 *blockdistance));
skystone1Detected = false;
targetVisible = false;

Expand Down Expand Up @@ -413,19 +413,19 @@ public class DetectingSkystones extends Movement {
telemetry.update();
stopWithSleep("Wahoo 2", 10);
goForward(0.5, 50);
Turnleft(1, 1,575);
turnLeft(1, 1,575);
frontServo.setPosition(0.4);
goForward(0.4, 550);
stopWithSleep("stop", 100);
armclamp();
armClamp();
stopWithSleep("stopafterarmclamp", 200);
goBackward(0.6, 575);
stopWithSleep("stop", 100);
Turnleft(1,1, 650);
turnLeft(1,1, 600);
stopWithSleep("stop", 100);
goForward(0.4, (int) (0.25 *blockdistance2));
goForward(0.8, (int) (0.3 *blockdistance2));
goForward(1, 2000);
armrelease();
armRelease();
stopWithSleep("stop", 100);
goBackward(1, 300);

Expand All @@ -437,7 +437,7 @@ public class DetectingSkystones extends Movement {
telemetry.addData("Visible Target", "none");
goForward(0.1, 1);
blockdistance2 += 1;
telemetry.addData("blockdistance", String.valueOf(blockdistance2));
telemetry.addData("blockdistance 2", String.valueOf(blockdistance2));
telemetry.update();
}

Expand Down
21 changes: 9 additions & 12 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControl.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,11 +1,8 @@
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.util.Range;
import com.qualcomm.robotcore.hardware.DcMotor.ZeroPowerBehavior;

@TeleOp(name="DriverControl", group="Linear Opmode")
public class DriverControl extends Movement {
Expand All @@ -28,10 +25,10 @@ public void runOpModeImpl() {
// Gamepad 1 controls:

// Left trigger - to move left sideways
goLeft(-gamepad1.left_trigger, 0);
strafeRight(-gamepad1.left_trigger, 0);

// Right trigger - to move right sideways
goRight(-gamepad1.right_trigger, 0);
strafeLeft(-gamepad1.right_trigger, 0);

// Left stick y - to go forward or backward
double drive = -(-gamepad1.left_stick_y);
Expand All @@ -40,14 +37,14 @@ public void runOpModeImpl() {
double turn = (-gamepad1.right_stick_x);

// to drive and turn left?
double leftPower = Range.clip(drive + turn, -1.0, 1.0) ;
leftfront.setPower(leftPower);
leftback.setPower(leftPower);
double leftPower = Range.clip(drive + turn, -1.0, 1.0) ;
frontLeft.setPower(leftPower);
backLeft.setPower(leftPower);

// to drive and turn right?
double rightPower = Range.clip(drive - turn, -1.0, 1.0) ;
rightfront.setPower(rightPower);
rightback.setPower(rightPower);
frontRight.setPower(rightPower);
backRight.setPower(rightPower);

// Gamepad 2 Controls

Expand Down Expand Up @@ -82,8 +79,8 @@ public void runOpModeImpl() {
// x - to move back servo down
if (gamepad2.x) {

leftConstruction.setPosition(0.5);
rightConstruction.setPosition(0.4);
leftConstruction.setPosition(0.35);
rightConstruction.setPosition(0.43);
telemetry.addData("back servos down", "servoposition: 0,0" );
}

Expand Down
Loading