API tools faq
paste
Advertisement
ZulRocky

Blynk Local Server (Line Follower)

ZulRocky
May 6th, 2020
1,187
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Arduino 8.56 KB | None | 0 0
raw download clone embed print report
  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 = 5.5, ki = 1.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.   Blynk.run();
  176.   timer.run();
  177.  
  178.   updateSensor();
  179.   updateMinMax();
  180.   //calculateError();
  181.  
  182.   //ShowSensor();
  183.  
  184.  
  185.   /*digitalWrite(in1, HIGH);
  186.   digitalWrite(in3, HIGH);
  187.   analogWrite(enA, motorSpeedA);
  188.   analogWrite(enB, motorSpeedB);*/
  189.  
  190.   /* ERROR
  191.    *  -4 : White
  192.    *  -2 : Turn Left 100%
  193.    *  -1 : Turn Left 50%
  194.    *   0 : Forward
  195.    *   1 : Turn Right 50%
  196.    *   2 : Turn Right 100%
  197.    */
  198. if(engine_on == HIGH){
  199.   led_engine.on();
  200.   if(manual_mode == 0){
  201.   led_auto.on();
  202.   error = 0;
  203.   short sum = 0;
  204.   for (short i = 0; i < 6; i++)
  205.   {
  206.     sum += digitalValue[i];
  207.     error += digitalValue[i] * (i + 1);
  208.   }
  209.   error = error / sum - 3;
  210.   //lcd.print(error);
  211.   if(error == -4){
  212.     smotor();
  213.   }else if(error == -2){
  214.     fmotor();
  215.     fmotorpwm(200, 0);
  216.     delay(50);
  217.   }else if(error == -1){
  218.     fmotor();
  219.     fmotorpwm(150, 0);
  220.   }else if(error == 0){
  221.     fmotor();
  222.     fmotorpwm(100, 100);
  223.   }else if(error == 1){
  224.     fmotor();
  225.     fmotorpwm(0, 150);
  226.   }else if(error == 2){
  227.     fmotor();
  228.     fmotorpwm(0, 200);
  229.     delay(50);
  230.   }
  231.  
  232.   //PROXIMITY SENSOR
  233.   long duration, cm;
  234.  
  235.     pinMode(trigPin, OUTPUT);
  236.     digitalWrite(trigPin, LOW);
  237.     delayMicroseconds(2);
  238.     digitalWrite(trigPin, HIGH);
  239.     delayMicroseconds(10);
  240.     digitalWrite(trigPin, LOW);
  241.  
  242.     pinMode(echoPin, INPUT);
  243.     duration = pulseIn(echoPin, HIGH);
  244.     cm = microsecondsToCentimeters(duration);
  245.     if(cm<10){
  246.       ok=1;
  247.     }else{
  248.       ok=0;
  249.     }
  250.  
  251.   if(ok==1){
  252.     led_obstacle.on();
  253.     /*fmotor();
  254.     fmotorpwm(100, 0);
  255.     delay(750);
  256.     fmotorpwm(100, 100);
  257.     delay(100);
  258.     fmotorpwm(0, 100);
  259.     delay(600);
  260.     fmotorpwm(100, 100);
  261.     delay(500);
  262.     fmotorpwm(0, 100);
  263.     delay(600);
  264.     fmotorpwm(100, 100);
  265.     delay(100);
  266.     fmotorpwm(100, 0);
  267.     delay(300);*/
  268.     ok=0;
  269.     led_obstacle.off();
  270.   }else{
  271.     //smotor();
  272.   }
  273.   } // END manual_mode == false
  274.   else
  275.   {
  276.     led_auto.off();
  277.     if(forward == HIGH){
  278.       fmotorpwm(motor_speed_l, motor_speed_r);
  279.     }else if(backward == HIGH){
  280.       bmotor();
  281.     }else if(left == HIGH){
  282.       fmotorpwm(motor_speed_r, 0);
  283.     }else if(right == HIGH){
  284.       fmotorpwm(0, motor_speed_l);
  285.     }else{
  286.       smotor();
  287.     }
  288.   }
  289. }else{
  290.   smotor();
  291.   led_engine.off();
  292. }
  293. }
  294.  
  295. void sensorDataSend()
  296. {
  297.   Blynk.virtualWrite(V2, analogRead(A5));
  298.   Blynk.virtualWrite(V3, analogRead(A4));
  299.   Blynk.virtualWrite(V4, analogRead(A3));
  300.   Blynk.virtualWrite(V5, analogRead(A2));
  301.   Blynk.virtualWrite(V6, analogRead(A1));
  302.   Blynk.virtualWrite(V7, analogRead(A0));
  303. }
  304.  
  305. void fmotor(){
  306.   analogWrite(in2, 255);
  307.   analogWrite(in4, 255);
  308. }
  309.  
  310. void bmotor(){
  311.   digitalWrite(in1, HIGH);
  312.   digitalWrite(in3, HIGH);
  313. }
  314.  
  315. void smotor(){
  316.   digitalWrite(in1, LOW);
  317.   digitalWrite(in3, LOW);
  318.   analogWrite(in2, 0);
  319.   analogWrite(in4, 0);
  320. }
  321.  
  322. void fmotorpwm(int motora, int motorb){
  323.   analogWrite(in2, motora);
  324.   analogWrite(in4, motorb);
  325. }
  326.  
  327. void ShowPing(){
  328.   if(millis() >= time_now + period){
  329.     lcd.clear();
  330.     time_now += period;
  331.     long duration, cm;
  332.  
  333.     pinMode(trigPin, OUTPUT);
  334.     digitalWrite(trigPin, LOW);
  335.     delayMicroseconds(2);
  336.     digitalWrite(trigPin, HIGH);
  337.     delayMicroseconds(10);
  338.     digitalWrite(trigPin, LOW);
  339.  
  340.     pinMode(echoPin, INPUT);
  341.     duration = pulseIn(echoPin, HIGH);
  342.     cm = microsecondsToCentimeters(duration);
  343.    
  344.     lcd.setCursor(0,0);
  345.     lcd.print(cm);
  346.     lcd.print(" CM");
  347.   }
  348. }
  349.  
  350. long microsecondsToCentimeters(long microseconds)
  351. {
  352.   return microseconds / 29 / 2;
  353. }
  354.  
  355. void ShowSensor(){
  356.   if(millis() >= time_now + period){
  357.     lcd.clear();
  358.     time_now += period;
  359.   lcd.setCursor(0,0);
  360.   lcd.print(analogRead(A5));
  361.   lcd.setCursor(5,0);
  362.   lcd.print(analogRead(A4));
  363.   lcd.setCursor(10,0);
  364.   lcd.print(analogRead(A3));
  365.  
  366.   lcd.setCursor(0,1);
  367.   lcd.print(analogRead(A2));
  368.   lcd.setCursor(5,1);
  369.   lcd.print(analogRead(A1));
  370.   lcd.setCursor(10,1);
  371.   lcd.print(analogRead(A0));
  372.  
  373.   }
  374.   //A5 , A4 , A3 , A2 , A1, A0
  375. }
  376.  
  377. void ShowPID(){
  378.   updateSensor();
  379.   updateMinMax();
  380.   calculateError();
  381.  
  382.   // Calculate PID
  383.   PID = error * kp + integ * ki + deriv * kd;
  384.   integ += error;
  385.   deriv = error - last_error;
  386.   last_error = error;
  387.   lcd.print(SPD);
  388.   lcd.print("  ");
  389.   lcd.print(PID);
  390.  
  391.   //rotate Motors
  392.  /* LeftMotor.rotate(SPD + PID);
  393.   RightMotor.rotate(SPD - PID); */
  394.   delay(100);
  395.   lcd.clear();
  396. }
  397.  
  398. void calculateError()
  399. {
  400.   error = 0;
  401.   short sum = 0;
  402.   for (short i = 0; i < 6; i++)
  403.   {
  404.     sum += digitalValue[i];
  405.     error += digitalValue[i] * (i + 1);
  406.   }
  407.   error = error / sum - 3;
  408. }
  409.  
  410.  
  411. void updateSensor()
  412. {
  413.   for (short i = 0; i < 6; i++)
  414.   {
  415.     sensorValue[i] = analogRead(sensorPins[i]);
  416.     digitalValue[i] = sensorValue[i] > (sensorValueAvg[i] + threshold[i]);
  417.   }
  418. }
  419. void updateMinMax()
  420. {
  421.   for (short i = 0; i < 6; i++)
  422.   {
  423.     sensorValueMin[i] = sensorValue[i] < sensorValueMin[i] ? sensorValue[i] : sensorValueMin[i];
  424.     sensorValueMax[i] = sensorValue[i] > sensorValueMax[i] ? sensorValue[i] : sensorValueMax[i];
  425.     sensorValueAvg[i] = (sensorValueMin[i] + sensorValueMax[i]) / 2; // update the avarage value
  426.     threshold[i] = (sensorValueMax[i] - sensorValueMax[i]) * 0.1; // Threshold is 10% of total sensor range
  427.   }
  428. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!