Ejemplo n.º 1
0
  // ****************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()), "");
 }