Freescale芯片智能小车程序分析

#include <hidef.h>
#include <MC9S12XS128.h>
#pragma LINK_INFO DERIVATIVE "mc9s12xs128"
#define PITTIME 5000      //50MS定时中断
#define speed_set1  3200
#define speed_set2  2000
#define NB 150              //直道最小速度
#define NS 250             //直道中小速度
#define Z  350            //中速
#define PS 500             //中高速
#define PB 600                //高速  
#define NB1 275           //弯道最小速度
#define Z1  325             //中速
#define PS1 350             //中高速
#define PB1 375             //高速 
int speed_data[5][7] =            //直道速度表

{

	{ NB, NB, NS, Z, PS, Z, NS },

	{ NB, NS, Z, PS, PB, PB, Z },

	{ NS, Z, PS, PB, PS, Z, NS },

	{ Z, PS, PB, PS, Z, NS, NB },

	{ NS, Z, NS, Z, NS, NB, NB }

};



int speed_data0[5][7] =            //弯道速度表

{

	{ NB1, NB1, NS1, Z1, PS1, Z1, NS1 },

	{ NB1, NS1, Z1, PS1, PB1, PB1, Z1 },

	{ NS1, Z1, PS1, PB1, PS1, Z1, NS1 },

	{ Z1, PS1, PB1, PS1, Z1, NS1, NB1 },

	{ NS1, Z1, NS1, Z1, NS1, NB1, NB1 }

};





int ad_data[10][11];

int ad_data1[11];

int pd_data[11];



int ad_min[11] = { 40, 20, 20, 30, 50, 50, 50, 50, 60, 50, 50 };      //新

int ad_mid[11];

int state;

int start_flag = 1;                 //启停标志

int straight_flag = 1;              //直弯标志

int count = 0;

int B_cnt;

int B_cnt_last;

int count1 = 0;             //赛道特殊状况检测次数

int count3 = 0;             //直弯检测累计

int count4 = 0;

int state_first = 0;       //赛道状态传递

int state_last = 0;

int state_now;

int state_e = 0;

int state_e_last;

int u;

int v = 200;

int steer_e;          //舵机增量         

float Kp = 2;           //舵机K

float Kp_data[11] = { 0.6, 0.65, 0.82, 0.83, 0.84, 0.90, 0.95, 1.1, 1.35, 1.47, 1.59 };



float Kp_data2[11] = { 0 };

float Kd = 0.5;          //舵机D

float Kp_s = 0.008;              //速度K



float Ki_s = 0;                    //速度I

float Kd_s = 0;                  //速递D

int steer = 2600;

int speed;            //光电编码器测速值

int speed_e_first = 0;

int speed_e_last = 0;

int speed_e;

int speed_set;        //速度设定值                     

int motor_e;

int motor_pwm;









//初始化//



void PLL_Init(void)       //PLLClK=2*OSCCLK*(SYNR+1/REFDV+1)

{

	//锁相环=2*16=32MHz

	REFDV = 1;            //总线时钟=32/2=16MHz

	SYNR = 1;

	while (!(CRGFLG & 0x08));

	CLKSEL = 0X08;      //时钟选择，等待模式下锁相环停止工作

}



void PWM_Init(void)

{

	PWME = 0x00;

	PWMCTL = 0x70;   //级联方式

	PWMPOL = 0xff;

	PWMCAE = 0xff;   //对齐方式左对齐

	PWMCLK = 0x30;  //PWM时钟选择，23通道CLOCKSB

	PWMPRCLK = 0x11; //PWM预分频，时钟源A=时钟源B=8MHz                 

	PWMSCLA = 5;

	PWMSCLB = 5;

	PWMPER01 = 1000;

	PWMDTY01 = 0;

	PWMPER45 = 1000;

	PWMDTY45 = 200;

	PWMPER23 = 16000;

	PWMDTY23 = 0;

	PWME = 0x3f;

}



void AD_Init(void)

{

	ATD0CTL0 = 0x0A;

	ATD0CTL1 = 0x00; //7:1-外部触发,65:00-8位精度,4:放电,3210:ch

	ATD0CTL2 = 0x40; //禁止外部触发, 中断禁止

	ATD0CTL3 = 0xD8; //右对齐无符号,每次转换11个序列, No FIFO, Freeze模式下继续转

	ATD0CTL4 = 0x01; //765:采样时间为4个AD时钟周期,ATDClock=[BusClock*0.5]/[PRS+1]

	ATD0CTL5 = 0x30; //6:0特殊通道禁止,5:1连续转换 ,4:1多通道轮流采样

	ATD0DIEN = 0x00; //禁止数字输入 

}



void PIT_Init(void)

{

	PITCFLMT_PITE = 0;     //PIT关

	PITCE_PCE0 = 1;        //定时器通道0使能

	PITMTLD0 = 160 - 1;    //8位定时器初值设定。160分频，在16MHzBusClock下，为0.1MHz。即 10us.

	PITLD0 = PITTIME - 1;  //16位定时器初值设定。PITTIME*0.01MS   50ms定时

	PITINTE_PINTE0 = 1;    //定时器中断通道0中断使能

	PITCFLMT_PITE = 1;     //PIT使能

}



void PACA_Init(void)        //初始化脉冲累加器A

{

	TCTL3 = 0X40;           //下降沿捕捉脉冲

	PACTL = 0x40;           //脉冲累加使能

	PACNT = 0X0000;

}



//功能函数//



void delay_ms(int ms)      //延时函数

{

	int i, j;

	for (i = 0; i < ms; i++)

		for (j = 0; j < 2770; j++); //16MHz--2ms

}



int abs(int num)        //绝对值

{

	if (num<0)

		return -num;

	else

		return num;

}

void Reverse(int* arr, int b, int e)  //数组元素对换

{

	for (; b < e; b++, e--)

	{

		int temp = arr[e];

		arr[e] = arr[b];

		arr[b] = temp;

	}

}

void RightShift(int* arr, int N, int K)  //数组循环移位

{

	K %= N;

	Reverse(arr, 0, N - K - 1);

	Reverse(arr, N - K, N - 1);

	Reverse(arr, 0, N - 1);

}



//单片机处理/

void ATD_data(void)     //AD采集

{



	int i;



	for (i = 0; i<10; i++)

	{

		while (!ATD0STAT0_SCF);



		ad_data[i][0] = ATD0DR0L;

		ad_data[i][1] = ATD0DR1L;

		ad_data[i][2] = ATD0DR2L;

		ad_data[i][3] = ATD0DR3L;

		ad_data[i][4] = ATD0DR4L;

		ad_data[i][5] = ATD0DR5L;

		ad_data[i][6] = ATD0DR6L;

		ad_data[i][7] = ATD0DR7L;

		ad_data[i][8] = ATD0DR8L;

		ad_data[i][9] = ATD0DR9L;

		ad_data[i][10] = ATD0DR10L;

		ATD0CTL5 = 0x30;

	}

}



void PD_data(void)            //AD数据处理

{

	int i, j, k, temp;

	for (i = 0; i<10; i++)         //每个传感器采样10次的数据进行排序

	{

		for (j = 0; j<11; j++)

			for (k = j + 1; k<10; k++)

			{

				if (ad_data[j][i]>ad_data[k][i])

				{

					temp = ad_data[j][i];

					ad_data[j][i] = ad_data[k][i];

					ad_data[k][i] = temp;

				}

			}

	}

	for (j = 0; j<11; j++)                  //对中间的两个值求平均作为本次采集的结果

		ad_data1[j] = (ad_data[4][j] + ad_data[5][j]) / 2;

}



void STATE_data(void)      //位置判断

{

	int i;



	B_cnt_last = B_cnt;      //B_cnt为当前检测到黑线传感器的个数

	B_cnt = 0;

	for (i = 0; i<11; i++)       //对采集的数据进行二值化处理

	{

		ad_mid[i] = ad_min[i] + 60;

		(ad_data1[i]<ad_mid[i]) ? (pd_data[i] = 1) : (pd_data[i] = 0);

	}



	for (i = 0; i<11; i++)             //位置计算，判断采集黑线传感器的个数

	{

		if (pd_data[i] == 1)

			B_cnt++;

	}

	if (B_cnt == 1)

		for (i = 0; i<11; i++)    //判断黑线的位置

		{

			if (pd_data[i] == 1)

				state = 2 * i - 10;

		}

	if (B_cnt == 2)

		for (i = 0; i<10; i++)

		{

			if (pd_data[i] == 1 && pd_data[i + 1] == 1)

				state = 2 * i + 1 - 10;

		}

	if (B_cnt>5 && B_cnt_last<6)   //特殊状况，十字，起跑线

		count1++;



}





void START_judge(void)         //起跑线识别

{

	int i, knum, SATurn;

	int kA[4] = { 0 };

	if (count1>2)

		for (i = 0, knum = 0, SATurn = 0; i<10; i++)

			if (pd_data[i] ^ pd_data[i + 1])

			{

				SATurn++;

				kA[knum] = i;

				knum++;

			}

	if (SATurn == 4)

		if ((kA[1] - kA[0]) <= 2 && (kA[1] - kA[0]) >= 1 &&   //起跑线左白区域

			(kA[2] - kA[1]) <= 2 && (kA[2] - kA[1]) >= 1 &&      //起跑线中黑区域

			(kA[3] - kA[2]) <= 2 && (kA[3] - kA[2]) >= 1 &&      //起跑线右白区域

			(B_cnt <= 9 && B_cnt >= 7))                     //状态1传感器总数



			start_flag = 0;



}





void STATE_judge(void)     //滤波&&直弯判断

{

	state_last = state_now;

	state_now = state;

	if (abs(state_now - state_last)>2)

	{

		state_now = state_last;

	}

	if (count3<40)

		count3++;

	if (abs(state_now)<5)

		count4++;

	if (count3 == 40 && count4>35)

	{

		straight_flag = 1;

		count3 = 0;

		count4 = 0;

	}

	if (count3 == 40 && count4<30)

	{

		straight_flag = 0;

		count3 = 0;

		count4 = 0;

	}



	Kp = Kp_data[abs(state_now)];



}





void STATE_e(void)          //误差变化计算

{

	if (count == 0)

	{

		state_first = state_now;

	}

	count++;



	if (count>5)

	{

		state_e_last = state_e;

		state_e = state_now - state_first;

		count = 0;

	}

}





void STEER_pd(void)         //舵机PD控制

{

	/* if(abs(state_now)<4)     //速度对转角的影响，速度大时，直道K值取小

	Kp-=speed/speed_set1;

	if(abs(state_now)>4)       //弯道K值随速度增大

	Kp+=speed/speed_set2;    */

	//Kp=Kp_data[abs(state_now)];

	steer = (int)(2550 + Kp*state_now * 70 + Kd*(state_e - state_e_last) * 70);

	// steer+=(int)(Kp*(state_now-state_last)*80+Kd*(state_e-state_e_last)*80);





	if (steer > 3400)

	{

		steer = 3400;

	}

	if (steer < 1700)

	{

		steer = 1700;

	}



	PWMDTY23 = steer;

}



void SPEED_set(void)

{

	int i, j;

	if (state_e>4)

		i = 4;

	else if (state_e<-4)

		i = 0;

	else

	{

		i = (state_e + 4) / 2;

		j = (state_now + 10) / 3;

	}

	if (straight_flag == 1)

		speed_set = speed_data[i][j];

	if (straight_flag == 0)

		speed_set = speed_data0[i][j];

	// speed_e_first=speed_e_last;

	speed_e_last = speed_e;

	speed_e = speed_set - speed;





}







void SPEED_pd(void)

{

	if (start_flag == 0)

	{

		delay_ms(50);

		PWMDTY01 = 200;

	}

	else

	{

		if (speed_set - speed>300)

		{

			motor_pwm = 700;

		}

		else if (speed_set - speed<-300)

		{

			motor_pwm = 0;

		}

		else

		{

			motor_e = (int)(Kp_s*speed_e + Kd_s*(speed_e - speed_e_last));

			//motor_e = (int)(Kp_s*(speed_e-speed_e_last)+Ki_s*speed_e+

			// Kd_s*(speed_e-2*speed_e_last+speed_e_first));

			motor_pwm = motor_e + v;

			v = motor_pwm;

		}

		PWMDTY01 = motor_pwm;

	}



}



/* 定时中断 */

\#pragma CODE_SEG __NEAR_SEG NON_BANKED

void interrupt 66 PIT0(void)

{

	PITTF_PTF0 = 1;           //清中断标志位

	speed = PACNT;

	PACNT = 0x0000;

}









/********************************/

/*         主函数               */

/********************************/

void main(void)

{

	PLL_Init();

	PWM_Init();

	AD_Init();

	PIT_Init();

	PACA_Init();

	EnableInterrupts;

	for (;;)

	{

		ATD_data();

		PD_data();

		STATE_data();

		START_judge();

		STATE_judge();

		STATE_e();

		SPEED_set();

		STEER_pd();

		SPEED_pd();

	}

}

这份源代码的不足和错误还是比较多， for(i=0;i<10;i++) //每个传感器采样10次的数据进行排序

排序采用了冒泡排序，效率大打折扣，而且采用排序的方法处理数据稳定但并不是特别好。更改排序算法效果也不明显。

for(j=0;j<11;j++) //对中间的两个值求平均作为本次采集的结果

ad_data1[j]=(ad_data[4][j]+ad_data[5][j])/2;

效果 这句求平均值浪费一个循环，完全可以将这个求平均放在排序循环下。

换一种思路，不排序，选出接受的最大最小的数据，排除后求平均，这样少了排序，理论上能减少时间复杂度，不过效果仍不明显。

改进算法似乎不能限于程序的微调，可以建立跑道模型，分析历年的跑道无非直道，90度弯道，s弯道，小幅度弯道，回环型道。对这些跑道建立最优模型，通过接受的数据大概估算出恰当的模型，然后利用最优的方法执行。再利用定时中断接受数据看看和估算的模型差别大不大，不大就继续执行，差别大就判断其他模型，如果都没有再利用这种及时探测行驶。