Skip to content

Instantly share code, notes, and snippets.

@battis
Created May 21, 2020 19:17
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 battis/eba7e76b964c52513b5d751291a74ddd to your computer and use it in GitHub Desktop.
Save battis/eba7e76b964c52513b5d751291a74ddd to your computer and use it in GitHub Desktop.
Now with a first pass at turning right...
package org.firstinspires.ftc.teamcode.autonomous;
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.DcMotorSimple;
@Autonomous
public class PrettySimpleAutonomous extends LinearOpMode {
private final int LF = 0, LR = 1, RF = 2, RR = 3;
private DcMotor[] motors;
private static final String[] motorNames = {"Left Front", "Left Rear", "Right Front", "Right Rear"};
private static final double distancePerTick = 0.00875; /* inches */
private void initializeMotors() {
// initialize motors
motors = new DcMotor[4];
int i = 0;
for (String deviceName : motorNames) {
motors[i] = hardwareMap.get(DcMotor.class, deviceName);
motors[i].setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
}
// reverse left motors so that we get the directional output we "expect"
motors[LF].setDirection(DcMotorSimple.Direction.REVERSE);
motors[LR].setDirection(DcMotorSimple.Direction.REVERSE);
}
private boolean motorsAreBusy() {
for (DcMotor motor : motors) {
if (motor.isBusy()) return true;
}
return false;
}
private void driveDistanceInInches(double distance) {
// set target distance (in ticks)
int encoderDistanceInTicks = (int) (distance / distancePerTick);
for (DcMotor motor : motors) {
motor.setTargetPosition(motor.getCurrentPosition() + encoderDistanceInTicks);
motor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
motor.setPower(0.5); // half-speed
}
while (opModeIsActive() && motorsAreBusy()) { /* wait */ }
for (DcMotor motor : motors) {
motor.setPower(0);
}
}
private void turnRight() {
// run the two left motors 1/4 of the circle
motors[LF].setTargetPosition(
motors[LF].getCurrentPosition() +
(int) (0.25 * 2 * Math.PI * 14 / distancePerTick)
);
motors[LF].setPower(0.5);
motors[LR].setTargetPosition(
motors[LR].getCurrentPosition() +
(int) (-0.25 * 2 * Math.PI * 14 / distancePerTick)
);
motors[LR].setPower(0.5);
// reset all motor encoder counters
for (DcMotor motor : motors) {
motor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
// wait for the motors to finish this process
while (opModeIsActive() && motorsAreBusy()) { /* wait */ }
for (DcMotor motor : motors) {
motor.setPower(0);
}
}
@Override
public void runOpMode() throws InterruptedException {
initializeMotors();
waitForStart();
driveDistanceInInches(24);
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment