#include <QTRSensors.h>

QTRSensors qtr;

const uint8_t SensorCount = 8;
uint16_t sensorValues[SensorCount];


int MODE = 8;

int left_motor_in = 6;
int left_motor_out = 9;

int right_motor_in = 3;
int right_motor_out = 5;

void setup() {
  Serial.begin(9600);
  Serial.println("Start");
  init_motor();
  
  qtr.setTypeRC();
  qtr.setSensorPins((const uint8_t[]){10, 11, 12, 14, 15, 16, 18, 19}, SensorCount);
  qtr.setEmitterPin(7);

  delay(500);
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, HIGH);// turn on Arduino's LED to indicate we are in calibration mode
  Serial.println("Starting Calibration");

  // 2.5 ms RC read timeout (default) * 10 reads per calibrate() call
  // = ~25 ms per calibrate() call.
  // Call calibrate() 400 times to make calibration take about 10 seconds.
  for (uint16_t i = 0; i < 400; i++)
  {
    qtr.calibrate();
  }
  digitalWrite(LED_BUILTIN, LOW); // turn off Arduino's LED to indicate we are through with calibration
  Serial.println("Calibration finished");

  // print the calibration minimum values measured when emitters were on
  Serial.begin(9600);
  for (uint8_t i = 0; i < SensorCount; i++)
  {
    Serial.print(qtr.calibrationOn.minimum[i]);
    Serial.print(' ');
  }
  Serial.println();

  // print the calibration maximum values measured when emitters were on
  for (uint8_t i = 0; i < SensorCount; i++)
  {
    Serial.print(qtr.calibrationOn.maximum[i]);
    Serial.print(' ');
  }
  Serial.println();
  Serial.println();
  delay(1000);
}

void loop() {
  uint16_t position = qtr.readLineBlack(sensorValues);
  
   for (uint8_t i = 0; i < SensorCount; i++)
      {
        Serial.print(sensorValues[i]);
        Serial.print('\t');
      }
      Serial.println(position);

      delay(250);

      if (position >= 3000 and position <= 4300) {
        left_motor_go(200);
        right_motor_go(200);
        Serial.print(position);Serial.println("center");
      }

      if (position < 3000) {
        left_motor_go(200);
        right_motor_go(0);
        Serial.print(position);Serial.println("left");
      }

      if (position > 4300) {
        left_motor_go(0);
        right_motor_go(200);
        Serial.print(position);Serial.println("right");
      }

      
}
  
void init_motor(){
  pinMode(left_motor_in, OUTPUT);
  pinMode(left_motor_out, OUTPUT);
  
  pinMode(right_motor_in, OUTPUT);
  pinMode(right_motor_out, OUTPUT);

  pinMode(MODE, OUTPUT);
  digitalWrite(MODE, HIGH);
  }

//range:0<value<255
void left_motor_go(int motor_speed){
  analogWrite(left_motor_in, motor_speed);
  digitalWrite(left_motor_out, LOW);
 }
//range:0<value<255
void right_motor_go(int motor_speed){
  analogWrite(right_motor_in, motor_speed);
  digitalWrite(right_motor_out, LOW);
}