Skip to content

Instantly share code, notes, and snippets.

@soulreverie

soulreverie/Autonomous Mode

Last active November 28, 2018 03:40
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save soulreverie/02b1fbfb125000ba7216d5816ee21008 to your computer and use it in GitHub Desktop.
Save soulreverie/02b1fbfb125000ba7216d5816ee21008 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
Autonomous Mode
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.hardware.DcMotor;
/*Autonomous Code:
This code is assuming that the left motor is mounted backwards. If the right motor is mounted
backwards, commend out the line motorLeft.setDirection(DcMotor.Direction.REVERSE); and un-commend
the line motorRight.setDirection(DcMotor.Direction.REVERSE);
*/
@com.qualcomm.robotcore.eventloop.opmode.Autonomous(name = "AutoOnBot2", group = "Autonomous")
public class Autonomous2 extends LinearOpMode {
private DcMotor motorLeft;
private DcMotor motorRight;
private DcMotor armLeft;
private DcMotor armRight;
//private Servo armServoLeft;
//private Servo armServoRight;
//private Servo dumpServo;
// Detector object
//private GoldAlignDetector detector;
//Static Variables
// private static final double ARM_RETRACTED_POSITION = 0.2;
// private static final double ARM_EXTENDED_POSITION = 0.8;
static final int DRIVE_ENCODER_CLICKS = 1140;
//private static final int ARM_ENCODER_CLICKS = 2240;
static final double ROBOT_DIAM = 40.0; // Robot diameter in cm
static final double ROBOT_CIRC = ROBOT_DIAM * Math.PI;
static final double WHEEL_DIAM = 10.0; //Wheel diameter in cm
static final double WHEEL_CIRC = WHEEL_DIAM * Math.PI;
static final double CLICKS_PER_CM = DRIVE_ENCODER_CLICKS / WHEEL_CIRC;
// CORRECTED GEAR RATIO
static final double DRIVE_GEAR_RATIO = 2D/1D; // No spaces, Driven:Driver
@Override
public void runOpMode() throws InterruptedException
{
motorLeft = hardwareMap.dcMotor.get("Port0");
motorRight = hardwareMap.dcMotor.get("Port1");
//one of the motors will be mounted backwards, so it should be reversed.
motorLeft.setDirection(DcMotor.Direction.REVERSE);
motorLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
motorRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
//dumpServo = hardwareMap.servo.get("dumpServo");
//dumpServo.setPosition(ARM_RETRACTED_POSITION);
//code before waitForStart is run when Init button is pressed
telemetry.addData("status", "op mode init");
telemetry.update();
telemetry.addData("encoder status", motorLeft.getCurrentPosition());
telemetry.update();
waitForStart();
telemetry.addData("status", "op mode started");
telemetry.update();
//code after waitForStart is run when the start button is pressed
motorLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
motorRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// First Turn: 25 cm
motorLeft.setTargetPosition(13971);
motorRight.setTargetPosition(13971);
motorLeft.setPower(1.0);
motorRight.setPower(1.0);
sleep(5000);
telemetry.addData("encoder status", motorLeft.getCurrentPosition());
telemetry.update();
/*
motorLeft.setPower(0.5);
motorRight.setPower(0.5);
sleep(3000);
motorLeft.setPower(0);
motorRight.setPower(0.);
*/
/*
Drive(25,.5);
sleep(500);
telemetry.addData("Mode","running");
telemetry.update();
TurnLeft(50, .5, true);
sleep(500);
Drive(116.84, .5);
sleep(500);
TurnRight(90, .5, true);
sleep(500);
Drive(93.98,.5);
sleep(500);
TurnLeft(90, .5, true);
sleep(500);
//dumpServo.setPosition(90);
//dumpServo.setPosition(0);
TurnLeft(90, .5, true);
sleep(500);
Drive(92.98, .5);
sleep(500);
TurnLeft(90, .5, true);
sleep(500);
Drive(116.84, .5);
sleep(500);
TurnLeft(90,.5,true);
sleep(500);
Drive(116.84,.5);
sleep(500);
TurnRight(90,.5,true);
sleep(500);
Drive( 106.68,1);
sleep(500);
*/
/* PSEUDOCODE: *********
The detector has two features we can use just based on sample code: getAligned(0
returns a boolean, getXPosition(0 returns a number from 0 t0 640 depending on the cube's
position. So the code should read something like :
while getAligned is false, getXPosition. Depending on whether it indicates left or right,
turn the robot slightly to the left or the right. Once getAligned is true, move forward.
PSEUDOCODE **************
*/
}
public static int getEncoderClicks (double distanceInCM) // Distance in centimeters
{
// telemetry.addData("encoder clicks cm", "distance in cm" );
int outputClicks = ((int)Math.floor((CLICKS_PER_CM * distanceInCM)* DRIVE_GEAR_RATIO));
return outputClicks;
}
public void Drive( double distanceInCM, double power) throws InterruptedException {
/* Entering positive values into distance and power will drive the robot forward. Entering
negative values will drive the robot backward.
*/
//telemetry.addData("status","drive start");
//telemetry.update();
motorLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
motorRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
telemetry.addData("status","encoder reset");
telemetry.update();
motorLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
motorRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
motorLeft.setTargetPosition(getEncoderClicks(distanceInCM));
motorRight.setTargetPosition(getEncoderClicks(distanceInCM));
motorLeft.setPower(power);
motorRight.setPower(power);
while (opModeIsActive() && motorLeft.isBusy() && motorRight.isBusy())
{
telemetry.addData("encoder-fwd" , motorLeft.getCurrentPosition()
+ "busy" + motorLeft.isBusy() + motorRight.getCurrentPosition()
+ "busy" + motorRight.isBusy());
telemetry.update();
idle();
}
while(motorLeft.isBusy() && motorRight.isBusy())
{
int a=motorRight.getCurrentPosition();
Thread.sleep(200);
int b=motorRight.getCurrentPosition();
if (a == b) {
motorRight.setPower(0.0);
}
int c=motorLeft.getCurrentPosition();
Thread.sleep(200);
int d=motorLeft.getCurrentPosition();
if (c == d) {
motorLeft.setPower(0.0);
}
}
}
public void TurnLeft( double degrees, double power, boolean turnStyle)
{
double turn = ROBOT_CIRC * (degrees / 360);
if (turnStyle == true)
{
motorLeft.setDirection(DcMotor.Direction.FORWARD);
motorRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
motorRight.setPower(power);
motorRight.setTargetPosition(getEncoderClicks(turn));
motorLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
motorLeft.setPower(power);
motorLeft.setTargetPosition(getEncoderClicks(turn));
} else {
motorRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
motorRight.setPower(power);
motorRight.setTargetPosition(getEncoderClicks(turn));
}
motorLeft.setPower(0.0);
motorRight.setPower(0.0);
while (opModeIsActive() && motorRight.isBusy())
{
telemetry.addData("encoder-turn-left" , motorRight.getCurrentPosition()
+ "busy" + motorRight.isBusy());
telemetry.update();
idle();
}
}
public void TurnRight(double degrees, double power, boolean turnStyle) {
double turn = ROBOT_CIRC * (degrees / 360);
if (turnStyle == true) {
motorRight.setDirection(DcMotor.Direction.REVERSE);
motorRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
motorRight.setPower(power);
motorRight.setTargetPosition(getEncoderClicks(turn));
motorLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
motorLeft.setPower(power);
motorLeft.setTargetPosition(getEncoderClicks(turn));
} else{
motorLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
motorLeft.setPower(power);
motorLeft.setTargetPosition(getEncoderClicks(turn));}
motorLeft.setPower(0.0);
motorRight.setPower(0.0);
while (opModeIsActive() && motorLeft.isBusy())
{
telemetry.addData("encoder-turn-right" , motorLeft.getCurrentPosition() + "busy" + motorLeft.isBusy());
telemetry.update();
idle();
}
}
//-
//Adding a comment to test commit from Windows CL
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment