智能车制作

标题: 我的平衡车设计--软件篇 [打印本页]
作者: Malc    时间: 2013-7-30 00:58
标题: 我的平衡车设计--软件篇
本帖最后由 Malc 于 2013-7-30 01:00 编辑

大赛结束了，由于时间紧迫，做车的时间很短，所以留下了一些遗憾，趁这段时间有空，整理一下几个月来的资料，既是对自己几个月来的总结，也算上和大家的一些交流吧，这篇文章就谈谈我的平衡车所用到的一些方案和算法吧

首先我用的方案是卓晴老师（在常大看到卓老师啦~）的方案，这次方案是做了一些修改和拓展，比如滤波我采用的互补滤波，速度控制用的是P控制而非PI，方向控制使用了转向陀螺仪。控制周期5ms，除了蓝牙发送程序外，其他代码均放在中断中，这样的好处是控制周期准确，5ms周期中，CCD采样572us，ACC、Gyro采样+计算 784us，直立、方向、速度控制 208us，CCD计算500us左右，总共时间略大于2ms。曝光时间固定5ms，这个时间在白天以及比赛现场是很合适的，但是到了晚上就不行了，到了晚上一般我会将曝光时间x2，并用运放对CCD信号进行2~5倍的放大。下面详细说一下一些我认为比较重要的部分吧。

FIR滤波器
首先介绍的FIR滤波器，在我的代码中，我大量使用了FIR低通滤波器，主要是这么5个地方：加速度计角度、速度、直立控制中的Error、赛道中心线、速度控制中的Error。  关于FIR的介绍和设计估计可以重开一贴了，所以这里主要说一下FIR的效果，在直立控制中，我对Error=Setpoint-angle进行了一次低通滤波，带来的效果就是直立控制的P、D参数可以使用更高的值，也就是说小车立的更 硬 了，平常跑的时候基本1.6~1.8m/s过单障碍，FIR之前，kp=1250,kd=18，FIR之后kp=1800,kd=30。速度值通过低通滤波之后杂波明显少了很多，曲线也更光滑了。计算出来的赛道中心线之所以使用低通滤波进行滤波主要是为了防止赛道中心突变，因为整个赛道是连续的，即使是在十字弯。下面附上一张加速度计经过FIR低通滤波之后的效果图：
滤波之前，黄色的是加速度计角度，红色是融合之后的角度：
[attach]49366[/attach]
滤波之后：
[attach]49367[/attach]
当然FIR滤波器也有他的缺点，比如频率选择特性没有IIR好，会有固定的相位延迟，延迟为拍数/2*采样间隔

互补滤波
平衡车角度的获取我并没有采用清华方案，而是采用了动态权值互补滤波，好处是可以得到小车的真实角度，计算量有所增加，但几百us还是可以接受的。互补滤波关键代码：
tempX=angleAX*5/4096*1000/AccSense;//5v,12bit ADC,AccSense查datasheet可得
tempZ=angleAZ*5/4096*1000/AccSense;
tempZ=angleAZ*5/4096*1000/AccSense;
angleA=-atan2(angleAX,angleAZ)*180/PI;//加速度计计算出的角度
angleA=FIR(angleA);
gravity=sqrt(tempX*tempX+tempY*tempY+tempZ*tempZ);
gravityError=fabs(gravity-gravityG);
if(gravityError>gravityVibrationGate)// avoid vibration
   weight=1;
else{
      weight=weight2+(weight1-weight2)*gravityError/gravityVibrationGate;//动态权重
    }
time=micros()-gyroTimer;//micros()用于读取当前的时间，单位us
gyroTimeTest=time;
angleDelta=(testGyroZ1-TLYLDZ1)*5/4096*1.5/5.1/GyroSense2*time/1000;//自制的陀螺仪，放大倍数=5.1/1.5
angleFinal=angleA*(1-weight)+(angleDelta+angleFinal)*weight;//这就是互补滤波了
在这里weight的变化范围是0.98~0.995

黑线提取
黑线提取我采用的是跳变沿检测+动态阀值，好处是可以减少共模干扰，建议采集的128个点经过一次中值滤波，3点足够，5点的时候边沿已经变得略平滑了，代码如下:
for(i=0,CCDAvr0=0;i<128;i++)
           CCDAvr0=CCDa+CCDBuf2;
CCDAvr0=CCDAvr0/128;
FZ=CCDa*FZBL;//阀值=赛道亮度平均值*阀值比例（0.4~0.8）
for(i=LineCenter;i+DCCD<128;i++)  //Rblack
{
        if(CCDBuf2-CCDBuf2[i+DCCD]>FZ)
           Rblack=i+DCCD;
}
值得注意的是边沿检测不一定是两两相邻的点进行比较，因为这样可能会因为边沿特征的不明显而识别失败，建议取3~8个点。另外上面的搜索中，并不是从64点往两边搜索，而是从上次的centerline往两边搜索。
补线：
if(Lblack>50)
  LineCenter=Lblack+trackWidth/2;
else if(Rblack<78)
  LineCenter=Rblack-trackWidth/2;
else
  LineCenter=(Lblack+Rblack)/2;
至于trackWidth，我采用的是动态值，因为它会随车体的前倾角度二变化，我的小车只用了一个CCD，几次尝试使用一前一后的双CCD检测，最后蛋疼的放弃了，就是因为前瞻和前倾的蛋疼关系。。
起跑线的识别，使用的是“夏日的冰”的方法，检测跳变沿个数是否大于5，这个方法非常好用，在此表示感谢，原帖地址：http://www.znczz.com/thread-141798-1-1.html

时间的获取
在很多地方我使用了micros()、millis()这两个函数，这两个函数来自于Arduino，作用就是利用一个定时器，在单片机开机的时候进行计时，通过这两个函数可以获取当前的时间，unsigned long的格式，micros()70分钟后溢出，millis()50天后溢出，用在小车上足够。有了这两个函数可以很方便的计算出一段代码的运行时间、gyro的积分时间等，加上SerialChart、XS在线调试，示波器被彻底取代了：） 代码：
void PIT_Init(void) {
  PITCFLMT_PITE=0; //定时中断通道0 关
  PITCE_PCE0=1;//定时器通道0 使能
  PITMTLD0=80-1;//8 位定时器初值设定。80 分频，在80MHzBusClock 下，为1MHz。即1us.
  PITLD0=54321-1;//16 位定时器初值设定。50ms溢出
  PITINTE_PINTE0=1;//定时器中断通道0 中断使能
  PITCFLMT_PITE=1;//定时器通道0 使能
  
  PITLD1=5000-1;//16 位定时器初值设定。2ms溢出
  PITCE_PCE1=1;//定时器通道1 使能
  PITINTE_PINTE1=0; //定时器中断通道1 中断
}
unsigned long micros() {
return time_us+(54321-PITCNT0);
}
unsigned long millis() {
unsigned long mm=micros();
if(mm>500)
return (54321-PITCNT0)/1000+time_ms+1;
return (54321-PITCNT0)/1000+time_ms;
}
     
#pragma CODE_SEG __NEAR_SEG NON_BANKED
void interrupt 66 PIT0(void)
{
    PITTF_PTF0 = 1; //clear interrupts flag
    time_ms_mod+=321;
    if(time_ms_mod>=1000){
     time_ms_mod-=1000;
     time_ms+=1;
    }
    time_ms+=54;
    time_us+=54321;
      
}
#pragma CODE_SEG DEFAULT  
这段代码中，利用PIT0作为定时时钟，溢出周期54321us，而控制周期是5ms，为什么会用这么奇葩的数字，因为我发现如果溢出周期若为50ms,就会导致每隔10次就使得控制中断被忽略一次，没办法，只好增大他们的最小公倍数了。

关于软件，我想写的就这么多吧，时间仓促肯定有遗漏，以后再补充吧，欢迎大家谈谈自己的看法，而不是只留一个“顶”or“路过”~
作者: 奋斗小青年、    时间: 2013-7-30 07:21
我们的起跑线也很给力   而且不需要更改曝光时间啊
作者: wen123    时间: 2013-7-30 08:13
奋斗小青年、 发表于 2013-7-30 07:21
我们的起跑线也很给力   而且不需要更改曝光时间啊

分享出来不是更好么

作者: wen123    时间: 2013-7-30 08:25
好帖！！lz分享的精神令我佩服！！，你用了这么多的低通滤波会不会有滞后了，，，，我也用了低通滤波没有用你的这么多，我的比例是1：1，不知道你用多大的比例，还有trackWidth动态的和入弯姿势有关也就是你所说的小车姿态角度，这个有点误差的，不知道你怎么处理的，，，，，，建立数学模型算出精确的误差，还是直接算出这个trackwidth再通过trackwidth算出误差，，这样是有误差的，而且会随着丢线的次数而累积误差！！！！！希望交流！
作者: A楼飘房    时间: 2013-7-30 08:45
要向你學習啦。
作者: w717342899    时间: 2013-7-30 08:55
前排支持。。。。。。。。。。。
作者: vip101vip    时间: 2013-7-30 08:57
顶
作者: 理工小别    时间: 2013-7-30 09:17
w717342899 发表于 2013-7-30 08:55
前排支持。。。。。。。。。。。

有些东西你还是没考虑到吧 多学学吧 数学很重要哦
作者: wenshuai536    时间: 2013-7-30 09:18
感觉好深奥的样子，刚刚开始接触这个，现在也没办法理解，等我学会了就能理解了吧
作者: 暗里着迷    时间: 2013-7-30 09:46
学习了！谢过LZ！
作者: wen123    时间: 2013-7-30 10:14
你1.6到1.8过障碍速度开环没？？？？？？？？？？
作者: 嘸敵，尒骉垯    时间: 2013-7-30 10:16
学习了！
作者: qurter    时间: 2013-7-30 12:12
wen123 发表于 2013-7-30 10:14
你1.6到1.8过障碍速度开环没？？？？？？？？？？

大神你是开环过的障碍？效果怎么样？
作者: qurter    时间: 2013-7-30 12:28
首先感谢大神的分享，在此想求教大神几个问题：

直立用FIR的话相位滞后对直立调节实时性的影响大么？会不会出现因为滞后导致直立角度调节超调增加之类的？

大神你速度用FIR是用来引入反馈的车速还是编码器脉冲的计数值？会不会使得速度调节的实时性不够？
作者: wen123    时间: 2013-7-30 13:49
qurter 发表于 2013-7-30 12:28
首先感谢大神的分享，在此想求教大神几个问题：

直立用FIR的话相位滞后对直立调节实时性的影响大么？会不 ...

其实，我感觉，这个速度低通是有必要的，，你说的不实时性也就行滞后，低通滞后是必然的，但这个不会太影响系统的运行，其实个人感觉整个系统就是个滞后的系统，不可能 做到百分之百的实时性的，再说速度用pid控制本身就是个误差的消除过程，误差总存在的，速度滞后点是没多大关系的，开环也能跑的，何须在意这点滞后，不要忘记你的速度采集时间是ms级的，不知道lz怎么看？你又怎么能看？
作者: qurter    时间: 2013-7-30 14:06
wen123 发表于 2013-7-30 13:49
其实，我感觉，这个速度低通是有必要的，，你说的不实时性也就行滞后，低通滞后是必然的，但这个不会太影 ...

大神说的是，不过我之前试过给编码器加惯性滤波致使车速调节过程很不好，经常出现调节比速度滞后很多……
作者: a1005020208    时间: 2013-7-30 14:46
qurter 发表于 2013-7-30 14:06
大神说的是，不过我之前试过给编码器加惯性滤波致使车速调节过程很不好，经常出现调节比速度滞后很多…… ...

我总感觉主要的干扰出现在陀螺仪和加速度计上，难不成阁下认为编码器的值也不可靠？也要虑？那都虑一遍的话。。。。。
作者: qurter    时间: 2013-7-30 15:27
a1005020208 发表于 2013-7-30 14:46
我总感觉主要的干扰出现在陀螺仪和加速度计上，难不成阁下认为编码器的值也不可靠？也要虑？ ...

其实不是认为编码器不可靠，只是以前测得车速会有毛刺，测出来是编码器的问题……
作者: c6661176    时间: 2013-7-30 17:33
谢谢LZ
作者: Malc    时间: 2013-7-30 18:15
奋斗小青年、 发表于 2013-7-30 07:21
我们的起跑线也很给力   而且不需要更改曝光时间啊

别吊胃口啊，说吧
作者: Malc    时间: 2013-7-30 18:23
wen123 发表于 2013-7-30 08:25
好帖！！lz分享的精神令我佩服！！，你用了这么多的低通滤波会不会有滞后了，，，，我也用了低通滤波没有用 ...

你说的比例是什么?不是很明白
FIR低通滤波我用的都是5阶的，再高一点比如11阶、15阶就会出现明显滞后了，5阶的效果还不错吧，波形基本没有毛刺，但是还是会有一些高频噪声。
trackWidth的更新条件：50<LineCenter<78 && |Setpoint-angle|<8°，然后再constrain(trackWidth,min,max)
作者: Malc    时间: 2013-7-30 18:28
wen123 发表于 2013-7-30 10:14
你1.6到1.8过障碍速度开环没？？？？？？？？？？

速度一直是闭环的，速度控制我只用了P，没有PI，调了整整一个星期，加了积分各种问题，干脆去掉了I，单纯的P，实际跑起来速度会略低于给定速度
作者: ℡单身旅行    时间: 2013-7-30 18:35
前排支持，，，
作者: Malc    时间: 2013-7-30 18:39
qurter 发表于 2013-7-30 12:28
首先感谢大神的分享，在此想求教大神几个问题：

直立用FIR的话相位滞后对直立调节实时性的影响大么？会不 ...

阶数要合适，但是FIR的滤波效果是和阶数成正比的，具体取多少你可以自己测试一下，如果取得太大确实会导致直立超调--震荡--然后倒下
我说的速度指的就是编码器累计值，通过计算可以转换成mm/s，因为电机高速转动的噪声、轮胎的不均匀等原因会使得速度曲线带有大量毛刺，通过5阶FIR低通滤波之后，波形可以得到很好的改善，而且没有明显滞后，你也可以加大阶数再减小控制周期，看看实际效果怎么样
作者: wen123    时间: 2013-7-30 22:38
a1005020208 发表于 2013-7-30 14:46
我总感觉主要的干扰出现在陀螺仪和加速度计上，难不成阁下认为编码器的值也不可靠？也要虑？ ...

我认为，其实在没有障碍的情况下。速度没必要加什么低通滤波，过障碍是角度的突变和编码器的积分的突变，要想过必须加个低通滤波！！
作者: wen123    时间: 2013-7-30 22:39
Malc 发表于 2013-7-30 18:28
速度一直是闭环的，速度控制我只用了P，没有PI，调了整整一个星期，加了积分各种问题，干脆去掉了I，单纯 ...

你若用pi估计过障碍1.8的速度是过不去的，，
作者: Malc    时间: 2013-7-30 22:59
wen123 发表于 2013-7-30 22:38
我认为，其实在没有障碍的情况下。速度没必要加什么低通滤波，过障碍是角度的突变和编码器的积分的突变， ...

不，我认为最根本的原因是车轮撞击障碍的一瞬间，车轮速度陡然降低，但是车体上半部分因为惯性的原因维持原来的速度，小车来不及调节，最后扑街，所以解决的办法就是将重心尽量的降低，然后增大直立控制输出。
作者: 奋斗小青年、    时间: 2013-7-31 07:32
Malc 发表于 2013-7-30 18:15
别吊胃口啊，说吧

我们也是检测跳变  当检测到黑白黑白（两个黑之间的距离小于40）或者白黑白黑（两个黑之间的距离小于40） 的时候算作起跑线
作者: a1005020208    时间: 2013-7-31 10:01
wen123 发表于 2013-7-30 22:39
你若用pi估计过障碍1.8的速度是过不去的，，

对于角度的突变，我能想到的就是增大直立控制的输出。至于编码器积分的突变，可以加一个积分分离器在突变的时候把积分项舍了。你怎么看？
作者: a1005020208    时间: 2013-7-31 10:53
Malc 发表于 2013-7-30 22:59
不，我认为最根本的原因是车轮撞击障碍的一瞬间，车轮速度陡然降低，但是车体上半部分因为惯性的原因维持 ...

LZLZ，求FIR和IIR滤波器的资料，望不吝共享！  {:soso_e183:}  {:soso_e183:}
作者: wen123    时间: 2013-7-31 11:10
a1005020208 发表于 2013-7-31 10:01
对于角度的突变，我能想到的就是增大直立控制的输出。至于编码器积分的突变，可以加一个积分分离器在突变 ...

额，，这就是lz为啥只用p的原因吧！！！！！！
作者: a1005020208    时间: 2013-7-31 11:26
wen123 发表于 2013-7-31 11:10
额，，这就是lz为啥只用p的原因吧！！！！！！

你不是说PI过不去么，你倒是说说在过的时候哪些方面是该重点考虑的，哪些方面又是我们这些众人没有想到的呢？
作者: 幽溪居士    时间: 2013-7-31 23:42
Malc 发表于 2013-7-30 18:39
阶数要合适，但是FIR的滤波效果是和阶数成正比的，具体取多少你可以自己测试一下，如果取得太大确实会导致 ...

楼主的速度控制周期是多少啊？是5ms还是官方的100ms分20次输出？个人感觉100ms的控制周期没有必要低通滤波，5ms读一次编码器的值20次累加再求均值也起到了低通滤波的作用。另外如果用FIR低通滤波器窗函数怎么选取？楼主用的是啥呀？
作者: Malc    时间: 2013-8-1 19:36
幽溪居士 发表于 2013-7-31 23:42
楼主的速度控制周期是多少啊？是5ms还是官方的100ms分20次输出？个人感觉100ms的控制周期没有必要低通滤波 ...

速度控制周期5x8ms,再均匀输出，速度控制加不加fir感觉都差不多吧滤波用的是汉明窗、矩形窗，过段时间我整理一下FIR的资料发个贴吧
作者: yyyzrxh    时间: 2013-8-3 09:38
今年做的电磁，明年可能做直立。
作者: good_luck    时间: 2013-8-4 15:50
现在这个论坛气氛越来越差了，但是楼主这种分享精神真的很令我佩服，希望大家都能像你一样，让论坛的气氛变得积极起来！
作者: 沫XY    时间: 2013-8-4 16:52
Malc 发表于 2013-7-30 22:59
不，我认为最根本的原因是车轮撞击障碍的一瞬间，车轮速度陡然降低，但是车体上半部分因为惯性的原因维持 ...

你们面临的就是两个问题！一个是因为触碰障碍时速度不能很好地保持，另一个是因为车子过了障碍飞了（导致速度积分趋近饱和）
作者: 沫XY    时间: 2013-8-4 16:56
沫XY 发表于 2013-8-4 16:52
你们面临的就是两个问题！一个是因为触碰障碍时速度不能很好地保持，另一个是因为车子过了障碍飞了（导致 ...

我只是随口说一下，也不知对不对！以后希望大家多多交流！QQ：929976868
作者: /sun一    时间: 2013-8-8 08:39
求FIR滤波器资料，谢谢。
作者: 爱走来的那天    时间: 2013-8-18 21:55
顶...
作者: ℡单身旅行    时间: 2013-8-21 12:09
作者: zhaoliujian78    时间: 2013-8-21 16:05
好东西啊。
作者: 乜許錯ㄋ    时间: 2013-10-13 10:59
谢谢lz
作者: 青青水草    时间: 2013-10-16 10:27
作者: 爱可死/X    时间: 2013-12-14 20:48
谢谢分享。楼主的代码风格不错哦{:soso_e183:}
作者: 532902675    时间: 2013-12-15 18:30
评论都看着好深奥啊
作者: luxiaojian    时间: 2013-12-22 12:09
楼主，这段程序能讲下吗？？
for(i=LineCenter;i+DCCD<128;i++)  //Rblack
{
        if(CCDBuf2-CCDBuf2[i+DCCD]>FZ)
           Rblack=i+DCCD;
}
作者: 727589751    时间: 2013-12-22 13:04
作者: 淡定戈    时间: 2013-12-23 10:45
直立控制其实没必要这么复杂，用一个陀螺仪就够了
作者: 雨晴    时间: 2013-12-25 15:53
赞！lz
作者: zhou1994    时间: 2013-12-25 21:17
:lol:lol:lol
作者: wmslecz    时间: 2014-2-18 19:59
来一次过来支持一次...
作者: ccs5433    时间: 2014-3-1 19:39
楼主能解释一下为什么“溢出周期若为50ms,就会导致每隔10次就使得控制中断被忽略一次”吗？非常感谢~
作者: colson    时间: 2014-3-6 17:17
学习！
作者: wmslecz    时间: 2014-3-21 19:44
Malc 发表于 2013-8-1 19:36
速度控制周期5x8ms,再均匀输出，速度控制加不加fir感觉都差不多吧滤波用的是汉明窗、矩形窗，过段时间我整 ...

学姐..我想请问下你..关于那个F.I.R滤波.采样频率是200..低通频率你选的多少呢?50hz吗?仅仅是5阶的话那个效果应该不怎么好吧?
作者: 烽魔渣中渣    时间: 2014-3-21 20:16
能详细一点吗?
作者: wmslecz    时间: 2014-3-22 19:39
学姐..多次研究过后终于懂了...不过我发现延迟很重呢...我的FIR滤波阶数只有5呢..我发现效果还能接受..就是滞后...呜呜呜...[attach]57359[/attach]学姐你能看看这个还差多远吗?
作者: Malc    时间: 2014-3-23 08:59
wmslecz 发表于 2014-3-22 19:39
学姐..多次研究过后终于懂了...不过我发现延迟很重呢...我的FIR滤波阶数只有5呢..我发现效果还能接受..就是 ...

你这个是速度的波形么，怎么这么乱
这个波形滞后有点小大，我记得我原来的滞后没那么大的
你把控制周期改小点试试，周期小，意味着滞后也小
作者: wmslecz    时间: 2014-3-23 11:21
Malc 发表于 2014-3-23 08:59
你这个是速度的波形么，怎么这么乱
这个波形滞后有点小大，我记得我原来的滞后没那么大的
你把控制周期 ...

嘿嘿..学姐..这个是加速度计出来的波形...然会我测试是将车子来回走...加速度计有非常大的抖动.....红线是加速度计滤波出来后的效果...这个延迟还很大吗?我现在用的是5阶...wp=1 ws=2的汉明窗的....FS=200..这种滞后问题很大吗?
作者: Malc    时间: 2014-3-23 11:47
wmslecz 发表于 2014-3-23 11:21
嘿嘿..学姐..这个是加速度计出来的波形...然会我测试是将车子来回走...加速度计有非常大的抖动.....红线是 ...

哦是加速的计的啊
我用的也是5阶，wp=3 ws=9首先对加速度计进行一次fir，再在直立控制中对error进行一次fir，直立效果很好，那点延迟完全没影响
作者: 光速小子    时间: 2014-3-23 12:22
我想问一下在keil里面，micros()、millis()这两个函数可以用吗
作者: Malc    时间: 2014-3-23 12:31
光速小子 发表于 2014-3-23 12:22
我想问一下在keil里面，micros()、millis()这两个函数可以用吗

不能，因为这是我自己写的
其实很简单的，开个定时器不停地计时，然后再用两个unsigned long分别存储micros和millis就行了
作者: wmslecz    时间: 2014-3-23 14:41
Malc 发表于 2014-3-23 11:47
哦是加速的计的啊
我用的也是5阶，wp=3 ws=9首先对加速度计进行一次fir，再在直立控制中对error进行一次 ...

嗯嗯...要不是学姐..我也不知道什么是FIR..也不会去了解这个..希望以后再技术的道路能多多向你学习..能加QQ或者其他你常用的方式交流吗?我的腾讯企鹅是1044811851..谢谢学姐..
作者: Malc    时间: 2014-3-23 18:40
wmslecz 发表于 2014-3-23 14:41
嗯嗯...要不是学姐..我也不知道什么是FIR..也不会去了解这个..希望以后再技术的道路能多多向你学习..能加 ...

不用谢~！在论坛交流就好啦
作者: wmslecz    时间: 2014-3-24 12:15
Malc 发表于 2014-3-23 18:40
不用谢~！在论坛交流就好啦

嗯嗯..好的..那就先谢谢学姐咯..O(∩_∩)O~~...
作者: 光速小子    时间: 2014-3-24 18:48
Malc 发表于 2014-3-23 12:31
不能，因为这是我自己写的
其实很简单的，开个定时器不停地计时，然后再用两个unsigned long分别存储mic ...

是否是在一段程序开始前开定时器，结束后关定时器然后看返回的值？

作者: 苍天吐血    时间: 2014-3-24 20:40
前面几位大神讲的太深奥了，但感觉很受启发有木有
作者: Malc    时间: 2014-3-25 09:28
光速小子 发表于 2014-3-24 18:48
是否是在一段程序开始前开定时器，结束后关定时器然后看返回的值？

没必要，我写的这个函数是从上电初始化后就开始不停的计时，只要time=micros()或time=millis()就能读到当前时间
然后，micros()-time就能计算出一段代码的运行时间，用来查看每个函数运行时间、计算时间都挺方便的

代码：
Time_Function
  1. #include"Time_Function.h"
  2. #include <hidef.h>      /* common defines and macros */
  3. #include "derivative.h"      /* derivative-specific definitions */


  6. void setbusclock_80M(void)
  7. {      
  8.   CLKSEL = 0X00;    //disengage PLL to system
  9.   PLLCTL_PLLON = 1;   //turn on PLL
  10.   SYNR = 0xc0 | 0x09;                        
  11.   REFDV = 0x80 | 0x01;
  12.   POSTDIV = 0x00;       //pllclock=2*osc*(1+SYNR)/(1+REFDV)=160MHz;
  13.   _asm(nop);          //BUS CLOCK=80M
  14.   _asm(nop);
  15.   while(!(CRGFLG_LOCK==1));   //when pll is steady ,then use it;
  16.   CLKSEL_PLLSEL = 1;          //engage PLL to system;
  17. }

  19. void Dly_ms(unsigned int ms)
  20. {
  21.    unsigned int i,j;
  22.    if (ms<1) ms=1;
  23.    for(i=1;i<=ms;i++)
  24.    for(j=0;j<1000;j++) {
  25.     _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  26.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  27.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  28.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  29.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  30.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  31.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  32.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);  
  33.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  34.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  35.       
  36.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  37.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  38.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  39.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  40.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  41.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);  
  42.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  43.    }
  44.    
  45. }

  47. void Dly_us(unsigned int us)
  48. {
  49.    unsigned int ii;   
  50.    for(ii=0;ii<us;ii++)
  51.    {
  52.       
  53. #ifdef BUSCLOCK32M
  54.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  55.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  56.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  57.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  58.       
  59.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  60.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  61.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  62.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);  
  63. #endif

  65. #ifdef BUSCLOCK64M
  66.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  67.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  68.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  69.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  70.       
  71.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  72.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  73.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  74.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  75.       
  76.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  77.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  78.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  79.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  80.       
  81.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  82.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  83.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  84.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);  
  85. #endif

  87. #ifdef BUSCLOCK80M
  88.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  89.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  90.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  91.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  92.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  93.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  94.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  95.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);  
  96.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  97.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  98.       
  99.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  100.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  101.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  102.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  103.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  104.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);  
  105.       _asm(nop);_asm(nop);_asm(nop);_asm(nop);
  106.       //_asm(nop);_asm(nop);_asm(nop);_asm(nop);     
  107. #endif
  108.       
  109.    }
  110. } //end of Dly_us()



  114. void Dly_25ns(unsigned int ns)
  115. {
  116.    unsigned int ii;   
  117.    for(ii=0;ii<ns;ii++)
  118.    {
  119. #ifdef BUSCLOCK80M
  120.   
  121.       _asm(nop);
  122.       //_asm(nop);_asm(nop);_asm(nop);_asm(nop);     
  123. #endif
  124.       
  125.    }
  126. } //end of Dly_us()



  130.   
  131.   


  134. /*   for micros(),millis()
  135. void PIT_Init(void) {
  136.   PITCFLMT_PITE=0; //定时中断通道0 关
  137.   PITCE_PCE0=1;//定时器通道0 使能
  138.   PITMTLD0=80-1;//8 位定时器初值设定。80 分频，在80MHzBusClock 下，为1MHz。即1us.
  139.   PITLD0=50000-1;//16 位定时器初值设定。50ms溢出
  140.   PITINTE_PINTE0=1;//定时器中断通道0 中断使能
  141.   PITCFLMT_PITE=1;//定时器通道0 使能
  142. }
  143. */

  145. void PIT_Init(void) {
  146.   PITCFLMT_PITE=0; //定时中断通道0 关
  147.   PITCE_PCE0=1;//定时器通道0 使能
  148.   PITMTLD0=80-1;//8 位定时器初值设定。80 分频，在80MHzBusClock 下，为1MHz。即1us.
  149.   PITLD0=54321-1;//16 位定时器初值设定。50ms溢出
  150.   PITINTE_PINTE0=1;//定时器中断通道0 中断使能
  151.   PITCFLMT_PITE=1;//定时器通道0 使能
  152.   
  153.   PITLD1=5000-1;//16 位定时器初值设定。2ms溢出
  154.   PITCE_PCE1=1;//定时器通道1 使能
  155.   PITINTE_PINTE1=0; //定时器中断通道1 中断
  156. }

  158. /*
  159. void setTimer0(unsigned long)//定时中断初始化函数50MS 定时中断设置
  160. {
  161.   
  162. }
  163. */

  165. unsigned long micros() {
  166. return time_us+(54321-PITCNT0);
  167. }

  169. unsigned long millis() {
  170. unsigned long mm=micros();
  171. if(mm>500)
  172. return (54321-PITCNT0)/1000+time_ms+1;
  173. return (54321-PITCNT0)/1000+time_ms;

  175. }


  178.      
  179. #pragma CODE_SEG __NEAR_SEG NON_BANKED

  181. void interrupt 66 PIT0(void)
  182. {

  184.     PITTF_PTF0 = 1; //clear interrupts flag
  185.     time_ms_mod+=321;
  186.     if(time_ms_mod>=1000){
  187.      time_ms_mod-=1000;
  188.      time_ms+=1;
  189.     }
  190.     time_ms+=54;
  191.     time_us+=54321;
  192.       
  193. }

  195. #pragma CODE_SEG DEFAULT      

复制代码



Time_Function.h:
  1. #ifndef Time_Function_h
  2. #define Time_Function_h

  4. #define BUSCLOCK80M


  7. #define EnableControlPIT PITINTE_PINTE1=1; //定时器中断通道1 中断使能
  8. #define DisableControlPIT PITINTE_PINTE1=0; //定时器中断通道1 中断使能

  10. #define EnableTimerPIT PITINTE_PINTE0=1; //定时器中断通道1 中断使能
  11. #define DisableTimerPIT PITINTE_PINTE0=0; //定时器中断通道1 中断使能

  13. static unsigned long time_ms=0;
  14. static unsigned long time_ms_mod=0;
  15. static unsigned long time_us=0;

  17. void setbusclock_80M(void);
  18. void Dly_us(unsigned int);
  19. void Dly_ms(unsigned int);
  20. void PIT_Init(void);
  21. unsigned long micros(void);
  22. unsigned long millis(void);

  24. static unsigned signa=0;

  26. #endif
复制代码


作者: 光速小子    时间: 2014-3-25 10:30
Malc 发表于 2013-8-1 19:36
速度控制周期5x8ms,再均匀输出，速度控制加不加fir感觉都差不多吧滤波用的是汉明窗、矩形窗，过段时间我整 ...

5*8ms是这个样子吗？40ms采集一次然后速度分8次融合到直立控制中（5ms控制一次）是吗。恳求lz的回复。
作者: 光速小子    时间: 2014-3-25 10:31
ccs5433 发表于 2014-3-1 19:39
楼主能解释一下为什么“溢出周期若为50ms,就会导致每隔10次就使得控制中断被忽略一次”吗？非常感谢~

我也想问这个问题。
作者: ccs5433    时间: 2014-3-25 10:57
光速小子 发表于 2014-3-25 10:31
我也想问这个问题。

我的理解是，首先有两个PIT中断，一个1ms，一个10ms；然后初始化结束后，两个PIT同时开启，计数器自减；之后第一个1ms时溢出进入中断一次，第二个1ms时溢出进入中断一次...第十个1ms时，两个PIT同时溢出，单片机只能进入其中的一个中断，导致另一个被忽略。忽略的周期是两个PIT的最小公倍数...
作者: 光速小子    时间: 2014-3-25 11:03
ccs5433 发表于 2014-3-25 10:57
我的理解是，首先有两个PIT中断，一个1ms，一个10ms；然后初始化结束后，两个PIT同时开启，计数器自减；之 ...

有道理
作者: projecta    时间: 2014-3-25 17:04
最近直立车老抖，加了低通就好了，有用
作者: wmslecz    时间: 2014-3-27 20:51
Malc 发表于 2014-3-25 09:28
没必要，我写的这个函数是从上电初始化后就开始不停的计时，只要time=micros()或time=millis()就能读到当 ...

学姐,你太厉害了..看来你这个帖子不久就会被我等的新手所爆棚...恳请学姐鉴定下..这样的融合角度是否合适了?[attach]57574[/attach][attach]57573[/attach][attach]57572[/attach]
作者: Malc    时间: 2014-3-27 21:33
wmslecz 发表于 2014-3-27 20:51
学姐,你太厉害了..看来你这个帖子不久就会被我等的新手所爆棚...恳请学姐鉴定下..这样的融合角度是否合适 ...

嗯 挺好 波形基本没有滞后，但是在上升和下降的边缘有过冲，估计是你给的加速度计比例太大了一点吧
作者: wmslecz    时间: 2014-3-27 22:42
Malc 发表于 2014-3-27 21:33
嗯 挺好 波形基本没有滞后，但是在上升和下降的边缘有过冲，估计是你给的加速度计比例太大了一点吧

嘿嘿..好的..谢谢学姐...我用的是FIR 9阶 wp=2 ws=3  然后加之卡尔曼出来的..我试试把陀螺仪的信任度弄大点O(∩_∩)O~~我发现卡尔曼神奇之处了...我现在板子刚送出去做..调试设备都是零时搭建起来了..学姐你说我们这样来得及吗?嘿嘿...
作者: 直立摄像头    时间: 2014-3-27 23:43
wmslecz 发表于 2014-3-27 22:42
嘿嘿..好的..谢谢学姐...我用的是FIR 9阶 wp=2 ws=3  然后加之卡尔曼出来的..我试试把陀螺仪的信任度弄大 ...

求交流FIR滤波，还不大懂
作者: allenanswerzq    时间: 2014-3-28 00:37
speed = speed * 0.2 + speedlast * 0.8；这也算是低通滤波吧，求教
作者: Malc    时间: 2014-3-28 10:27
wmslecz 发表于 2014-3-27 22:42
嘿嘿..好的..谢谢学姐...我用的是FIR 9阶 wp=2 ws=3  然后加之卡尔曼出来的..我试试把陀螺仪的信任度弄大 ...

没问题的，加油！
作者: Malc    时间: 2014-3-28 10:29
直立摄像头 发表于 2014-3-27 23:43
求交流FIR滤波，还不大懂

过段时间我把fir资料整理一下上传
作者: Malc    时间: 2014-3-28 10:30
allenanswerzq 发表于 2014-3-28 00:37
speed = speed * 0.2 + speedlast * 0.8；这也算是低通滤波吧，求教

你这个应该叫做加权递推平均滤波法吧
作者: 直立摄像头    时间: 2014-3-28 10:31
Malc 发表于 2014-3-28 10:29
过段时间我把fir资料整理一下上传

好，期待着
作者: allenanswerzq    时间: 2014-3-28 12:24
Malc 发表于 2014-3-28 10:30
你这个应该叫做加权递推平均滤波法吧

对于控制小车来说，我觉得还是有效果的，你说的那个FIR低通滤波，不是没看懂，是根本没看懂啊，哈哈哈
作者: 直立摄像头    时间: 2014-4-2 11:30
Malc 发表于 2014-3-28 10:30
你这个应该叫做加权递推平均滤波法吧

学姐请问你的直立车加速度计FIR低通截止频率取多少啊，采样频率为200，根据采样定理，是不是取100HZ为截止频率比较适合呢？
作者: Malc    时间: 2014-4-2 13:27
直立摄像头 发表于 2014-4-2 11:30
学姐请问你的直立车加速度计FIR低通截止频率取多少啊，采样频率为200，根据采样定理，是不是取100HZ为截止 ...

截止频率9hz
作者: qymsdo0000    时间: 2014-4-2 16:24
求FIR低通滤波器的相关资料，万分感谢:lol:lol
作者: 直立摄像头    时间: 2014-4-4 09:47
Malc 发表于 2014-3-28 10:30
你这个应该叫做加权递推平均滤波法吧

惯性滤波吧
作者: 60飘过    时间: 2014-4-4 10:52
mark
作者: 直立摄像头    时间: 2014-4-4 11:03
Malc 发表于 2014-4-2 13:27
截止频率9hz

学姐请问你计算系数时是不是用matlab的FDAtool去算的
作者: wmslecz    时间: 2014-4-8 16:31
本帖最后由 wmslecz 于 2014-4-8 16:36 编辑
Malc 发表于 2014-4-2 13:27
截止频率9hz

学姐.我们遇到一个问题.就是发现陀螺仪抖动很大...这个你们是怎么解决的?能说一下吗?[attach]58144[/attach]..我已经不能再降低干扰了...现在是各种调整..我发现在没开电机的情况下还是干扰很大...学姐....求确认..呜呜呜...
作者: prothesman    时间: 2014-4-8 16:56
收藏本帖，留存，常来看看，下载学习，储备知识，换积分啊
作者: Malc    时间: 2014-4-9 08:51
直立摄像头 发表于 2014-4-4 11:03
学姐请问你计算系数时是不是用matlab的FDAtool去算的

不是，是我根据老外写的一篇文章写了个参数自动生成小程序，下次附上来
作者: Malc    时间: 2014-4-9 08:51
wmslecz 发表于 2014-4-8 16:31
学姐.我们遇到一个问题.就是发现陀螺仪抖动很大...这个你们是怎么解决的?能说一下吗?..我已经不能再降低干 ...

你这波形。。是不是因为你用电脑的5v供电的，如果是，拔掉5v供电只用电池就好
作者: wmslecz    时间: 2014-4-9 13:06
Malc 发表于 2014-4-9 08:51
你这波形。。是不是因为你用电脑的5v供电的，如果是，拔掉5v供电只用电池就好

千万次实验最终确定是摄像头的场中断干扰了AD端口.....一直在苦思解决办法中....[attach]58176[/attach]这加速度计陀螺仪的波形都和这个差不多..断开摄像头就好了....谢谢学姐哈..那个JLINK我改成不供电的模式了..O(∩_∩)O~~
作者: wmslecz    时间: 2014-4-10 22:13
Malc 发表于 2014-4-9 08:51
你这波形。。是不是因为你用电脑的5v供电的，如果是，拔掉5v供电只用电池就好

学姐..俺又来了..哈哈..上次的自立摄像头中断都搞定了..现在车子稍微有点抖...都还好...我现在车子倒下60°(我的车设计最大的只有60度的倒角)后都可以直接立起来..这个算立的硬吗??学姐.再问下你一个问题哈..那个速度P参数调出来的情况应该是怎么样的..如果说我拿着车子倒向一边的话..轮子转动的情况是怎么样的?...我现在是直接疯转(倒向哪一边就向哪一边转..但是直立状态下扰动车子能很快的平衡住..不会继续跑)..但是我觉得应该是一个影响了直立的值..应该不是满PWM波..但是实际测试出来还是满波...这个是属于正常情况吗?
作者: Malc    时间: 2014-4-11 10:00
wmslecz 发表于 2014-4-10 22:13
学姐..俺又来了..哈哈..上次的自立摄像头中断都搞定了..现在车子稍微有点抖...都还好...我现在车子倒下60 ...

这个直立效果是不错的，可以调下一步了
车子倒下了电机肯定会疯转的，因为直立输出会达到满幅，你可以做个保护，如果车身角度离平衡点过大，就关掉电机输出
加速度p的效果是，直立状态下不会一直往一个方向加速跑，这个速度pid的调试你可以参考卓晴老师的调试视频，非常详细
作者: wmslecz    时间: 2014-4-11 11:18
Malc 发表于 2014-4-11 10:00
这个直立效果是不错的，可以调下一步了
车子倒下了电机肯定会疯转的，因为直立输出会达到满幅，你可以做 ...

好的好的..谢谢学姐..我再去仔细分析分析..O(∩_∩)O~~
作者: wmslecz    时间: 2014-4-13 10:27
本帖最后由 wmslecz 于 2014-4-13 10:29 编辑
Malc 发表于 2014-4-2 13:27
截止频率9hz

学姐..嘿嘿...我又过来啦..嘿嘿...又来打扰你了....这次又遇见一个问题..一直不知道那个转向陀螺仪拿来干嘛的?我的摄像头描出来的引导线量值乘以一个P参数可以直接给..让车子转动...我看官方文档说是角度D参数是给陀螺仪的....然后就不知道怎么用了....麻烦学姐解答一下啦啦啦...嘿嘿...
作者: Malc    时间: 2014-4-13 11:34
wmslecz 发表于 2014-4-13 10:27
学姐..嘿嘿...我又过来啦..嘿嘿...又来打扰你了....这次又遇见一个问题..一直不知道那个转向陀螺仪拿来干 ...

pid里的d就是微分项，陀螺仪的输出就是转动角速度，也就是转动角度的微分
作者: wmslecz    时间: 2014-4-13 11:55
Malc 发表于 2014-4-13 11:34
pid里的d就是微分项，陀螺仪的输出就是转动角速度，也就是转动角度的微分

没有理解耶..学姐..我就像问下...[attach]58333[/attach]这个里面的D参数带来的实际效果...(如果我的D参数弄的很大..这个效果是什么?)




欢迎光临 智能车制作 (http://111.231.132.190/) Powered by Discuz! X3.2