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