 double Setpoint, Input, Output;
 
 //Specify the links and initial tuning parameters
-double Kp = 32609.0;
-double Ki = 34517.0;
-double Kd = 7086;
+double Kp = 0.1; //32609.0;
+double Ki = 5.0; //34517.0;
+double Kd = 1; //7086;
 
 PID pid(&Input, &Output, &Setpoint, Kp,Ki,Kd, DIRECT);
 

  Setpoint = 0;

  //tell the PID to range between 0 and the full window size
  pid.SetOutputLimits(0, 1);

  //turn the PID on
  pid.SetMode(AUTOMATIC);  
}

  }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);
}

/*
