|
|
/*
* 走黑線
* 鏈接方法：L298N：ENA,IN1,IN2，IN3，IN4，ENB
引腳12-IN1；引腳13-IN2；引腳10-IN3；引腳11-IN4；
引腳9-ENA；引腳6-ENB；
引腳A0-線左傳感器；引腳A1-線右傳感器；引腳A2-外左傳感器；引腳A3-外右傳感器；遇到黑線
經過試驗：遇到黑線：指示燈不亮，io=1
遇到地板：指示燈亮， io=0*/
#define left_motor_go digitalWrite(10,1),digitalWrite(11,0) //左輪前進
#define left_motor_back digitalWrite(10,0),digitalWrite(11,1) //左輪后退
#define right_motor_go digitalWrite(12,0),digitalWrite(13,1) //右輪前進
#define right_motor_back digitalWrite(12,1),digitalWrite(13,0)//右輪后退
#define line_left digitalRead(A0) //線左傳感器
#define line_right digitalRead(A1) //線右傳感器
#define out_left digitalRead(A2) //外左傳感器
#define out_right digitalRead(A3) //外右傳感器
#define Stop_left digitalWrite(9,0)
#define Stop_right digitalWrite(6,0)
#define n0 (out_left==1)&&(out_right==1) // 外傳感器全部檢測到黑線
#define n1 (out_left==1)&&(out_right==0) //外左傳感器檢測到黑線
#define n2 (out_left==0)&&(out_right==1) // 外右傳感器檢測到黑線
#define n3 ((out_left==1)&&(out_right==1)&&(line_right==1)&&(line_left==1)) //傳感器全部檢測到黑
#define n4 ((out_left==0)&&(out_right==0)&&(line_right==0)&&(line_left==0)) //傳感器全部檢測到bai
//////////////////////////////////////////////////////////////////////
void zitai(boolean f1,byte s1,boolean f2,byte s2)//(左輪方向：1前進，0后退。速度：0~255；右輪方向：1前進，0后退。速度：0~255.)
{
if(f1)
left_motor_go; //左電機前進
else
left_motor_back; //左電機后退
if(f2)
right_motor_go;//右電機前進
else
right_motor_back;//右電機后退
analogWrite(9,s1);//0~255,值越大電壓越高
analogWrite(6,s2);
}
//跟著黑線走
void zhixing(){
if((line_left == 1)&& (line_right == 1)) //go strite
{
zitai(1,200,1,200);
}
else if((out_left==0)&&(out_right==0)&&(line_left == 0)&& (line_right ==1)) //turn right
{
zitai(1,190,1,190);
while((out_left==0)&&(out_right==0)&&(line_right==0)&&(line_left==0)) //line_right == 1,line_left==1,線右，線左探測到地板，一直左轉
{
zitai(1,200,0,200); //慢慢原地右轉
}
}
else if((out_left==0)&&(out_right==0)&&(line_left == 1)&& (line_right == 0)) //turn left
{
zitai(1,190,1,190);
while((out_left==0)&&(out_right==0)&&(line_right==0)&&(line_left==0)) //line_right == 1,line_left==1,線右，線左探測到地板，一直左轉
{
zitai(0,200,1,200); //慢慢原地左轉
}
}
}
void wangline(void) //跟著直線走
{
if((line_left == 1)&& (line_right == 1)) //go strite
{
zitai(1,230,1,230);
}
while((out_left==0)&&(out_right==0)&&(line_left == 0)&& (line_right ==1)) //turn right
{
zitai(1,230,0,200);
zitai(1,230,0,200);
zitai(1,220,1,220);
while((out_left==0)&&(out_right==0)&&(line_right==0)&&(line_left==0)) //line_right == 1,line_left==1,線右，線左探測到地板，一直左轉
{
zitai(1,230,0,230); //慢慢原地右轉
}
}
while((out_left==0)&&(out_right==0)&&(line_left == 1)&& (line_right == 0)) //turn left
{
zitai(0,200,1,230);
zitai(0,200,1,230);
zitai(1,220,1,220);
while((out_left==0)&&(out_right==0)&&(line_right==0)&&(line_left==0)) //line_right == 1,line_left==1,線右，線左探測到地板，一直左轉
{
zitai(0,230,1,230); //慢慢原地左轉
}
}
/*else //stop
{
zitai(0,100,0,100);
}*/
}
|
-
-
_3.zip
2017-11-1 20:30 上傳
點擊文件名下載附件
下載積分: 黑幣 -5
1.82 KB, 下載次數: 14, 下載積分: 黑幣 -5
|
