中级会员
 
- 积分
- 398
- 威望
- 213
- 贡献
- 101
- 兑换币
- 117
- 注册时间
- 2017-2-8
- 在线时间
- 42 小时
|
/***********************定义变量***************************/#define black 0
#define white 1
#define line 80
#define hang 60
#define lilun 40 //摄像头理论中心位置60
int32 left,right,mid,leftblack,rightblack;
int32 Imagesum;
int32 Imageave;
int32 error0=0,error1=0,error;
uint16 LinCout =0;
uint8 Atem8B0 = 0 ;
uint8 ALineOverCout = 0 ; //采集完成黑线行数
uint8 ALineCal = 0 ;
uint8 ADdata[DATALINE][DATACOUNT] ={0} ; //黑线AD数组存储
uint8 Buffer[hang][line]={0}; //解压后的数组
uint16 Atemp0 ;
uint8 checkflg = 0 ;
uint16 i=0,j=0;
uint8 ii ;
uint8 steer_turn;
uint16 black_zuo[line],black_you[line]; //只能看到一条线时的数组
uint8 left_flag [30]={0},right_flag[30]={0};
uint16 Line_Center[60]; //计算出来的中线值存放数组
uint16 lineable=0; //有效行
uint16 temp=0;
uint8 zuo, you;
uint8 hang1=20,line1=20;
uint8 err_cnt;
uint8 Road_mid[ hang ][line]={0,0};//赛道中心 【显示用】
int32 search_end_flg=0;
int32 controler_steer;
vuint8 right_lose=1;
vuint8 left_lose=1;
/***************image process***************************/
int8 err[ 60 ]={0};
int8 slop1;//斜率
int8 slop2;//
int8 slop_add;//
#define STEER_LEFT 166//100 左极限
#define STEER_RIGHT 130//50 右极限
#define STEER_CENTER 145 //舵机中值
vuint8 H_youwan=0,H_zuowan=0,L_youwan=0,L_zuowan=0;
/******************加速度模块变量****************/
uint16 AAngleAcce[6] ;
uint8 AangleCount ;
/****************速度变量****************/
int16 speedcout1,speedcount2 ;
extern SPEEDCOUT speed1[10],speed2[10];
extern uint8 turncount1 ,turncount2 ;
extern vuint8 LPT_INT_count ; //速度2的count
extern vuint8 DMA_count_Flg ; //速度1的count
extern vint16 DMALastValue;
float ultrlen ;
extern int32 ultrtime;
/***********************图像处理的行数***************************/
uint8 table[20]={3,6,9,12,15,18,
21,24,27,30,33,
36,39,42,45,48,
51,54,57,60};
//将图像解压扩张
void Imge_Extend() //字节扩张，1字节8位，以位显示
{
uint8 i,j;
for(i=0;i<=60;i++)
{
for(j=0;j<DATACOUNT;j++)
{
if(ADdata[i][j]&0x80) Buffer[i][j*8+0]=0;
else Buffer[i][j*8+0]=1;
if(ADdata[i][j]&0x40) Buffer[i][j*8+0]=0;
else Buffer[i][j*8+0]=1;
if(ADdata[i][j]&0x20) Buffer[i][j*8+0]=0;
else Buffer[i][j*8+0]=1;
if(ADdata[i][j]&0x10) Buffer[i][j*8+0]=0;
else Buffer[i][j*8+0]=1;
if(ADdata[i][j]&0x08) Buffer[i][j*8+0]=0;
else Buffer[i][j*8+0]=1;
if(ADdata[i][j]&0x04) Buffer[i][j*8+0]=0;
else Buffer[i][j*8+0]=1;
if(ADdata[i][j]&0x02) Buffer[i][j*8+0]=0;
else Buffer[i][j*8+0]=1;
if(ADdata[i][j]&0x01) Buffer[i][j*8+0]=0;
else Buffer[i][j*8+0]=1;
}
}
}
/*****************PWM波的配置*****************************/
void PWM_peizhi(void)
{
FTM_PWM_init(FTM1, CH1 , 100,145); //舵机占空比设置初始化 MOD =19531 ;
FTM_PWM_init(FTM0 , CH0, 80000,0); //电机占空比设置初始化 MOD =313 ; 占空比 = duty /(MOD +1 ) ;FTM_CnV_REG(FTMx[ftmn], ch) = cv;
FTM_PWM_init(FTM0 , CH1, 80000,0); //电机占空比设置初始化 MOD =313 ; 占空比 = duty /(MOD +1 ) ;FTM_CnV_REG(FTMx[ftmn], ch) = cv;
FTM_PWM_init(FTM0 , CH2, 80000,0); //电机占空比设置初始化 MOD =313 ; 占空比 = duty /(MOD +1 ) ;FTM_CnV_REG(FTMx[ftmn], ch) = cv;
FTM_PWM_init(FTM0 , CH3, 80000,0); //电机占空比设置初始化 MOD =313 ; 占空比 = duty /(MOD +1 ) ;FTM_CnV_REG(FTMx[ftmn], ch)
}
/*************舵机打中***********************/
void duoji(uint16 dj )
{ steer_turn=dj;
FTM_PWM_Duty(FTM2, CH0,steer_turn);
}
/*************电机正转***********************/
void zhengzhuan_dj(uint32 D1,uint32 D2)
{
FTM_PWM_Duty(FTM0, CH0,D1); //PWM2
FTM_PWM_Duty(FTM0, CH1,0); //PWM1
FTM_PWM_Duty(FTM0, CH2,D2); //PWM2
FTM_PWM_Duty(FTM0, CH3,0); //PWM1
}
/*************电机反转***********************/
void fanzhuan_dj(uint32 D1,uint32 D2)
{
FTM_PWM_Duty(FTM0, CH0,0); //PWM2
FTM_PWM_Duty(FTM0, CH1,D1); //PWM1
FTM_PWM_Duty(FTM0, CH2,0); //PWM2
FTM_PWM_Duty(FTM0, CH3,D2); //PWM1
}
/*************电机停止***********************/
void stop_dj()
{
FTM_PWM_Duty(FTM0, CH0,0); //PWM2
FTM_PWM_Duty(FTM0, CH1,0); //PWM1
FTM_PWM_Duty(FTM0, CH2,0); //PWM2
FTM_PWM_Duty(FTM0, CH3,0); //PWM1
}
uint16 steer_value,last_steer_value;
uint16 m=0; //换行变量
//***************************************************************************************************************
//没有偏振片 实验室里屋桌子内侧三个灯亮，外侧一个灯亮 内侧 阈值最好为191 舵机>1500中间值向左打角，<1500右打角 左偏减小 右偏增大
//****************************************************************************************************
//************* 黑线的中值滤波辅助程序***********************//
int get_mid(unsigned int a,unsigned int b,unsigned int c) //三个数排序 取最大和最小之间的中间值
{ int32 x=0;
if(a>b){x=b;b=a;a=x;}
if(b>c){x=c;c=b;b=x;}
if(a>b){x=b;b=a;a=x;}
return b ;
}
//************* 求绝对值***********************//
uint16 f_abs(int16 t)
{
if(t>0)
return t;
else
{
t=-t;
return t;
}
}
uint8 n=0;
///////////////////找黑线算法///////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
void vedge()
{
static u8 i;
static u8 left=0,right=0;
H_youwan=0;
H_zuowan=0;
L_youwan=0;
L_zuowan=0;
err_cnt=0;
Imge_Extend();
for(hang1=0;hang1<60;hang1++)
{
right=0;
left=0;
right_lose=0;
left_lose=0;
for(line1=40;line1<80;line1++) //两边向中间找 扫描范围比较宽 要求图像质量要非常好。
{
if((Buffer[hang1][line1-2]==white)&&
(Buffer[hang1][line1-1]==white)&&
Buffer[hang1][line1]==white&&
Buffer[hang1][line1+1]==black&&
Buffer[hang1][line1+2]==black )
{
right=j;
right_lose=1;
break;
}
}
for(line1=40;line1>0;line1--)
{
if((Buffer[hang1][line1+2]==black)&&
(Buffer[hang1][line1+1]==black)&&
Buffer[hang1][line1]==white&&
Buffer[hang1][line1-1]==white&&
Buffer[hang1][line1-2]==white )
{
left=j;
left_lose=1;
break;
}
}
if((left_lose==1)&&(right_lose==1)) // （左边界+右边界)/2
{
Line_Center[n]=(right+left)>>1; //((right+left)>>1); 表示除以2
}
/*右弯*/if((left_lose==1 )&&(right_lose==0)) //可能的情况是弯道一边全丢,只扫描到一边另外一边扫描到赛道外面的白点干扰
{
H_youwan++;
}
/*左弯*/if((left_lose==0 )&&(right_lose==1)) //可能的情况是弯道一边全丢,只扫描到一边另外一边扫描到赛道外面的白点干扰
{
H_zuowan++;
}
if((left_lose==0 )&&(right_lose==0))
{
Line_Center[n]=40;
err_cnt++;
}
}
////////////////////////////////////////一行图像的信息处理完毕///////////////////////////// ////////////
//分段部分
/////////////////////////////黑线的中值滤波程序////////////////// /////////////////////////////////////////
for(i=1;i<=19;i++)
{
if(f_abs(Line_Center[i]-Line_Center[i+1])>=3)
{
Line_Center[i]=(Line_Center[i-1]+Line_Center[i+1]*3)<<2;
}
Line_Center[i]=get_mid(Line_Center[i-1],Line_Center[i],Line_Center[i+1]);
Road_mid[i][ Line_Center[i] ]=black;//中心线显示用
}
}
//////////////////////////找黑线完毕///////////////////////////////////////////////////
/*
void shi()
{
uint8 i;
vedge();
for(i=0;i<=20;i++)
{
Line_Center[i]=black;
}
}
void xianshi()
{
uint8 i,j;
shi();
for(i=0;i<20;i++)
{
for(j=0;j<20;j++)
{
uart_putchar(UART3,Buffer[i][Line_Center[j]]);
}
}
}*/
//-----根据处理的中心线数据控制------------------------------------------------------
void imageProc(void)
{
unsigned int mm=0; // unsigned int sum;
/* 计算小车偏离象素点个数 */
for(mm=0;mm<hang;mm++)
{
err[mm]=lilun-Line_Center[mm]; /*40 为摄像头象素中间位置 */
}
slop1 = Line_Center[5]-Line_Center[20];//远端斜率
slop2 = Line_Center[20]-Line_Center[35];//近端斜率
slop_add = slop1+slop2;
if(H_youwan<10&&H_zuowan<10)//直道或小S弯
{
if(slop1*slop2<0) //判断赛道类型为小S
{
if (slop_add>=-3&&slop_add<=3)
{
controler_steer=err[9];
}
else if((slop_add>=-7&&slop_add<-3)||(slop_add>3&&slop_add<=7))
{
controler_steer=err[15];
}
else if((slop_add>=-10&&slop_add<-7)||(slop_add>7&&slop_add<=10))
{
controler_steer=(3*err[0]+4*err[15]+3*err[30])/10;
}
else if((slop_add>=-20&&slop_add<-10)||(slop_add>10&&slop_add<=20))
{
controler_steer=(5*err[0]+3*err[15]+2*err[30])/10;
}
}
else
{
if(slop_add>=-5&&slop_add<=5) // 直道
{
controler_steer=err[0];
}
else if ((slop_add>-8&&slop_add<-5)||(slop_add>5&&slop_add<8))
{
controler_steer=err[0];
}
else if((slop_add>=-15&&slop_add<=-8)||(slop_add>=8&&slop_add<=15))
{
controler_steer=(3*err[0]+4*err[15]+3*err[30])/10;
}
else if((slop_add>=-25&&slop_add<=-15)||(slop_add>=15&&slop_add<=25))
{
controler_steer=err[0];
}
else if((slop_add>=-40&&slop_add<=-25)||(slop_add>=25&&slop_add<=40))
{
controler_steer=(6*err[0]+4*err[30])/10;
}
}
}
else //大S
{
if(H_youwan>=10 ) //S右偏 有出赛道情况
{
controler_steer=-(H_youwan*2+L_youwan);
}
if(H_zuowan>=10 ) //S左偏
{
controler_steer=H_zuowan*2+L_zuowan;
}
if(err_cnt>=10)
{
stop_dj();
}
else
{
zhengzhuan_dj(300,300);//1500
}
// 舵机控制
if (controler_steer > 15)
{
controler_steer=15;
}
if (controler_steer < -15)
{
controler_steer=-15;
}
steer_turn=(int)(controler_steer*5+STEER_CENTER);//
if(steer_turn<=STEER_RIGHT)
{
steer_turn=STEER_RIGHT;
}
if(steer_turn>=STEER_LEFT)
{
steer_turn=STEER_LEFT;
}
duoji(steer_turn);
}
}
这是主函数上面的子函数，求高手帮忙分析下，被这个问题困扰太久了
|
|
发表于 2017-3-27 21:45:48
微信
收藏
转播
分享
顶
踩
提升卡
变色卡
抢沙发
来自 14#
楼主
发表于 2017-3-27 23:20:54
发表于 2017-3-28 07:37:09