safwan092

Code-Simple-4WD-Ultrasonic+Servo

Jul 29th, 2020
1,849
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /*
  2.    ARDUINO CONNECTIONS:
  3.  
  4.    ENA ---> 9
  5.    IN1 ---> 8
  6.    IN2 ---> 7
  7.    IN3 ---> 6
  8.    IN4 ---> 5
  9.    ENB ---> 4
  10.  
  11.    SERVO ---> 3
  12.  
  13.    TRIG ---> 11
  14.    ECHO ---> 10
  15.  
  16. */
  17.  
  18.  
  19. #include <Servo.h>
  20. int pinLB = 8;     // defining pin 12
  21. int pinLF = 7;     // defining pin 3
  22. int pinRB = 6;    // defining pin 13
  23. int pinRF = 5;    // defining pin 11
  24. int Lpwm_pin = 9;  //adjusting speed
  25. int Rpwm_pin = 4;   //adjusting speed //
  26. unsigned char Lpwm_val = 200;
  27. unsigned char Rpwm_val = 200;
  28.  
  29. int inputPin = 10;    // defining receiving pin of ultrasonic signal
  30. int outputPin = 11;   // defining emitting pin of ultrasonic signal
  31.  
  32. int Fspeedd = 0;      // forward speed
  33. int Rspeedd = 0;      // right speed
  34. int Lspeedd = 0;      // left speed
  35. int directionn = 0;   // front=8 back=2 left=4 right=6
  36. Servo myservo;        // setting myservo
  37. int delay_time = 250; // time for servo motor turning backward
  38.  
  39. int Fgo = 8;         // going forward
  40. int Rgo = 6;         // turning right
  41. int Lgo = 4;         // turning left
  42. int Bgo = 2;         // turning backward
  43.  
  44. void setup()
  45. {
  46.   Serial.begin(9600);     // defining output pin of motor
  47.   pinMode(pinLB, OUTPUT); // pin 12
  48.   pinMode(pinLF, OUTPUT); // pin 3 (PWM)
  49.   pinMode(pinRB, OUTPUT); // pin 13
  50.   pinMode(pinRF, OUTPUT); // pin 11 (PWM)
  51.   pinMode(inputPin, INPUT);    // defining input pin of ultrasonic
  52.   pinMode(outputPin, OUTPUT);  // defining output pin of ultrasonic
  53.   myservo.attach(3);    // defining output pin9 of motor
  54. }
  55. void advance(){
  56.   digitalWrite(pinLB, LOW);
  57.   digitalWrite(pinLF, HIGH);
  58.   digitalWrite(pinRB, HIGH);
  59.   digitalWrite(pinRF, LOW);
  60. }
  61. void back(){
  62.   digitalWrite(pinLB, HIGH);
  63.   digitalWrite(pinLF, LOW);
  64.   digitalWrite(pinRB, LOW);
  65.   digitalWrite(pinRF, HIGH);
  66. }
  67. void right(){
  68.   digitalWrite(pinRB, HIGH);
  69.   digitalWrite(pinRF, LOW);
  70.   digitalWrite(pinLB, HIGH);
  71.   digitalWrite(pinLF, LOW);
  72. }
  73. void left(){
  74.   digitalWrite(pinRB, LOW);
  75.   digitalWrite(pinRF, HIGH);
  76.   digitalWrite(pinLB, LOW);
  77.   digitalWrite(pinLF, HIGH);
  78. }
  79. void stopp(){
  80.   digitalWrite(pinRB, HIGH);
  81.   digitalWrite(pinRF, HIGH);
  82.   digitalWrite(pinLB, HIGH);
  83.   digitalWrite(pinLF, HIGH);
  84. }
  85.  
  86. void detection()        //measuring 3 angles(0.90.179)
  87. {
  88.   int delay_time = 250;   // time for servo motor turning backward
  89.   ask_pin_F();            // reading out the front distance
  90.   if (Fspeedd < 20)        // assuming the front distance less than 10cm
  91.   {
  92.     stopp();               // clear output material
  93.     delay(100);
  94.     back();                // going backward for 0.2 second
  95.     delay(200);
  96.   }
  97.  
  98.   if (Fspeedd < 40)        // assuming the front distance less than 25cm
  99.   {
  100.     stopp();
  101.     delay(100);             // clear output material
  102.     ask_pin_L();            // reading out the left distance
  103.     delay(delay_time);      // waiting servo motor to be stable
  104.     ask_pin_R();            // reading out the right distance
  105.     delay(delay_time);      // waiting servo motor to be stable
  106.  
  107.     if (Lspeedd > Rspeedd)  //assuming left distance more than  right distance
  108.     {
  109.       directionn = Lgo;      //turning left
  110.     }
  111.  
  112.     if (Lspeedd <= Rspeedd)  //assuming left distance less than or equal to right distance
  113.     {
  114.       directionn = Rgo;      //turning right
  115.     }
  116.  
  117.     if (Lspeedd < 15 && Rspeedd < 15)   //assuming both left distance and right distance less than  10cm
  118.     {
  119.       directionn = Bgo;      //going backward
  120.     }
  121.   }
  122.   else                      //assuming the front distance  more than 25 cm
  123.   {
  124.     directionn = Fgo;        //going forward
  125.   }
  126.  
  127. }
  128. void ask_pin_F()   // measuring the front distance
  129. {
  130.   myservo.write(90);
  131.   digitalWrite(outputPin, LOW);   // ultrasonic launching low voltage at 2μs
  132.   delayMicroseconds(2);
  133.   digitalWrite(outputPin, HIGH);  // ultrasonic launching high voltage at 10μs,at least at10μs
  134.   delayMicroseconds(10);
  135.   digitalWrite(outputPin, LOW);    // keeping ultrasonic launching low voltage
  136.   float Fdistance = pulseIn(inputPin, HIGH);  // time of error reading
  137.   Fdistance = Fdistance / 5.8 / 10;  // converting time into distance(unit:cm)
  138.   Fspeedd = Fdistance;              // reading-in Fspeedd(fore speed) with distance
  139. }
  140. void ask_pin_L()   // measuring left distance
  141. {
  142.   myservo.write(5);
  143.   delay(delay_time);
  144.   digitalWrite(outputPin, LOW);   // ultrasonic launching low voltage at 2μs
  145.   delayMicroseconds(2);
  146.   digitalWrite(outputPin, HIGH);  // ultrasonic launching high voltage at 10μs,at least at10μs
  147.   delayMicroseconds(10);
  148.   digitalWrite(outputPin, LOW);    // keeping ultrasonic launching low voltage
  149.   float Ldistance = pulseIn(inputPin, HIGH);  // time of error reading
  150.   Ldistance = Ldistance / 5.8 / 10;  // converting time into distance(unit:cm)
  151.   Lspeedd = Ldistance;              //reading-in Lspeedd(left speed) with distance
  152. }
  153. void ask_pin_R()   // measuring right distance
  154. {
  155.   myservo.write(177);
  156.   delay(delay_time);
  157.   digitalWrite(outputPin, LOW);   // ultrasonic launching low voltage at 2μs
  158.   delayMicroseconds(2);
  159.   digitalWrite(outputPin, HIGH);  // ultrasonic launching high voltage at 10μs,at least at10μs
  160.   delayMicroseconds(10);
  161.   digitalWrite(outputPin, LOW);    // keeping ultrasonic launching low voltage
  162.   float Rdistance = pulseIn(inputPin, HIGH);  // time of error reading
  163.   Rdistance = Rdistance / 5.8 / 10;  // onverting time into distance(unit:cm)
  164.   Rspeedd = Rdistance;              // reading-in Rspeedd(right speed) with distance
  165. }
  166.  
  167. void loop() {
  168.   myservo.write(90);  //making motor regression, being ready for next measurement
  169.   detection();        //measuring angle and deciding which direction it moves towards
  170.   if (directionn == 2) //supposing direction = 2(back up)
  171.   {
  172.     back();
  173.     delay(800);                    //  back up
  174.     left() ;
  175.     delay(200);              //moving slightly towards left(avoiding locked)
  176.   }
  177.   if (directionn == 6)          //supposing direction = 6(turning right)
  178.   {
  179.     back();
  180.     delay(100);
  181.     right();
  182.     delay(600);                 // turning right
  183.   }
  184.   if (directionn == 4)         //supposing direction = 4(turning left)
  185.   {
  186.     back();
  187.     delay(600);
  188.     left();
  189.     delay(600);                  // turning left
  190.   }
  191.   if (directionn == 8)         //supposing direction =  = 8(going forwards)
  192.   {
  193.     advance();                 // going forwards normally
  194.     delay(100);
  195.   }
  196. }
RAW Paste Data