SHARE
TWEET

Untitled

a guest Jan 18th, 2019 63 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. //Arduino Obstacle Avoiding
  2.  
  3. #include <Servo.h>          //Servo motor library. This is standard library
  4. #include <NewPing.h>        //Ultrasonic sensor function library. You must install this library
  5.  
  6. //our L298N control pins
  7. const int LeftMotorForward = 4;
  8. const int LeftMotorBackward = 5;
  9. const int RightMotorForward = 6;
  10. const int RightMotorBackward = 7;
  11.  
  12. //sensor pins
  13. #define trig_pin A1 //analog input 1
  14. #define echo_pin A2 //analog input 2
  15.  
  16. #define maximum_distance 200
  17. boolean goesForward = false;
  18. int distance = 100;
  19.  
  20. NewPing sonar(trig_pin, echo_pin, maximum_distance); //sensor function
  21. Servo servo_motor; //our servo name
  22.  
  23.  
  24. void setup(){
  25.  
  26.   pinMode(RightMotorForward, OUTPUT);
  27.   pinMode(LeftMotorForward, OUTPUT);
  28.   pinMode(LeftMotorBackward, OUTPUT);
  29.   pinMode(RightMotorBackward, OUTPUT);
  30.  
  31.   servo_motor.attach(10); //our servo pin
  32.  
  33.   servo_motor.write(115);
  34.   delay(2000);
  35.   distance = readPing();
  36.   delay(100);
  37.   distance = readPing();
  38.   delay(100);
  39.   distance = readPing();
  40.   delay(100);
  41.   distance = readPing();
  42.   delay(100);
  43. }
  44.  
  45. void loop(){
  46.  
  47.   int distanceRight = 0;
  48.   int distanceLeft = 0;
  49.   delay(50);
  50.  
  51.   if (distance <= 27){ //jarak
  52.     moveStop();
  53.     delay(300);
  54.     moveBackward();
  55.     delay(400);
  56.     moveStop();
  57.     delay(300);
  58.     distanceRight = lookRight();
  59.     delay(300);
  60.     distanceLeft = lookLeft();
  61.     delay(300);
  62.  
  63.     if (distance >= distanceLeft){
  64.       turnRight();
  65.       moveStop();
  66.     }
  67.     else{
  68.       turnLeft();
  69.       moveStop();
  70.     }
  71.   }
  72.   else{
  73.     moveForward();
  74.   }
  75.     distance = readPing();
  76. }
  77.  
  78. int lookRight(){  
  79.   servo_motor.write(50);
  80.   delay(500);
  81.   int distance = readPing();
  82.   delay(100);
  83.   servo_motor.write(115);
  84.   return distance;
  85. }
  86.  
  87. int lookLeft(){
  88.   servo_motor.write(170);
  89.   delay(500);
  90.   int distance = readPing();
  91.   delay(100);
  92.   servo_motor.write(115);
  93.   return distance;
  94.   delay(100);
  95. }
  96.  
  97. int readPing(){
  98.   delay(70);
  99.   int cm = sonar.ping_cm();
  100.   if (cm==0){
  101.     cm=250;
  102.   }
  103.   return cm;
  104. }
  105.  
  106. void moveStop(){
  107.  
  108.   digitalWrite(RightMotorForward, LOW);
  109.   digitalWrite(LeftMotorForward, LOW);
  110.   digitalWrite(RightMotorBackward, LOW);
  111.   digitalWrite(LeftMotorBackward, LOW);
  112. }
  113.  
  114. void moveForward(){
  115.  
  116.   if(!goesForward){
  117.  
  118.     goesForward=true;
  119.    
  120.     digitalWrite(LeftMotorForward, HIGH);
  121.     digitalWrite(RightMotorForward, HIGH);
  122.  
  123.     digitalWrite(LeftMotorBackward, LOW);
  124.     digitalWrite(RightMotorBackward, LOW);
  125.   }
  126. }
  127.  
  128. void moveBackward(){
  129.  
  130.   goesForward=false;
  131.  
  132.   digitalWrite(LeftMotorBackward, HIGH);
  133.   digitalWrite(RightMotorBackward, HIGH);
  134.  
  135.   digitalWrite(LeftMotorForward, LOW);
  136.   digitalWrite(RightMotorForward, LOW);
  137.  
  138. }
  139.  
  140. void turnRight(){
  141.  
  142.   digitalWrite(LeftMotorForward, HIGH);
  143.   digitalWrite(RightMotorBackward, HIGH);
  144.  
  145.   digitalWrite(LeftMotorBackward, LOW);
  146.   digitalWrite(RightMotorForward, LOW);
  147.  
  148.   delay(500);
  149.  
  150.   digitalWrite(LeftMotorForward, HIGH);
  151.   digitalWrite(RightMotorForward, HIGH);
  152.  
  153.   digitalWrite(LeftMotorBackward, LOW);
  154.   digitalWrite(RightMotorBackward, LOW);
  155.  
  156.  
  157.  
  158. }
  159.  
  160. void turnLeft(){
  161.  
  162.   digitalWrite(LeftMotorBackward, HIGH);
  163.   digitalWrite(RightMotorForward, HIGH);
  164.  
  165.   digitalWrite(LeftMotorForward, LOW);
  166.   digitalWrite(RightMotorBackward, LOW);
  167.  
  168.   delay(500);
  169.  
  170.   digitalWrite(LeftMotorForward, HIGH);
  171.   digitalWrite(RightMotorForward, HIGH);
  172.  
  173.   digitalWrite(LeftMotorBackward, LOW);
  174.   digitalWrite(RightMotorBackward, LOW);
  175. }
RAW Paste Data
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
 
Top