Freescale芯片智能小车程序分析
Freescale芯片智能小车程序分析
July 19, 2010
#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弯道，小幅度弯道，回环型道。对这些跑道建立最优模型，通过接受的数据大概估算出恰当的模型，然后利用最优的方法执行。再利用定时中断接受数据看看和估算的模型差别大不大，不大就继续执行，差别大就判断其他模型，如果都没有再利用这种及时探测行驶。
最后更新于