// ****************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);
}
}
// returns sonar values in inches!!!
public double readSonar(AnalogInput sonar) {
double sValue = sonar.getValue();
sValue = sValue / 2;
return sValue;
}
/*
* This method will be called repeatedly in a loop
* @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop()
*/
@Override
public void loop() {
telemetry.addData(Integer.toString(range.getValue()), "");
}
