Untitled

static void addSensorReadingsToMap(MapperApp mapper, Field translationField) {
    // Get the actual robot locationd and direction
    double values[] = translationField.getSFVec3f();
    double x = (values[0]*100);
    double y = -(values[2]*100);
    double a = getCompassReadingInDegrees(compass);
    
    // Offsets for all IR sensors (indices 0-8) and Ultrasonic sensors (indices 9-13)
    float xOffsets[] = {-4.038f, 1.481f, 4.885f, 6.560f, 6.560f, 4.885f, 1.481f, -4.038f, -5.502f, 0.476f, 4.442f, 5.643f, 4.268f, 0.476f};
    float yOffsets[] = {4.956f, 6.296f, 4.538f, 1.552f, -1.552f, -4.538f, -6.296f, -4.956f, 0.000f, 5.961f, 4.268f, 0.000f, -4.442f, -5.961f};
    int   aOffsets[] = {140, 76, 43, 13, -13, -43, -76, -140, 180, 90, 46, 0, -46, -90};

    // Compute 9 object locations ... one for each IR sensor reading
    for (int i=0; i<9; i++) {
      double d = rangeSensors[i].getValue() * 100;
      double sensorX = x + xOffsets[i]*Math.cos(Math.toRadians(a)) - yOffsets[i]*Math.sin(Math.toRadians(a));
      double sensorY = y + yOffsets[i]*Math.cos(Math.toRadians(a)) + xOffsets[i]*Math.sin(Math.toRadians(a));

          
      //System.out.printf("(X,Y),A = (%2.1f, %2.1f, %2.1f)    OBJ(%2.1f,%2.1f)    d=%2.1f\n", X, Y, A, objX, objY, d);
      if (d < 120){ // This only records on the map if it was within the specified distance
        // mapper.addObjectPoint((float)(sensorX + d*Math.cos(Math.toRadians(a + aOffsets[i]))), 
                              // (float)(sensorY + d*Math.sin(Math.toRadians(a + aOffsets[i]))));
          int sensorAngle = (int)a+aOffsets[i];
          double beamWidthInDegrees = 1.72;
          double distanceErrorAsPercent = 0.03;
          // mapper.applySensorModelReading(sensorX, sensorY, sensorAngle, d, beamWidthInDegrees, distanceErrorAsPercent);
        }
    }
    
    // Compute 5 object locations ... one for each Ultrasonic sensor reading
    for (int i=9; i<14; i++) {
      double d = ultrasonicSensors[i-9].getValue() * 100;
      double sensorX = x + xOffsets[i]*Math.cos(Math.toRadians(a)) - yOffsets[i]*Math.sin(Math.toRadians(a));
      double sensorY = y + yOffsets[i]*Math.cos(Math.toRadians(a)) + xOffsets[i]*Math.sin(Math.toRadians(a));
      
      if (d < 120) {
        int sensorAngle = (int)a+aOffsets[i];
        double beamWidthInDegrees = 22.9;
        double distanceErrorAsPercent = 0.066;
        mapper.applySensorModelReading(sensorX, sensorY, sensorAngle, d, beamWidthInDegrees, distanceErrorAsPercent);
      }
    }
  }