Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- //匯入
- #include <Arduino.h>
- #include <Wire.h>
- #include <SoftwareSerial.h>
- #include <MeMCore.h>
- #include <Timer.h>
- //自訂參數
- MBotDCMotor motor_left(9); //左邊馬達
- MBotDCMotor motor_right(10); //右邊馬達
- MeLineFollower IF1(2); // ss1 ss2
- MeLineFollower IF2(1); // ss3
- MeLineFollower IF3(4); // ss4 ss5
- MeUltrasonicSensor UTS(3); //超音波
- MeBuzzer buzzer; //嗡鳴器
- byte ReturnIF1; //設定左邊感測器的回傳值
- byte S3; //callback使用判斷值
- byte ReturnIF3; //設定右邊感測器的回傳值
- int buttom; //設定一個參數為buttom
- int distance; //設定一個參數為distance
- bool times = false; //設定一個是否值
- const int Ttimes = 3; //目標計算次數
- bool times2 = false; //設定一個是否值
- const int Ttimes2 = 3; //設定計算次數
- byte S3Output = 0; //循線使用回傳值
- Timer t; //設定Timer為t
- int add = 0; //callback使用計算值
- int outadd = 0; //設定callback外部使用設定值
- bool Buttomif =true; //設定按下按鈕後的判斷條件
- int S3times = 0;
- void setup()
- {
- buzzer.tone(262,200);
- buzzer.tone(294,200);
- buzzer.tone(330,200); //輸出已(重)開機訊號
- t.every(80,callback); //設定timer參數
- }
- void callback() //timer使用的callback函數
- {
- outadd++;
- if(S3 == 0) //S3感測為黑時
- {
- add++; //add計算值+1
- S3times++;
- }
- if(S3times == 3)
- {
- if(add>=3) //如果add大於等於3時
- {
- S3Output = 0; //輸入訊號為黑
- }
- else{
- S3Output = 1; //輸入訊號為白
- }
- if(outadd >= 3)
- {
- add = 0;
- outadd = 0;
- }
- }
- }
- void loop()
- {
- buttom=analogRead(A7); //設定buttom為板載按鈕
- ReturnIF1=IF1.readSensors();
- S3=IF2.readSensors(); //callback判對值為中間感測器資料
- ReturnIF3=IF3.readSensors(); //設定五路的感測資料位置
- distance=UTS.distanceCm(); //令超音波資料為distance
- int Atimes = 0;
- int Atimes2 = 0;
- t.update();
- if(buttom<10) //如果按鈕按下去
- {
- Buttomif = false;
- }
- if(Buttomif == false)
- { //按下板載按鈕後執行所有loop程式
- if(distance <= 5) //距離小於5cm 程式避障
- {
- motor_left.run(255);
- motor_right.run(-255);
- delay(200); //後退
- motor_left.run(-55);
- motor_right.run(255);
- delay(500); //左轉
- motor_left.run(-255);
- motor_right.run(255);
- delay(200); //前進
- motor_left.run(-255);
- motor_right.run(55);
- delay(700); //右轉
- motor_left.run(-255);
- motor_right.run(255);
- delay(150); //接線時請調150,鋰電池時請調250(前進)
- motor_left.run(255);
- motor_right.run(0);
- delay(680); //回復路線
- }
- else{ //無感測到障礙物時
- if(ReturnIF1!=0 && S3Output!=0 && ReturnIF3!=0)
- {
- while(Atimes < Ttimes) //計算次數(次數(0~3)<目標次數)
- {
- if (ReturnIF1!=0 && S3Output!=0 && ReturnIF3!=0) // 全感白
- {
- motor_left.run(-255); //直走
- motor_right.run(255);
- delay(20); //0.02秒(未測試)
- Atimes++; //次數+1
- }
- else{
- Atimes = 0;
- break; //就跳出
- }
- }
- if(Atimes == Ttimes) //次數=目標次數
- {
- while(ReturnIF1!=0 && S3Output!=0 && ReturnIF3!=0) //假如是白
- {
- Atimes = 0;
- motor_left.run(-255);
- motor_right.run(255);
- //直走
- }
- }
- }
- if(ReturnIF1==0 && S3Output==0 && ReturnIF3==0)
- {
- while(Atimes2 < Ttimes2) //計算次數(次數(0~3)<目標次數)
- {
- if (ReturnIF1==0 && S3Output==0 && ReturnIF3==0) // 全感黑
- {
- motor_left.run(-255); //直走
- motor_right.run(255);
- delay(20); //0.02秒(未測試)
- Atimes2++; //次數+1
- }
- else{
- Atimes2 = 0;
- break; //就跳出
- }
- }
- if(Atimes2 == Ttimes2) //次數=目標次數
- {
- while(ReturnIF1==0 && S3Output==0 && ReturnIF3==0) //假如是黑
- {
- Atimes2 = 0;
- motor_left.run(-255);
- motor_right.run(255);
- //直走
- }
- }
- }
- //以下為新型循線
- // ==0 = 黑 !=0 = 白
- if(ReturnIF1!=0 && S3Output==0 && ReturnIF3!=0) //ss3感黑 其他感白
- {
- //白黑白
- motor_left.run(-255);
- motor_right.run(255);
- //直走
- }
- else if (ReturnIF1==0 && S3Output!=0 && ReturnIF3!=0) // 一二路感黑 其他感白
- {
- //黑白白
- motor_left.run(0);
- motor_right.run(255);
- //往左 (未測試)
- }
- else if (ReturnIF1==0 && S3Output==0 && ReturnIF3!=0) // 四五路感白 其他感黑
- {
- //黑黑白
- motor_left.run(255);
- motor_right.run(0);
- //往右(未測試)
- }
- else if (ReturnIF1!=0 && S3Output!=0 && ReturnIF3==0) // 四五路感黑 其他感白
- {
- //白白黑
- motor_left.run(0);
- motor_right.run(255);
- //往左 (未測試)
- }
- else if (ReturnIF1!=0 && S3Output==0 && ReturnIF3==0) // 一二路感白 其他感黑
- {
- //白黑黑
- motor_left.run(-255);
- motor_right.run(0);
- //往右(未測試)
- }
- }
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement