// ****************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; }
/* * Main loop */ @Override public void runOpMode() throws InterruptedException { // lookup table to find distance compared to voltage based on our measurements. We may need to // recalculate these using a more exact measurement tool, but they are within 1 cm. // // We measured these by taking physical measurements with the sensor. double[] distanceCm = { 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30 }; // These need to be re-calibrated. Will update in next release. double[] voltage = { 3.051757813, 2.685546875, 2.255859375, 1.93359375, 1.684570313, 1.489257813, 1.357421875, 1.240234375, 1.137695313, 1.044921875, 0.9814453125, 0.9228515625, 0.8740234375, 0.8349609375, 0.7958984375, 0.76171875, 0.7177734375, 0.6982421875, 0.6591796875, 0.6396484375, 0.6201171875, 0.576171875, 0.556640625, 0.5322265625, 0.5126953125, 0.5126953125, 0.4931640625, 0.4736328125 }; /* * Initialize the hardware */ AnalogInput distanceSensor; distanceSensor = hardwareMap.analogInput.get("distance"); // wait for the start button to be pressed. waitForStart(); while (opModeIsActive()) { // Reading voltage double voltreading = (float) distanceSensor.getVoltage(); // convert voltage to distance (cm) double distance = -999; // check to see if reading is out of range if (voltreading > 3.05 || voltreading < 0.48) { distance = -999; // missing value } else { // loop through our lookup table to get the two voltage points on either side // of the voltage we read. int arrayLength = distanceCm.length; for (int i = 0; i < (arrayLength - 1); i++) { // when we find the voltage, use linear interpolation to calculate the exact // value if (voltreading <= voltage[i] && voltreading >= voltage[i + 1]) { distance = distanceCm[i] + ((voltreading - voltage[i]) * (distanceCm[i + 1] - distanceCm[i])) / (voltage[i + 1] - voltage[i]); } } } telemetry.addData("time", "elapsed time: " + Double.toString(this.time)); telemetry.addData("raw val", "reflection: " + Double.toString(voltreading)); // this is our calculated value telemetry.addData("Distance", "cm: " + Double.toString(distance)); telemetry.update(); } }
/* * 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()), ""); }