会arduino 或c++或c的高手赶快帮忙啊!
程序代码:
int OUT1=2; int OUT2=3; int IN4=8; // 定义数字第8腳 接右边的MOTOR方向控制位IN4 int IN3=9; //定义数字第9腳 接右边的MOTOR方向控制位IN3 int IN2=10; // 定义数字第10腳 接左边的MOTOR方向控制位IN2 int IN1=11; // 定义数字第11腳 接左边的MOTOR方向控制位IN1 int MotorA=5; // PWMA引脚定义为数字5脚 int MotorB=6; // PWMB引脚定义为数字6脚 int Lspeed=230;//量让车子走成直线 int Rspeed=230;//让车子走成直线 int IR1=14; //左寻线传感接 A0 当数字端口 D14使用 int IR2=15; //中间寻线传感接 A1 当数字端口 D15使用 int IR3=16; //右边寻线传感接 A2 当数字端口 D16使用 int IR4=17; int IR5=18; int c1,c2; int a1,a2; int temp=0; //临时变量,用于存储上一次的状态 int temp1=5;//表示检测到00000的次数 int temp2=1;//表示走的圈数 int z;//奇数圈或偶数圈以及是第几次遇到00000的区别 int time=0,l,c,r,q,w; void setup() { Serial.begin(9600); // 定义串口输出的波特率 pinMode(IN4,OUTPUT); // 定义为输出,下面相同 pinMode(IN3,OUTPUT); pinMode(IN2,OUTPUT); pinMode(IN1,OUTPUT); pinMode(MotorA,OUTPUT); pinMode(MotorB,OUTPUT); /*pinMode(2,OUTPUT); pinMode(4,OUTPUT); pinMode(3,OUTPUT);*/ pinMode(IR1, INPUT); // 定义为寻线传感输出,下面相同/////////////////// pinMode(IR2, INPUT); pinMode(IR3, INPUT); ////////////////////////////////////////////////// pinMode(IR4,INPUT); pinMode(IR5,INPUT); /*pinMode(IR6,INPUT); pinMode(IR7,INPUT); pinMode(IR8,INPUT);*/ attachInterrupt(0,COUNT1,FALLING); attachInterrupt(1,COUNT2,FALLING); /*myservo.attach(2); //定义数字第2,4脚为舵机控制引脚 myservo.attach(4); */ } void COUNT1()//计数函数 { c1++; } void COUNT2() { c2++; } /*void ReLay()//继电器的收放 { digitalWrite(3,HIGH); delay(100); digitalWrite(3,LOW); } void SteeRing1(int m)//控制头的转动角度 { myservo.write(m); //写入舵机角度 } void SteeRing2(int p)//控制机器臂的转动角度 { myservo.write(p); //写入舵机角度 }*/ void advance(int a) // 前进 { digitalWrite(IN2,LOW); digitalWrite(IN1,HIGH); analogWrite(MotorB,Rspeed-70); digitalWrite(IN4,HIGH); digitalWrite(IN3,LOW); analogWrite(MotorA,Lspeed-70); delayMicroseconds;// delay(a*1); } void back(int g) { digitalWrite(IN2,HIGH); digitalWrite(IN1,LOW); analogWrite(MotorB,Rspeed-80); digitalWrite(IN4,LOW); //左轮前进,和超声波车不同 digitalWrite(IN3,HIGH); analogWrite(MotorA,Lspeed-80); delay(g*1);// ;//可以通过和参数相乘得出 } void right(int b) //右转(双轮模式) { digitalWrite(IN2,HIGH); digitalWrite(IN1,HIGH); analogWrite(MotorB,Rspeed-80); digitalWrite(IN4,HIGH); //左轮前进,和超声波车不同 digitalWrite(IN3,LOW); analogWrite(MotorA,Lspeed-80); delayMicroseconds;// ;//可以通过和参数相乘得出 } void right1(int d) { digitalWrite(IN2,HIGH); //右轮后退 digitalWrite(IN1,LOW); analogWrite(MotorB,Rspeed-100); digitalWrite(IN4,HIGH); //左轮前进,和超声波车不同 digitalWrite(IN3,LOW); analogWrite(MotorA,Lspeed-100); delay(d*1);//以通过和参数相乘得出 } void left(int c) //左转双轮模式 { digitalWrite(IN2,LOW); //右边轮子前进 digitalWrite(IN1,HIGH); analogWrite(MotorB,Rspeed-80); digitalWrite(IN4,HIGH); // digitalWrite(IN3,HIGH); analogWrite(MotorA,Lspeed-80); delayMicroseconds; } void left1(int f) { digitalWrite(IN2,LOW); //右边轮子前进 digitalWrite(IN1,HIGH); analogWrite(MotorB,Rspeed-20); digitalWrite(IN4,LOW); //左边的电机后退 digitalWrite(IN3,HIGH); analogWrite(MotorA,Lspeed-20); delay(f*1); } void stop(int e) //停止 { digitalWrite(IN2,HIGH); digitalWrite(IN1,HIGH); analogWrite(MotorB,Rspeed); digitalWrite(IN4,HIGH); //左边的电机停转 digitalWrite(IN3,HIGH); analogWrite(MotorA,Lspeed); delay(e*10); } int get()//获取得到6个红外检测10次并处理 { int t,i; int l1[10],c1[10],r1[10],q1[10],w1[10]; for(i=0;i<10;i++) if(time-millis()>=2) //每2ms检测一次当前巡线状态 { l1[10]=digitalRead(IR1); //读取左边的传感器/////////////////////////////////////////////////// c1[10] =digitalRead(IR2); //读取中间的传感器 r1[10] =digitalRead(IR3); //读取右边的传感器 q1[10]=digitalRead(IR4); w1[10] =digitalRead(IR5); /* x1=digitalRead(IR6); x2=digitalRead(IR7); x3=digitalRead(IR8);*/ time=millis(); }} int fix(int t)//处理红外0~1之间情况的函数 {int l=0,c=0,r=0,q=0,w=0; int l1[10],c1[10],r1[10],q1[10],w1[10]; int sum[5]; int i; get(); for(i=0;i<10;i++) {l=l+l1[i];} for(i=0;i<10;i++) {c=c+c1[i];} for(i=0;i<10;i++) {r=r+r1[i];} for(i=0;i<10;i++) {q=q+q1[i];} for(i=0;i<10;i++) {w=w+w1[i];} sum[0]=l;sum[1]=c;sum[2]=r;sum[4]=q;sum[5]=w; for(i=0;i<5;i++) {if(sum[i]>0) sum[i]=1; else sum[i]=0;} t=sum[0]*1000+sum[1]*1000+sum[2]*100+sum[3]*10+sum[4]; return (t);} void loop() {int t; fix(t); switch(t) { case 11111: /* case 10111: case 11011: case 11101:*/ { temp1=temp1+1;//记遇到00000的次数 temp2=temp2+((temp1-5)/15);//记圈数 z=(temp1%5)*10+(temp2%2);//记次数与奇数或偶数圈的次数 stop(500); if(z==11) //当走奇数圈且第一次遇到00000状况的时候 { advance(5); stop(100); a1=c1;a2=c2; while(c1<a1+6) right1(1);//转90度 stop(5); advance(5);} else if(z==10) //当走偶数圈且第一次遇到00000状况的时候 { advance(5); stop(100); a1=c1;a2=c2; while(c2<a2+6) left1(1); stop(5); advance(5);} else if(z==21) //当走奇数圈且第2次遇到00000状况的时候 //eeRing2(130); //teeRing1(60); //ReLay(); // delay(2000); { a1=c1;a2=c2; while(c2<a2+17) left1(1); stop(5); advance(5);} else if(z==20) //当走偶数圈且第2次遇到00000状况的时候 // SteeRing2(130); // SteeRing1(60);//实际45度要写入60,实际0度要写入30 // ReLay(); // delay(2000); {a1=c1;a2=c2; while(c1<a1+17) right1(1); advance(5);} else if(z==30) //第3次遇到00000状况的时候 advance(5); else if(z==31) advance(5); else if(z==41) //当走奇数圈且第4次遇到00000状况的时候 // SteeRing2(1); // SteeRing1(70); // ReLay(); // delay(2000); {a1=c1;a2=c2; while(c2<a2+17)//180 left1(1); advance(5);} else if(z==40) //当走偶数圈且第4次遇到00000状况的时候 // SteeRing2(1); // SteeRing1(70); // ReLay(1); // delay(2000); { a1=c1;a2=c2; while(c1<a1+17)//180 right1(1); advance(5);} else if(z==1) //当走奇数圈且第5次遇到00000状况的时候 { advance(5); stop(200); a2=c2;a1=c1; while(c2<a2+6) left1(1); advance(4);} else if(z==0) //当走偶数圈且第5次遇到00000状况的时候 { advance(5); stop(200); a2=c2;a1=c1; while(c1<a1+6) right1(1); advance(4);} else advance(1); temp=t; fix(t); break; case 111: right(2); temp=t; fix(t); break; case 110: right(2); temp=t; fix(t); break; case 101: right(2); temp=t; fix(t); break; case 1: right1(2); temp=t; fix(t); break; case 10: case 11: right1(2); temp=t; fix(t); break; case 100://黑线在中间 advance(4); fix(t); break; case 11100: //黑线在you边 left(2); temp=t; fix(t); break; case 10100: left(2); temp=t; fix(t); break; case 1100: left(2); temp=t; fix(t); break; case 11000: left1(2); temp=t; fix(t); break; case 10000: left1(2); temp=t; fix(t); break; case 1000: left1(2); temp=t; fix(t); break; case 0: back(5); temp=t; fix(t); break; default: //其他情况 break; } /* if(x>1) { advance(4); delay(200); temp=t;} else temp=t; }*/}}