---

#include <QuantSensors.h>
int sensors[6];
QuantLine line;
void setup() {
pinMode(6, OUTPUT);
pinMode(9, OUTPUT);
Serial.begin(9600);
line.begin(true);
line.setLevel(600);
}
int speed_A = 170;
int speed_B = 155;

void loop() {
  readS();
   if(!sensors[0] && !sensors[1] && !sensors[2] && !sensors[3] && !sensors[4] && !sensors[5])  forward();
  else if((sensors[0] || sensors[1])&& (!sensors[2] && !sensors[3]) && (sensors[4] || sensors[5])) forward();
  else if(!sensors[5] && (sensors[0] || sensors[1] || sensors[2] || sensors[3] || sensors[4])) right();
  else if(!sensors[0] && (sensors[5] || sensors[1] || sensors[2] || sensors[3] || sensors[4])) left();
  else if(!sensors[4] && (sensors[0] || sensors[1] || sensors[2] || sensors[3] || sensors[5])) rightAlt();
  else if(!sensors[3] && !sensors[4] && (sensors[0] || sensors[1] || sensors[2] || sensors[5]))rightAlt();
  else if(!sensors[1] && !sensors[2] && (sensors[0] || sensors[3] || sensors[4] || sensors[5])) leftAlt();
  else if(!sensors[1] && (sensors[0] || sensors[5] || sensors[2] || sensors[3] || sensors[4])) leftAlt();
}
void moveTest(){
  forward();
  delay(2000);
  backward();
  delay(2000);
  left();
  delay(2000);
  right();
  delay(2000);
}
void readS(){
  for (uint8_t i = 0; i < 6; i++) {
      sensors[i] = line.readLineSensorBool(i);
      Serial.print(sensors[i]);
    }
    Serial.println();
}
void forward(){
  Serial.println("F");
  digitalWrite(6, LOW);
  analogWrite(5, speed_A);
  digitalWrite(9, LOW);
  analogWrite(10, speed_B);
}
void backward(){
  Serial.println("B");
  digitalWrite(6, HIGH);
  analogWrite(5, map(speed_A, 0, 255, 255, 0));
  digitalWrite(9, HIGH);
  analogWrite(10, map(speed_B, 0, 255, 255, 0));
  delay(2000);
}

void right(){
  Serial.println("R");
  digitalWrite(6, LOW);
  analogWrite(5, speed_A);
  digitalWrite(9, HIGH);
  analogWrite(10, map(speed_B/1.6, 0, 255, 255, 0));
}
void rightAlt(){
  Serial.println("AR");
  digitalWrite(6, LOW);
  analogWrite(5, speed_A - 30);
  digitalWrite(9, LOW);
  analogWrite(10, speed_B+30);
}
void left(){
  Serial.println("L");
  digitalWrite(6, HIGH);
  analogWrite(5, map(speed_A/1.6, 0, 255, 255, 0));
  digitalWrite(9, LOW);
  analogWrite(10, speed_B);
}
void leftAlt(){
  Serial.println("AL");
  digitalWrite(6, LOW);
  analogWrite(5, speed_A+30);
  digitalWrite(9, LOW);
  analogWrite(10, speed_B - 30);
}
void stp(){
  Serial.println("S");
  digitalWrite(6, LOW);
  analogWrite(5, 0);
  digitalWrite(9, LOW);
  analogWrite(10, 0);
}