   }
   else digitalWrite(6,LOW);
 }
-
+

volatile int address = 0;

volatile float dist[3] = {655.35, 655.35, 655.35};
float beacons[3][2] = {{0.0f, 2.94f},
                       {0.0f, 0.0f},
                       {2.94f, 0.0f}};

float A[3][3];
void setup() {

}

void newRange() {
    
  address = DW1000Ranging.getDistantDevice()->getShortAddress();

  range = DW1000Ranging.getDistantDevice()->getRange();
  if(range < 0) address = NULL;
  if(address == BEACON_1_ADDRESS){
    range += CALIBRATE_OFFSET?0.0f:BEACON_1_OFFSET;
    range /= CALIBRATE_MULTIPLIER?1.0f:BEACON_1_MULTIPLIER;
    dist[0] = range;
    if(CALIBRATE_OFFSET){
      if(dist[0] < shortestDistanceBeacon1 && dist[0] > 0){
        shortestDistanceBeacon1 = dist[0];
      }
    }
  }
  else if(address == BEACON_2_ADDRESS){
    range += CALIBRATE_OFFSET?0.0f:BEACON_2_OFFSET;
    range /= CALIBRATE_MULTIPLIER?1.0f:BEACON_2_MULTIPLIER;
    dist[1] = range;
    if(CALIBRATE_OFFSET){
      if(dist[1] < shortestDistanceBeacon2 && dist[1] > 0){
        shortestDistanceBeacon2 = dist[1];
      }
    }
  }
  else if(address == BEACON_3_ADDRESS){
    range += CALIBRATE_OFFSET?0.0f:BEACON_3_OFFSET;
    range /= CALIBRATE_MULTIPLIER?1.0f:BEACON_3_MULTIPLIER;
    dist[2] = range;
    if(CALIBRATE_OFFSET){
      if(dist[2] < shortestDistanceBeacon3 && dist[2] > 0){
        shortestDistanceBeacon3 = dist[2];
      }
    }
  }
  if(CALIBRATE_OFFSET){
    Serial.print("Offset 1: ");Serial.print(shortestDistanceBeacon1);Serial.print(" Offset 2: ");Serial.print(shortestDistanceBeacon2);Serial.print(" Offset 3: ");Serial.print(shortestDistanceBeacon3);
  }
  else if(CALIBRATE_MULTIPLIER){
    Serial.print("MULT 1: ");Serial.print(dist[0]/CALIBRATION_DISTANCE);Serial.print(" MULT 2: ");Serial.print(dist[1]/CALIBRATION_DISTANCE);Serial.print(" MULT 3: ");Serial.print(dist[2]/CALIBRATION_DISTANCE);
  }
  else{
    Serial.print("1: ");Serial.print(dist[0]);Serial.print(" 2: ");Serial.print(dist[1]);Serial.print(" 3: ");Serial.print(dist[2]);
  }
  
  // ITERATIVE STEPS BELOW

