int s0 = 0, s1 = 0, s2 = 0, s3 = 0, s4 = 0, s5 = 0, s6 = 0, s7 = 0; int line_value = 0, sum = 0; const int mrp = 11, mr1 = 9, mr2 = 10, ml1 = 6, ml2 = 5, mlp = 3; int j = 0, junction = 0; int StopTrigger=0; double error = 0, p_error = 0, d_error = 0, last_error = 0, pid_error = 0; double kd=31 ,kp=1.91; int left_pwm = 65, right_pwm = 60; void setup() { // put your setup code here, to run once: pinMode(mlp, OUTPUT); pinMode(ml1, OUTPUT); pinMode(ml2, OUTPUT); pinMode(mr1, OUTPUT); pinMode(mr2, OUTPUT); pinMode(mrp, OUTPUT); Serial.begin(9600); } void loop() { // put your main code here, to run repeatedly: //// CheckEnd(); // if(StopTrigger>20) // stop_(); j = read_sensors(); go_straight(); if (j > 4) { // delay(50); junction = check_junction(); Serial.print(" jN** "); Serial.println(junction); // stop_(); if (!junction) { left(); } else if (junction == 1) { go_straight(); } } if (j == 0) { right(); } // if (j==8) // { // delay(200); // if(j==8) // { // stop_(); // Serial.print("stop"); // } // // } // Serial.print(" sens: "); // Serial.print(s0); // Serial.print(" "); // Serial.print(s1); // Serial.print(" "); // Serial.print(s2); // Serial.print(" "); // Serial.print(s3); // Serial.print(" "); // Serial.print(s4); // Serial.print(" "); // Serial.print(s5); // Serial.print(" "); // Serial.print(s6); // Serial.print(" "); // Serial.print(s7); // Serial.print(" s="); // Serial.print(sum); // Serial.print(" s_num="); // Serial.print(j); // Serial.print(" | E="); // Serial.print(error); // Serial.print(" DE "); // Serial.print(d_error); // Serial.print(" PD_er="); // Serial.print(p_error + d_error); // Serial.print(" L="); // Serial.print(left_pwm); // Serial.print(" R="); // Serial.print(right_pwm); // Serial.print(" "); // Serial.println(line_value); } int read_sensors() { s0 = analogRead(A0) / 10; s1 = analogRead(A1) / 10; s2 = analogRead(A2) / 10; s3 = analogRead(A3) / 10; s4 = analogRead(A4) / 10; s5 = analogRead(A5) / 10; s6 = analogRead(A6) / 10; s7 = analogRead(A7) / 10; sum = s0 + s1 + s2 + s3 + s4 + s5 + s6 + s7; line_value = (((10 * s0) + (20 * s1) + (30 * s2) + (40 * s3) + (50 * s4) + (60 * s5) + (70 * s6) + (80 * s7)) / sum); line_value = constrain(line_value, 0, 80); int i = 0; if (s0 > 5) i++; if (s1 > 5) i++; if (s2 > 5) i++; if (s3 > 5) i++; if (s4 > 5) i++; if (s5 > 5) i++; if (s6 > 5) i++; if (s7 > 5) i++; return i; } void go_straight() { error = line_value - 40; p_error = kp * error; // i_error = ki * d_error = kd * (error - last_error); last_error = error; pid_error = p_error + d_error; left_pwm = 70 - pid_error; right_pwm = 60 + pid_error; left_pwm = constrain(left_pwm, 0, 255); right_pwm = constrain(right_pwm, 0, 255); digitalWrite(ml1, HIGH); digitalWrite(ml2, LOW); digitalWrite(mr1, HIGH); digitalWrite(mr2, LOW); analogWrite(mlp, left_pwm); analogWrite(mrp, right_pwm); // StopTrigger=0; } void stop_() { digitalWrite(ml1, LOW); digitalWrite(ml2, LOW); digitalWrite(mr1, LOW); digitalWrite(mr2, LOW); delay(3000); stop_(); } int check_junction() { if ((!((s6 > 5) || (s7 > 5))) && (s0 > 5)) { return 1; // go straight } else { return 0; // take left } } void left() { digitalWrite(ml1, LOW); digitalWrite(ml2, HIGH); digitalWrite(mr1, HIGH); digitalWrite(mr2, LOW); analogWrite(mlp,0); analogWrite(mrp, 0); delay(20); // Serial.print(" \n Entered in left \n "); digitalWrite(ml1, LOW); digitalWrite(ml2, HIGH); digitalWrite(mr1, HIGH); digitalWrite(mr2, LOW); analogWrite(mlp, 60); analogWrite(mrp, 90); delay(200); while (1) { // Serial.print(" \nleft "); // Serial.println(line_value); digitalWrite(ml1, LOW); digitalWrite(ml2, HIGH); digitalWrite(mr1, HIGH); digitalWrite(mr2, LOW); analogWrite(mlp, 60); analogWrite(mrp, 90); j = read_sensors(); if ((line_value >= 30 && line_value <= 50 && ( j < 3)) ) { Serial.print("left"); break; } } } void right() { digitalWrite(ml1, LOW); digitalWrite(ml2, HIGH); digitalWrite(mr1, HIGH); digitalWrite(mr2, LOW); analogWrite(mlp, 0); analogWrite(mrp, 0); delay(20); // Serial.print(" \n Entered in right\n "); digitalWrite(ml1, HIGH); digitalWrite(ml2, LOW); digitalWrite(mr1, LOW); digitalWrite(mr2, HIGH); analogWrite(mlp, 70); analogWrite(mrp, 65); while (1) { // Serial.print(" \nright \n"); digitalWrite(ml1, HIGH); digitalWrite(ml2, LOW); digitalWrite(mr1, LOW); digitalWrite(mr2, HIGH); analogWrite(mlp, 65); analogWrite(mrp, 60); j = read_sensors(); if ((line_value >= 30 && line_value <= 50 && ( j < 3)) ) { break; } } } //void CheckEnd() //{ // if( j==8) // StopTrigger++; //}