#include "processing.h"
extern uint8 img[CAMERA_H][CAMERA_W];//圖像數組
extern uint8 add_mid;//顯示中線標志位
extern struct PID sPID;//舵機PID結構體
extern int center_10;
extern uint8 var1, var2;//按鍵調試變量
extern uint16 var3, var4;//按鍵調試變量
extern uint32 var5, var6;//按鍵調試變量
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;//舵機保持
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]; //左丟線標志,LOSTLINE:丟線，GETLINE:捕捉到
uint8 right_lost[CAMERA_H]; //右丟線標志,LOSTLINE:丟線，GETLINE:捕捉到
uint8 leftflag; //找右線發現左線
uint8 rightflag; //找左線發現右線
uint8 left_valid; //左有效行
uint8 right_valid; //右有效行
uint8 left_invalid; //左無效行
uint8 right_invalid; //右無效行
}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 ;//下左右邊界點
//static uint8 centerline = CAMERA_W/2;
//static uint8 last_btm_mid = CAMERA_H/2;//上次圖像底部中點
static int16 steer_control;//舵機控制靜態變量，當未更新它時，它的值保持與上一次相同
static uint8 load_mid;//賽道中心
uint8 left_stop = 0, right_stop = 0, stop = 0;//停止線檢測變量
uint8 CROSS = 0, OBSTACLE = 0, CURVE = 0;//賽道元素檢測變量
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;//白點數
extern int time_start;
uint8 sleep=0; //延時變量，此處以處理一幅圖片為單位
uint8 ring_overlap = 0;//覆蓋變量，此用于覆蓋左圓環
uint16 ring_center_memory;//緩存變量，此處用于記錄舵機打角
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();//找底部三行線
// search_blckline();//找黑線
search_centerline();//從中線往兩邊找線
if(add_mid){
display_com(H_MIN);//上位機顯示
}
load_mid = calculate_mid(H_MIN);//計算中心點,采取加權方式也許會好點
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);//計算舵機輸出
return steer_control;
}
void search_centerline(void)//從中心點往兩邊找線
{
//————————————————————標志位初始化，找基礎元素值————————
// 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++)//找右底線邊線
// {
// if(img[H_MAX][W_CNT] == 255 && img[H_MAX][W_CNT+1] == 0)//找到右底線邊線
// {
// 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--)//找左底線邊線
// {
// if(img[W_MAX][W_CNT] == 0 && img[W_MAX][W_CNT-1] == 255)//找到左底線邊線
// {
// 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--)//圖像處理
{
/*————————————————————以下為找底線邊線————————————————————————*/
if(H_CNT == H_MAX)
{
for(W_CNT = W_MIN;W_CNT < W_MAX;W_CNT++)//找左邊線
{
if(img[H_CNT][W_CNT] == 0 && img[H_CNT][W_CNT+1] == 255)//找到左邊線
{
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)//找到右邊線
{
left_lost_flag=1;
left_curve = 1;
break;
}
}
if(W_CNT == W_MAX || left_lost_flag == 1)//左丟線
{
load.left[H_CNT] = W_MIN;
load.left_invalid++;
load.left_valid = 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)//找到右邊線
{
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)//找到左邊線
{
right_lost_flag = 1;
right_curve = 1;
break;
}
}
if(W_CNT == W_MIN || right_lost_flag == 1)//右丟線
{
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)//找到右邊線
{
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)//找到左邊線
{
load.left[H_MAX]=W_CNT;
break;
}
}
}
}
load.center[H_CNT]=(uint8)((load.left[H_CNT] + load.right[H_CNT])/2); //底線中點
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;
}
/*————————————————————————以上為找底線邊線——————————————————————————*/
/*————————————————————————以下為剩下部分處理——————————————————————————*/
else//此部分可繼續分化處理
{
/*————————————————————————以下為十字補線——————————————————————————————————*/
W_CNT = load.center[H_CNT+1];
if(W_CNT != W_MIN && W_CNT != W_MAX)//防止數組越界
{
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)//找到右邊線
{
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)//找到左邊線
{
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)//找到左邊線
// {
// 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); //行中點
CROSS = 0;
continue;
}
}
/*————————————————————————以上為十字補線——————————————————————————————————*/
for(W_CNT = load.center[H_CNT+1];W_CNT > W_MIN;W_CNT--)//找左邊線
{
if(img[H_CNT][W_CNT] == 255 && img[H_CNT][W_CNT-1] == 0)//找到左邊線
{
load.left[H_CNT]=W_CNT;
load.left_valid++;
load.left_invalid = 0;
break;
}
}
if(W_CNT == W_MIN)//左丟線
{
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++)//找右邊線
{
if(img[H_CNT][W_CNT] == 255 && img[H_CNT][W_CNT+1] == 0)//找到右邊線
{
load.right[H_CNT]=W_CNT;
load.right_valid++;
load.right_invalid = 0;
break;
}
}
if(W_CNT == W_MAX)//右丟線
{
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); //行中點
}
}//搜線for循環
/*———————————————————————————以下停止線檢測———————————-*/
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);//右電機
// ftm_pwm_duty(FTM0,FTM_CH1,0);
// ftm_pwm_duty(FTM0,FTM_CH4,0);//右電機
// 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);//右電機
// ftm_pwm_duty(FTM0,FTM_CH1,0);
// ftm_pwm_duty(FTM0,FTM_CH4,3000);//右電機
// ftm_pwm_duty(FTM0,FTM_CH5,0);
// systick_delay_ms(200);
// while(1)
// {
// ftm_pwm_duty(FTM0,FTM_CH0,0);//右電機
// ftm_pwm_duty(FTM0,FTM_CH1,0);
// ftm_pwm_duty(FTM0,FTM_CH4,0);//右電機
// ftm_pwm_duty(FTM0,FTM_CH5,0);
// }
}
stop_right_add = 0;
stop_left_add = 0;
}
}
/*——————————————————————————以上停止線檢測——————————*/
/*——————————————————————————以下會車區檢測——————————*/
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);
}
/*——————————————————————————以上會車區檢測————————*/
/*—————————————————————————以下為圓環檢測————————*/
if(kaiguan4 == 1) {
for(H_CNT = 59; H_CNT > 0;H_CNT--) //測量左下限 //試試if(ring_start == 0)下檢測
{
if(img[H_CNT-1][0] == 0)
{
left_btm = H_CNT -1;
break;
}
}
for(H_CNT = 59; H_CNT > 0;H_CNT--)//測量右下限
{
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) //入環結束檢測，此時清除相關入環標志位
{
clear = 1;
}
/////////////////////////////////以上為入環前準備
if(ring_start == 1 || ring_start == 2) //入環
{
if(left_btm <= right_btm) //先測左環
{
if(ring_overlap == 0 || ring_overlap == 1)//檢測左圓環,覆蓋右圓環
{
for(H_CNT = 2; H_CNT < 30; H_CNT++)
{
if(load.left[H_CNT-1] - load.left[H_CNT] >= 4)//左圓環
{
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++)//右圓環
{
if(load.right[H_CNT] - load.right[H_CNT-1] >= 4 )//檢測右圓環,覆蓋左圓環
{
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 //先測右環
{
if(ring_overlap == 0 || ring_overlap == 2)
{
for(H_CNT = 3; H_CNT < 30; H_CNT++)//右圓環
{
if(load.right[H_CNT] - load.right[H_CNT-1] >= 44 )//&& (ring_overlap == 0 || ring_overlap == 2))//檢測右圓環,覆蓋左圓環
{
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++)//左圓環
{
if(load.left[H_CNT-1] - load.left[H_CNT] >= 3 )//檢測左圓環,覆蓋右圓環
{
//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++;
}
}
// /* 出環二
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 ) //出環
{
if(ring_overlap == 1) //出左環
{
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++; //此處白色點計數
}
}
}
if(ring_overlap == 2) ////出右環
{
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++; //此處白色點計數
}
}
// 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++; //此處黑色點計數
// }
// }
// }
// black_count /=(15);
}
}
if((black_count >= 30))// || (black_count <= 10 && ring_overlap == 2)) //開始保持打角
{
ring_off = 1;
sleep = 0;
}
black_count = 0;
if(ring_off == 1)
{
if(load_center > ring_center_memory && ring_overlap == 1) //左圓環保持右打角
{
ring_off = 2;
ring_delay = 1;
ring_overlap = 0;
}
if(load_center < ring_center_memory && ring_overlap == 2) //右圓環保持左打角
{
ring_off = 2;
ring_delay = 1;
ring_overlap = 0;
}
}
// if(ring_off == 2 )
// {
// ring_overlap = 0;
// // sleep = 0;
// }
*/
ring_center_memory = load_center;
}
//記錄中心值
/*————————————————————————以上為圓環檢測——————————*/
}
/*
所有行找完后的處理
*/
/*以下停止線檢測*/
/*_____________________________________________________________________________________
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;
}
}//找左停止線
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;
}
}//找右停止線
if(left_stop == 1 && right_stop == 1 && img[50][40] == 255){
if(gpio_get(PTA17) == 0) { stop = 1;}//開第一個撥碼開關就開停止線檢測
else stop = 0;//等于0關掉停車
}//滿足條件了
}
________________________________________________________________________________________________*/
/*以上停止線檢測*/
/*以下十字檢測*/
/*__________________________________________________________________________________________________
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;
}
___________________________________________________________________________________________________*/
/*以上十字檢測*/
/*以下障礙檢測*/
/*_____________________________________________________________________________________________
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){//有突變,沒丟線
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){//很直, 底線靠中間 有黑塊
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;
}
}
}
__________________________________________________________________________________________*/
/*以上障礙檢測*/
/*__________________________________________________________________________________________
memset( (void *)load.left_lost, GETLINE, CAMERA_H );
memset( (void *)load.right_lost, GETLINE, CAMERA_H );
}
____________________________________________________________________________________________*/
/*!
* @brief 計算中心點函數
* @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) //入環標志，此時改變中值計算方式
{
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)//在上位機上顯示中線
{
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)//在液晶屏上顯示三條線
{
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);//一堆點
LCD_points(left,CAMERA_H, BLUE);//一堆點
LCD_points(right,CAMERA_H, BLUE);//一堆點
} |
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
