Skip to content

Movement Changes #13

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

Open
nikhilmantrala wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Movement Changes #13

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
5877918
Movement Changes
nikhilmantrala Nov 12, 2021
40333fc
Autonomous Edits
nikhilmantrala Nov 13, 2021
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
250 changes: 212 additions & 38 deletions ...ain/java/org/firstinspires/ftc/teamcode/opmodes/autonomous/AutonomousBlueStorageUnit.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,21 +20,35 @@
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
import org.firstinspires.ftc.robotcore.external.tfod.TfodCurrentGame;
import com.qualcomm.robotcore.util.ElapsedTime;

import java.util.List;

@Autonomous
public class AutonomousRedStorageunit extends LinearOpMode {
public class AutonomousBlueStorageUnit extends LinearOpMode {
TurtleRobot turtlerobot = new TurtleRobot(); // Use a Pushbot's hardware
private ElapsedTime runtime = new ElapsedTime();
static final double COUNTS_PER_MOTOR_REV = 28; // eg: TETRIX Motor Encoder
static final double DRIVE_GEAR_REDUCTION = 19.2; // This is < 1.0 if geared UP
static final double WHEEL_DIAMETER_INCHES = 120 / 25.4; // For figuring circumference
static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
(WHEEL_DIAMETER_INCHES * 3.1415);
static final double DRIVE_SPEED = 0.5;
static final double DRIVE_SPEED = 0.01;
static final double TURN_SPEED = 0.1;
int counter = 0;
Recognition recognition;

VuforiaCurrentGame vuforiaFreightFrenzy;
TfodCurrentGame tfodFreightFrenzy;

@Override
public void runOpMode() {
List<Recognition> recognitions;
int index;
int pos;
int duckPos = 3;

initCam();
turtlerobot.init(hardwareMap);
telemetry.addData("Status", "Resetting Encoders"); //
telemetry.update();
Expand All @@ -54,31 +68,52 @@ public void runOpMode() {

// Wait for the game to start (driver presses PLAY)
waitForStart();
ElapsedTime duckruntime = new ElapsedTime();
duckruntime.reset();

EncoderDrive(turtlerobot,turtlerobot.DRIVE_SPEED, 2.5, 2.5, 2.5, 2.5, 1);
// Get the duck position
while (duckruntime.seconds() < 3) {
EncoderDrive(turtlerobot,turtlerobot.DRIVE_SPEED, 1.5, 1.5, 1.5, 1.5, 1);
EncoderDrive(turtlerobot,turtlerobot.DRIVE_SPEED, -1, -1, -1, -1, 1);
sleep(333);
duckPos = recognizeDuckPosition();
}
telemetry.addData("Duck Position: %d", duckPos);
telemetry.update();



// Field is 144 by 144 inches
// Each square floor tile is 24 by 24 inches
// Each square floor tile is 2 4 by 24 inches
EncoderDrive(turtlerobot,turtlerobot.DRIVE_SPEED, 10, 10, 10, 10, 3);
GyroTurn(turtlerobot, -90); //right
EncoderDrive(turtlerobot,turtlerobot.DRIVE_SPEED, 20, 20, 20, 20, 5); // S1: Forward 47 Inches with 5 Sec timeout
moveCarousel(turtlerobot, false);
sleep(2000);
EncoderDrive(turtlerobot,turtlerobot.DRIVE_SPEED, -65, -65, -65, -65, 6);
GyroTurn((turtlerobot, 90)); //left
EncoderDrive(turtlerobot, turtlerobot.DRIVE_SPEED, 24, 24, 24, 24, 4);
collectdrop(turtlerobot, false);
sleep(1000);
EncoderDrive(turtlerobot, turtlerobot.DRIVE_SPEED, -24, -24, -24, -24, 4);
GyroTurn(turtlerobot, -90); //right
EncoderDrive(turtlerobot, turtlerobot.DRIVE_SPEED, 10, 10, 10, 10, 4);
GyroTurn(turtlerobot, 90); //left
EncoderDrive(turtlerobot, turtlerobot.DRIVE_SPEED, 20, 20, 20, 20, 3);
GyroTurn(turtlerobot, -90); //right
EncoderDrive(turtlerobot, turtlerobot.DRIVE_SPEED, 24, 24, 24, 24, 7);
EncoderDrive(turtlerobot, turtlerobot.TURN_SPEED, -5, -5, 5, 5, 3); // left
EncoderDrive(turtlerobot,turtlerobot.DRIVE_SPEED, 15, 15, 15, 15, 5); // S1: Forward 13 Inches with 5 Sec timeout
//EncoderDrive(turtlerobot, turtlerobot.TURN_SPEED, -12.5, -12.5 , 12.5, 12.5, 3); //left
arm(turtlerobot, turtlerobot.DRIVE_SPEED, (duckPos*11), 3);
collectdrop(turtlerobot, true);
EncoderDrive(turtlerobot, turtlerobot.DRIVE_SPEED, -16, -16 ,-16, -16, 3);
arm(turtlerobot, turtlerobot.FAST_SPEED, (duckPos*-2), 1);
arm(turtlerobot, turtlerobot.FAST_SPEED, 53, 6);
//EncoderDrive(turtlerobot, turtlerobot.DRIVE_SPEED, -12.5, -12.5 ,-12.5, -12.5, 3);
EncoderDrive(turtlerobot, turtlerobot.TURN_SPEED, -5, -5, 5, 5, 3); // right
EncoderDrive(turtlerobot, turtlerobot.DRIVE_SPEED, -23, -23, -23, -23, 5);
while(counter != 7) {
startCarousel(turtlerobot, false);
sleep(500);
EncoderDrive(turtlerobot, turtlerobot.SLOW_SPEED, -0.2/counter, -0.2/counter, -0.2/counter, -0.2/counter, 1);
counter += 1;
}
stopCarousel(turtlerobot);

EncoderDrive(turtlerobot,turtlerobot.DRIVE_SPEED, 2, 2, 2, 2, 3);
EncoderDrive(turtlerobot, turtlerobot.TURN_SPEED, 12.5, 12.5, -12.5, -12.5, 3); // left
EncoderDrive(turtlerobot, turtlerobot.DRIVE_SPEED, 15, 15, 15, 15, 3);
//EncoderDrive(turtlerobot, turtlerobot.TURN_SPEED, -10, -10 , 10, 10, 3); //left
//EncoderDrive(turtlerobot, turtlerobot.DRIVE_SPEED, 16, 16, 16, 16, 4);
}


//EncoderDrive(turtlerobot,turtlerobot.TURN_SPEED, -10, -10, -3, -3, 4.0); // S2: Turn Right 12 Inches with 4 Sec timeout
}
public void EncoderDrive(TurtleRobot turtlerobot, double speed,
double leftfrontInches, double leftbackInches,
double rightfrontInches, double rightbackInches,
Expand Down Expand Up @@ -110,10 +145,10 @@ public void EncoderDrive(TurtleRobot turtlerobot, double speed,

// reset the timeout time and start motion.
runtime.reset();
turtlerobot.leftfrontmotor.setPower(Math.abs(speed));
turtlerobot.leftbackmotor.setPower(Math.abs(speed));
turtlerobot.rightfrontmotor.setPower(Math.abs(speed));
turtlerobot.rightbackmotor.setPower(Math.abs(speed));
turtlerobot.leftfrontmotor.setPower((Math.abs(speed)));
turtlerobot.leftbackmotor.setPower((Math.abs(speed)));
turtlerobot.rightfrontmotor.setPower((Math.abs(speed)));
turtlerobot.rightbackmotor.setPower((Math.abs(speed)));

// keep looping while we are still active, and there is time left, and both motors are running.
// Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits
Expand Down Expand Up @@ -158,7 +193,7 @@ public void EncoderDrive(TurtleRobot turtlerobot, double speed,
}
}

public void GyroTurn(TurtleRobot turtlerobot, double yawangle) {
public void GyroTurn(TurtleRobot turtlerobot, int turnangle) {
BNO055IMU.Parameters IMU_Parameters;
ElapsedTime ElapsedTime2;
double Left_Power;
Expand Down Expand Up @@ -194,16 +229,16 @@ public void GyroTurn(TurtleRobot turtlerobot, double yawangle) {
sleep(1000);
}
// Report calibration complete to Driver Station.
telemetry.addData("Status", "Calibration Complete");
telemetry.addData("Action needed:", "Please press the start triangle");
telemetry.update();
//telemetry.addData("Status", "Calibration Complete");
//telemetry.addData("Action needed:", "Please press the start triangle");
//telemetry.update();
// Wait for Start to be pressed on Driver Station.
ElapsedTime2 = new ElapsedTime();
// Set motor powers to the variable values.
Yaw_Angle = turtlerobot.imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).thirdAngle;
while (Yaw_Angle <= 90 || isStopRequested()) {
Yaw_Angle = turtlerobot.imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES).thirdAngle;
while (Yaw_Angle <= turnangle || isStopRequested()) {
// Update Yaw-Angle variable with current yaw.
Yaw_Angle = turtlerobot.imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).thirdAngle;
Yaw_Angle = turtlerobot.imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES).thirdAngle;
turtlerobot.leftfrontmotor.setPower(-0.2);
turtlerobot.leftbackmotor.setPower(-0.2);
turtlerobot.rightfrontmotor.setPower(0.2);
Expand All @@ -218,20 +253,30 @@ public void GyroTurn(TurtleRobot turtlerobot, double yawangle) {
turtlerobot.leftbackmotor.setPower(0);
sleep(1000);
}
public void moveCarousel(TurtleRobot turtlerobot, boolean direction) {
public void startCarousel(TurtleRobot turtlerobot, boolean direction) {
if (direction == true) {
turtlerobot.Carouselmotor1.setPower(-0.25);
turtlerobot.Carouselmotor1.setPower(-0.35);
} else if (direction == false) {
turtlerobot.Carouselmotor1.setPower(0.25);
turtlerobot.Carouselmotor1.setPower(0.35);
}
}

public void stopCarousel(TurtleRobot turtlerobot) {
turtlerobot.Carouselmotor1.setPower(0);
}

public void collectdrop(TurtleRobot turtlerobot, boolean cargo) {
if (cargo == true) {
turtlerobot.intakeservo.setPower(-0.25);
turtlerobot.armservo.setPower(-1);
sleep(1000);
turtlerobot.armservo.setPower(0);
} else if (cargo == false) {
turtlerobot.intakeservo.setPower(0.25);
turtlerobot.armservo.setPower(1);
sleep(1000);
turtlerobot.armservo.setPower(0);
}
}

/**
* Function that becomes true when gyro is calibrated and
* reports calibration status to Driver Station in the meantime.
Expand All @@ -242,5 +287,134 @@ private boolean IMU_Calibrated() {
telemetry.addData("System Status", turtlerobot.imu.getSystemStatus().toString());
return turtlerobot.imu.isGyroCalibrated();
}
}

public int recognizeDuckPosition() {
int pos = 4;
List<Recognition> recognitions;
int index;
recognitions = tfodFreightFrenzy.getRecognitions();
// If list is empty, inform the user. Otherwise, go
// through list and display info for each recognition.
if (recognitions.size() == 0) {
telemetry.addData("TFOD", "No items detected.");
pos = 1;
} else {
index = 0;
// Iterate through list and call a function to
// display info for each recognized object.
for (Recognition recognition_item : recognitions) {
recognition = recognition_item;
// Display info.
displayInfo(index);
if (recognition.getLabel().equals("Duck")) {
if (recognition.getLeft() < 150) {
telemetry.addData("Duck", "Middle");
pos = 2;
} else if (recognition.getLeft() > 150) {
telemetry.addData("Duck", "Right");
pos= 3;
}
} else {
telemetry.addData("Duck", "Left");
pos=2;
}
// Increment index.
index = index + 1;
}
}
telemetry.update();
return pos;
}

public void initCam() {
vuforiaFreightFrenzy = new VuforiaCurrentGame();
tfodFreightFrenzy = new TfodCurrentGame();

// Sample TFOD Op Mode
// Initialize Vuforia.
vuforiaFreightFrenzy.initialize(
"", // vuforiaLicenseKey
hardwareMap.get(WebcamName.class, "Webcam 1"), // cameraName
"", // webcamCalibrationFilename
false, // useExtendedTracking
false, // enableCameraMonitoring
VuforiaLocalizer.Parameters.CameraMonitorFeedback.AXES, // cameraMonitorFeedback
0, // dx
0, // dy
0, // dz
90, // xAngle
0, // yAngle
90, // zAngle
true); // useCompetitionFieldTargetLocations
// Set min confidence threshold to 0.7
tfodFreightFrenzy.initialize(vuforiaFreightFrenzy, (float) 0.7, true, true);
// Initialize TFOD before waitForStart.
// Init TFOD here so the object detection labels are visible
// in the Camera Stream preview window on the Driver Station.
tfodFreightFrenzy.activate();
// Enable following block to zoom in on target.\
tfodFreightFrenzy.setZoom(1, 16/9);
telemetry.addData(">", "Press Play to start");
telemetry.update();
}
public void arm(TurtleRobot turtlerobot, double speed,
double arminches,
double timeoutS) {
int armtarget;

// Ensure that the opmode is still active
if (opModeIsActive()) {

// Determine new target position, and pass to motor controller
armtarget = turtlerobot.ArmMotor.getCurrentPosition() + (int) (arminches * turtlerobot.COUNTS_PER_INCH);
turtlerobot.ArmMotor.setTargetPosition(armtarget);


// Turn On RUN_TO_POSITION
turtlerobot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);

// reset the timeout time and start motion.
runtime.reset();
turtlerobot.ArmMotor.setPower((Math.abs(speed)));

// keep looping while we are still active, and there is time left, and both motors are running.
// Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits
// its target position, the motion will stop. This is "safer" in the event that the will
// always end the motion as soon as possible.
// However, if you require that BOTH motors have finished their moves before the continues
// onto the next step, use (isBusy() || isBusy()) in the loop test.
while (opModeIsActive() &&
(runtime.seconds() < timeoutS)
&& (turtlerobot.ArmMotor.isBusy())) {

// Display it for the driver.
telemetry.addData("Path1", "Running to :%7d",
armtarget);
telemetry.addData("Path2", "Running at :%7d",
turtlerobot.ArmMotor.getCurrentPosition());
telemetry.update();
}

// Stop all motion;
turtlerobot.ArmMotor.setPower(0);

// Turn off RUN_TO_POSITION
turtlerobot.ArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// sleep(250); // optional pause after each move
}
}

public void displayInfo(int i) {
// Display label info.
// Display the label and index number for the recognition.
telemetry.addData("label " + i, recognition.getLabel());
// Display upper corner info.
// Display the location of the top left corner
// of the detection boundary for the recognition
telemetry.addData("Left, Top " + i, recognition.getLeft() + ", " + recognition.getTop());
// Display lower corner info.
// Display the location of the bottom right corner
// of the detection boundary for the recognition
telemetry.addData("Right, Bottom " + i, recognition.getRight() + ", " + recognition.getBottom());
}
}
Loading