|
|
#include "processing.h"
extern uint8 img[CAMERA_H][CAMERA_W];//圖像數(shù)組
extern uint8 add_mid;//顯示中線(xiàn)標(biāo)志位
extern struct PID sPID;//舵機(jī)PID結(jié)構(gòu)體
extern int center_10;
extern uint8 var1, var2;//按鍵調(diào)試變量
extern uint16 var3, var4;//按鍵調(diào)試變量
extern uint32 var5, var6;//按鍵調(diào)試變量
extern uint8 ring_start;
extern uint8 ring_off;
extern uint32 ring_add;
extern uint32 xh;
extern int gg;
int flag = 0;
extern int kaiguan1;
extern int kaiguan2;
extern int kaiguan3;
extern int kaiguan4;
extern int kaiguan5;
uint32 clear_add=0;
uint8 clear=0;
uint8 zhi = 0;//舵機(jī)保持
uint8 zhi_start = 0;
struct LOAD
{
int16 left[CAMERA_H]; //左邊沿
int16 center[CAMERA_H]; //中心
int16 right[CAMERA_H]; //右邊沿
float width[CAMERA_H]; //寬度
uint8 left_lost[CAMERA_H]; //左丟線(xiàn)標(biāo)志,LOSTLINE:丟線(xiàn)，GETLINE:捕捉到
uint8 right_lost[CAMERA_H]; //右丟線(xiàn)標(biāo)志,LOSTLINE:丟線(xiàn)，GETLINE:捕捉到
uint8 leftflag; //找右線(xiàn)發(fā)現(xiàn)左線(xiàn)
uint8 rightflag; //找左線(xiàn)發(fā)現(xiàn)右線(xiàn)
uint8 left_valid; //左有效行
uint8 right_valid; //右有效行
uint8 left_invalid; //左無(wú)效行
uint8 right_invalid; //右無(wú)效行
}load;
static uint8 H_MIN = 0;//圖像邊界變量
static uint8 H_MAX = 59;
static uint8 W_MIN = 0;
static uint8 W_MAX = 79;
uint16 load_center;
extern uint8 ring_delay;
uint8 left_btm, right_btm ;//下左右邊界點(diǎn)
//static uint8 centerline = CAMERA_W/2;
//static uint8 last_btm_mid = CAMERA_H/2;//上次圖像底部中點(diǎn)
static int16 steer_control;//舵機(jī)控制靜態(tài)變量，當(dāng)未更新它時(shí)，它的值保持與上一次相同
static uint8 load_mid;//賽道中心
uint8 left_stop = 0, right_stop = 0, stop = 0;//停止線(xiàn)檢測(cè)變量
uint8 CROSS = 0, OBSTACLE = 0, CURVE = 0;//賽道元素檢測(cè)變量
uint8 H_CNT;
uint8 W_CNT;
uint8 H_CNT_MIN;
uint8 H_CNT_MAX;
uint8 MINCOUNT;
uint8 left_lost_flag;
uint8 right_lost_flag;
uint8 left_curve;
uint8 right_curve;
uint8 stop_flag = 0;
uint8 ring_flag = 0;
uint8 stop_right_add;//add-->累加型變量
uint8 stop_left_add;
uint8 hc_add;
uint8 hc_get_add;
extern int hc_flag;
int black_count=0;//白點(diǎn)數(shù)
extern int time_start;
uint8 sleep=0; //延時(shí)變量，此處以處理一幅圖片為單位
uint8 ring_overlap = 0;//覆蓋變量，此用于覆蓋左圓環(huán)
uint16 ring_center_memory;//緩存變量，此處用于記錄舵機(jī)打角
void delay()
{
uint32 x;
uint32 y;
for(y =500; y > 1; y--)
{
for(x=10000 ;x > 1; x--);
}
}
int8 img_handle(void)
{
load.left_valid = 0;
load.right_valid = 0;
load.left_invalid = 0;
load.right_invalid = 0;
left_lost_flag = 0;
right_lost_flag = 0;
CROSS = 0;
left_curve = 0;
right_curve = 0;
stop_right_add = 0;
stop_left_add = 0;
hc_add=0;
// search_btmline();//找底部三行線(xiàn)
// search_blckline();//找黑線(xiàn)
search_centerline();//從中線(xiàn)往兩邊找線(xiàn)
if(add_mid){
display_com(H_MIN);//上位機(jī)顯示
}
load_mid = calculate_mid(H_MIN);//計(jì)算中心點(diǎn),采取加權(quán)方式也許會(huì)好點(diǎn)
if(load_mid > 0 && load_mid <= 23)
{
sPID.Kp = 28;
sPID.Kd = 18;
}
else if(load_mid > 59 && load_mid <= 79)
{
sPID.Kp = 28;
sPID.Kd = 18;
}
else if(load_mid > 23 && load_mid <= 27){
sPID.Kp = var1;
sPID.Kd = var2;
}
else if(load_mid > 48 && load_mid <= 59){
sPID.Kp = var1;
sPID.Kd = var2;
}
else if(load_mid > 27 && load_mid <= 48){
sPID.Kp = var3;
sPID.Kd = var4;
}
else {
sPID.Kp = var5;
sPID.Kd = var6;
}
steer_control = Steer_PIDCalc(load_mid);//計(jì)算舵機(jī)輸出
return steer_control;
}
void search_centerline(void)//從中心點(diǎn)往兩邊找線(xiàn)
{
//————————————————————標(biāo)志位初始化，找基礎(chǔ)元素值————————
// load.leftflag = 0;
// load.rightflag = 0;
// left_btm_flag = 0;
// right_btm_flag = 0;
//
// for(W_CNT = W_MIN;W_CNT < W_MAX;W_CNT++)//找右底線(xiàn)邊線(xiàn)
// {
// if(img[H_MAX][W_CNT] == 255 && img[H_MAX][W_CNT+1] == 0)//找到右底線(xiàn)邊線(xiàn)
// {
// right_btm_flag = 1;
// H_CNT_MAX = H_MAX;
// load.right[W_MAX]=W_CNT+1;
// }
// }
//
// for(W_CNT = W_MAX;W_CNT > W_MIN;W_CNT--)//找左底線(xiàn)邊線(xiàn)
// {
// if(img[W_MAX][W_CNT] == 0 && img[W_MAX][W_CNT-1] == 255)//找到左底線(xiàn)邊線(xiàn)
// {
// left_btm_flag = 1;
// H_CNT_MAX = W_MAX;
// load.left[W_MAX]=W_CNT;
// }
// }
//
// if(left_btm_flag == 0 && right_btm_flag == 0 && right_btm_flag == 0)
// {
for(H_CNT_MAX = H_MAX;H_CNT_MAX > H_MIN;H_CNT_MAX--)//找底限
{
if(img[H_CNT_MAX][W_MIN] == 0)break;
if(img[H_CNT_MAX][W_MAX] == 0) break;
}
// }
//
for(H_CNT_MIN = H_MAX;H_CNT_MIN >H_MIN;H_CNT_MIN--)//找上限
{
MINCOUNT = 0;
for(W_CNT = W_MIN;W_CNT < W_MAX;W_CNT++)
{
if(img[H_CNT_MIN][W_CNT] == 0) MINCOUNT++;
}
if(MINCOUNT >= 70) break;
if(H_CNT_MIN == H_CNT_MIN+1) H_CNT_MIN = 0;
}
for(H_CNT = H_MAX;H_CNT > H_CNT_MIN;H_CNT--)//圖像處理
{
/*————————————————————以下為找底線(xiàn)邊線(xiàn)————————————————————————*/
if(H_CNT == H_MAX)
{
for(W_CNT = W_MIN;W_CNT < W_MAX;W_CNT++)//找左邊線(xiàn)
{
if(img[H_CNT][W_CNT] == 0 && img[H_CNT][W_CNT+1] == 255)//找到左邊線(xiàn)
{
load.left[H_CNT] = W_CNT;
load.left_valid++;
load.left_invalid = 0;
right_curve = 1;
break;
}
if(img[H_CNT][W_CNT] == 255 && img[H_CNT][W_CNT+1] == 0)//找到右邊線(xiàn)
{
left_lost_flag=1;
left_curve = 1;
break;
}
}
if(W_CNT == W_MAX || left_lost_flag == 1)//左丟線(xiàn)
{
load.left[H_CNT] = W_MIN;
load.left_invalid++;
load.left_valid = 0;
}
for(W_CNT = W_MAX;W_CNT > W_MIN;W_CNT--)//找右邊線(xiàn)
{
if(img[H_CNT][W_CNT] == 0 && img[H_CNT][W_CNT-1] == 255)//找到右邊線(xiàn)
{
load.right[H_CNT]=W_CNT;
load.right_valid++;
load.right_invalid = 0;
left_curve = 1;
break;
}
if(img[H_CNT][W_CNT] == 255 && img[H_CNT][W_CNT-1] == 0)//找到左邊線(xiàn)
{
right_lost_flag = 1;
right_curve = 1;
break;
}
}
if(W_CNT == W_MIN || right_lost_flag == 1)//右丟線(xiàn)
{
load.right[H_CNT] = W_MAX;
load.right_invalid++;
load.right_valid = 0;
}
if(left_lost_flag == 1 && right_lost_flag ==1)//大彎處理
{
for(H_CNT = 0;H_CNT < H_MAX;H_CNT++)
{
if(img[H_CNT][W_MIN] == 0)
{
left_lost_flag = 0;
break;
}
if(img[H_CNT][W_MAX] == 0)
{
right_lost_flag = 0;
break;
}
}
if(left_lost_flag == 0)
{
load.left[H_MAX]=0;
for(W_CNT = W_MIN;W_CNT < W_MAX;W_CNT++)
{
if(img[H_CNT][W_CNT] == 255 && img[H_CNT][W_CNT+1] == 0)//找到右邊線(xiàn)
{
load.right[H_MAX]=W_CNT;
break;
}
}
}
if(right_lost_flag == 0)
{
load.right[H_MAX]=0;
for(W_CNT = W_MAX;W_CNT > W_MIN;W_CNT--)
{
if(img[H_CNT][W_CNT] == 0 && img[H_CNT][W_CNT-1] == 255)//找到左邊線(xiàn)
{
load.left[H_MAX]=W_CNT;
break;
}
}
}
}
load.center[H_CNT]=(uint8)((load.left[H_CNT] + load.right[H_CNT])/2); //底線(xiàn)中點(diǎn)
if(left_curve) //左彎道濾反光
{
for(H_CNT = 59;H_CNT < H_CNT_MIN;H_CNT--)
{
if(img[H_CNT][W_MIN] == 0)
{
H_CNT_MIN = H_CNT;
break;
}
if(H_CNT == H_CNT_MIN)
H_CNT_MIN = (uint8)((H_MAX+H_MIN)/2);
}
}
if(right_curve) //右彎道濾反光
{
for(H_CNT = 59;H_CNT < H_CNT_MIN;H_CNT--)
{
if(img[H_CNT][W_MAX] == 0)
{
H_CNT_MIN = H_CNT;
break;
}
if(H_CNT == H_CNT_MIN)
H_CNT_MIN = (uint8)((H_MAX+H_MIN)/2);
}
}
H_CNT=H_MAX;
}
/*————————————————————————以上為找底線(xiàn)邊線(xiàn)——————————————————————————*/
/*————————————————————————以下為剩下部分處理——————————————————————————*/
else//此部分可繼續(xù)分化處理
{
/*————————————————————————以下為十字補(bǔ)線(xiàn)——————————————————————————————————*/
W_CNT = load.center[H_CNT+1];
if(W_CNT != W_MIN && W_CNT != W_MAX)//防止數(shù)組越界
{
if(img[H_CNT][W_CNT] == 0 && img[H_CNT][W_CNT+1] == 0 && img[H_CNT][W_CNT-1] == 0)
{
for(W_CNT = 1;W_CNT < W_MAX;W_CNT++)
{
if(img[H_CNT][W_CNT] == 255 && img[H_CNT][W_CNT+1] == 0)//找到右邊線(xiàn)
{
load.right[H_CNT]=W_CNT;
load.left[H_CNT]=W_MIN;
CROSS = 1;
break;
}
if(img[H_CNT][W_CNT-1] == 255 && img[H_CNT][W_CNT] == 0)//找到左邊線(xiàn)
{
load.right[H_CNT]=W_MAX;
load.left[H_CNT]=W_CNT;
CROSS = 1;
break;
}
}
// if(W_CNT == W_MAX)
// {
// for(W_CNT = W_MAX;W_CNT > W_MIN;W_CNT--)
// {
// if(img[H_CNT][W_CNT-1] == 255 && img[H_CNT][W_CNT] == 0)//找到左邊線(xiàn)
// {
// load.right[H_CNT]=W_MAX;
// load.left[H_CNT]=W_CNT;
// break;
// }
// }
// }
}
if(CROSS == 1)
{
load.center[H_CNT]=(uint8)((load.left[H_CNT] + load.right[H_CNT])/2); //行中點(diǎn)
CROSS = 0;
continue;
}
}
/*————————————————————————以上為十字補(bǔ)線(xiàn)——————————————————————————————————*/
for(W_CNT = load.center[H_CNT+1];W_CNT > W_MIN;W_CNT--)//找左邊線(xiàn)
{
if(img[H_CNT][W_CNT] == 255 && img[H_CNT][W_CNT-1] == 0)//找到左邊線(xiàn)
{
load.left[H_CNT]=W_CNT;
load.left_valid++;
load.left_invalid = 0;
break;
}
}
if(W_CNT == W_MIN)//左丟線(xiàn)
{
load.left_invalid++;
load.left_valid = 0;
load.left[H_CNT]=W_MIN;
}
for(W_CNT = load.center[H_CNT+1];W_CNT < W_MAX;W_CNT++)//找右邊線(xiàn)
{
if(img[H_CNT][W_CNT] == 255 && img[H_CNT][W_CNT+1] == 0)//找到右邊線(xiàn)
{
load.right[H_CNT]=W_CNT;
load.right_valid++;
load.right_invalid = 0;
break;
}
}
if(W_CNT == W_MAX)//右丟線(xiàn)
{
load.right_invalid++;
load.right_valid = 0;
load.right[H_CNT]=W_MAX;
}
// if(load.left_invalid != 0 && load.right_invalid == 0)
// load.center[H_CNT]= load.left[H_CNT] + 20;
// else if(load.right_invalid != 0 && load.left_invalid == 0)
// load.center[H_CNT]= load.right[H_CNT] - 20;
// else
load.center[H_CNT]=(uint8)((load.left[H_CNT] + load.right[H_CNT])/2); //行中點(diǎn)
}
}//搜線(xiàn)for循環(huán)
/*———————————————————————————以下停止線(xiàn)檢測(cè)———————————-*/
if(xh>0xf4 && kaiguan5 == 1)//if(xh>0x44)
{
for(H_CNT = 25; H_CNT > H_MIN + 1; H_CNT--)
{
for(W_CNT = 0; W_CNT <W_MAX-1;)
{
W_CNT++;
if(img[H_CNT][W_CNT] == 255 && img[H_CNT][W_CNT+1] == 0)
stop_left_add++;
}
for(W_CNT = W_MAX; W_CNT >W_MIN+1;)
{
W_CNT--;
if(img[H_CNT][W_CNT] == 255 && img[H_CNT][W_CNT-1] == 0)
stop_right_add++;
}
if(stop_right_add >= 6 && stop_left_add >= 6)
{
//stop_flag = 1;
H_CNT_MIN = H_CNT;
flag = 1;
// break;
// ftm_pwm_duty(FTM0,FTM_CH0,0);//右電機(jī)
// ftm_pwm_duty(FTM0,FTM_CH1,0);
// ftm_pwm_duty(FTM0,FTM_CH4,0);//右電機(jī)
// ftm_pwm_duty(FTM0,FTM_CH5,0);
// ftm_pwm_duty(FTM3, FTM_CH0, INIT_STEER_DUTY - 55);
// systick_delay_ms(250);
// ftm_pwm_duty(FTM3, FTM_CH0, INIT_STEER_DUTY + 60);
// systick_delay_ms(400);
// ftm_pwm_duty(FTM3, FTM_CH0, INIT_STEER_DUTY );
// //Dsystick_delay_ms(200);
// ftm_pwm_duty(FTM0,FTM_CH0,3000);//右電機(jī)
// ftm_pwm_duty(FTM0,FTM_CH1,0);
// ftm_pwm_duty(FTM0,FTM_CH4,3000);//右電機(jī)
// ftm_pwm_duty(FTM0,FTM_CH5,0);
// systick_delay_ms(200);
// while(1)
// {
// ftm_pwm_duty(FTM0,FTM_CH0,0);//右電機(jī)
// ftm_pwm_duty(FTM0,FTM_CH1,0);
// ftm_pwm_duty(FTM0,FTM_CH4,0);//右電機(jī)
// ftm_pwm_duty(FTM0,FTM_CH5,0);
// }
}
stop_right_add = 0;
stop_left_add = 0;
}
}
/*——————————————————————————以上停止線(xiàn)檢測(cè)——————————*/
/*——————————————————————————以下會(huì)車(chē)區(qū)檢測(cè)——————————*/
if(hc_flag == 0 && kaiguan1 == 1 && xh >0xe0)
{
if(hc_add >=3)
{
hc_add = 0;
hc_get_add = 0;
}
/* else if(img[0][30] == 255 && (img[0][0] ==0 && img[0][79] == 0))
{
hc_add++;
for(H_CNT = H_CNT_MAX;H_CNT > H_CNT_MIN+2;H_CNT--)
{
if((load.right[H_CNT] - load.right[H_CNT-1]) >= 3)
{
H_CNT += 3;
for(H_CNT=H_CNT+1;H_CNT > H_CNT_MIN+1;H_CNT--)
{
if(load.right[H_CNT-1] - load.right[H_CNT] >=3)
{
hc_get_add++;
break;
}
}
break;
}
}
}
*/
else if(img[0][30] == 255 && (img[0][0] ==0 && img[0][79] == 0))
{
hc_add++;
for(H_CNT = H_CNT_MAX;H_CNT > 2;H_CNT--)
{
if((load.right[H_CNT] - load.right[H_CNT-1]) >= 2)
{
H_CNT += 1;
for(H_CNT=H_CNT+1;H_CNT > H_CNT_MIN+1;H_CNT--)
{
if(load.right[H_CNT-1] - load.right[H_CNT] >=1)
{
hc_get_add++;
break;
}
}
break;
}
}
for(H_CNT = H_CNT_MAX;H_CNT > 2;H_CNT--)
{
if((load.left[H_CNT] - load.left[H_CNT-1]) >=2)
{
H_CNT += 1;
for(H_CNT=H_CNT+1;H_CNT > H_CNT_MIN+1;H_CNT--)
{
if(load.left[H_CNT-1] - load.left[H_CNT] >=1)
{
hc_get_add++;
break;
}
}
break;
}
}
}
}
if(hc_flag == 0 && kaiguan2 == 1)
{
if(hc_add >=3)
{
hc_add = 0;
hc_get_add = 0;
}
else if(img[0][30] == 255 && (img[0][0] ==0 && img[0][79] == 0))
{
hc_add++;
for(H_CNT = H_CNT_MAX;H_CNT > H_CNT_MIN+2;H_CNT--)
{
if((load.right[H_CNT] - load.right[H_CNT-1]) >= 3)
{
H_CNT += 3;
for(H_CNT=H_CNT+1;H_CNT > H_CNT_MIN+1;H_CNT--)
{
if(load.right[H_CNT-1] - load.right[H_CNT] >=3)
{
hc_get_add++;
break;
}
}
break;
}
}
for(H_CNT = H_CNT_MAX;H_CNT > H_CNT_MIN+2;H_CNT--)
{
if((load.left[H_CNT] - load.left[H_CNT-1]) >= 3)
{
H_CNT += 3;
for(H_CNT=H_CNT+1;H_CNT > H_CNT_MIN+1;H_CNT--)
{
if(load.left[H_CNT-1] - load.left[H_CNT] >=3)
{
hc_get_add++;
break;
}
}
break;
}
}
}
// else if(img[0][30] == 255 && (img[0][0] ==0 && img[0][79] == 0))
// {
// hc_add++;
//
// }
}
if(hc_get_add >= 3)
{
// gpio_set(PTC4,0);
// delay();
// gpio_set(PTC8,0);
hc_get_add=0;
hc_flag = 2;
gpio_set (PTC4, 0);
gg=1;
// ftm_pwm_duty(FTM0,FTM_CH0,0);
// ftm_pwm_duty(FTM0,FTM_CH1,0);
// ftm_pwm_duty(FTM0,FTM_CH5,0);
// ftm_pwm_duty(FTM0,FTM_CH4,0);
//
// while(1);
//
//// systick_delay_ms(200);
// for(y=20;y>0;)
// {
// y--;
// for(x=0xfffff;x > 0;x--);
// }
// ftm_pwm_duty(FTM0,FTM_CH0,0);
// ftm_pwm_duty(FTM0,FTM_CH1,0);
// ftm_pwm_duty(FTM0,FTM_CH5,0);
// ftm_pwm_duty(FTM0,FTM_CH4,0);
//
// while(1);
}
/*——————————————————————————以上會(huì)車(chē)區(qū)檢測(cè)————————*/
/*—————————————————————————以下為圓環(huán)檢測(cè)————————*/
if(kaiguan4 == 1) {
for(H_CNT = 59; H_CNT > 0;H_CNT--) //測(cè)量左下限 //試試if(ring_start == 0)下檢測(cè)
{
if(img[H_CNT-1][0] == 0)
{
left_btm = H_CNT -1;
break;
}
}
for(H_CNT = 59; H_CNT > 0;H_CNT--)//測(cè)量右下限
{
if(img[H_CNT-1][79] == 0)
{
right_btm = H_CNT -1;
break;
}
}
clear = 0;
clear_add = 0;
for(H_CNT = 20;H_CNT > 0;H_CNT--)
{
for(W_CNT = 0;W_CNT < 79; W_CNT++)
{
if(img[H_CNT][W_CNT] == 0)
{
clear_add++;
}
}
}
if(clear_add >= 800) //入環(huán)結(jié)束檢測(cè)，此時(shí)清除相關(guān)入環(huán)標(biāo)志位
{
clear = 1;
}
/////////////////////////////////以上為入環(huán)前準(zhǔn)備
if(ring_start == 1 || ring_start == 2) //入環(huán)
{
if(left_btm <= right_btm) //先測(cè)左環(huán)
{
if(ring_overlap == 0 || ring_overlap == 1)//檢測(cè)左圓環(huán),覆蓋右圓環(huán)
{
for(H_CNT = 2; H_CNT < 30; H_CNT++)
{
if(load.left[H_CNT-1] - load.left[H_CNT] >= 4)//左圓環(huán)
{
ring_start = 2;
ring_overlap = 1;
if(H_CNT < 8)
{
load_center = 38 - H_CNT;
break;
}
else
{
load_center = 38 - H_CNT;
break;
}
}
}
}
if(ring_overlap == 0 || ring_overlap == 2)
{
for(H_CNT = 3; H_CNT < 30; H_CNT++)//右圓環(huán)
{
if(load.right[H_CNT] - load.right[H_CNT-1] >= 4 )//檢測(cè)右圓環(huán),覆蓋左圓環(huán)
{
ring_start = 2;
ring_overlap = 2;
//if(load.left[H_CNT] - load.left[H_CNT+1] >= 0 && load.left[H_CNT] - load.left[H_CNT+1] <= 2 && load.left[H_CNT-2] - load.left[H_CNT-1] >= 0 && load.left[H_CNT-2] - load.left[H_CNT-1] <= 2)
if(1)
{
if(H_CNT <= 7)
{
load_center = 35 +H_CNT;
break;
}
else
{
load_center = 35 + H_CNT;
break;
}
}
}
}
}
}
else //先測(cè)右環(huán)
{
if(ring_overlap == 0 || ring_overlap == 2)
{
for(H_CNT = 3; H_CNT < 30; H_CNT++)//右圓環(huán)
{
if(load.right[H_CNT] - load.right[H_CNT-1] >= 44 )//&& (ring_overlap == 0 || ring_overlap == 2))//檢測(cè)右圓環(huán),覆蓋左圓環(huán)
{
ring_start = 2;
ring_overlap = 2;
//if(load.left[H_CNT] - load.left[H_CNT+1] >= 0 && load.left[H_CNT] - load.left[H_CNT+1] <= 2 && load.left[H_CNT-2] - load.left[H_CNT-1] >= 0 && load.left[H_CNT-2] - load.left[H_CNT-1] <= 2)
if(1)
{
if(H_CNT <= 7)
{
load_center = 35 +H_CNT;
break;
}
else
{
load_center = 35 + H_CNT;
break;
}
}
}
}
}
if(ring_overlap == 0 || ring_overlap == 1)
{
for(H_CNT = 2; H_CNT < 30; H_CNT++)//左圓環(huán)
{
if(load.left[H_CNT-1] - load.left[H_CNT] >= 3 )//檢測(cè)左圓環(huán),覆蓋右圓環(huán)
{
//if(load.left[H_CNT] - load.left[H_CNT+1] >= 0 && load.left[H_CNT] - load.left[H_CNT+1] <= 2 && load.left[H_CNT-2] - load.left[H_CNT-1] >= 0 && load.left[H_CNT-2] - load.left[H_CNT-1] <= 2)
if(1)
{
ring_start = 2;
ring_overlap = 1;
if(H_CNT < 8)
{
load_center = 38 - H_CNT;
break;
}
else
{
load_center = 38 - H_CNT;
break;
}
}
}
}
}
}
if(H_CNT == 30 && ring_start == 2 )//&& clear ==1)
{
//time_start = 1;
if(sleep == 2)
{
ring_start = 3;
time_start = 1;
}
sleep++;
}
}
// /* 出環(huán)二
if(ring_overlap == 1 && ring_start == 3 )//&& time_start == 0)
for(W_CNT = 79 ;W_CNT > 5; W_CNT --)
{
black_count = 0;
for(H_CNT = 59 ;H_CNT > 0;H_CNT--)
{
if(img[H_CNT][W_CNT] == 0)
{
black_count++;
}
}
if(black_count <= 3)
{
for(H_CNT = 59 ;H_CNT > 0;H_CNT--)
{
if(img[H_CNT][W_CNT] == 0)
break;
}
if(H_CNT > 5 && H_CNT <= 20)
{
if((load.center[H_CNT-5] - load.center[H_CNT]) > 8 && (load.center[H_CNT+5] - load.center[H_CNT]) >8)
{
ring_off = 2;
}
}
else if(H_CNT > 20 && H_CNT <= 40)
{
if((load.center[H_CNT-10] - load.center[H_CNT]) > 15 && (load.center[H_CNT+10] - load.center[H_CNT]) >15)
{
ring_off = 2;
load_center -= 5;
}
}
}
}
// */
/* if(sleep == 3 && time_start == 0 && ring_off != 2 ) //出環(huán)
{
if(ring_overlap == 1) //出左環(huán)
{
for(W_CNT = 0 ;W_CNT < 79; W_CNT++)
{
if(img[0][W_CNT] == 255 && img[5][W_CNT] == 255 )//&& img[9][W_CNT] == 255 )//|| img[3][W_CNT] == 255 || img[4][W_CNT] == 255 || img[5][W_CNT] == 255 || img[6][W_CNT] == 255 || img[7][W_CNT] == 255)
{
black_count++; //此處白色點(diǎn)計(jì)數(shù)
}
}
}
if(ring_overlap == 2) ////出右環(huán)
{
for(W_CNT = 0 ;W_CNT < 79; W_CNT++)
{
if(img[0][W_CNT] == 255 )// && img[5][W_CNT] == 255 )//&& img[9][W_CNT] == 255 )//|| img[3][W_CNT] == 255 || img[4][W_CNT] == 255 || img[5][W_CNT] == 255 || img[6][W_CNT] == 255 || img[7][W_CNT] == 255)
{
black_count++; //此處白色點(diǎn)計(jì)數(shù)
}
}
// for(W_CNT = 40 ;W_CNT < 55; W_CNT++)
// {
// for(H_CNT = 59 ;H_CNT > 0 ;H_CNT --)
// {
// if(img[H_CNT][W_CNT] == 0)// || img[2][W_CNT] == 255 || img[3][W_CNT] == 255 || img[4][W_CNT] == 255 || img[5][W_CNT] == 255 || img[6][W_CNT] == 255 || img[7][W_CNT] == 255)
// {
// black_count++; //此處黑色點(diǎn)計(jì)數(shù)
// }
// }
// }
// black_count /=(15);
}
}
if((black_count >= 30))// || (black_count <= 10 && ring_overlap == 2)) //開(kāi)始保持打角
{
ring_off = 1;
sleep = 0;
}
black_count = 0;
if(ring_off == 1)
{
if(load_center > ring_center_memory && ring_overlap == 1) //左圓環(huán)保持右打角
{
ring_off = 2;
ring_delay = 1;
ring_overlap = 0;
}
if(load_center < ring_center_memory && ring_overlap == 2) //右圓環(huán)保持左打角
{
ring_off = 2;
ring_delay = 1;
ring_overlap = 0;
}
}
// if(ring_off == 2 )
// {
// ring_overlap = 0;
// // sleep = 0;
// }
*/
ring_center_memory = load_center;
}
//記錄中心值
/*————————————————————————以上為圓環(huán)檢測(cè)——————————*/
}
/*
所有行找完后的處理
*/
/*以下停止線(xiàn)檢測(cè)*/
/*_____________________________________________________________________________________
for(H_CNT = 55; H_CNT > 45; H_CNT--){
for(W_CNT = 40 ; W_CNT > 25; W_CNT--){
if((img[H_CNT][W_CNT+1] == 255) && (img[H_CNT][W_CNT] == 0)){
left_stop = 1;
break;
}
}//找左停止線(xiàn)
for(W_CNT = 40 ; W_CNT < 55; W_CNT++){
if((img[H_CNT][W_CNT - 1] == 255) && (img[H_CNT][W_CNT] == 0)){
right_stop = 1;
break;
}
}//找右停止線(xiàn)
if(left_stop == 1 && right_stop == 1 && img[50][40] == 255){
if(gpio_get(PTA17) == 0) { stop = 1;}//開(kāi)第一個(gè)撥碼開(kāi)關(guān)就開(kāi)停止線(xiàn)檢測(cè)
else stop = 0;//等于0關(guān)掉停車(chē)
}//滿(mǎn)足條件了
}
________________________________________________________________________________________________*/
/*以上停止線(xiàn)檢測(cè)*/
/*以下十字檢測(cè)*/
/*__________________________________________________________________________________________________
if(load.left_lost[48] == LOSTLINE && load.left_lost[50] == LOSTLINE && load.left_lost[52] == LOSTLINE &&
load.right_lost[48] == LOSTLINE && load.right_lost[50] == LOSTLINE && load.right_lost[52] == LOSTLINE &&
img[48][40] == 255 && img[50][40] == 255 && img[52][40] == 255){
CROSS = 1;
}
___________________________________________________________________________________________________*/
/*以上十字檢測(cè)*/
/*以下障礙檢測(cè)*/
/*_____________________________________________________________________________________________
for(H_CNT = 35; H_CNT > 20; H_CNT--)
{
if(gpio_get(PTA15) == 1 && (img[H_CNT - 1][55] == 0 || img[H_CNT - 1][25] == 0) && (abs(load.center[H_CNT+3] - load.center[H_CNT]) > 8) && load.left_lost[H_CNT+1] == GETLINE && load.right_lost[H_CNT+1] == GETLINE){//有突變,沒(méi)丟線(xiàn)
if(abs(load.center[H_CNT+3] - load.center[H_CNT+2]) < 5 && abs(load.center[H_CNT-2] - load.center[H_CNT-1] < 5) &&
abs(load.center[H_CNT+3] - CAMERA_W/2) < 10){//很直, 底線(xiàn)靠中間 有黑塊
OBSTACLE = 1;
if(img[H_CNT - 1][55] == 0 && img[H_CNT - 1][25] == 255){//障礙在右邊
ftm_pwm_duty(FTM2, FTM_CH0, 450 - 55);
}
else if(img[H_CNT - 1][25] == 0 && img[H_CNT - 1][55] == 255){//障礙在左邊
ftm_pwm_duty(FTM2, FTM_CH0, 450 + 55);
}
break;
}
}
}
__________________________________________________________________________________________*/
/*以上障礙檢測(cè)*/
/*__________________________________________________________________________________________
memset( (void *)load.left_lost, GETLINE, CAMERA_H );
memset( (void *)load.right_lost, GETLINE, CAMERA_H );
}
____________________________________________________________________________________________*/
/*!
* @brief 計(jì)算中心點(diǎn)函數(shù)
* @since v1.0
* @note CSUST
*/
uint8 calculate_mid(uint8 end)
{
uint8 count=0;
int x;
for(H_CNT = 59; H_CNT>0;H_CNT--)
{
x=0;
for(W_CNT = 0;W_CNT<79;W_CNT ++)
{
if(img[H_CNT][W_CNT] == 0 && img[H_CNT][W_CNT+1]==255)
{
x++;
}
}
if(x>=3)
load.center[H_CNT] =39;
}
if(ring_start != 2) //入環(huán)標(biāo)志，此時(shí)改變中值計(jì)算方式
{
for( H_CNT = H_MAX;H_CNT > H_CNT_MIN;)
{
H_CNT--;
load_center += load.center[H_CNT];
count++;
}
load_center = load_center / (count);
}
if(ring_off == 2)
load_center = ring_center_memory;
return (uint8)load_center;
}
void display_com(uint8 end)//在上位機(jī)上顯示中線(xiàn)
{
uint16 H_CNT;
for(H_CNT = H_MAX; H_CNT > H_MIN;){
H_CNT--;
img[H_CNT][load.center[H_CNT]] = 0;
}
// for(H_CNT = CAMERA_H;H_CNT>0;)
// {
// H_CNT--;
// for(W_CNT = CAMERA_W;W_CNT>0;)
// {
// W_CNT--;
// img[H_CNT][W_CNT] = 255;
// }
// }
// for(H_CNT = CAMERA_H;H_CNT>0;)
// {
// H_CNT--;
// img[H_CNT][load.left[H_CNT]] =0;
// img[H_CNT][load.right[H_CNT]] = 0;
// img[H_CNT][load.center[H_CNT]] = 0;
// }
}
void display_lcd(void)//在液晶屏上顯示三條線(xiàn)
{
uint8 H_CNT;
Site_t center[CAMERA_H];
Site_t left[CAMERA_H];
Site_t right[CAMERA_H];
for(H_CNT = CAMERA_H; H_CNT>0;)
{
H_CNT--;
center[H_CNT].y = H_CNT;
center[H_CNT].x = load.center[H_CNT];
left[H_CNT].y = H_CNT;
left[H_CNT].x = load.left[H_CNT];
right[H_CNT].y = H_CNT;
right[H_CNT].x = load.right[H_CNT];
}
LCD_points(center,CAMERA_H, GREEN);//一堆點(diǎn)
LCD_points(left,CAMERA_H, BLUE);//一堆點(diǎn)
LCD_points(right,CAMERA_H, BLUE);//一堆點(diǎn)
} |
評(píng)分
-
查看全部評(píng)分
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
