|
四、主程序框架
其实一开始是想使用rt-thread操作系统的，不过这样以来就增加了程序的复杂性了。程序处于调试阶段，还是老老实实的使用大循环吧。帖几段伪代码，还有一些注释，作为程序框架的介绍。
----------------------------------------
int main(void)
{
~~~~//各种系统资源的初始化
while(1)
{
if(tick_flag)// SysTick时钟中断每10ms使tick_flag置1。
{//大部分程序都采用10ms的控制周期，不过也可以考虑更高的控制频率，毕竟cortex-m3的运算能力比起前人的51、avr强太多了。
tick_flag = 0;
balance_proc();//根据9轴模块的数据，PID调整pwm的输出
control_proc();//读取ir输入，调整pwm的输出
}
}
return 0;
}
----------------------------------------
void control_proc(void)
{
//----------遥控器反馈控制部分----------
int i = ir_key_proc(); // 将红外接收到的按键值，转换为小车控制的相应按键值。
switch(i)
{
~~~~//根据按键值，修改pwm_speed和pwm_turn的值。
}
pwm_turn *= 0.9; // pwm_turn的值以0.9的比例衰减，使小车在接收到一个转向信号后只转动一定的时间后停止转动。
//----------编码器反馈控制部分----------
speed = speed*0.7 +0.3*(encoder_read()); // 定周期（10ms）读取编码器数值得到实时速度，再对速度进行平滑滤波
encoder_write(0); // 编码器值重新设为0
distance += speed; // 对速度进行积分，得到移动距离
if(distance>6000) distance = 6000; // 减少小车悬空、空转对控制的影响
else if(distance<-6000) distance = -6000;
}
----------------------------------------
void balance_proc(void)
{
static unsigned int err_cnt=0;
int pwm_balance;
float radian, radian_pt; // 当前弧度及弧度的微分(角速度，角度值用弧度表示）
adxl345_read(&acc); // 读取当前加速度。由于传感器按照的位置原因，传感器的值在函数内部经过处理，变为小车的虚拟坐标系。
l3g4200d_read(&gyr); // 读取当前角速度。同样经过坐标系变换。
// 此段程序用于传感器出错时停止小车
err_cnt = err_cnt*115>>7; // err_cnt以0.9的比例系数衰减(115>>7的值约为0.9，避免浮点数，提高速度)
if(acc.flag != 0x0F || gyr.flag != 0x0F) // 读取的角度、角速度值有误。可能是电磁干扰、iic线太长等导致出错。
{
LED0_ON(); // 亮红灯
err_cnt +=100; // 等比数列，比例系数0.9(115>>7)，常数项100；根据公式，连续10项的和约为657
if(err_cnt>657) goto err; // 当连续发生约10次（约0.1秒）错误则超过657而溢出。
}
// 此段程序用于倒立或失重时停止小车
if(acc.z<=0)
{
goto err;
}
// 小车的虚拟x轴方向为小车前进方向，虚拟y轴为小车左边，虚拟z轴为小车上升方向。
// 前倾角度为负，后倾角度为正。
// 通过计算加速度分量，得到小车倾斜弧度（未滤波）
radian = (float)(acc.x)/acc.z; // 一阶展开：Q =f(x)=x-x^3/3+x^5/5+...+(-1)^k*x^(2k+1)/(2k+1)+...
// 通过角速度传感器，得到小车的角速度（单位为 弧度/秒）
radian_pt = gyr.y*RADPT;
radian_filted = ofme_filter(radian_filted, radian, radian_pt); // 互补滤波得到小车的倾斜角度
// 此段程序用于小车倾斜角度过大时，停止小车
if(radian_filted> ANGLE_RANGE_MAX || radian_filted<ANGLE_RANGE_MIN)
{
goto err;
}
// 通过PID计算，得到保持小车角度为零所需要的电机pwm输出
pwm_balance = pid_proc(&sPID, radian_filted, radian_pt);
//小车角度为零不代表小车重心为零，还需要通过小车移动速度与移动距离等信息，调整小车平衡所需的pwm输出
pwm_balance += (speed*6+distance*0.1);
// 在pwm_balance的基础上，加上速度分量与转动分量，调整小车两个电机的转速。
pwm_control(pwm_balance+pwm_speed+pwm_turn, pwm_balance+pwm_speed-pwm_turn);
// 如果pwm超出有效值，红灯亮。用于调试，了解系统状态。
if(pwm_balance>=1000||pwm_balance<=-1000) LED1_ON();
LED0_OFF();
return;
err:
puts("balance error.\r\n");
pwm_control(0, 0); // 关闭电机
return;
}
----------------------------------------
程序列表说明：
| 
[复制链接]
/2 
|手机版|OpenEdv-开源电子网
( 粤ICP备12000418号-1 )
发表于 2013-5-5 23:24:08
楼主
发表于 2013-5-6 10:11:30
