Untitled

  static void addSensorReadingsToMap(MapperApp mapper, Field translationField, double x, double y, double a) {
    // Use the actual location.  Comment out these three lines when using estimated position
    double values[] = translationField.getSFVec3f();
    x = (values[0]*100);
    y = -(values[2]*100);

    // Offsets for all IR sensors (indices 0-8)
    float xOffsets[] = {-4.038f, 1.481f, 4.885f, 6.560f, 6.560f, 4.885f, 1.481f, -4.038f, -5.502f};
    float yOffsets[] = {4.956f, 6.296f, 4.538f, 1.552f, -1.552f, -4.538f, -6.296f, -4.956f, 0.000f};
    int   aOffsets[] = {140, 76, 43, 13, -13, -43, -76, -140, 180};
    
    // ADD YOUR CODE HERE TO READ ALL 9 SENSORS, COMPUTE THE OBJECT LOCATIONS,
    // AND APPLY THEM TO THE MAP Y ADDING OBJECT POINTS
    for (int i = 0; i < 9; i++) {
        double dSensor = rangeSensors[i].getValue()*100; 

        if (dSensor > 1000) continue;
        double aTotal = Math.toRadians(a + aOffsets[i]);  
        double xRel = xOffsets[i] + dSensor * Math.cos(aTotal); 
        double yRel = yOffsets[i] + dSensor * Math.sin(aTotal); 
        double xObject = x + xRel;
        double yObject = y + yRel;

        mapper.addObjectPoint((float)xObject, (float)yObject);

    }
}