#include <QTRSensors.h>

#define Kp 0 // experiment to determine this, start by something small that just makes your bot follow the line at a slow speed
#define Kd 0 // experiment to determine this, slowly increase the speeds and adjust this value. ( Note: Kp < Kd)

#define rightMaxSpeed 200 // max speed of the robot
#define leftMaxSpeed 200 // max speed of the robot
#define rightBaseSpeed 150 // this is the speed at which the motors should spin when the robot is perfectly on the line
#define leftBaseSpeed 150  // this is the speed at which the motors should spin when the robot is perfectly on the line

#define NUM_SENSORS  8     // number of sensors used
#define TIMEOUT       2500  // waits for 2500 us for sensor outputs to go low
#define EMITTER_PIN   2     // emitter is controlled by digital pin 2

#define mRF   9
#define mRB   6
#define rightMotorPWM // UNUSED
#define mLF   10
#define mLB   11
#define leftMotorPWM // UNUSED
#define motorPower   // UNUSED

QTRSensorsRC qtr((unsigned char[]) {
  5, 4, 3, 2, 0, A1, A2, A3
} , NUM_SENSORS);//, TIMEOUT, EMITTER_PIN);

unsigned int sensorValues[NUM_SENSORS];

void setup()
{
  pinMode(mRF, OUTPUT);
  pinMode(mRB, OUTPUT);
  //pinMode(rightMotorPWM, OUTPUT);
  pinMode(mLF, OUTPUT);
  pinMode(mLB, OUTPUT);
  //pinMode(leftMotorPWM, OUTPUT);
  //pinMode(motorPower, OUTPUT);

  int i;
  for (int i = 0; i < 100; i++) // calibrate for sometime by sliding the sensors across the line, or you may use auto-calibration instead

    /* comment this part out for automatic calibration
      if ( i  < 25 || i >= 75 ) // turn to the left and right to expose the sensors to the brightest and darkest readings that may be encountered
       turn_right();
      else
       turn_left(); */
    qtr.calibrate();
  delay(20);
  wait();
  delay(2000); // wait for 2s to position the bot before entering the main loop

  /* comment out for serial printing

    Serial.begin(9600);
    for (int i = 0; i < NUM_SENSORS; i++)
    {
    Serial.print(qtr.calibratedMinimumOn[i]);
    Serial.print(' ');
    }
    Serial.println();

    for (int i = 0; i < NUM_SENSORS; i++)
    {
    Serial.print(qtr.calibratedMaximumOn[i]);
    Serial.print(' ');
    }
    Serial.println();
    Serial.println();
  */
}

int lastError = 0;

void loop()
{
  unsigned int sensors[8];
  int position = qtr.readLine(sensors,QTR_EMITTERS_ON,1); // get calibrated readings along with the line position, refer to the QTR Sensors Arduino Library for more details on line position.
  int error = position - 3500;

  int motorSpeed = Kp * error + Kd * (error - lastError);
  lastError = error;

  int rightMotorSpeed = rightBaseSpeed + motorSpeed;
  int leftMotorSpeed = leftBaseSpeed - motorSpeed;

  if (rightMotorSpeed > rightMaxSpeed ) rightMotorSpeed = rightMaxSpeed; // prevent the motor from going beyond max speed
  if (leftMotorSpeed > leftMaxSpeed ) leftMotorSpeed = leftMaxSpeed; // prevent the motor from going beyond max speed
  if (rightMotorSpeed < 0) rightMotorSpeed = 0; // keep the motor speed positive
  if (leftMotorSpeed < 0) leftMotorSpeed = 0; // keep the motor speed positive

  {
    //digitalWrite(motorPower, HIGH); // move forward with appropriate speeds
    analogWrite(mRF, rightMotorSpeed);
    digitalWrite(mRB, LOW);
    //analogWrite(rightMotorPWM, rightMotorSpeed);
    //digitalWrite(motorPower, HIGH);
    analogWrite(mLF, leftMotorSpeed);
    digitalWrite(mLB, LOW);
    //analogWrite(leftMotorPWM, leftMotorSpeed);
  }
}

void wait() {
  digitalWrite(mRF, LOW);
  digitalWrite(mRB, LOW);
  digitalWrite(mLF, LOW);
  digitalWrite(mLB, LOW);
}