ZulRocky

line follower

Sep 25th, 2020
1,138
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <LiquidCrystal_SR_LCD3.h>
  2. const int PIN_LCD_STROBE  = 2;
  3. const int PIN_LCD_DATA  = 3;
  4. const int PIN_LCD_CLOCK = 4;
  5. LiquidCrystal_SR_LCD3 lcd(PIN_LCD_DATA, PIN_LCD_CLOCK, PIN_LCD_STROBE);
  6. /*
  7. #include <BlynkSimpleStream.h>
  8.  
  9. char auth[] = "W9KxUhtSLkGGAyxYmiDImNjGC0DKiKmF";
  10.  
  11. WidgetLED led_obstacle(V1);
  12. WidgetLED led_auto(V15);
  13. WidgetLED led_engine(V13);
  14.  
  15. BlynkTimer timer; // Announcing the timer
  16. */
  17. //#include <SoftwareSerial.h>
  18.  
  19. //SoftwareSerial mySerial(0, 1); // RX, TX
  20.  
  21.  
  22. /* MOTOR */
  23. int mt1=5, mt2=6, mt3=12, mt4=8;
  24. #define enA NULL
  25. #define in1 8
  26. #define in2 6
  27. #define enB NULL
  28. #define in3 12
  29. #define in4 5
  30.  
  31. int motorSpeedA = 0;
  32. int motorSpeedB = 0;
  33.  
  34. // LS = Left Sensor , RS = Right Sensor
  35. // LS3 - LS2 - LS1 -- RS1 - RS2 - RS3
  36. int sensorPins[] = {A5 , A4 , A3 , A2 , A1, A0};
  37.  
  38. float sensorValue[6];
  39. float sensorValueMin[6] = {1024, 1024, 1024, 1024, 1024, 1024};
  40. float sensorValueMax[6] = {0, 0, 0, 0, 0, 0};
  41. float threshold[6] = {10, 10, 10, 10, 10, 10};
  42. float sensorValueAvg[6] = {512, 512, 512, 512, 512, 512};
  43. boolean digitalValue[6];
  44. short error = 0;
  45.  
  46. /* BUTTON SW3 SW4 SW5 SW6*/
  47. int sw3=8, sw4=13, sw5=11, sw6=12;
  48. /* END BUTTON */
  49.  
  50. /* Sensor Ping */
  51. const int trigPin = 9;
  52. const int echoPin = 10;
  53.  
  54. int push=0, ok=0;
  55.  
  56. int period = 300;
  57. unsigned long time_now = 0;
  58.  
  59. int manual_mode = 0;
  60. int engine_on = LOW;
  61. /*
  62. BLYNK_CONNECTED(){
  63.   Blynk.syncVirtual(V0);
  64.  
  65.   Blynk.syncVirtual(V20);
  66.   Blynk.syncVirtual(V21);
  67.   Blynk.syncVirtual(V22);
  68.   Blynk.syncVirtual(V23);
  69. }
  70.  
  71. int forward = LOW,backward = LOW,left = LOW,right = LOW;
  72.  
  73. BLYNK_WRITE(V20){
  74.   forward = param.asInt();
  75. }
  76.  
  77. BLYNK_WRITE(V21){
  78.   backward = param.asInt();
  79. }
  80.  
  81. BLYNK_WRITE(V22){
  82.   left = param.asInt();
  83. }
  84.  
  85. BLYNK_WRITE(V23){
  86.   right = param.asInt();
  87. }
  88.  
  89. int motor_speed_r = 0,motor_speed_l = 0;
  90. // LEFT MOTOR SPEED MANUAL
  91. BLYNK_WRITE(V11){
  92.   motor_speed_r = param.asInt();
  93. }
  94.  
  95. // RIGHT MOTOR SPEED MANUAL
  96. BLYNK_WRITE(V12){
  97.   motor_speed_l = param.asInt();
  98. }
  99.  
  100. BLYNK_WRITE(V0){
  101.   int buttonState = param.asInt();
  102.   engine_on = buttonState;
  103. }
  104.  
  105. BLYNK_WRITE(V10) {
  106.   switch(param.asInt()){
  107.     case 1: // Manual
  108.      manual_mode = 1;
  109.      break;
  110.     case 2: // AUTO
  111.      manual_mode = 0;
  112.      break;
  113.      default:
  114.      manual_mode = 1;
  115.      break;
  116.   }
  117. }*/
  118.  
  119. void setup() {
  120.   Serial.begin(9600);
  121.   //Blynk.begin(auth, Serial);
  122.  
  123.   lcd.clear();
  124.   //timer.setInterval(1000L, sensorDataSend);
  125.  
  126.   pinMode(7, OUTPUT);
  127.   for (short i = 0; i < 6; i++)
  128.   {
  129.     pinMode(sensorPins[i], INPUT);
  130.   }
  131.   /* BUTTON */
  132.   //pinMode(sw3, INPUT);
  133.   pinMode(sw4, INPUT);
  134.   pinMode(sw5, INPUT);
  135.   //pinMode(sw6, INPUT);
  136.   /* END BUTTON */
  137.  
  138.   /* MOTOR */
  139.   pinMode(enA, OUTPUT);
  140.   pinMode(enB, OUTPUT);
  141.   pinMode(in1, OUTPUT);
  142.   pinMode(in2, OUTPUT);
  143.   pinMode(in3, OUTPUT);
  144.   pinMode(in4, OUTPUT);
  145.   /* END MOTOR */
  146.  
  147.  
  148.   /* SENSOR */
  149.  
  150.   /* END SENSOR */
  151.   lcd.begin(16, 2);
  152.   lcd.home();
  153.   lcd.setCursor(4,0);
  154.   lcd.print("*Welcome*"); delay(1000);
  155.   lcd.clear();
  156.   lcd.setCursor(1,0);
  157.   lcd.print("Line  Follower");
  158.   lcd.setCursor(6,1);
  159.   lcd.print("v1.0");
  160.   delay(3000);
  161.   lcd.clear();
  162. }
  163.  
  164. int SPD = 100;
  165. float PID;
  166. float kp = 10.0, ki = 5.0, kd = 0.5;
  167. float last_error, integ, deriv;
  168.  
  169. unsigned long time_now2 = 0;
  170.  
  171. bool Prox = false;
  172. int kecepatan = 100;
  173.  
  174. void loop() {
  175.  
  176.   updateSensor();
  177.   updateMinMax();
  178.  
  179.   //calculateError();
  180.  
  181.   //ShowSensor();
  182.  
  183.  
  184.   /*digitalWrite(in1, HIGH);
  185.   digitalWrite(in3, HIGH);
  186.   analogWrite(enA, motorSpeedA);
  187.   analogWrite(enB, motorSpeedB);*/
  188.  
  189.   /* ERROR
  190.    *  -4 : White
  191.    *  -2 : Turn Left 100%
  192.    *  -1 : Turn Left 50%
  193.    *   0 : Forward
  194.    *   1 : Turn Right 50%
  195.    *   2 : Turn Right 100%
  196.    */
  197.   error = 0;
  198.   short sum = 0;
  199.   for (short i = 0; i < 6; i++)
  200.   {
  201.     sum += digitalValue[i];
  202.     error += digitalValue[i] * (i + 1);
  203.   }
  204.   if(error < 0 || error > 0){
  205.   error = error / sum - 3;
  206.   }
  207.   /*
  208.   PID = error * kp + integ * ki + deriv * kd;
  209.   integ += error;
  210.   deriv = error - last_error;
  211.   last_error = error;*/
  212.  
  213.  
  214.  if(error == -4){
  215.     smotor();
  216.   }else if(error == -2){
  217.     fmotor();
  218.     fmotorpwm(200, 0);
  219.     delay(50);
  220.   }else if(error == -1){
  221.     fmotor();
  222.     fmotorpwm(150, 0);
  223.   }else if(error == 0){
  224.     fmotor();
  225.     fmotorpwm(130, 130);
  226.   }else if(error == 1){
  227.     fmotor();
  228.     fmotorpwm(0, 150);
  229.   }else if(error == 2){
  230.     fmotor();
  231.     fmotorpwm(0, 200);
  232.     delay(50);
  233.   }
  234.  
  235.   //PROXIMITY SENSOR
  236.   long duration, cm;
  237.  
  238.     pinMode(trigPin, OUTPUT);
  239.     digitalWrite(trigPin, LOW);
  240.     delayMicroseconds(2);
  241.     digitalWrite(trigPin, HIGH);
  242.     delayMicroseconds(10);
  243.     digitalWrite(trigPin, LOW);
  244.  
  245.     pinMode(echoPin, INPUT);
  246.     duration = pulseIn(echoPin, HIGH);
  247.     cm = microsecondsToCentimeters(duration);
  248.     if(cm<5){
  249.       ok=1;
  250.     }else{
  251.       ok=0;
  252.     }
  253.  
  254.   /*if(ok==1){
  255.     bmotor();
  256.     delay(200);
  257.     smotor();
  258.     fmotor();
  259.     fmotorpwm(100, 0);
  260.     delay(600);
  261.     fmotorpwm(100, 100);
  262.     delay(100);
  263.     fmotorpwm(0, 100);
  264.     delay(460);
  265.     fmotorpwm(100, 100);
  266.     delay(450);
  267.     fmotorpwm(0, 100);
  268.     delay(500);
  269.     fmotorpwm(100, 100);
  270.     delay(50);
  271.     fmotorpwm(100, 0);
  272.     delay(600);
  273.     ok=0;
  274.   }else{
  275.     smotor();
  276.   }*/
  277. }
  278.  
  279. void fmotor(){
  280.   analogWrite(in2, 255);
  281.   analogWrite(in4, 255);
  282. }
  283.  
  284. void bmotor(){
  285.   digitalWrite(in1, HIGH);
  286.   digitalWrite(in3, HIGH);
  287. }
  288.  
  289. void smotor(){
  290.   digitalWrite(in1, LOW);
  291.   digitalWrite(in3, LOW);
  292.   analogWrite(in2, 0);
  293.   analogWrite(in4, 0);
  294. }
  295.  
  296. void fmotorpwm(int motora, int motorb){
  297.   analogWrite(in2, motora);
  298.   analogWrite(in4, motorb);
  299. }
  300.  
  301. void ShowPing(){
  302.   if(millis() >= time_now + period){
  303.     lcd.clear();
  304.     time_now += period;
  305.     long duration, cm;
  306.  
  307.     pinMode(trigPin, OUTPUT);
  308.     digitalWrite(trigPin, LOW);
  309.     delayMicroseconds(2);
  310.     digitalWrite(trigPin, HIGH);
  311.     delayMicroseconds(10);
  312.     digitalWrite(trigPin, LOW);
  313.  
  314.     pinMode(echoPin, INPUT);
  315.     duration = pulseIn(echoPin, HIGH);
  316.     cm = microsecondsToCentimeters(duration);
  317.    
  318.     lcd.setCursor(0,0);
  319.     lcd.print(cm);
  320.     lcd.print(" CM");
  321.   }
  322. }
  323.  
  324. long microsecondsToCentimeters(long microseconds)
  325. {
  326.   return microseconds / 29 / 2;
  327. }
  328.  
  329. void ShowSensor(){
  330.   if(millis() >= time_now + period){
  331.     lcd.clear();
  332.     time_now += period;
  333.   lcd.setCursor(0,0);
  334.   lcd.print(analogRead(A5));
  335.   lcd.setCursor(5,0);
  336.   lcd.print(analogRead(A4));
  337.   lcd.setCursor(10,0);
  338.   lcd.print(analogRead(A3));
  339.  
  340.   lcd.setCursor(0,1);
  341.   lcd.print(analogRead(A2));
  342.   lcd.setCursor(5,1);
  343.   lcd.print(analogRead(A1));
  344.   lcd.setCursor(10,1);
  345.   lcd.print(analogRead(A0));
  346.  
  347.   }
  348.   //A5 , A4 , A3 , A2 , A1, A0
  349. }
  350.  
  351. void ShowPID(){
  352.   updateSensor();
  353.   updateMinMax();
  354.   calculateError();
  355.  
  356.   // Calculate PID
  357.   PID = error * kp + integ * ki + deriv * kd;
  358.   integ += error;
  359.   deriv = error - last_error;
  360.   last_error = error;
  361.   lcd.print(SPD);
  362.   lcd.print("  ");
  363.   lcd.print(PID);
  364.  
  365.   //rotate Motors
  366.  /* LeftMotor.rotate(SPD + PID);
  367.   RightMotor.rotate(SPD - PID); */
  368.   delay(100);
  369.   lcd.clear();
  370. }
  371.  
  372. void calculateError()
  373. {
  374.   error = 0;
  375.   short sum = 0;
  376.   for (short i = 0; i < 6; i++)
  377.   {
  378.     sum += digitalValue[i];
  379.     error += digitalValue[i] * (i + 1);
  380.   }
  381.   error = error / sum - 3;
  382. }
  383.  
  384.  
  385. void updateSensor()
  386. {
  387.   for (short i = 0; i < 6; i++)
  388.   {
  389.     sensorValue[i] = analogRead(sensorPins[i]);
  390.     digitalValue[i] = sensorValue[i] > (sensorValueAvg[i] + threshold[i]);
  391.   }
  392. }
  393. void updateMinMax()
  394. {
  395.   for (short i = 0; i < 6; i++)
  396.   {
  397.     sensorValueMin[i] = sensorValue[i] < sensorValueMin[i] ? sensorValue[i] : sensorValueMin[i];
  398.     sensorValueMax[i] = sensorValue[i] > sensorValueMax[i] ? sensorValue[i] : sensorValueMax[i];
  399.     sensorValueAvg[i] = (sensorValueMin[i] + sensorValueMax[i]) / 2; // update the avarage value
  400.     threshold[i] = (sensorValueMax[i] - sensorValueMax[i]) * 0.1; // Threshold is 10% of total sensor range
  401.   }
  402. }
RAW Paste Data