#define DR1 7

#define DR2 8
#define PWMR 6
#define DL1 9
#define DL2 4
#define PWML 5
int button = 2;
int numSensor = 6;

int sensor_pin[] = {0, 1, 2, 3, 4, 5};

int min_sensor_values[] = {260, 375, 77, 337, 718, 217,};
int max_sensor_values[] = {979, 981, 520, 982, 985, 979};
int REF01 = (min_sensor_values[0]+max_sensor_values[0])/2;
int REF02 = (min_sensor_values[1]+max_sensor_values[1])/2;
int REF03 = (min_sensor_values[2]+max_sensor_values[2])/2;
int REF04 = (min_sensor_values[3]+max_sensor_values[3])/2;
int REF05 = (min_sensor_values[4]+max_sensor_values[4])/2;
int REF06 = (min_sensor_values[5]+max_sensor_values[5])/2;
int state_on_Line = 1;
float Kp = 3;
float Ki = 0;
float Kd = 16;

int speed_max =100;

uint16_t setpoint;
uint32_t _lastPosition;
float present_position;
float errors = 0;
float output = 0;
float integral ;
float derivative ;
float previous_error ;
boolean buttonState = 0;

//unsigned long long int time = 10000;

void setup() {
Lotus_nano();

//////////////////////////////////////////////////

LR();LL();LL();LR();

/////////////////////////////////////////////////

run(0,0); delay(1);

void loop() {
// pid();
readanalog();
}
void readanalog()
{
int S1 = analogRead(0);
int S2 = analogRead(1);
int S3 = analogRead(2);
int S4 = analogRead(3);
int S5 = analogRead(4);
int S6 = analogRead(5);
Serial.print("S1="); Serial.print(S1);
Serial.print("||"); Serial.print("S2="); Serial.print(S2);
Serial.print("||"); Serial.print("S3="); Serial.print(S3);
Serial.print("||"); Serial.print("S4="); Serial.print(S4);
Serial.print("||"); Serial.print("S5="); Serial.print(S5);
Serial.print("||"); Serial.print("S6="); Serial.print(S6);
Serial.println(); delay(1);
}
void TN()
{
while (1)
{
if (analogRead(0) < REF01 && analogRead(5) < REF06 )
{
while (1)
{
if (analogRead(0) > REF01 && analogRead(5) > REF06 )
{

break;
}
pid();
}
break;
}
pid();
}
}

void LL()
{
while(1){
if(analogRead(0) < REF01 and analogRead(1)< REF02 and analogRead(2)<REF03)
{delay(100);l();break;}
pid();
}
}

void LR()
{
while(1){
if(analogRead(3) < REF04 and analogRead(4)< REF05 and analogRead(5)<REF06)
{delay(100);r();break;}
pid();
}
}

void T()
{
while(1){
if((analogRead(1)< REF02) and (analogRead(2)<REF03) and ((analogRead(3)< REF04)
or (analogRead(2)< REF03) and (analogRead(3)<REF04) and (analogRead(4)< REF05)))
{run(speed_max,speed_max);delay(200);break;}
pid();
}
}

//void T()
//{ int j = 0;
// while(1)
// {
// if(analogRead(3) < REF04 and analogRead(4)< REF05 and analogRead(5)<REF06)
{ R(); }
// if(analogRead(0) < REF01 and analogRead(1)< REF02 and analogRead(2)<REF03)
{ L(); }
//
// if(analogRead(1)< REF02 and analogRead(2)<REF03 and
// analogRead(3)< REF04 and analogRead(4)<REF05)
{ run(speed_max,speed_max);delay(200); }
//
// if (j >= time)
// {
// /////
// run(0,0); delay(100*100);
// /////
// break;
// }
// j++;
// pid();
// }
//}
void l()
{
while(1){
if(analogRead(0)<REF01){run(-350,350);}
if(analogRead(1)<REF02){run(-350,350);}
if(analogRead(3)<REF04){run(-200,200);break;}
else {run(-350,350);}
}
}
void r()
{ run(0,0);delay(20);
while(1){
if(analogRead(5)<REF06){run(350,-350);}
if(analogRead(4)<REF05){run(350,-350);}
if(analogRead(2)<REF03){run(200,-200);break;}
else {run(350,-350);}
}
}
void sw_button()
{
while (1)
{
buttonState = digitalRead(2);
if (buttonState == LOW) {
break;
}
else {
run(0, 0);
}
}
}
int readline()
{
bool onLine = false;
long avg = 0;
long sum = 0;
for (uint8_t i = 0; i < numSensor ; i++)
{
long value = map(analogRead(sensor_pin[i]), min_sensor_values[i],
max_sensor_values[i], 1000, 0);
if (value > 200) {
onLine = true;
}
if (value > 50)
{
avg += (long)value * (i * 1000);
sum += value;
}
}
if (!onLine)
{
if (_lastPosition < (numSensor - 1) * 1000 / 2)
{
return 0 - 2000;
}
else
{
return ((numSensor - 1) * 1000) + 2000 ;
}
}
_lastPosition = avg / sum;
return _lastPosition;
}
void pid() {

present_position = readline() / ((numSensor - 1) * 10) ;

setpoint = 50.0;
errors = setpoint - present_position;
integral = integral + errors ;
derivative = (errors - previous_error) ;
output = Kp * errors + Ki * integral + Kd * derivative;
previous_error = errors;
float motorL = speed_max - output;
float motorR = speed_max + output;
motorL = constrain(motorL, -100, 250);
motorR = constrain(motorR, -100, 250);
run(motorL, motorR);

void run(int spl, int spr)

{
if (spl > 0)
{
digitalWrite(DL1, LOW);
digitalWrite(DL2, HIGH);
analogWrite(PWML, spl);
}
else if (spl < 0)
 {
digitalWrite(DL1, HIGH);
digitalWrite(DL2, LOW);
analogWrite(PWML, -spl);
}
else
{
digitalWrite(DL1, LOW);
digitalWrite(DL2, LOW);
analogWrite(PWML, -255);
}
//////////////////////////////////////
if (spr > 0)
{
digitalWrite(DR1, LOW);
digitalWrite(DR2, HIGH);
analogWrite(PWMR, spr);
}
else if (spr < 0)
{
digitalWrite(DR1, HIGH);
digitalWrite(DR2, LOW);
analogWrite(PWMR, -spr);
}
else
{
digitalWrite(DR1, LOW);
digitalWrite(DR2, LOW);
analogWrite(PWMR, -255);
}
}
void Lotus_nano()
{
Serial.begin(9600);
pinMode(2, INPUT);
pinMode(DL1, OUTPUT);
pinMode(DL2, OUTPUT);
pinMode(PWML, OUTPUT);
pinMode(DR1, OUTPUT);
pinMode(DR2, OUTPUT);
pinMode(PWMR, OUTPUT);
sw_button();
}