zjz000
获得 0 次赞
帮助过0人
#include //舵机控制库 #include DistanceSRF04 Dist; int distance; int EN2 = 14; int EN3 = 15; int EN4 = 16; int EN5 = 17; int inputPin = 9; // 定義超音波信號接收腳位 int outputPin =8; // 定義超音波信號發射腳位 int EA = 3; int EB = 5; int lkf; Servo myservo; // 設 myservo int directionn = 0; // 前=8 後=2 左=4 右=6 int distanceL=0; int distanceF=0; int distanceR=0; int Fgo = 8; // 前進 int Rgo = 6; // 右轉 int Lgo = 4; // 左轉 int Bgo = 2; // 倒車 /////////////////////////////////////// void ting(void) { analogWrite(EA,0); analogWrite(EB,0); digitalWrite(EN2,LOW); digitalWrite(EN3,LOW); digitalWrite(EN4,LOW); digitalWrite(EN5,LOW); } void qian(void) { analogWrite(EA,254);//调节参数可调速度(0-255) analogWrite(EB,255);//调节参数可调速度(0-255) digitalWrite(EN2,HIGH); digitalWrite(EN3,LOW); digitalWrite(EN4,HIGH); digitalWrite(EN5,LOW); } void hou(void) { analogWrite(EA,255);//调节参数可调速度(0-255) analogWrite(EB,255); //调节参数可调速度(0-255) digitalWrite(EN2,LOW); digitalWrite(EN3,HIGH); digitalWrite(EN4,LOW); digitalWrite(EN5,HIGH); } void zuo(void) { analogWrite(EA,150);//调节参数可调速度(0-255) analogWrite(EB,150); //调节参数可调速度(0-255) digitalWrite(EN2,HIGH); digitalWrite(EN3,LOW); digitalWrite(EN4,LOW); digitalWrite(EN5,HIGH); } void you(void) { analogWrite(EA,150);//调节参数可调速度(0-255) analogWrite(EB,150); //调节参数可调速度(0-255) digitalWrite(EN2,LOW); digitalWrite(EN3,HIGH); digitalWrite(EN4,HIGH); digitalWrite(EN5,LOW); } void ask_pin_F() // 量出前方距離 { myservo.write(90); digitalWrite(outputPin, LOW); // 讓超聲波發射低電壓2μs delayMicroseconds(2); digitalWrite(outputPin, HIGH); // 讓超聲波發射高電壓10μs,這裡至少是10μs delayMicroseconds(10); digitalWrite(outputPin, LOW); // 維持超聲波發射低電壓 float Fdistance = pulseIn(inputPin, HIGH); // 讀差相差時間 Fdistance= Fdistance/5.8/10; // 將時間轉為距離距离(單位:公分) Serial.print("F distance:"); //輸出距離(單位:公分) Serial.println(Fdistance); //顯示距離 distanceF= Fdistance; // 將距離 讀入Fspeedd(前速) } void ask_pin_L() // 量出左邊距離 { myservo.write(5); delay(100); digitalWrite(outputPin, LOW); // 讓超聲波發射低電壓2μs delayMicroseconds(2); digitalWrite(outputPin, HIGH); // 讓超聲波發射高電壓10μs,這裡至少是10μs delayMicroseconds(10); digitalWrite(outputPin, LOW); // 維持超聲波發射低電壓 float Ldistance = pulseIn(inputPin, HIGH); // 讀差相差時間 Ldistance= Ldistance/5.8/10; // 將時間轉為距離距离(單位:公分) Serial.print("L distance:"); //輸出距離(單位:公分) Serial.println(Ldistance); //顯示距離 distanceL = Ldistance; // 將距離 讀入Lspeedd(左速) } void ask_pin_R() // 量出右邊距離 { myservo.write(177); delay(100); digitalWrite(outputPin, LOW); // 讓超聲波發射低電壓2μs delayMicroseconds(2); digitalWrite(outputPin, HIGH); // 讓超聲波發射高電壓10μs,這裡至少是10μs delayMicroseconds(10); digitalWrite(outputPin, LOW); // 維持超聲波發射低電壓 float Rdistance = pulseIn(inputPin, HIGH); // 讀差相差時間 Rdistance= Rdistance/5.8/10; // 將時間轉為距離距离(單位:公分) Serial.print("R distance:"); //輸出距離(單位:公分) Serial.println(Rdistance); //顯示距離 distanceR = Rdistance; // 將距離 讀入Rspeedd(右速) } void detection() //測量3個角度(0.90.179) { int delay_time = 250; // 伺服馬達轉向後的穩定時間 ask_pin_F(); // 讀取前方距離 if(distanceFdistanceR) //假如 左邊距離大於右邊距離 { directionn = Rgo; //向右走 } if(distanceL