SHARE
TWEET

Untitled

a guest Jun 26th, 2019 64 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #define IZQTRIGGER 2
  2. #define IZQECHO 3
  3. #define DERTRIGGER 4
  4. #define DERECHO 5
  5. #define ADETRIGGER 6
  6. #define ADEECHO 7
  7.  
  8. int ENA = 19;
  9. int IN1 = 18;
  10. int IN2 = 17;
  11.  
  12. int ENB = 14;
  13. int IN3 = 16;
  14. int IN4 = 15;
  15.  
  16. const float sonido = 34300.0; // Velocidad del sonido en cm/s
  17.  
  18. void setup ()
  19. {
  20.  Serial.begin(9600);
  21.  pinMode (ENA, OUTPUT);
  22.  pinMode (ENB, OUTPUT);
  23.  pinMode (IN1, OUTPUT);
  24.  pinMode (IN2, OUTPUT);
  25.  pinMode (IN3, OUTPUT);
  26.  pinMode (IN4, OUTPUT);
  27.  pinMode (DERECHO, INPUT);
  28.  pinMode (DERTRIGGER, OUTPUT);
  29.  pinMode (IZQECHO, INPUT);
  30.  pinMode (IZQTRIGGER, OUTPUT);
  31.  pinMode (ADEECHO, INPUT);
  32.  pinMode (ADETRIGGER, OUTPUT);
  33. }
  34.  
  35.  
  36. void loop() {
  37.   //delay (5000);
  38.   //while (true){
  39.     izqIniciarTrigger();
  40.     float distanciaizq = calcularDistanciaIzq();
  41.     derIniciarTrigger();
  42.     float distanciader = calcularDistanciaDer();
  43.     adeIniciarTrigger();
  44.     float distanciaade = calcularDistanciaAde();
  45.  
  46.     if (distanciaade < 10){
  47.       Adelante();
  48.       Serial.println ("RUUUUUUUUUUUUUN");
  49.     } else {
  50.       Parar();
  51.       Serial.println ("bip");
  52.     }
  53.    
  54.   //}
  55. }
  56.  
  57.  
  58. void izqIniciarTrigger()
  59. {
  60.   // Ponemos el Triiger en estado bajo y esperamos 2 ms
  61.   digitalWrite(IZQTRIGGER, LOW);
  62.   //delayMicroseconds(1);
  63.  
  64.   // Ponemos el pin Trigger a estado alto y esperamos 10 ms
  65.   digitalWrite(IZQTRIGGER, HIGH);
  66.   //delayMicroseconds(2);
  67.  
  68.   // Comenzamos poniendo el pin Trigger en estado bajo
  69.   digitalWrite(IZQTRIGGER, LOW);
  70. }
  71.  
  72. void derIniciarTrigger()
  73. {
  74.   digitalWrite(DERTRIGGER, LOW);
  75.   //delayMicroseconds(1);
  76.  
  77.   digitalWrite(DERTRIGGER, HIGH);
  78.   //delayMicroseconds(2);
  79.  
  80.   digitalWrite(DERTRIGGER, LOW);
  81. }
  82.  
  83. void adeIniciarTrigger()
  84. {
  85.   digitalWrite(ADETRIGGER, LOW);
  86.   //delayMicroseconds(1);
  87.  
  88.   digitalWrite(ADETRIGGER, HIGH);
  89.   //delayMicroseconds(2);
  90.  
  91.   digitalWrite(ADETRIGGER, LOW);
  92. }
  93.  
  94.  
  95.  
  96. float calcularDistanciaIzq()
  97. {
  98.   unsigned long tiempo = pulseIn(IZQECHO, HIGH);
  99.  
  100.   float distancia1 = tiempo * 0.000001 * sonido / 2.0;
  101.   Serial.print(distancia1);
  102.   Serial.print("cm");
  103.   Serial.println();
  104.  
  105.   return distancia1;
  106. }
  107.  
  108. float calcularDistanciaDer()
  109. {
  110.   unsigned long tiempo = pulseIn(DERECHO, HIGH);
  111.  
  112.   float distancia2 = tiempo * 0.000001 * sonido / 2.0;
  113.   Serial.print(distancia2);
  114.   Serial.print("cm");
  115.   Serial.println();
  116.  
  117.   return distancia2;
  118. }
  119.  
  120. float calcularDistanciaAde()
  121. {
  122.   // La función pulseIn obtiene el tiempo que tarda en cambiar entre estados, en este caso a HIGH
  123.   unsigned long tiempo = pulseIn(ADEECHO, HIGH);
  124.  
  125.   // Obtenemos la distancia en cm, hay que convertir el tiempo en segudos ya que está en microsegundos
  126.   // por eso se multiplica por 0.000001
  127.   float distancia3 = tiempo * 0.000001 * sonido / 2.0;
  128.   Serial.print(distancia3);
  129.   Serial.print("cm ade");
  130.   Serial.println();
  131.  
  132.   return distancia3;
  133. }
  134.  
  135.  
  136.  
  137. void Adelante ()
  138. {
  139.  //Direccion motor A
  140.  digitalWrite (IN1, HIGH);
  141.  digitalWrite (IN2, LOW);
  142.  analogWrite (ENA, 255); //Velocidad motor A
  143.  //Direccion motor B
  144.  digitalWrite (IN3, HIGH);
  145.  digitalWrite (IN4, LOW);
  146.  analogWrite (ENB, 255); //Velocidad motor B
  147. }
  148.  
  149. void Atras ()
  150. {
  151.  //Direccion motor A
  152.  digitalWrite (IN1, LOW);
  153.  digitalWrite (IN2, HIGH);
  154.  analogWrite (ENA, 128); //Velocidad motor A
  155.  //Direccion motor B
  156.  digitalWrite (IN3, LOW);
  157.  digitalWrite (IN4, HIGH);
  158.  analogWrite (ENB, 128); //Velocidad motor B
  159. }
  160.  
  161. void Derecha ()
  162. {
  163.  //Direccion motor A
  164.  digitalWrite (IN1, HIGH);
  165.  digitalWrite (IN2, LOW);
  166.  analogWrite (ENA, 200); //Velocidad motor A
  167.  //Direccion motor B
  168.  digitalWrite (IN3, LOW);
  169.  digitalWrite (IN4, HIGH);
  170.  analogWrite (ENB, 100); //Velocidad motor A
  171. }
  172.  
  173. void Izquierda ()
  174. {
  175.  //Direccion motor A
  176.  digitalWrite (IN1, LOW);
  177.  digitalWrite (IN2, HIGH);
  178.  analogWrite (ENA, 50); //Velocidad motor A
  179.  //Direccion motor B
  180.  digitalWrite (IN3, HIGH);
  181.  digitalWrite (IN4, LOW);
  182.  analogWrite (ENB, 150); //Velocidad motor A
  183. }
  184.  
  185. void Parar ()
  186. {
  187.  //Direccion motor A
  188.  digitalWrite (IN1, LOW);
  189.  digitalWrite (IN2, LOW);
  190.  analogWrite (ENA, 0); //Velocidad motor A
  191.  //Direccion motor B
  192.  digitalWrite (IN3, LOW);
  193.  digitalWrite (IN4, LOW);
  194.  analogWrite (ENB, 0); //Velocidad motor A
  195. }
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
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!
 
Top