注册 登录
查看: 10991|回复: 106

[平衡组] 【分享资料和源码】平衡直立组堆积木般的教程和源码(1)

  [复制链接]
发表于 2014-11-2 11:19:49 | 显示全部楼层 |阅读模式
本帖最后由 gsc617 于 2014-11-2 12:45 编辑

强调:作者是大连海事大学电航学社的黄翊峰。源码用的是K60山外编写的新库,废话少说,直接上猛料
目录:

  

前言
本次大赛基本是从0开始做直立,其间有很多经验,也有很多问题,只是写出来能与大家分享,基本就是我的方法的叙述,不能算多么权威的方案,大家以后参考着做,是肯定能把车子立起来走起来的,但如果有更好的方法能让车子走的更稳定,一定要时刻更新!我尽量用最简单的话语表达实现车模直立的步骤,一般初学者肯定是可以跟着做出来的。再次强调一次,以后大家做车,跟着这个方案做出来后,肯定还有大量的时间尝试更多的更好的算法或者思考出自己的更好的想法,一定要补充,这样才能使我们的整体水平一届比一届强吧。
车模直立运行从第七届飞思卡尔开始出现,到现在第九届已经3届了,第七届是电磁直立,第八届光电直立,第九届摄像头直立,传感器不同但对于车模的直立、速度、方向控制是完全相通的,我今年做的摄像头组直立,如果以后的比赛出现了其他组别的直立大家仍然可以参考这个文档使自己的车跑起来。直立车是一门大学问,里边有很多东西自己没能弄清楚,还得等以后大家慢慢完善。
这里说明一些东西,为了阅读方便和对应程序,对陀螺仪加速度计方向做以下规定(这与硬件上的标注也是对应的):
l   1:测量车模倾角那个方向,就是直立PID所用的方向,称为Y轴(变量都是以Y标定的,例如GYRO_Y就是这个方向的角速度)。
l   2:测量车模转弯的那个方向,例如车模转弯产生的角速度,称为X轴。   

硬件安装与机械结构
一个原则就是重心越低越好,越集中越好,重心低会使控制更顺滑,车模极限速度更高,抗干扰能力更强(速度越快,赛道颠簸越大也就是干扰越大,抗干扰能力直接影响着提速过程!),而重心集中则会让车模转弯更加灵活,不需要太大的改变就可以完成较大的转弯,但最好不要随意增加配重,车子太沉会影响很多东西,我曾经尝试过使用配重将车子重心极限的降低(不需要开电就直接立在那里,很重),但带来的影响就是转弯很迟钝,加速很慢(如果想要变速肯定是不利的),最终还是将配重都取消掉了。
机械结构的调试对于智能车是很重要的,首页那张图便是今年拿冠军的哈尔滨工业大学的车子,他们的车据说在实验室跑到了2.8m\s,我的车最快才跑到过2.4,可以看到他们也是加了配重的,但没有我一次性加的那么多,我的机械结构并不是最优的那一种,最终版本的车子过于轻了(基本就是和理工那一组差不多的结构,但比他们的要轻,要低),导致弯道跳轮,坡道飞起,而之前的是过于重了,导致过度迟钝。
总之,机械结构建议最好在一开始就仿照之前大牛成熟的结构(如哈工大那种结构),以上边的原则定下来,这样有助于长时间的调试。我们今年的一大败笔就是机械不停的改变方案,导致控制方案没有太大的进展。
买到车模后(E车模)
今年我们主要做的是E车模,为了不让大家买太多的车模(囧),有必要说说这车的坑爹之处:
1:一定要将所有的内六角螺丝更换成十字螺丝或者淘宝上买航模级的那种优质内六角螺丝(大连电子城李姐家斜对面,K6刚进去左边那家螺丝挺全的都能买到),原车的螺丝质量很差,很容易脱扣,对以后机械的调整很不利。
  2:固定电机的那两个座和地盘连接的那10个螺丝内部很容易脱扣,不到迫不得已不要拆卸这两个地方,大约拆装5次左右这车就废了!
3:电机安装前看把轮子拆掉,看一下旋转轮轴时候那个大的齿轮是不是圆的,如果不圆基本就是大齿轮上的四个螺丝没有拧紧,一定要保证这个旋转的顺滑,基本上都拧紧就好了
4:安装电机的时候,那两个螺丝千万不要直接拧,力气一大螺丝会把塑料挤断,要弄一个垫片!还有就是齿轮之间要有一定的空程(有一点点即可,不能太大了),否则两个轮子阻力很难一样,有空程其实无所谓,跑起来是不受任何影响的。
5:关于安装好电机后旋转轮子产生的咔咔声,如果你留了足够的空程,大齿轮上的螺丝都紧过的话,这种声音是很小的,基本可以忽略。有这种声音基本就是两个齿轮的母线不平行造成的。
6:安装编码器的时候,店家给的齿轮与支架直接安装是无法与大齿轮接触的,需要适当的打磨电机支架部分。就可以无压力咬合了,齿轮咬合还是需要一点点空程。
陀螺仪与加速度计
这两个东西我是用胶枪固定到摄像头杆子上的,如下图所示


需要注意的是安装陀螺仪时一定要保证陀螺仪Y轴与车模轮轴平行!否则会出现转弯速度的改变,这个可以用推力轴承测试,把车模水平放置在推力轴承上,旋转车模,通过上位机观察Y轴的角速度,在车模旋转时候理论应该只有X轴的角速度,不应该有Y轴角速度的,如果有了一定是安装斜了。
这两个东西要保持在一条竖直线上,竖直线要与车轮轴心相交,尽量保持在车子中间,这样可以很大程度减少运动对信号采集的影响,具体怎么影响没有理论上分析过,总之之前试过当两个不再一起,车子出现了剧烈抖动,最后陀螺仪与加速度计还是保持了原来的计划,如果有人可以从理论上解释这俩该怎么安装那一定要添加进来。总之就是要安装的尽可能的正,能让对应的信号最舒服的采集。
电池固定方式
我用了两种方式,一种是用铝片弯折(这种铝片是从大工的一个老爷爷那里要来的,就在大工体育场底下有一个金工的老爷爷,人特别好,也很乐意帮助同学们,铝片硬度适中很适合做支架,加工铝片可以去大活弄,那里的设备比较多),另一种是直接困扎在电路板上,都差不多,铝片那种方式比较复杂,也不容易平衡,但调整好了重心会更低(见哈工大神车的固定方式),或者就直接将驱动板子开的大一些,就和我们车一样,驱动&电源板也是地盘固定板和电池支架,我觉得这个想法还是不错的
下图是我的电池固定方案:


摄像头杆子固定
这个没什么说的,原则就是不要让它晃动就好,各种三角形各种支架。图:

轮胎处理方式:
1:淘宝上买软化剂,各种店铺都有,模型店,这里给个链接,非常给力的液体:
2:打磨,尽量把纹路打磨少了,这个轮子外表皮不均匀,得用细砂纸打磨,具体方法就是将轮子空转,然后用砂纸放上去打磨,看我的都这个样子了:

3:填充,在轮毂里边的两条楞之间,放一个B车轮胎(用小刀修到合适大小正好放进去的),然后把海绵拆掉(否则轮子不圆!)。
4:粘住,用3m胶水将轮胎与轮毂彻底粘死,这样会让轮子更结实,摩擦力更强
胶水链接:
这样处理下来基本不会打滑了,我提速的瓶颈不像别人在摩擦力,摩擦力足够大到翻车。
加速度计与陀螺仪信号采集(模块说明&程序)陀螺仪模块简介(最开始用的ENC03)
这算是使用最成熟的飞思卡尔陀螺仪了,最一开始也是用它做的,输出模拟量,操作很简单,直接供电后用示波器就可以观察波形。说到程序就直接AD采集呗,但缺点也很多,温漂和线性度都不如大多数数字陀螺仪好。尤其温漂很大。不建议使用,但这个用着真是简单,特别方便。所以还是提一下了。
陀螺仪模块简介(最终使用的L3G4200D)
便宜、木有温漂,总之这个陀螺仪特别好,我们是在比赛的中后期换的,使用模拟I2C通信(无视上边的SPI接口吧,我们用SDA和SCL那两根线和电源线接入单片机就可以了),换了以后车子运行明显更加平稳了(应该是线性度的问题),建议直接使用这个型号的陀螺仪吧,亲测很给力。
加速度模块简介(MMA7361)
这个也很简单,配置两路AD即可,需要注意的是这货有个使能端,就是SL那个管脚,要给他接到高才能用。否则啥都采集不到的
MMA7361程序详解&加速度计得到角度方式
说明一下我的程序写得不是规范,代码全部都分布在两个文件中,只是为了自己调整的方便,没有考虑可读性神马的,代码基本上都在main.c中,只有Get_Ang函数(得到角度、调整PID)和Calculate_Speed函数(测速)还有控制电机的函数Go_S在Read_Data.c中。
首先是加速度计的AD采集:
第一步:在初始化部分中,初始化AD
adc_init (ADC1_SE6a);                                    //初始化ADC1 采集陀螺仪和加速度计传回来的值
adc_init(ADC1_SE7a);
第二步:配置一个定时器,用于定时采集并计算角度、速度等值
pit_init_ms(PIT0,1);                                   //初始化PIT0,定时时间为:1ms
set_vector_handler(PIT0_VECTORn,PIT0_IRQHandler);      //设置PIT0的中断服务函数
enable_irq(PIT0_IRQn);                              //使能PIT0中断
PS:这是野火最新库的中断初始化方式。
第三步:在定时器中断服务函数中,运行Get_Ang函数计算角度,在这一步我们先只是采集加速度计的值
Get_Ang();//1ms调整一次姿态
第四步:编写Get_Ang函数(在我的工程中,这个函数在Read_Data.c中),在这个函数中运行AD采集函数,得到两路加速度的值,并用atan2函数(在math.h中)计算通过加速度计得到的角度值。
PS:计算角度,有几种方法,首先说一下math.h中,atan函数原型是doubleatan(double x),就是直接求x的反正切,还有一个atan2,它的原型是double atan2(double y, double x), 求角度方法如下图,

如果粉色是z轴,蓝色是x轴,竖直线为0位置,当车模处于0位置时候,x轴为0,z轴为g,那么当它有角度  时,可用以下方式求角度:
1:ang=arctan(x/z)
2:ang=arcsin(x/g)
3:ang=arccos(z/g)
用第二种和第三种要求ad采集到的值再乘以一个系数或者自己设置g的值才可以求出角度,而第一种直接ad值就可以用了,而且用上了两个方向的传感器的值,也会更准一些,所以我用了atan2函数。
uint16ADresult;  //保存ADC转换结果
/******读取一次 ******/
ADresult    =  adc_once(ADC1_SE6a, ADC_16bit);        //读取 ADC1_SE6a ,16位精度
A_X=ADresult-22000;
ADresult    =  adc_once(ADC1_SE7a, ADC_16bit);        //读取 ADC1_SE7a ,16位精度
A_Z=ADresult-28000;
PS:AD减去那个值是实验出来的,当加速度计水平时读数应该为0,而AD采集的范围是0到65535,所以就要减去一个系数喽。
然后计算角度:
Ang_Acc=173*atan2(A_X,A_Z);//角度  
PS:前边乘着的那个系数是将这个数放大一下让他基本符合角度,可以叫做灵敏度系数,由于atan2出来的值是弧度,太小了不利于和一些整型数计算所以才这么弄。
L3G4200D程序详解&陀螺仪积分得到角度方式
L3G4200D通信采用I2C协议,具体协议没有必要背会,直接调用成熟的模块即可,我们用的是大工写的IIC模块,其间有些小插曲,之前用自己以前在STM32上用的IIC程序没调出来,不知道为什么,然后大工的车友帮着调,用他的程序就调好了,我看了看真没啥区别,就是IO口用的不一样,他用的我板子上用作串口的那两个口(A14和A15),我就试着把管脚换到之前预留的IO口(之前用模拟陀螺仪,AD口,E2和E3)上,然后怪了,又出问题了,读数很奇怪,要么是-1,要么是乱七八糟的数字,结论就是那俩IO口有问题,不知道为啥!!!换了IO口就好了!!!然后解决方案就是把加速度那两个口用来i2c了,陀螺仪那两个口还是用作AD,接了模拟加速度计。。。
IIC移植完毕后直接调用函数,就可以采集到X 与Y 方向的角速度值了:
GYRO_X=Get_Gyro(1,'X')-30;                                   //用于转弯的角速度
GYRO_Y=Get_Gyro(1,'Y')-20;                           
直接调用Get_Gyro函数即可,后边的常数是0偏值。由于是模拟IIC,运行速度取决于IIC.h中最上边nop5与nops后边nop指令的数量(延时),延时越短,运行越快。默认的这么多已经足够快了。
得到角速度后,乘一个系数得到真正的角速度后积分:
Ang_IGyro=Ang_IGyro+GYRO_Y*0.00017;//积分得到的角度
这个系数也是通过上位机调出来的,直到积分出来的角度大致符合真实的角度。
至此,两种角度值就都获得了,然后想要得到更精准的角度值需要经过互补滤波。
PS:模拟陀螺仪的采集方式和模拟加速度计采集一样的,这里就不叙述了。

没完:续贴http://www.vcan123.com/forum.php?mod=viewthread&tid=6496也就是本帖的一楼
附件在续贴里





本帖被以下淘专辑推荐:

回复

使用道具 举报

 楼主| 发表于 2014-11-2 11:21:10 | 显示全部楼层

【分享资料和源码】平衡直立组堆积木般的教程和源码(2)

本帖最后由 gsc617 于 2014-11-2 11:22 编辑

续贴:


互补滤波-一种很好的控制思想
  两种获取角度的方式及他们的特点
使用加速度计获得角度(上文中的Ang变量),毛刺很多,而且受运动影响极大,一旦运动起来,加速度计会将运动的震动体现出来,毛刺更是多,如图:

        
加速度计获得的角度波形

   
   
  之前调试时没有截图,便取材其它资料了,此图来自第七届官方直立方案,这幅图中蓝色就是通过加速度计获得的角度波形,红色是实际的角度,一旦运动,毛刺会更多,无法直接用它控制电机的。
而用角速度积分得到的角度,则没有这么多的毛刺,也不会受到运动的影响,但是陀螺仪有温漂,角速度的静偏差,导致这个角度值会一直不停地增大或者减小
  互补滤波思路图解
Ang_Acc整体趋势的正确使其可以准确反映角度变化的正确走势,于是将其与Ang_IGyro做差就得到了一个误差值,这个值就是陀螺仪此时的静误差,将这个误差值修复到Ang_IGyro上,得出来的角度值就相对准确了,这就是互补滤波的思路,效果如下图:

黄色是Ang_Acc红色是滤波后的输出,这个角度完全可以用作PID的位置值了。

PS:官网方案与之略有不同,偏差值不是直接修复到角度上,而是修复到陀螺仪采集值,效果是一样的,官网的框图是比较规范的控制流程图,如下图:

下面是我的程序,直接用偏差修复角度的,整体来说还是比较简单的:
//互补滤波
Ang_Error=Ang_Acc-Ang_IGyro;
Ang_IGyro=Ang_IGyro+Ang_Error*0.0005;
//绿的那个参数越大曲线跟随效应越好但是毛刺越多需要调
Ang=Ang_IGyro;
PS:互补滤波在1ms中断函数中,实时得出角度
得到Ang_Error后,将其乘以一个系数修复到Ang_IGyro上,就可以消除Ang_IGyro的静差了,那个系数越小,曲线修复的越少,也就是说反应越迟钝,曲线越圆滑,当车子有突然变化时,角度不是很突然的改变,而是慢慢的变化过去,当系数较大时候,最终曲线更加跟随加速度曲线,当车子有突然变化时候,角度也会很快的改变过去,但有时候就像加速度计一样有一些毛刺,大家做的时候可以试试这两种特点的曲线,建议这个值还是小一些,这样曲线会更加的平稳,陀螺仪积分得到的角度成分更大,而这个算法是一个积累过程,在上电时只要加速度计得出的角度也在0附近(将车子摆到平衡位置上电即可),很快曲线就可以跟随过去。如果上电时没有摆正,在上电的一瞬间加速度得出的角度是其真实角度,而陀螺仪从0开始积分,得出的角度是不准确的,需要互补滤波中加速度计的成分逐渐将其修复过去,此处大家可以通过上位机看看波形我就不一一截图了,还是比较简单的。
电机控制
使用K60内部FTM模块发出PWM波控制电机,对于直立车推荐使用成熟的电机驱动芯片,因为这些芯片线性度更好,更稳定,没有必要像趴着的车那样追求加速性能而用MOS管自己搭H桥电路,4块BTN7971就足够稳定足够用了。
    这里需要看一下K60库中对FTM的一些描述,电机控制需要协调好管脚的,控制电机的PWM频率是3Khz(不知道是不是最佳,最优频率需要根据电机参数计算的,反正这个值科学),只需要一种频率,而需要4种不同的占空比,所以控制两个电机只需要一个FTM模块,一个拥有4个通道输出的FTM模块,而K60的FTM模块如下(在FIRE_PORT_cfg.h中):
/**********************************  FTM   ***************************************/

//      模块通道   端口          可选范围              建议
#define FTM0_CH0    PTC1       //PTC1、PTA3            PTA3不要用(与Jtag、SWD冲突)
#define FTM0_CH1    PTA4       //PTC2、PTA4
#defineFTM0_CH2    PTA5        //PTC3、PTA5
#defineFTM0_CH3    PTA6        //PTC4、PTA6
#defineFTM0_CH4    PTA7        //PTD4、PTA7
#define FTM0_CH5    PTD5       //PTD5、PTA0            PTA0不要用(与Jtag、SWD冲突)
#define FTM0_CH6    PTD6       //PTD6、PTA1            PTA1不要用(与Jtag冲突)
#define FTM0_CH7    PTD7       //PTD7、PTA2            PTA2不要用(与Jtag冲突)


//      模块通道   端口          可选范围              建议
#defineFTM1_CH0    PTA8       //PTA8、PTA12、PTB0
#defineFTM1_CH1    PTA13       //PTA9、PTA13、PTB1

//      模块通道   端口          可选范围              建议
#defineFTM2_CH0    PTB18       //PTA10、PTB18
#defineFTM2_CH1    PTA11       //PTA11、PTB19

//正交解码模块通道  端口         可选范围              建议
#defineFTM1_QDPHA  PTA12       //PTA8、PTA12、PTB0
#defineFTM1_QDPHB  PTA13       //PTA9、PTA13、PTB1

#defineFTM2_QDPHA  PTA10       //PTA10、PTB18
#defineFTM2_QDPHB  PTA11       //PTA11、PTB19

从上面能看出FTM0有8个通道(!!!),而FTM1和FTM2都是各有2个,所以FTM0正好可以用来控制电机,而FTM1和FTM2以后用来测速更合适。
然后就是首先初始化FTM0的4个通道:
   FTM_PWM_init(FTM0, FTM_CH4, 3000, 0);                  //四路pwm去电机
   FTM_PWM_init(FTM0, FTM_CH1, 3000, 0);
   FTM_PWM_init(FTM0, FTM_CH2, 3000, 0);
   FTM_PWM_init(FTM0, FTM_CH3, 3000, 0);
初始化为3k,%0,这个比较简单不多说,初始化了后就可以通过使用FTM_PWM_Duty函数改变各个通道的频率来控制电机的转速了,我编写了Go函数,接受一个数字,如果是正正转,负则反转,方便用PID调整时直接出来的值就可以给到函数中去做出正确的运动。Go函数中的代码是:
If(Speed>0)
  {
     FTM_PWM_Duty(FTM0,FTM_CH2,(uint32)Speed);
     FTM_PWM_Duty(FTM0,FTM_CH4,(uint32)Speed);
     FTM_PWM_Duty(FTM0,FTM_CH3,0);
     FTM_PWM_Duty(FTM0,FTM_CH1,0);
  }
  else if(Speed<=0)
  {
              FTM_PWM_Duty(FTM0,FTM_CH2,0);        
FTM_PWM_Duty(FTM0,FTM_CH4,0);
FTM_PWM_Duty(FTM0,FTM_CH3,0+(uint32)(0-Speed));           FTM_PWM_Duty(FTM0,FTM_CH1,0+(uint32)(0-Speed));

  }
嗯,就是这些,这样就可以动了,至于硬件连线就不用多说了吧。
PID与车模直立原理调整角度0位置值
在直立之前,需要先调整一下角度的0位置值,更改一下上面的加速度计获取角度那句话:
Ang_Acc=173*atan2(A_X,A_Z) -Ang_Set;//角度  
Ang_Set是一个常数,可以用小键盘调整,调整方式后边有解释,这个变量的作用主要是修复加速度计的安装误差,如果加速度计的竖直方向没有与车模平衡位置竖直方向平行,那么车模处在平行位置时测得的角度不是0,所以需要加上一个变量将这个误差修复,而实际上由于器件难以避免的温漂,这个值是需要经常调整的,否则会造成车子直立时的前进或后退,也会影响车模运行的速度与设定速度产生静误差。
这一步虽然重要但相当简单,只需要将车模摆在平衡位置,看一下上位机就能知道这个值取什么了。最终的结果就是车模处在平衡位置时角度输出要是0。
直立原理简介
这里再简单说一下车模可以直立的原理,还是比较好动的,官网也有详细的理论分析,我就几句话讲一下这原理吧,没有那么复杂那么乱的,理解一下也方便调试:
首先大家想一想单摆,为什么空气中的单摆拉起来后可以很快的静止下来。
这里边有两个条件,第一是有一个回复力拉着他向平衡位置运动。
而只有这一个条件是不够的,它会不停的做往复运动,还有一个条件就是空气阻力,是因为阻力的存在才使单摆停下来。
而我们的车模基本上就是一个倒过来的单摆了,如果想让车模直立住(停留在平衡位置),需要一个回复力,而此时重力提供的回复力是远离平衡位置的,也就是和我们想要的力相反,于是这就需要电机提供一个回复力了,这个力就是kp*Ang。
如果大家把这个值直接给电机,你会发现当kp值合适时,车子会立住的(机械结构要好要不然很难调),用手给车模一个干扰,它会前后摆动,最终再次回到平衡。回到平衡的原理就是它有一个回复力了,而空气和机械摩擦又提供了一点点的阻尼力,所以他可以震荡着回去。
阻力的作用就是减速,刚才只加了kp*Ang,阻力完全靠空气和机械摩擦,如果我们再把kd*GYRO_Y加到电机上就相当于用电机提供了一个令其减速的阻尼力,这就是D,当D合适时,车子就根本倒不了了,最终的效果就是只要轮子在地上它肯定不会倒否则肯定是程序有bug了。
上面就是简单的原理简介,看过后应该大致能知道直立车直立的原理了,详细的理论分析请见第七届官方直立指南,我这个是白话,人家那个才专业呢。
直立程序(就一句话)
将上面的那些写成程序就是这一句话了,同样放在1ms中断中,1ms调整一次姿态:
Speed_L = (0-Ang)*35 -GYRO_Y*0.27;//直立PID
Go(Speed_L);
这就是让车子直立起来的那句话,35是kp,0.27是kd,调整这两个参数即可让车模非常稳定的直立。
调整方式:首先将kd设为0,调整kp使车子能够直立,然后用手按倒它可以很快的立起来再反复震荡几次再立在哪里。Kp其实很小就可以立住,然后给kd,直到调整到你觉得很稳定为止。对于直立应该硬还是软,我是取个折中,因为都有好处,不是越硬越好的。
调整角度零位值与为什么不用自动校正
这里在说一下自己调了一星期的自动校正零位,最后没有成功:
首先这个校正需要刚开始就摆到很正的位置,读取一个角度值后设定为Ang_Set,校正后如果一开始拜访的有误差,车子角度略有误差而摩擦力又足够让其停在赛道上,这个误差就无法消除,所以在这里不建议大家搞自动校正,除非有更好的算法能彻底消除器件的温度漂移。用两个按键来调整是最好最方便的。
速度控制!让车子自己走起来
直立车的速度控制如果从理论的角度分析会很复杂,具体的理论分析参见第七届官方直立指南,我这里只是说一下程序中的实现方法,大家照着这个来即可,还有一些我自己的调试经验,理论部分我也没弄清楚,真的比较复杂,大家有兴趣可以去研究,实现方式其实也不难,首先是要得到两个轮子的转速:
编码器简介
注意要一并买对应的齿轮与支架,要不不好安装,这种编码器的好处一是体积小,质量轻,二是有一个管脚直接输出高低来表示旋转方向,不需要配置正交解码,具体管脚如下:


只有前四个管教有用。旋转方向输出高低电平表示了旋转方向。
测速程序讲解
也是用k60的FTM模块,这时候用FTM1和FTM2(PS,测速不能用一个模块的两个通道,因为用的是它的捕获模块)。
首先初始化两路FTM和中断函数:

FTM_Input_init(FTM1,FTM_CH0, FTM_Falling);              //初始化FTM输入捕捉模式(用于第一路测速)
    port_init_NoALT(FTM1_CH0 ,PULLUP);
            set_vector_handler(FTM1_VECTORn,FTM1_INPUT_IRQHandler);//设置FTM1的中断服务函数
    enable_irq (FTM1_IRQn);

    FTM_Input_init(FTM2, FTM_CH0,FTM_Rising);              //初始化FTM输入捕捉模式(用于第二路测速)
    port_init_NoALT(FTM2_CH0 ,PULLUP);
            set_vector_handler(FTM2_VECTORn,FTM2_INPUT_IRQHandler);//设置FTM2的中断服务函数
    enable_irq (FTM2_IRQn);

初始化两个IO口用来读取旋转方向:
gpio_init  (PTB10, GPI, 0);                             //初始化 PTB10(第一路测速方向位)
gpio_init  (PTB11, GPI, 0);                             //初始化 PTB11(第二路测速方向位)

然后编写两路输入捕捉的中断函数(以一路为例):
voidFTM1_INPUT_IRQHandler(void)    //第一路测速,右轮
{
   uint8 s = FTM1_STATUS;          //读取捕捉和比较状态     
uint8CHn;

   FTM1_STATUS = 0x00;             //清中断标志位

   CHn = 0;
   if( s & (1 << CHn) )
   {

        /*    用户任务       */
        if(gpio_get(PTB10))            
       {
          RightWheel_Count--;
       }
       else
       {
          RightWheel_Count++;
       };

        /*********************/
   }


}

上面的函数里边,关键只有一句话,就是让RightWheel_Count自增或自减,这个变量是全局变量,两路测速不停的捕获,配合定时器中断服务函数中的速度计算函数就可以不停的得出车轮转动速度了,同样用1ms定时器中断来计算,不过设置了时间标志位,50ms测速一次,在1ms中断服务函数中的代码如下:
time_counter1++;                     //时间标志位
    if(50==time_counter1)                //50ms测一次速度
    {
      Calculate_Speed();                 //测速函数,展开是:
Check_Speed_Left=(LeftWheel_Count+RightWheel_Count)/2;  //平均左右轮的数据,得到切向的速度
                LeftWheel_Count=0;//清零测速数据,等于是每50ms计数一次
            RightWheel_Count=0;
      D_Check_Speed_Left=Check_Speed_Left-Check_Speed_Left_Last;  //速度的D
      Check_Speed_Left_Last=Check_Speed_Left;//储存本次速度,下次用来算D
      I_Check_Speed_Left+=Check_Speed_Left;  //速度的I
      time_counter1=0; //标志位清零
    }
这样获得的Check_Speed_Left就是车子运行的速度了,所谓速度控制就是用这个速度反馈了,具体原理参考官方指南,我只是照着官网的方案,自己摸索了一个比较简单的程序实现方法,实测还是好用的,首先在获得速度的代码处加一个变量作为速度设定值:
Check_Speed_Left=(LeftWheel_Count+RightWheel_Count)/2-Speed_Set_Left;
这个值是目标速度(不要在意那个Left),设定的值就是车子所要达到的速度值。下一步是第二层PID,放在Get_Ang函数中:
Speed_L=Speed_L-4.0*(float)(Check_Speed_Left)-0.0*(float)D_Check_Speed_Left-(0.001)*I_Check_Speed_Left;
函数中,此处为增量式PID,调整P可以使车子往前走达到目标速度,I可以消除静误差,D可以消除震荡,直接用即可,速度控制后人还需要再好好研究,我是没搞懂具体的控制原理,只是跟着官网的方案,不停的调参数。下面我只能说一说调整参数的时候的一些现象了:
首先是P,就是上边的4.0,这个值小了加速特别慢,而且加减速,P越小,加速越慢,加减速周期越长,P大了有一个值可以使之匀速运行,再大就开始晃动了,而且这回是越晃越大直到摔倒!我是调到正好匀速那里,一点不晃动,也不加减速(可能机械比较稳定,所以一个P可以匀速走的)
D的话加上一点可能抗干扰?尝试过改变它但总觉得效果很乱,最后实验出了个范围来,真不懂D改怎么给
I比较奇怪,I大了开始起步过冲很明显!!!I小了有时候上下午速度不一样,而且有一段时间发现I的大小会影响速度!
车模方向控制
很简单,官网方案,通过一个Turning变量控制转弯力度,这个变量通过你的传感器检测的赛道信息得出,通过X轴陀螺仪反馈消除转弯的过冲,代码如下:
FTM_PWM_Duty(FTM0,FTM_CH3,0+((uint32)(0-Speed-4.7*Turning+0.035*GYRO_X)));     
FTM_PWM_Duty(FTM0,FTM_CH1,0+((uint32)(0-Speed+4.7*Turning-0.035*GYRO_X)));
两个轮子一个加,一个减,就实现了差速转弯,这个值直接加到电机输出即可。通过调整PD值控制转弯的走线。
需要注意的是直立车的转弯比较迟缓,所以过冲严重,刚开始调整时先将D弄成0,然后给P,通常这样会走直线时不停的左右晃动,这时候就要加上D了,直到它不再晃动,然后过第一个弯的时候如果出现了转向不足则是D大了或者P不足,理论上D是有一个预判的作用,转弯调整的很硬,就是PD参数都很大,走线可以非常给力,小S弯直走,但由于预判过于明显会导致小弯忽略,十字出问题(除非十字处理的很稳定).还是需要大家权衡的。
尾声
2014年8月9日星期六于大连海事大学电航学社。作者黄翊峰。

最后附上原链接地址:http://www.9mcu.com/9mcubbs/forum.php?mod=viewthread&tid=1063884和附件

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复 支持 3 反对 0

使用道具 举报

发表于 2014-12-7 20:36:47 | 显示全部楼层
gsc617 发表于 1417955737
模拟的可能稍微简单一些
温飘飘的脑袋疼!正在参照这个模块看看
来自PC客户端 来自PC客户端
回复 支持 1 反对 0

使用道具 举报

 楼主| 发表于 2014-11-2 11:22:24 | 显示全部楼层
@山外メ雲ジ山外哥这个帖子必须是精华了吧不做硬盘控,只收集精华,打造最强势的堆积木教程,让智能车越来越简单。
回复 支持 1 反对 0

使用道具 举报

发表于 2014-12-7 16:32:31 | 显示全部楼层
陀螺仪模块简介(最终使用的L3G4200D)
便宜、木有温漂,总之这个陀螺仪特别好!
真的木有温飘?
回复 支持 0 反对 1

使用道具 举报

发表于 2014-11-17 21:51:13 | 显示全部楼层
Object not found!

The requested URL was not found on this server. The link on the referring page seems to be wrong or outdated. Please inform the author of that page about the error.

If you think this is a server error, please contact the webmaster.

Error 404

www.9mcu.com
Mon 17 Nov 2014 09:52:28 PM CST
Apache/2.2.14 (Unix) DAV/2 mod_ssl/2.2.14 OpenSSL/0.9.8l PHP/5.3.1 mod_apreq2-20090110/2.7.1 mod_perl/2.0.4 Perl/v5.10.1
回复 支持 1 反对 0

使用道具 举报

发表于 2014-11-2 11:36:54 | 显示全部楼层
gsc617 发表于 2014-11-2 11:22
@山外メ雲ジ山外哥这个帖子必须是精华了吧不做硬盘控,只收集精华,打造最强势的堆积木教程,让智能车越来 ...

不错的资料,谢谢分享啊,收集一下淘贴,加精。
回复 支持 反对

使用道具 举报

发表于 2014-11-2 11:50:23 | 显示全部楼层
gsc617 发表于 2014-11-2 11:23
没完:续贴地址http://www.vcan123.com/forum.php?mod=viewthread&tid=6495请移步
附件在续贴里

我把帖子合并一下,方便大家看帖子。
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-11-2 12:49:54 | 显示全部楼层
山外メ雲ジ 发表于 2014-11-2 11:50
我把帖子合并一下,方便大家看帖子。

好的,我还在纠结怎么发这么多字的帖子呢,看来以后还是放在评价回复下面吧。
回复 支持 反对

使用道具 举报

发表于 2014-11-2 12:50:53 | 显示全部楼层
gsc617 发表于 2014-11-2 12:49
好的,我还在纠结怎么发这么多字的帖子呢,看来以后还是放在评价回复下面吧。

放到2楼帖子就好。
来自安卓客户端来自安卓客户端
回复 支持 反对

使用道具 举报

发表于 2014-11-2 14:57:08 | 显示全部楼层
正需要中!!!
回复 支持 反对

使用道具 举报

发表于 2014-11-2 16:09:51 | 显示全部楼层
谢谢楼主的分享,赞一个
回复 支持 反对

使用道具 举报

发表于 2014-11-2 16:27:03 | 显示全部楼层

果然是猛料啊!
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-11-2 17:10:54 | 显示全部楼层
乁_year 发表于 2014-11-2 16:27

果然是猛料啊!

下了人家的资料然后顶一下帖子,这是一种礼仪
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-11-2 17:11:27 | 显示全部楼层
7swkr4795z 发表于 2014-11-2 16:09
谢谢楼主的分享,赞一个

下了人家的资料然后顶一下帖子,这是一种礼仪
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-11-2 17:11:57 | 显示全部楼层

不谢, 不过下了人家的资料然后顶一下帖子,这是一种礼仪
回复 支持 反对

使用道具 举报

发表于 2014-11-2 18:08:59 | 显示全部楼层
靠 这么吊 我们怎么办
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-11-2 18:20:48 | 显示全部楼层
秦时明月94 发表于 2014-11-2 18:08
靠 这么吊 我们怎么办

你也可以成为大神,只要你愿意和努力我也正在努力
来自安卓客户端来自安卓客户端
回复 支持 反对

使用道具 举报

发表于 2014-11-3 14:03:21 | 显示全部楼层
满怀感激的收下了
回复 支持 反对

使用道具 举报

发表于 2014-11-11 14:41:00 | 显示全部楼层
其实我还是不明白为啥角速度乘kd是阻尼力
来自安卓客户端来自安卓客户端
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-11-11 16:25:15 | 显示全部楼层
Ray______ 发表于 2014-11-11 14:41
其实我还是不明白为啥角速度乘kd是阻尼力

这是不严格的一种关系,很多公式的一些不变参数或者从某种角度看就可以不严格的忽略。仅仅参考
回复 支持 反对

使用道具 举报

发表于 2014-11-11 19:43:56 | 显示全部楼层
gsc617 发表于 2014-11-11 16:25
这是不严格的一种关系,很多公式的一些不变参数或者从某种角度看就可以不严格的忽略。仅仅参考

还是没明白是微分影响小然后就找角速度来微分吗
来自安卓客户端来自安卓客户端
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-11-11 20:20:40 | 显示全部楼层
本帖最后由 gsc617 于 2014-11-11 20:22 编辑
Ray______ 发表于 2014-11-11 19:43
还是没明白是微分影响小然后就找角速度来微分吗

那就别管了,就像pid算法一样,很多人一开始怎么也无法理解,用着用着就开窍豁然开朗了

当然用自己的理解编程最好了
回复 支持 反对

使用道具 举报

发表于 2014-11-11 20:43:17 | 显示全部楼层
gsc617 发表于 2014-11-11 20:20
那就别管了,就像pid算法一样,很多人一开始怎么也无法理解,用着用着就开窍豁然开朗了

当然用自 ...

总是有股想了解透彻的心,嘿嘿
来自安卓客户端来自安卓客户端
回复 支持 反对

使用道具 举报

发表于 2014-11-12 15:51:01 | 显示全部楼层
楼主,请问你那个哈工大的帖子是在哪里找到的,能发个链接吗
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-11-12 16:12:30 | 显示全部楼层
bill 发表于 2014-11-12 15:51
楼主,请问你那个哈工大的帖子是在哪里找到的,能发个链接吗

哈工大的帖子????我没发过y
回复 支持 反对

使用道具 举报

发表于 2014-11-12 16:27:26 | 显示全部楼层
好资料,不得不顶,谢谢分享啦
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-11-12 17:05:03 | 显示全部楼层
徐清馨 发表于 2014-11-12 16:27
好资料,不得不顶,谢谢分享啦

拿了人家的资料然后顶一下帖子,这是一种礼仪。不谢,那就好好研究吧。
回复 支持 反对

使用道具 举报

发表于 2014-11-15 17:27:57 | 显示全部楼层
顶顶。。。。。。。。。。。。。
回复 支持 反对

使用道具 举报

发表于 2014-11-19 20:21:55 | 显示全部楼层
好帖子,值得一赞!
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-11-19 20:28:50 | 显示全部楼层
200835421 发表于 2014-11-19 20:21
好帖子,值得一赞!

耐心点调吧,不用着急的
回复 支持 反对

使用道具 举报

发表于 2014-11-19 22:21:56 | 显示全部楼层
要是在有点图像处理的讲解就更好了
回复 支持 反对

使用道具 举报

发表于 2014-11-20 22:29:16 | 显示全部楼层
关于轮胎那里不太清楚,你用的是新B车的轮胎还是老b车那种海绵轮胎
来自安卓客户端来自安卓客户端
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-11-20 22:34:03 | 显示全部楼层
tongzd 发表于 2014-11-20 22:29
关于轮胎那里不太清楚,你用的是新B车的轮胎还是老b车那种海绵轮胎

新的车子我不知道,帖子是第九届的车子
来自安卓客户端来自安卓客户端
回复 支持 反对

使用道具 举报

发表于 2014-11-20 22:47:22 | 显示全部楼层
就是你帖子里面提到的剪了一块b车轮胎填充到里面,是第九届用的那种b车轮胎吗
来自安卓客户端来自安卓客户端
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-11-20 22:53:24 | 显示全部楼层
tongzd 发表于 2014-11-20 22:47
就是你帖子里面提到的剪了一块b车轮胎填充到里面,是第九届用的那种b车轮胎吗

这不是我的车子,自己细细看看吧。我已经没空在深入探讨这些问题了抱歉
来自安卓客户端来自安卓客户端
回复 支持 反对

使用道具 举报

发表于 2014-11-25 12:56:34 | 显示全部楼层
楼主好厉害啊
回复 支持 反对

使用道具 举报

发表于 2014-11-26 17:35:03 | 显示全部楼层
为啥点不开额
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-11-26 18:33:14 | 显示全部楼层

什么意思,,,
回复 支持 反对

使用道具 举报

发表于 2014-11-27 02:19:09 | 显示全部楼层
gsc617 发表于 2014-11-26 18:33
什么意思,,,

链接点击打不开~
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-11-27 08:36:03 | 显示全部楼层

我这里能打开,假如你那边还是不能就不用看原帖了吧。都全部转过来了,做成了附件了
回复 支持 反对

使用道具 举报

发表于 2014-11-27 18:34:24 | 显示全部楼层
gsc617 发表于 2014-11-27 08:36
我这里能打开,假如你那边还是不能就不用看原帖了吧。都全部转过来了,做成了附件了

嗯  好的   谢啦~
回复 支持 反对

使用道具 举报

发表于 2014-11-28 13:11:08 | 显示全部楼层
为何加了指定速度,还是直立状态
来自安卓客户端来自安卓客户端
回复 支持 反对

使用道具 举报

发表于 2014-11-28 13:14:42 | 显示全部楼层
Ray______ 发表于 2014-11-28 13:11
为何加了指定速度,还是直立状态

调速度,无非就是调小车的倾斜角度。
回复 支持 反对

使用道具 举报

发表于 2014-11-28 13:16:58 | 显示全部楼层
7swkr4795z 发表于 2014-11-28 13:14
调速度,无非就是调小车的倾斜角度。

我参考的时帖子里的这种速度控制,给定速度的值给好大,速度都不明显
来自安卓客户端来自安卓客户端
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-12-7 18:19:44 | 显示全部楼层
ii童话Bū说话 发表于 2014-12-7 16:32
陀螺仪模块简介(最终使用的L3G4200D)
便宜、木有温漂,总之这个陀螺仪特别好!
真的木有温飘?

哪有绝对没有温飘的,陀螺仪一般都有温飘吧。
来自安卓客户端来自安卓客户端
回复 支持 反对

使用道具 举报

发表于 2014-12-7 18:21:50 | 显示全部楼层
gsc617 发表于 1417947584
哪有绝对没有温飘的,陀螺仪一般都有温飘吧。
貌似也有人说数字的没有温飘!刚才还有人说……
来自PC客户端 来自PC客户端
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-12-7 20:28:17 | 显示全部楼层
ii童话Bū说话 发表于 2014-12-7 18:21
貌似也有人说数字的没有温飘!刚才还有人说……

那我不知道了,,,没温飘最好不过了
回复 支持 反对

使用道具 举报

发表于 2014-12-7 20:29:26 | 显示全部楼层
gsc617 发表于 1417955297
那我不知道了,,,没温飘最好不过了
你用的数字的还是模拟的!你是不是参加过了啊!直觉告诉我你参加过了!
来自PC客户端 来自PC客户端
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-12-7 20:35:37 | 显示全部楼层
ii童话Bū说话 发表于 2014-12-7 20:29
你用的数字的还是模拟的!你是不是参加过了啊!直觉告诉我你参加过了!

模拟的可能稍微简单一些
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则

快速回复 返回列表 返回顶部