// ****************SENSOR METHODS****************// public void moveToClosestObject() { double ultraRight, ultraLeft; double rightSpeed, leftSpeed; while (true) { ultraRight = sensorUltra_1.getValue(); // set these values to sensor readout ultraLeft = sensorUltra_2.getValue(); rightSpeed = SPEED_CONST * (ultraRight - RIGHT_TARGET_DISTANCE) + -RIGHT_STEERING_CONST * (ultraLeft - ultraRight); // speedl = kpd * (dl - tl) + kps * (dl - dr) leftSpeed = SPEED_CONST * (ultraLeft - LEFT_TARGET_DISTANCE) + -LEFT_STEERING_CONST * (ultraRight - ultraLeft); drive((float) rightSpeed, (float) leftSpeed); if (Math.abs(ultraRight - RIGHT_TARGET_DISTANCE) + Math.abs(ultraLeft - LEFT_TARGET_DISTANCE) < STOP_CONST) { break; } // wait(100); } }
@Override public void runOpMode() throws InterruptedException { resQLibrary.drive(1.0f, 1.0f); }