|
int led1=0,led2=0,led3=0,led4=0,led5=0,led6=0,led7=0; //電壓7.8V ;led1~7 分別用來接收七個探頭檢測回來的值 int in1=9,in2=3,in3=10,in4=11; //數(shù)字IO口9,3,10,11用來控制電機 int sum=0; void setup() { pinMode(0,INPUT);pinMode(7,INPUT); pinMode(1,INPUT);pinMode(8,INPUT); pinMode(2,INPUT);pinMode(3,OUTPUT); pinMode(6,INPUT);pinMode(9,OUTPUT); pinMode(4,INPUT);pinMode(10,OUTPUT); pinMode(13,INPUT);pinMode(11,OUTPUT); pinMode(12,INPUT);pinMode(5,INPUT); digitalWrite(in2,LOW);digitalWrite(in4,LOW); } void motor(char pwmpin,char pin,int val) //給電機兩個輸入口賦PWM值 { pinMode(pin, OUTPUT); analogWrite(pwmpin,val); digitalWrite(pin,LOW); } void turn_led1_0() //in1給20，in2給低電平 { //in3給250，in4給低電平 motor(in1,in2,20); //PWM滿值255，差值決定轉(zhuǎn)角度，大小決定速度 motor(in3,in4,250); } void turn_led1_1() { motor(in1,in2,20); motor(in3,in4,250); } void turn_led2_0() { motor(in1,in2,70); motor(in3,in4,250); } void turn_led2_1() { motor(in1,in2,130); motor(in3,in4,250); } void turn_led3_0() { motor(in1,in2,180); motor(in3,in4,250); } void turn_led3_1() { motor(in1,in2,250); motor(in3,in4,250); } void turn_led4_0() { motor(in1,in2,250); motor(in3,in4,250); } void turn_led4_1() { motor(in1,in2,250); motor(in3,in4,250); } void turn_led5_0() { motor(in1,in2,250); motor(in3,in4,180); } void turn_led5_1() { motor(in1,in2,250); motor(in3,in4,130); } void turn_led6_0() { motor(in1,in2,250); motor(in3,in4,70); } void turn_led6_1() { motor(in1,in2,250); motor(in3,in4,20); } void turn_led7_0() { motor(in1,in2,250); motor(in3,in4,20); } void turn_stop() { motor(in1,in2,0); motor(in1,in2,0); } void test() //檢測光電管探頭。是黑線返回高電平（1），白線返回低電平（0），這與實際電路相關(guān) { //0,1,2,6,4,7,8是數(shù)字IO口，讀取檢測值 led1=digitalRead(0); led2=digitalRead(1); led3=digitalRead(2); led4=digitalRead(6); led5=digitalRead(4); led6=digitalRead(7); led7=digitalRead(8); } int deal() //對每個探頭賦值：從左往右-6，-4，-2,0,2,4,6 記錄檢測到黑線的探頭個數(shù)并將值相加 { int a=0,b=0,c=0,d=0,e=0,f=0,g=0; int i=0,value=0; test(); if(led1==1) { a=-6; i++; } if(led2==1) { b=-4; i++; } if(led3==1) { c=-2; i++; } if(led4==1) { d=0; i++; } if(led5==1) { e=2; i++; } if(led6==1) { f=4; i++; } if(led7==1) { g=6; i++; } sum=a+b+c+d+e+f+g; //求平均值，較精確的確定黑線位置 if(i==0) value=8; //分母不能為0，沖出跑道均沒檢測到的特殊情況,讓小車保持上一次的狀態(tài) else if(i<3) value=sum/i; //一般的情況：1個或2個檢測到黑線 else { if(sum<0) value=-6; //特殊情況：左急彎，右急彎，直道 else if(sum>0) value=6; else value=0; } return value; } void loop() { static char a[5],j=0; //數(shù)組記錄上次值 if(j==5) j=0; if(deal()==8) { if(j==0) a[j]=a[4]; else a[j]=a[j-1]; } switch(a[j]) //對各種返回值的響應(yīng)速度和轉(zhuǎn)彎角度 { case -6:turn_led1_0();break; case -5:turn_led1_1();break; case -4:turn_led2_0();break; case -3:turn_led2_1();break; case -2:turn_led3_0();break; case -1:turn_led3_1();break; case 0:turn_led4_0();break; case 1:turn_led4_1();break; case 2:turn_led5_0();break; case 3:turn_led5_1();break; case 4:turn_led6_0();break; case 5:turn_led6_1();break; case 6:turn_led7_0();break; default:turn_led4_0();break; } j++; } /*相比較于數(shù)字信號的話，這個處理程序應(yīng)該比較好了。PWM值，角度，速度與實際情況有較大聯(lián)系，車的配置，摩擦，電壓，車重量，電機轉(zhuǎn)速，輪子大小等，所以各位下載后還需細(xì)調(diào)。不過，這個程序就在于很好調(diào)，你只要改變上面的PWM值就可實現(xiàn)轉(zhuǎn)彎與速度的實際搭配*/
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
