金牌会员
- 积分
- 1148
- 威望
- 529
- 贡献
- 281
- 兑换币
- 284
- 注册时间
- 2012-11-9
- 在线时间
- 169 小时
|
9#
楼主 |
发表于 2013-3-6 17:59:15
|
只看该作者
代码如下:
// 霍尔测速 (Hall Velocity Measurement)
// 通过定时取脉冲计数器里的值,计算后的出当前速度
// 所采集的最近一次速度放在SpeedTimeH 和 SpeedTimeL中
#include "includes.h"
// 车速计数器,以两次得到脉冲间隔的毫秒数计
INT16U carSpeedCounter[4];
INT16U carSpeedArrN = 0;
unsigned long lastAbsoluteTime;
//INT16U last_TC2, last_tc2_counter;
extern INT32U PerDistSrvTotal;
extern INT16U PerDistSrvN;
#define SRV_LEFT 0
#define SRV_CENTER 1
#define SRV_RIGHT 2
// 相差多少
#define DIS(x,y) ( ( (x) > (y) ) ? ( (x) - (y) ) : ( (y) - (x) ))
INT8U LastSrvAvg = 90;
INT16U PerDistSrvAvg = 90;
INT16U LastSrvState = SRV_CENTER;
// 以轮转圈数来计量路程
//#define ClearDistanceCounter() {PAC = 0;}
//#define GetDistance() (PAC)
//#define Distance PAC
INT16U GetSpeed(void) {
//如果还没开始跑,返回零
if (GetDistance() < 8)
return 0xFFFF;
return carSpeedCounter[0] +
carSpeedCounter[1] +
carSpeedCounter[2] +
carSpeedCounter[3];
}
void ProcessSpeed_FUNC(void) {
carSpeedCounter[carSpeedArrN = (carSpeedArrN + 1) & 0x3] = (INT16U)(absoluteTime - lastAbsoluteTime);
lastAbsoluteTime = absoluteTime;
/******************************************************/
/** TODO 路径记忆的间隔分析 **/
/* 分析前面的开始新的 */
if (PerDistSrvTotal != 0 && PerDistSrvN != 0) {
PerDistSrvAvg = (INT8U)(PerDistSrvTotal / PerDistSrvN);
// 排除错误
if (DIS(PerDistSrvAvg, LastSrvAvg) > 30) {
PerDistSrvAvg = LastSrvAvg;
}
if ((PerDistSrvAvg > 50 && PerDistSrvAvg < 130) ) {
LastSrvAvg = (INT8U)PerDistSrvAvg;
}
} else {
PerDistSrvAvg = LastSrvAvg;
}
// 发送调试数据
if (SCI0SR1 & 0x80) {
SCI0DRL = (INT8U)((INT16S)((3927 * 4 * 4) / GetSpeed() * _RTI_P / 4));
//(INT8U)PerDistSrvAvg;
}
/** 路径记忆入口点 **/
Process(GetDistance(), (INT8U)PerDistSrvAvg);
// 路径播放入口点
if (PathMemState == PLAY) {
PathPlayProcess(GetDistance());
PathPlayer(GetDistance());
}
// 判断特殊点
CheckNewPoint();
PerDistSrvTotal = 0;
PerDistSrvN = 0;
/******************************************************/
}
// 初始化测速器
void InitSpeeder(void) {
// 初始化和脉冲计数器
// WaitDisable();
WaitEnable();
InitPA();
PAI_FUNC = ProcessSpeed_FUNC;
lastAbsoluteTime = 0;
// 各位赋初值
ClearDistanceCounter();
}
有些看不懂
!!! |
|