#pragma config(Sensor, S1,     LS,                  sensorLightActive)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main(){
  float offset;
  float Kp = 7;
  float Ki = 0.00018;
  float Kd = 0.00018;
  float Tp = 40;
  float error;
  float integral;
  float derivative;
  float lastError;
  float turn;
  float powerA;
  float powerC;
  int tooLow;
   int tooHigh;

  wait10Msec(200);
  PlaySound(soundBlip);
  tooLow = SensorValue(LS);
  wait10Msec(200);
  tooHigh = SensorValue(LS);
  PlaySound(soundBlip);
  offset = (tooLow+tooHigh)/2;

  while(true){
    error = SensorValue(LS)-offset;
    integral = integral+error;
    derivative = error-lastError;
    turn = (Kp*error)+(Ki*integral)+(Kd*derivative);
    powerA = Tp-turn;
    powerC = Tp+turn;
    motor[motorA] = powerA;
    motor[motorC] = powerC;
    lastError = error;
  }
}