Created
May 21, 2020 19:17
-
-
Save battis/eba7e76b964c52513b5d751291a74ddd to your computer and use it in GitHub Desktop.
Now with a first pass at turning right...
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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