public void rest() {
fr.setPower(0);
fl.setPower(0);
bl.setPower(0);
br.setPower(0);
collector.setPower(0);
}
@Override
public void loop() {
/* reset the navX-Model device yaw angle so that whatever direction */
/* it is currently pointing will be zero degrees. */
if (first_iteration) {
first_iteration = false;
navx_device.zeroYaw();
yawPIDResult = new navXPIDController.PIDResult();
}
/* Wait for new Yaw PID output values, then update the motors
with the new PID value with each new output value.
*/
/* Drive straight forward at 1/2 of full drive speed */
double drive_speed = 0.5;
if (yawPIDController.isNewUpdateAvailable(yawPIDResult)) {
if (yawPIDResult.isOnTarget()) {
leftMotor.setPower(drive_speed);
rightMotor.setPower(drive_speed);
} else {
double output = yawPIDResult.getOutput();
leftMotor.setPower(drive_speed + output);
rightMotor.setPower(drive_speed - output);
}
} else {
/* No sensor update has been received since the last time */
/* the loop() function was invoked. Therefore, there's no */
/* need to update the motors at this time. */
}
}
public void run1() {
motorW();
rnctr++;
leftmotor.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
rightmotor.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
telemetry.addData("runct", rnctr);
while (togo < inchtorun) {
motorW();
rightmotor.setTargetPosition(rightPos * togo);
leftmotor.setTargetPosition(leftPos * togo);
leftmotor.setPower(.5);
rightmotor.setPower(.5);
while (ctr < 50000000) {
ctr++;
}
motorR();
while (leftmotor.getCurrentPosition() != leftPos) {}
togo++;
}
motorW();
leftmotor.setPower(0);
rightmotor.setPower(0);
// return;
}
@Override
public void stop(RobotContext ctx, LinkedList<Object> out) throws Exception {
driveForwardLeft.setPower(0);
driveForwardRight.setPower(0);
driveRearRight.setPower(0);
driveRearRight.setPower(0);
}
@Override
public void loop() {
telemetry.addData("rightEncoder = ", motorRight.getCurrentPosition());
if (motorRight.getCurrentPosition() <= -5000) {
if (finishedForward) {
finishedForward = false;
finishedTurning = true;
telemetry.addData("I did it!", motorRight.getCurrentPosition());
// wait(waitValue);
newValue = motorRight.getCurrentPosition() - 250;
motorRight.setTargetPosition(-newValue);
motorRight.setPower(.50);
motorLeft.setPower(-.50);
}
}
if (motorRight.getCurrentPosition() <= -newValue) {
if (finishedTurning) {
finishedTurning = false;
motorRight.setPower(0);
motorLeft.setPower(0);
}
}
}
private void lift(double power) {
try {
liftLeft.setPower(power);
liftRight.setPower(power);
} catch (Exception ex) {
}
}
@Override
public void loop() {
double rightDrive;
double leftDrive;
rightDrive = -gamepad1.right_stick_y;
leftDrive = -gamepad1.left_stick_y;
if (gamepad1.dpad_up) {
rightDrive = 0.3;
leftDrive = 0.3;
} else if (gamepad1.dpad_down) {
rightDrive = -0.3;
leftDrive = -0.3;
} else if (gamepad1.dpad_right) {
rightDrive = -0.3;
leftDrive = 0.3;
} else if (gamepad1.dpad_left) {
rightDrive = 0.3;
leftDrive = -0.3;
}
motorRight.setPower(rightDrive);
motorLeft.setPower(leftDrive);
if (gamepad1.a) {
servoClimberDumper.setPosition(1.0);
} else {
servoClimberDumper.setPosition(0.0);
}
telemetry.addData("trackLifter", Integer.toString(trackLifter.getCurrentPosition()));
telemetry.addData("liftCheck", liftCheck.isPressed());
telemetry.addData("ENCLeft", Integer.toString(motorLeft.getCurrentPosition()));
telemetry.addData("ENCRight", Integer.toString(motorRight.getCurrentPosition()));
telemetry.addData("trigger", gamepad1.right_trigger);
}
@Override
public void loop() {
// Gamepad 1 - Drive Controller
float rightDriveMotorSpeed = -gamepad1.right_stick_y;
float leftDriveMotorSpeed = -gamepad1.left_stick_y;
rightDriveMotorSpeed = Range.clip(rightDriveMotorSpeed, -1, 1);
leftDriveMotorSpeed = Range.clip(leftDriveMotorSpeed, -1, 1);
rightDriveMotorSpeed = (float) scaleInput(rightDriveMotorSpeed);
leftDriveMotorSpeed = (float) scaleInput(leftDriveMotorSpeed);
System.out.println(rightDriveMotorSpeed);
driveRight.setPower(rightDriveMotorSpeed);
driveLeft.setPower(leftDriveMotorSpeed);
if (gamepad1.a & hookServoClick == 0) {
hookServoPosition -= hookServoDelta;
System.out.println(hookServoPosition);
hookServoClick = 1;
hookServo.setPosition(hookServoPosition);
}
if (gamepad1.a & hookServoClick == 1) {
hookServoPosition += hookServoDelta;
System.out.println(hookServoPosition);
hookServoClick = 0;
hookServo.setPosition(hookServoPosition);
}
if (gamepad1.x & triggerServoClick == 0) {
triggerServoPosition -= triggerServoDelta;
System.out.println(triggerServoPosition);
triggerServoClick = 1;
triggerServo.setPosition(triggerServoPosition);
}
if (gamepad1.x & triggerServoClick == 1) {
triggerServoPosition += triggerServoDelta;
System.out.println(triggerServoPosition);
triggerServoClick = 0;
triggerServo.setPosition(triggerServoPosition);
}
// Gamepad 2
float armStage1MotorSpeed = -gamepad2.right_stick_y;
float armStage2MotorSpeed = -gamepad2.left_stick_y;
armStage1MotorSpeed = Range.clip(armStage1MotorSpeed, -1, 1);
armStage2MotorSpeed = Range.clip(armStage2MotorSpeed, -1, 1);
armStage1MotorSpeed = (float) scaleInput(armStage1MotorSpeed);
armStage2MotorSpeed = (float) scaleInput(armStage2MotorSpeed);
armStage1.setPower(armStage1MotorSpeed);
armStage2.setPower(armStage2MotorSpeed);
}
/* Initialize standard Hardware interfaces */
public void init(HardwareMap ahwMap) {
// Save reference to Hardware map
hwMap = ahwMap;
// Define and Initialize Motors
leftMotor = hwMap.dcMotor.get("left_drive");
rightMotor = hwMap.dcMotor.get("right_drive");
armMotor = hwMap.dcMotor.get("left_arm");
leftMotor.setDirection(DcMotor.Direction.FORWARD); // Set to REVERSE if using AndyMark motors
rightMotor.setDirection(DcMotor.Direction.REVERSE); // Set to FORWARD if using AndyMark motors
// Set all motors to zero power
leftMotor.setPower(0);
rightMotor.setPower(0);
armMotor.setPower(0);
// Set all motors to run without encoders.
// May want to use RUN_USING_ENCODERS if encoders are installed.
leftMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
rightMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
armMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
// Define and initialize ALL installed servos.
leftClaw = hwMap.servo.get("left_hand");
rightClaw = hwMap.servo.get("right_hand");
leftClaw.setPosition(MID_SERVO);
rightClaw.setPosition(MID_SERVO);
}
/**
* This method does all of the things required to make each side of the robot move forward by the
* given amounts. Seriously, It changes the channelMode, It calculates the pulse counts
* Everything! If you understand java and can't use this, you don't deserve to!
*/
public void measuredDrive(double Rdist, double Ldist) {
double frrot = Rdist / frontCirc;
double flrot = Ldist / frontCirc;
double rPow = .5, lPow = .5;
leftMotorf.setTargetPosition((int) flrot * pulsePerRot);
rightMotorf.setTargetPosition((int) frrot * pulsePerRot);
if (!encodersEnabled) {
enableEncoders(true);
}
if (Rdist < 0) {
rPow = -rPow;
} else if (Rdist == 0) {
rPow = 0;
}
if (Ldist < 0) {
lPow = -lPow;
} else if (Ldist == 0) {
lPow = 0;
}
leftMotorf.setPower(frontRatio * lPow);
rightMotorf.setPower(frontRatio * rPow);
}
@Override
public void loop() {
telemetry.addData("encoders", motor1.getCurrentPosition());
telemetry.addData("x", x);
switch (x) {
case 0:
motor1.setPower(0.2);
motor1.setTargetPosition(1120);
motor1.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
if (motor1.getCurrentPosition() < 1125 && motor1.getCurrentPosition() > 1115) {
motor1.setPower(0.0);
x = 1;
motor1.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
}
break;
case 1:
motor1.setPower(0.1);
motor1.setTargetPosition(2400);
motor1.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
if (motor1.getCurrentPosition() < 2405 && motor1.getCurrentPosition() > 2395) {
motor1.setPower(0.0);
x = 2;
motor1.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
}
}
}
@Override
public void runOpMode() throws InterruptedException {
LeftDrive = hardwareMap.dcMotor.get("LeftFront");
RightDrive = hardwareMap.dcMotor.get("RightFront");
RightDrive.setDirection(DcMotor.Direction.REVERSE);
waitForStart();
LeftDrive.setPower(0.5);
RightDrive.setPower(0.5);
sleep(2000);
LeftDrive.setPower(0);
RightDrive.setPower(0);
sleep(500);
LeftDrive.setPower(-0.5);
RightDrive.setPower(-0.5);
sleep(1100);
LeftDrive.setPower(1.0);
RightDrive.setPower(1.0);
sleep(2650);
LeftDrive.setPower(0);
RightDrive.setPower(0);
}
public void moveState(double power, double inches, int state, int change) {
int stateMove = 1;
telemetry.addData("Move state: ", stateMove);
switch (stateMove) {
case 1:
leftTread.setPower(power);
rightTread.setPower(power);
if (Math.abs(leftTread.getCurrentPosition()) >= (inches * TICKS_PER_INCH)) {
stateMove = 2;
}
break;
case 2:
leftTread.setPower(0.0);
rightTread.setPower(0.0);
stateMove = 3;
break;
case 3:
leftTread.setMode(DcMotor.RunMode.RESET_ENCODERS);
rightTread.setMode(DcMotor.RunMode.RESET_ENCODERS);
stateMove = 4;
break;
case 4:
if ((leftTread.getCurrentPosition() == 0) && (rightTread.getCurrentPosition() == 0)) {
leftTread.setMode(DcMotor.RunMode.RUN_USING_ENCODERS);
rightTread.setMode(DcMotor.RunMode.RUN_USING_ENCODERS);
}
state = change;
break;
default:
// End of switch (or bug).
break;
}
}
public void turnState(double power, double degrees, int state, int change) {
int stateTurn = 1;
telemetry.addData("Turn state: ", stateTurn);
switch (stateTurn) {
case 1:
leftTread.setPower(power);
rightTread.setPower(-power);
if (Math.abs(leftTread.getCurrentPosition()) >= (degrees * TICKS_PER_DEGREE)) {
stateTurn = 2;
}
break;
case 2:
leftTread.setPower(0.0);
rightTread.setPower(0.0);
stateTurn = 3;
break;
case 3:
leftTread.setMode(DcMotor.RunMode.RESET_ENCODERS);
rightTread.setMode(DcMotor.RunMode.RESET_ENCODERS);
stateTurn = 4;
break;
case 4:
if ((leftTread.getCurrentPosition() == 0) && (rightTread.getCurrentPosition() == 0)) {
leftTread.setMode(DcMotor.RunMode.RUN_USING_ENCODERS);
rightTread.setMode(DcMotor.RunMode.RUN_USING_ENCODERS);
}
state = change;
break;
default:
// End of switch (or bug).
break;
}
}
@Override
public void loop() {
if (this.time < 6.5d) {
goForward(-1.0);
} else if (this.time < 8.0d) {
turnRight(1.0);
} else if (this.time < 11d) {
goForward(-1.0);
} else if (this.time < 14d) {
turnRight(1.0);
armUp = true;
motorArm.setPower(0.5);
if (this.time > 11.2d) {
motorArm.setPower(0.1);
}
} else if (this.time < 16) {
goForward(1.0);
motorSlide.setPower(1.0);
} else if (this.time < 18) {
stopMotors();
motorSlide.setPower(0.0);
forwardBack.setPosition(0.75);
} else if (this.time < 22) {
motorSlide.setPower(-1.0);
forwardBack.setPosition(0.5);
}
telemetry.addData("Text", "*** Robot Data***");
telemetry.addData("..", "time: " + String.format("%.2f", this.time));
}
/*
* This method will be called repeatedly in a loop
*
* @see com.qualcomm.robotcore.eventloop.opmode.OpMode#run()
*/
@Override
public void loop() {
/*
* Gamepad 1
*
* Gamepad 1 controls the motors via the left stick, and it controls the
* wrist/claw via the a,b, x, y buttons
*/
// throttle: left_stick_y ranges from -1 to 1, where -1 is full up, and
// 1 is full down
// direction: left_stick_x ranges from -1 to 1, where -1 is full left
// and 1 is full right
float left_throttle = -gamepad1.left_stick_y;
float right_throttle = -gamepad1.right_stick_y;
// scale the joystick value to make it easier to control
// the robot more precisely at slower speeds.
left_throttle = (float) scaleInput(left_throttle);
right_throttle = (float) scaleInput(right_throttle);
// write the values to the motors
front_right.setPower(right_throttle);
back_right.setPower(right_throttle);
front_left.setPower(left_throttle);
back_left.setPower(left_throttle);
// update the position of the arm.
if (gamepad1.y) {
// if the A button is pushed on gamepad1, increment the position of
// the arm servo.
spinnerPosition += spinnerDelta;
}
if (gamepad1.a) {
// if the Y button is pushed on gamepad1, decrease the position of
// the arm servo.
spinnerPosition -= spinnerDelta;
}
// clip the position values so that they never exceed their allowed range.
spinnerPosition = Range.clip(spinnerPosition, 0.0, 1.0);
// write position values to the wrist and claw servo
spinner.setPosition(spinnerPosition);
/*
* Send telemetry data back to driver station. Note that if we are using
* a legacy NXT-compatible motor controller, then the getPower() method
* will return a null value. The legacy NXT-compatible motor controllers
* are currently write only.
*/
telemetry.addData("Text", "*** Robot Data***");
telemetry.addData("arm", "arm: " + String.format("%.2f", armPosition));
telemetry.addData("claw", "claw: " + String.format("%.2f", clawPosition));
telemetry.addData("left tgt pwr", "left pwr: " + String.format("%.2f", left));
telemetry.addData("right tgt pwr", "right pwr: " + String.format("%.2f", right));
}
public void driveDistance(double ldist, double rdist, double power) throws InterruptedException {
int enc1;
int enc2;
enc1 = (int) Math.round(ldist * 33.65);
enc2 = (int) Math.round(rdist * 33.65);
leftMotor2.setMode(DcMotorController.RunMode.RESET_ENCODERS);
rightMotor2.setMode(DcMotorController.RunMode.RESET_ENCODERS);
waitOneFullHardwareCycle();
waitOneFullHardwareCycle();
waitOneFullHardwareCycle();
leftMotor2.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
rightMotor2.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
waitOneFullHardwareCycle();
leftMotor2.setTargetPosition(enc1);
rightMotor2.setTargetPosition(enc2);
leftMotor2.setPower(power);
rightMotor2.setPower(power);
waitOneFullHardwareCycle();
waitOneFullHardwareCycle();
waitOneFullHardwareCycle();
waitOneFullHardwareCycle();
waitOneFullHardwareCycle();
leftMotorController.setMotorControllerDeviceMode(
DcMotorController.DeviceMode.READ_ONLY); // Change to read
rightMotorController.setMotorControllerDeviceMode(DcMotorController.DeviceMode.READ_ONLY);
waitOneFullHardwareCycle();
while (counter == 1) {
if (isCloseto(leftMotor2.getCurrentPosition(), enc1)
&& isCloseto(rightMotor2.getCurrentPosition(), enc2)) {
counter = 2;
}
waitOneFullHardwareCycle();
}
waitOneFullHardwareCycle();
leftMotorController.setMotorControllerDeviceMode(
DcMotorController.DeviceMode.WRITE_ONLY); // Change to read
rightMotorController.setMotorControllerDeviceMode(DcMotorController.DeviceMode.WRITE_ONLY);
waitOneFullHardwareCycle();
leftMotor2.setPower(0);
rightMotor2.setPower(0);
leftMotor2.setMode(DcMotorController.RunMode.RESET_ENCODERS);
rightMotor2.setMode(DcMotorController.RunMode.RESET_ENCODERS);
waitOneFullHardwareCycle();
leftMotorController.setMotorControllerDeviceMode(
DcMotorController.DeviceMode.READ_ONLY); // Change to read
rightMotorController.setMotorControllerDeviceMode(DcMotorController.DeviceMode.READ_ONLY);
waitOneFullHardwareCycle();
telemetry.addData("Currentvalue", leftMotor2.getCurrentPosition());
waitOneFullHardwareCycle();
leftMotorController.setMotorControllerDeviceMode(
DcMotorController.DeviceMode.WRITE_ONLY); // Change to read
rightMotorController.setMotorControllerDeviceMode(DcMotorController.DeviceMode.WRITE_ONLY);
waitOneFullHardwareCycle();
leftMotor2.setMode(DcMotorController.RunMode.RESET_ENCODERS);
rightMotor2.setMode(DcMotorController.RunMode.RESET_ENCODERS);
counter = 1;
}
/*
* This method will be called repeatedly in a loop
*
* @see com.qualcomm.robotcore.eventloop.opmode.OpMode#run()
*/
@Override
public void loop() {
double reflection = 0.0;
double left, right = 0.0;
// keep manipulator out of the way.
arm.setPosition(armPosition);
claw.setPosition(clawPosition);
/*
* Use the 'time' variable of this op mode to determine
* how to adjust the motor power.
*/
if (this.time <= 1) {
// from 0 to 1 seconds, run the motors for five seconds.
left = 0.15;
right = 0.15;
} else if (this.time > 5 && this.time <= 8.5) {
// between 5 and 8.5 seconds, point turn right.
left = 0.15;
right = -0.15;
} else if (this.time > 8.5 && this.time <= 15) {
// between 8 and 15 seconds, idle.
left = 0.0;
right = 0.0;
} else if (this.time > 15d && this.time <= 20.75d) {
// between 15 and 20.75 seconds, point turn left.
left = -0.15;
right = 0.15;
} else {
// after 20.75 seconds, stop.
left = 0.0;
right = 0.0;
}
/*
* set the motor power
*/
motorRight.setPower(left);
motorLeft.setPower(right);
/*
* read the light sensor.
*/
// reflection = reflectedLight.getLightLevel();
/*
* Send telemetry data back to driver station. Note that if we are using
* a legacy NXT-compatible motor controller, then the getPower() method
* will return a null value. The legacy NXT-compatible motor controllers
* are currently write only.
*/
telemetry.addData("Text", "*** Robot Data***");
telemetry.addData("time", "elapsed time: " + Double.toString(this.time));
telemetry.addData("reflection", "reflection: " + Double.toString(reflection));
telemetry.addData("left tgt pwr", "left pwr: " + Double.toString(left));
telemetry.addData("right tgt pwr", "right pwr: " + Double.toString(right));
}
private void tankDrive(double leftY, double rightY) throws InterruptedException {
rightY = -rightY; // flip the power of the right side
leftfrontMotor.setPower(leftY); // set the according power to each motor
leftbackMotor.setPower(leftY);
rightfrontMotor.setPower(rightY);
rightbackMotor.setPower(rightY);
}
/**
* Applies power to lift motors based on value in {@code double direction, angle} set in {@link
* DriverStation#getNextDrivesysCmd()}.
*
* @param driverCommand {@link DriverCommand} object with values.
*/
public void applyCmd(DriverCommand driverCommand) {
if (liftArmLengthMotorAvailable) {
liftArmLengthMotor.setPower(driverCommand.linliftcmd.armLength);
}
if (liftAngleMotorAvailable) {
liftAngleMotor.setPower(driverCommand.linliftcmd.angle);
}
}
/**
* This method just sets the power of the Left and right motors to the provided values It also
* scales the power to the front motors.
*/
public void tankDrive(double leftVal, double rightVal) {
if (encodersEnabled) {
enableEncoders(false);
}
leftMotorf.setPower(frontRatio * leftVal);
rightMotorf.setPower(frontRatio * rightVal);
}
@Override
public void loop() {
float leftY = -gamepad1.left_stick_y;
float rightY = -gamepad1.right_stick_y;
rightBackMotor.setPower(rightY);
rightFrontMotor.setPower(rightY);
leftBackMotor.setPower(leftY);
leftFrontMotor.setPower(leftY);
}
public void init(HardwareMap hardwareMap) {
motorRight = hardwareMap.dcMotor.get("Motor.2"); // call motors
motorLeft = hardwareMap.dcMotor.get("Motor.1");
motorLeft.setDirection(DcMotor.Direction.REVERSE);
motorRight.setDirection(DcMotor.Direction.FORWARD);
motorLeft.setPower(100);
motorRight.setPower(100); // one hunna//
}
public void setMotorPowerUniform(double power, boolean backwards) {
int direction = 1;
if (backwards) {
direction = -1;
}
fr.setPower(direction * power);
fl.setPower(direction * power);
bl.setPower(direction * power);
br.setPower(direction * power);
collector.setPower(-1 * Keys.COLLECTOR_POWER);
}
public void driveLimitless(double flspeed, double frspeed, double blspeed, double brspeed) {
// fl.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// fr.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// bl.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// br.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
fl.setPower(flspeed);
fr.setPower(frspeed);
bl.setPower(blspeed);
br.setPower(brspeed);
}
public void driveEncoder(double leftPower, double rightPower, double ticks) {
rightDrive.setTargetPosition(
(int) ticks); // may need to cast inches to int or long idk also may be in other units
rightDriveB.setTargetPosition((int) ticks);
leftDrive.setTargetPosition((int) ticks);
leftDriveB.setTargetPosition((int) ticks);
rightDrive.setPower(rightPower);
rightDriveB.setPower(rightPower);
leftDrive.setPower(leftPower);
leftDriveB.setPower(leftPower);
}
@Override
public void loop() {
float l_gp1_left_stick_y = gamepad1.left_stick_y;
float l_left_drive_power = (float) scale_motor_power(l_gp1_left_stick_y);
float l_gp1_right_stick_y = -gamepad1.right_stick_y;
float l_right_drive_power = (float) scale_motor_power(l_gp1_right_stick_y);
rightDrive.setPower(l_right_drive_power);
leftDrive.setPower(l_left_drive_power);
rightDriveB.setPower(l_right_drive_power);
leftDriveB.setPower(l_left_drive_power);
}
@Override
public void start() {
motorRight.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
motorLeft.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
// currentState = 1;
telemetry.addData("Text", "***Starting***");
motorRight.setTargetPosition(5000);
motorRight.setPower(.75);
// motorLeft.setTargetPosition(2500);
motorLeft.setPower(.75);
}
@Override
public void init() {
motorHangL = hardwareMap.dcMotor.get("motorHangL");
motorHangR = hardwareMap.dcMotor.get("motorHangR");
motorSpinner = hardwareMap.dcMotor.get("manipulator");
ramp = hardwareMap.servo.get("ramp");
drop = hardwareMap.servo.get("drop");
claw = hardwareMap.servo.get("claw");
motorHangL.setPower(0);
motorHangR.setPower(0);
motorSpinner.setPower(0);
boxBelt.setPosition(.5);
ramp.setPosition(0);
drop.setPosition(0);
}
public void turnRightgyro(double degreesToTurn, GyroSensor gyro, float power) {
long timeNow;
long timeLast = System.currentTimeMillis();
double gyroOffset = gyro.getRotation();
double degreesLeft = 0.0;
while (degreesLeft < degreesToTurn) {
timeNow = System.currentTimeMillis();
degreesLeft = (gyro.getRotation() - gyroOffset) * (timeNow - timeLast) / 1000;
timeLast = timeNow;
a.setPower(-power);
b.setPower(-power);
c.setPower(power);
c.setPower(power);
}
}
