自主避障代码.doc

intE1=5;
intM1=4;
intE2=6;
intM2=7;
intLightSensorPin=0;
intLeftDistanceSensorPin=9;
intRightDistanceSensorPin=8;
intLEDPin=13;
intspd=200;

voidstop(void){
analogWrite(E1,0);
analogWrite(E2,0);
}

voidforward(chara,charb){
analogWrite(E1,a);
digitalWrite(M1,HIGH);
analogWrite(E2,b);
digitalWrite(M2,HIGH);
}

voidbackward(chara,charb){
analogWrite(E1,a);
digitalWrite(M1,LOW);
analogWrite(E2,b);
digitalWrite(M2,LOW);
}

voidturn_L(chara,charb){
analogWrite(E1,a);
digitalWrite(M1,HIGH);
analogWrite(E2,b);
digitalWrite(M2,LOW);
}

voidturn_R(chara,charb){
analogWrite(E1,a);
digitalWrite(M1,LOW);
analogWrite(E2,b);
digitalWrite(M2,HIGH);
}

voidcheckEnv(){
intlight,R_dist,L_dist;

Serial.println("---checkEnv---");

light=analogRead(LightSensorPin);//ReadLightSensor
Serial.print("LightSensor:");
Serial.println(light,DEC);//Printresulttoserial

L_dist=digitalRead(LeftDistanceSensorPin);//ReadDistanceSensor
Serial.print("LeftDistanceSensor:");
Serial.println(L_dist,DEC);//Printresulttoserial

R_dist=digitalRead(RightDistanceSensorPin);//ReadDistanceSensor
Serial.print("RightDistanceSensor:");
Serial.println(R_dist,DEC);//Printresulttoserial

//IfLightSensorgreatthan400,lighttheLED
if(light>400)digitalWrite(LEDPin,HIGH);
elsedigitalWrite(LEDPin,LOW);

if(!L_dist&&R_dist)//turnright
move(1);
elseif(L_dist&&!R_dist)//turnleft
move(0);
elseif(!L_dist&&!R_dist)//randomturn
move(random(2));
}

intclearPath(){
intR_dist,L_dist;
Serial.println("---clearPath---");
L_dist=digitalRead(LeftDistanceSensorPin);//ReadDistanceSensor
R_dist=digitalRead(RightDistanceSensorPin);//ReadDistanceSensor
return(L_dist&&R_dist);
}

voidmove(intdir){
intflag;

Serial.println("---move---");
flag=clearPath();
while(!flag){
if(dir)//right
turn_R(spd,spd);
else
turn_L(spd,spd);
delay(50);
flag=clearPath();
}
}

voidsetup(){
pinMode(E1,OUTPUT);
pinMode(E2,OUTPUT);
pinMode(M1,OUTPUT);
pinMode(M2,OUTPUT);
pinMode(LeftDistanceSensorPin,INPUT);
pinMode(RightDistanceSensorPin,INPUT);
pinMode(LEDPin,OUTPUT);