 
 
 void loop() {
-  
+  loopIndex++;
+
   int position = qtra.readLine(sensorValues);
-  int M1 = 100;
-  int M2 = 100;
-  float KP = 0.001; //floating-point proportional constant
-  float KD = 3;   //floating-point derivative constant
-  int error = position - 2500;
+  int M1 = 200;
+  int M2 = 200;
+  float KP = 0.1; //floating-point proportional constant
+  float KD = 1;   //floating-point derivative constant
+  int propotionalToMid = position - 2500;
 
-  int motorSpeed = KP * error + KD * (error - lastError);
-  lastError = error;
+  int motorSpeed = KP * propotionalToMid + KD * (propotionalToMid - lastError);
+  lastError = propotionalToMid;
 
-  int m1Speed = M1 + motorSpeed;
-  int m2Speed = M2 - motorSpeed;
+  int m2Speed = M2 + motorSpeed;
+  int m1Speed = M1 - motorSpeed;
 
-  if (m1Speed < 0)
-    m1Speed = 0;
-  if (m2Speed < 0)
-    m2Speed = 0;
+  if (m1Speed < 0) m1Speed = 0;
+  if (m2Speed < 0) m2Speed = 0;
 
-  if (m1Speed > 180)
-    m1Speed = 180;
-  if (m2Speed > 180)
-    m2Speed = 180;
+  if (m1Speed > 180) m1Speed = 180;
+  if (m2Speed > 180) m2Speed = 180;
 
   motorA(m1Speed);
   motorB(m2Speed);
 
-   Serial.print(motorSpeed);
-   Serial.print('\t');
-    Serial.print(m1Speed);
-    Serial.print('\t');
-    Serial.print(m2Speed);
-    Serial.print('\t');
-    Serial.println();
-    delay(500);
-    
-    
+  sensorOutput();
+
+  //   Serial.print(motorSpeed);
+  //   Serial.print('\t');
+  //    Serial.print(m1Speed);
+  //    Serial.print('\t');
+  //    Serial.print(m2Speed);
+  //    Serial.print('\t');
+  //    Serial.println();
+
+
 }
 
 void motorA(int speed) {

#include <QTRSensors.h>

int lastError = 0;

#define NUM_SENSORS             6  // number of sensors used
#define NUM_SAMPLES_PER_SENSOR  4  // average 4 analog samples per sensor reading


void sensorOutput()
{
  if (loopIndex % 100 == 0) {
    // read calibrated sensor values and obtain a measure of the line position from 0 to 5000
    // To get raw sensor values, call:
    //  qtra.read(sensorValues); instead of unsigned int position = qtra.readLine(sensorValues);
    unsigned int position = qtra.readLine(sensorValues);

    // print the sensor values as numbers from 0 to 1000, where 0 means maximum reflectance and
    // 1000 means minimum reflectance, followed by the line position
    for (unsigned char i = 0; i < NUM_SENSORS; i++)
    {
      Serial.print(sensorValues[i]);
      Serial.print('\t');

    }
    //Serial.println(); // uncomment this line if you are using raw values
    Serial.println((int)position - 2500); // comment this line out if you are using raw values
  }
}

