// ****************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);
}
