 void loop() { 
   
   double yellow_dist = pollProximity();
-  yellow_dist /= 10000;
-  double black_dist = -yellow_dist;
+  double black_dist = FIDELITY - yellow_dist;
+  yellow_dist /= 10; // 10000;
+  black_dist  /= 10; // 10000;
+  
+
+  
   if(yellow_dist > 0.003){
     Input = yellow_dist;  
   }else if(black_dist > 0.003){

double Setpoint, Input, Output;

//Specify the links and initial tuning parameters
double Kp = 32609.0;
double Ki = 34517.0;
double Kd = 7086;

PID pid(&Input, &Output, &Setpoint, Kp,Ki,Kd, DIRECT);


  }else{
    Input = 0.0;  
  }
  
  pid.Compute();
  Serial.print("Input: "); Serial.print(Input, DEC); Serial.print("\t Output: "); Serial.print(Output, DEC); Serial.print("\r");
  //Serial.print("Input to PID: "); Serial.println(Input, DEC);
  //Serial.print("Ylw: "); Serial.print(yellow_dist, DEC); Serial.print("\tBlk: "); Serial.println(black_dist, DEC);
  delay(100);
}

/*
