luistavares

Robô Arduino Autônomo de Fuga Que Verifica os Lados

Aug 13th, 2018
120
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <Servo.h>          
  2. #include <NewPing.h>        
  3.  
  4. const int servoPin = 3;
  5. const int RightMotorForward = 6;
  6. const int RightMotorBackward = 7;
  7. const int LeftMotorForward = 5;
  8. const int LeftMotorBackward = 4;
  9.  
  10. #define trig_pin A0
  11. #define echo_pin A1
  12. #define maximum_distance 200
  13. int distance = 100;
  14.  
  15. NewPing sonar(trig_pin, echo_pin, maximum_distance); //sensor function
  16. Servo servo_motor; //our servo name
  17.  
  18. void setup(){
  19.   pinMode(RightMotorForward, OUTPUT);
  20.   pinMode(LeftMotorForward, OUTPUT);
  21.   pinMode(LeftMotorBackward, OUTPUT);
  22.   pinMode(RightMotorBackward, OUTPUT);
  23.  
  24.   servo_motor.attach(servoPin);
  25.   servo_motor.write(95);
  26.  
  27.   delay(200);
  28.   distance = readPing();
  29.   delay(20);
  30.   distance = readPing();  
  31.   Serial.begin(9600);
  32. }
  33.  
  34. void loop(){
  35.   int distanceRight = 0;
  36.   int distanceLeft = 0;
  37.   delay(50);
  38.  
  39.   if (distance <= 20){
  40.     moveStop();
  41.     delay(300);
  42.     moveBackward();
  43.     delay(200);
  44.     moveStop();
  45.     delay(300);
  46.     distanceRight = lookRight();
  47.     delay(200);
  48.     distanceLeft = lookLeft();
  49.     delay(200);
  50.    
  51.     if (distanceRight >= distanceLeft){
  52.       turnRight();
  53.       delay(200);  
  54.       moveStop();
  55.     }
  56.     else{
  57.       turnLeft();
  58.       delay(200);  
  59.       moveStop();
  60.     }
  61.   }    
  62.    delay(200);
  63.    moveForward();      
  64.    distance = readPing();
  65.   Serial.print("Leitura à frente: ");
  66.   Serial.println(distance);
  67. }
  68.  
  69. int lookRight(){  
  70.   servo_motor.write(50);
  71.   delay(500);
  72.   int distance = readPing();
  73.   for (int i=0; i<6; i++){  
  74.     int d = readPing();
  75.     if (d<distance){
  76.       distance = d;
  77.     }
  78.   }
  79.   servo_motor.write(95);
  80.   Serial.print("Leitura à direita: ");
  81.   Serial.println(distance);
  82.   return distance;
  83. }
  84.  
  85. int lookLeft(){
  86.   servo_motor.write(150);
  87.   delay(500);
  88.   int distance = readPing();
  89.   for (int i=0; i<6; i++){    
  90.     int d = readPing();
  91.     if (d<distance){
  92.       distance = d;
  93.     }
  94.   }  
  95.   servo_motor.write(95);  
  96.   Serial.print("Leitura à esquerda: ");
  97.   Serial.println(distance);
  98.   return distance;  
  99. }
  100.  
  101. int readPing(){
  102.   delay(70);
  103.   int cm = sonar.ping_cm();
  104.   if (cm==0){
  105.     cm=250;
  106.   }
  107.   return cm;
  108. }
  109.  
  110. void moveForward(){
  111.   for (int i=0; i<10; i++){  
  112.     motorLForward();
  113.     motorRForward();
  114.     delay(10);
  115.     moveStop();
  116.     delay(10);
  117.   }
  118. }
  119.  
  120. void moveBackward(){
  121.   motorLBackward();
  122.   motorRBackward();    
  123. }
  124.  
  125. void turnRight(){
  126.   Serial.println("Vira à direita");
  127.   motorLForward();
  128.   motorRBackward();    
  129. }
  130.  
  131. void turnLeft(){
  132.   Serial.println("Vira à esquerda");
  133.   motorRForward();
  134.   motorLBackward();
  135. }
  136.  
  137. void moveStop(){  
  138.   digitalWrite(RightMotorForward, LOW);
  139.   digitalWrite(LeftMotorForward, LOW);
  140.   digitalWrite(RightMotorBackward, LOW);
  141.   digitalWrite(LeftMotorBackward, LOW);
  142. }
  143.  
  144. void motorRForward(){
  145.   digitalWrite(RightMotorForward, HIGH);    
  146.   digitalWrite(RightMotorBackward, LOW);  
  147. }
  148.  
  149. void motorRBackward(){
  150.   digitalWrite(RightMotorForward, LOW);    
  151.   digitalWrite(RightMotorBackward, HIGH);  
  152. }
  153.  
  154. void motorLForward(){
  155.   digitalWrite(LeftMotorForward, HIGH);    
  156.   digitalWrite(LeftMotorBackward, LOW);  
  157. }
  158.  
  159. void motorLBackward(){
  160.   digitalWrite(LeftMotorForward, LOW);    
  161.   digitalWrite(LeftMotorBackward, HIGH);  
  162. }
RAW Paste Data