|
|
void motor(char pin,char pwmpin,char state,int val)
{
pinMode(pin, OUTPUT);
if(state==1)
{
analogWrite(pwmpin,val);
digitalWrite(pin,1);
}
else if(state==2)
{
analogWrite(pwmpin,val);
digitalWrite(pin,0);
}
else if(state==0)
{
analogWrite(pwmpin,0);
digitalWrite(pin,0);
}
}
void runfoward(int i) //前進(jìn)
{
motor(4,5,1,i);
motor(7,6,1,i);
}
void runback(int j) //后退
{
motor(4,5,2,j);
motor(7,6,2,j);
}
void turnL(int m) //左轉(zhuǎn)
{
motor(4,5,1,m);
motor(7,6,0,0);
}
void turnR(int n) //右轉(zhuǎn)
{
motor(4,5,0,0);
motor(7,6,1,n);
}
void stop() //停止
{
motor(4,5,0,0);
motor(7,6,1,0);
}
void setup()
{
pinMode(10,INPUT);//定義右邊傳感器,
pinMode(11,INPUT);//定義中間傳感器,
pinMode(12,INPUT);//定義左邊傳感器,
Serial.begin(9600);
}
void loop()
{
char num1,num2,num3;
num1=digitalRead(12);
num2=digitalRead(11);
num3=digitalRead(10);
if((num1==0)&&num2&&num3)
{
turnL(120); //檢測(cè)到左邊傳感器遇到黑線，說(shuō)明小車右偏了，立即往左轉(zhuǎn)一點(diǎn)
delay(2);
while(1){
num2=digitalRead(11); //循環(huán)判斷中間傳感器度數(shù)，
if(num2==1)
{ turnL(120); //如果num2==1說(shuō)明還沒有轉(zhuǎn)到中間位置，繼續(xù)左轉(zhuǎn)
delay(2);}
else
break; //檢測(cè)到num2==0說(shuō)明轉(zhuǎn)過(guò)頭了，跳出循環(huán)，檢測(cè)三個(gè)傳感器的狀態(tài)再做出相應(yīng)動(dòng)作
} //后面道理相同
}
else if(num2&&num1&&(num3==0))
{
turnR(120);
delay(2);
while(1)
{
num2=digitalRead(11);
if(num2==1){
turnR(120);
delay(2);}
else
break;
}
}
else
{
runfoward(120);
delay(2);
}
}
|
|
