fork download
copy 
  1. //超音波智能避障（ARDUINO）
  2. //    L = 左
  3. //    R = 右
  4. //    F = 前
  5. //    B = 後
  6.  
  7. #include <Servo.h> 
  8. int pinLB=6;     // 定義6腳位 左後
  9. int pinLF=9;     // 定義9腳位 左前
  10.  
  11. int pinRB=10;    // 定義10腳位 右後
  12. int pinRF=11;    // 定義11腳位 右前
  13.  
  14. int inputPin = A0;  // 定義超音波信號接收腳位
  15. int outputPin =A1;  // 定義超音波信號發射腳位
  16.  
  17. int Fspeedd = 0;      // 前速
  18. int Rspeedd = 0;      // 右速
  19. int Lspeedd = 0;      // 左速
  20. int directionn = 0;   // 前=8 後=2 左=4 右=6 
  21. Servo myservo;        // 設 myservo
  22. int delay_time = 250; // 伺服馬達轉向後的穩定時間
  23.  
  24. int Fgo = 8;         // 前進
  25. int Rgo = 6;         // 右轉
  26. int Lgo = 4;         // 左轉
  27. int Bgo = 2;         // 倒車
  28.  
  29. void setup()
  30.  {
  31.   Serial.begin(9600);     // 定義馬達輸出腳位 
  32.   pinMode(pinLB,OUTPUT); // 腳位 8 (PWM)
  33.   pinMode(pinLF,OUTPUT); // 腳位 9 (PWM)
  34.   pinMode(pinRB,OUTPUT); // 腳位 10 (PWM) 
  35.   pinMode(pinRF,OUTPUT); // 腳位 11 (PWM)
  36.  
  37.   pinMode(inputPin, INPUT);    // 定義超音波輸入腳位
  38.   pinMode(outputPin, OUTPUT);  // 定義超音波輸出腳位   
  39.  
  40.   myservo.attach(5);    // 定義伺服馬達輸出第5腳位(PWM)
  41.  }
  42. void advance(int a)     // 前進
  43.     {
  44.      digitalWrite(pinRB,LOW);  // 使馬達（右後）動作
  45.      digitalWrite(pinRF,HIGH);
  46.      digitalWrite(pinLB,LOW);  // 使馬達（左後）動作
  47.      digitalWrite(pinLF,HIGH);
  48.      delay(a * 100);     
  49.     }
  50.  
  51. void right(int b)        //右轉(單輪)
  52.     {
  53.      digitalWrite(pinRB,LOW);   //使馬達（右後）動作
  54.      digitalWrite(pinRF,HIGH);
  55.      digitalWrite(pinLB,HIGH);
  56.      digitalWrite(pinLF,HIGH);
  57.      delay(b * 100);
  58.     }
  59. void left(int c)         //左轉(單輪)
  60.     {
  61.      digitalWrite(pinRB,HIGH);
  62.      digitalWrite(pinRF,HIGH);
  63.      digitalWrite(pinLB,LOW);   //使馬達（左後）動作
  64.      digitalWrite(pinLF,HIGH);
  65.      delay(c * 100);
  66.     }
  67. void turnR(int d)        //右轉(雙輪)
  68.     {
  69.      digitalWrite(pinRB,LOW);  //使馬達（右後）動作
  70.      digitalWrite(pinRF,HIGH);
  71.      digitalWrite(pinLB,HIGH);
  72.      digitalWrite(pinLF,LOW);  //使馬達（左前）動作
  73.      delay(d * 100);
  74.     }
  75. void turnL(int e)        //左轉(雙輪)
  76.     {
  77.      digitalWrite(pinRB,HIGH);
  78.      digitalWrite(pinRF,LOW);   //使馬達（右前）動作
  79.      digitalWrite(pinLB,LOW);   //使馬達（左後）動作
  80.      digitalWrite(pinLF,HIGH);
  81.      delay(e * 100);
  82.     }    
  83. void stopp(int f)         //停止
  84.     {
  85.      digitalWrite(pinRB,HIGH);
  86.      digitalWrite(pinRF,HIGH);
  87.      digitalWrite(pinLB,HIGH);
  88.      digitalWrite(pinLF,HIGH);
  89.      delay(f * 100);
  90.     }
  91. void back(int g)          //後退
  92.     {
  93.  
  94.      digitalWrite(pinRB,HIGH);  //使馬達（右後）動作
  95.      digitalWrite(pinRF,LOW);
  96.      digitalWrite(pinLB,HIGH);  //使馬達（左後）動作
  97.      digitalWrite(pinLF,LOW);
  98.      delay(g * 100);     
  99.     }
  100.  
  101. void detection()        //測量3個角度(0.90.179)
  102.     {      
  103.       int delay_time = 250;   // 伺服馬達轉向後的穩定時間
  104.       ask_pin_F();            // 讀取前方距離
  105.  
  106.      if(Fspeedd < 10)         // 假如前方距離小於10公分
  107.       {
  108.       stopp(1);               // 清除輸出資料 
  109.       back(2);                // 後退 0.2秒
  110.       }
  111.  
  112.       if(Fspeedd < 25)         // 假如前方距離小於25公分
  113.       {
  114.         stopp(1);               // 清除輸出資料 
  115.         ask_pin_L();            // 讀取左方距離
  116.         delay(delay_time);      // 等待伺服馬達穩定
  117.         ask_pin_R();            // 讀取右方距離  
  118.         delay(delay_time);      // 等待伺服馬達穩定  
  119.  
  120.         if(Lspeedd > Rspeedd)   //假如 左邊距離大於右邊距離
  121.         {
  122.          directionn = Rgo;      //向右走
  123.         }
  124.  
  125.         if(Lspeedd <= Rspeedd)   //假如 左邊距離小於或等於右邊距離
  126.         {
  127.          directionn = Lgo;      //向左走
  128.         } 
  129.  
  130.         if (Lspeedd < 10 && Rspeedd < 10)   //假如 左邊距離和右邊距離皆小於10公分
  131.         {
  132.          directionn = Bgo;      //向後走        
  133.         }          
  134.       }
  135.       else                      //加如前方不小於(大於)25公分     
  136.       {
  137.         directionn = Fgo;        //向前走     
  138.       }
  139.  
  140.     }    
  141. void ask_pin_F()   // 量出前方距離 
  142.     {
  143.       myservo.write(90);
  144.       digitalWrite(outputPin, LOW);   // 讓超聲波發射低電壓2μs
  145.       delayMicroseconds(2);
  146.       digitalWrite(outputPin, HIGH);  // 讓超聲波發射高電壓10μs，這裡至少是10μs
  147.       delayMicroseconds(10);
  148.       digitalWrite(outputPin, LOW);    // 維持超聲波發射低電壓
  149.       float Fdistance = pulseIn(inputPin, HIGH);  // 讀差相差時間
  150.       Fdistance= Fdistance/5.8/10;       // 將時間轉為距離距离（單位：公分）
  151.       Serial.print("F distance:");      //輸出距離（單位：公分）
  152.       Serial.println(Fdistance);         //顯示距離
  153.       Fspeedd = Fdistance;              // 將距離 讀入Fspeedd(前速)
  154.     }  
  155.  void ask_pin_L()   // 量出左邊距離 
  156.     {
  157.       myservo.write(5);
  158.       delay(delay_time);
  159.       digitalWrite(outputPin, LOW);   // 讓超聲波發射低電壓2μs
  160.       delayMicroseconds(2);
  161.       digitalWrite(outputPin, HIGH);  // 讓超聲波發射高電壓10μs，這裡至少是10μs
  162.       delayMicroseconds(10);
  163.       digitalWrite(outputPin, LOW);    // 維持超聲波發射低電壓
  164.       float Ldistance = pulseIn(inputPin, HIGH);  // 讀差相差時間
  165.       Ldistance= Ldistance/5.8/10;       // 將時間轉為距離距离（單位：公分）
  166.       Serial.print("L distance:");       //輸出距離（單位：公分）
  167.       Serial.println(Ldistance);         //顯示距離
  168.       Lspeedd = Ldistance;              // 將距離 讀入Lspeedd(左速)
  169.     }  
  170. void ask_pin_R()   // 量出右邊距離 
  171.     {
  172.       myservo.write(177);
  173.       delay(delay_time);
  174.       digitalWrite(outputPin, LOW);   // 讓超聲波發射低電壓2μs
  175.       delayMicroseconds(2);
  176.       digitalWrite(outputPin, HIGH);  // 讓超聲波發射高電壓10μs，這裡至少是10μs
  177.       delayMicroseconds(10);
  178.       digitalWrite(outputPin, LOW);    // 維持超聲波發射低電壓
  179.       float Rdistance = pulseIn(inputPin, HIGH);  // 讀差相差時間
  180.       Rdistance= Rdistance/5.8/10;       // 將時間轉為距離距离（單位：公分）
  181.       Serial.print("R distance:");       //輸出距離（單位：公分）
  182.       Serial.println(Rdistance);         //顯示距離
  183.       Rspeedd = Rdistance;              // 將距離 讀入Rspeedd(右速)
  184.     }  
  185.  
  186. void loop()
  187.  {
  188.     myservo.write(90);  //讓伺服馬達回歸 預備位置 準備下一次的測量
  189.     detection();        //測量角度 並且判斷要往哪一方向移動
  190.  
  191.    if(directionn == 2)  //假如directionn(方向) = 2(倒車)             
  192.    {
  193.      back(8);                    //  倒退(車)
  194.      turnL(2);                   //些微向左方移動(防止卡在死巷裡)
  195.      Serial.print(" Reverse ");   //顯示方向(倒退)
  196.    }
  197.    if(directionn == 6)           //假如directionn(方向) = 6(右轉)    
  198.    {
  199.      back(1); 
  200.      turnR(6);                   // 右轉
  201.      Serial.print(" Right ");    //顯示方向(左轉)
  202.    }
  203.    if(directionn == 4)          //假如directionn(方向) = 4(左轉)    
  204.    {  
  205.      back(1);      
  206.      turnL(6);                  // 左轉
  207.      Serial.print(" Left ");     //顯示方向(右轉)   
  208.    }  
  209.    if(directionn == 8)          //假如directionn(方向) = 8(前進)      
  210.    { 
  211.     advance(1);                 // 正常前進  
  212.     Serial.print(" Advance ");   //顯示方向(前進)
  213.     Serial.print("   ");    
  214.    }
  215.  }
  216.  
  217.  
Compilation error #stdin compilation error #stdout 0s 0KB
comments (?)
stdin
copy 
Standard input is empty
compilation info 
prog.c:7:20: fatal error: Servo.h: No such file or directory
 #include <Servo.h> 
                    ^
compilation terminated.
stdout
copy 
Standard output is empty
https://ideone.com/5mlWWh
language:
C (gcc 8.3)
created:
10 years ago
visibility:
public

Share or Embed source code

Discover > Sphere Engine API

The brand new service which powers Ideone!

Discover > IDE Widget

Widget for compiling and running the source code in a web browser!