第九届飞思卡尔，调到现在才想起来总结，确实有点不应该。

之前和学长吃饭，飞哥和我说要把自己的成就(其实都微不足道啊)或者问题(关键是问题啊啊)与大家分享出来，自己也认真的记下了，于是乎从现在开始打算写点博客分享自己的经验了，其实才大二自个儿水平太差，只是分享出来，看看大家的吐槽，也能进步很多吧。

废话不多说了，大一上学期曾经做过一次飞思卡尔但由于水平真的有限调到头发都白了也没有过校赛，大一做的摄像头是趴着跑的4轮车，今年我们做的是摄像头直立组，难度大了很多，但一年的学习也让我们敢于挑战了吧，从去年冬天开始做，做废了3台车，曾经跑到国一的速度，但是没有考虑坡道与障碍、人字，由于底盘太低无法过坡道，被动的改了机械，就跑的不好了，从5月份到现在一个多月了，都很差，这之间换了一台车，改了电路板，程序没有大的改动因为一直没能跑到之前的水准，下面开始分析几个原因

1：之前跑到2米2时候，赛道相对简单，连续弯道只有一处(此处也是速度难以提升的瓶颈之一，经常在此处冲出跑道)也不是很难，而后两次的赛道均有较多的连续弯，欧姆弯，走线稍有不好就会多个弯道放大误差导致冲出跑道

2：车子机械迟迟定不下来，重心想要低，过坡道还不能刮底盘，上一台车尝试了用配重调低了重心结果导致了车子笨重走线很难调整，现在想想不一定是笨重的原因，之前赛道简单，用一条图像信息就跑的很好，而赛道复杂了后对这方面的算法要求更高了，所以不一定是配重的问题了，但由于还有不到一个月了，也真的不打算再改结构了，现在的结构比较折中，车也不重。

3：陀螺仪温漂！不知道算不算问题每次开始都手动调，打算这几天把开机自检搞定，横向陀螺仪温飘会影响转弯的反馈，所以走线也和这个有关！之前试过用两个编码器速度差来计算角速度反馈转弯，但站立时候总觉得不稳定，车子一有后退转向就乱，可能是bug，还没解决这个bug也不清楚哪里错了(排了很久了！！！)，还是乖乖开机自检吧。

4：坡道！过坡道车子直接前倾加速，不知道是不是我的控制速度不够快，上坡会使车子速度下降然后系统使车子加速，结果控制滞后了，加速时候车子已经到了坡上，下坡就冲的更快了根本调整不过来，之前的经验是速度控制越硬过坡道越难，速度控制相对软过坡道就容易些，于是写了一个上坡检测，当车子上坡由于速度反馈会让其前倾，角度加大，角度加大的时候就关掉速度控制1秒，这样速度在1.7左右的时候没问题，但速度再加上来就又悲剧了。有大神建议坡道不处理，但调参数真的过不去啊么么哒。可能还是控制滞后的问题，我在想如果用摄像头检测，提前关闭速度控制应该就会顺滑很多了的，这也是最近得做的

5：图像处理，之前用一条线跑，现在虽然把所有线都采集了(从12行到160行)，但实质还是用了一条，十字用了一个很屌丝的补线方式，挺好用的，后边会说。依然不知道如何使用多线，应为远处的线真的很可能乱掉，可能需要加一个滤波把线lu匀称了再进行操作。

哎，说这些都是假的，还是需要考虑一些对策的，下面先说说我的控制策略吧，从头开始，配以部分代码：(代码写的真是屌丝，但越写越多越乱也很难改正改又怕弄的bug更多，遂自己看的懂即可)

AD采集，电机输出就不多说了，从滤波开始

1：波形合成采用的是官方的互补滤波方案，整体效果还是比较理想的

首先是加速度计的采集：

Ang_Acc=573*atan2(A_X,A_Z)-Ang_Set;

用的arctan，采集了两个轴，这样据说效果最好，后边那个Ang_Set是用来调整0度位置的，将来开机自检，现在用键盘调(屌丝了。。)。

然后是陀螺仪积分得到的角度：

Ang_IGyro=Ang_IGyro-GYRO_Y_Fliter*0.000016;//积分得到的角度

对陀螺仪输出滑动滤波了一下，基本没什么明显的影响，*的那个很小的数使这个积分得到的角度尽可能与加速度计计算得到的大小或者说数量级吻合。

然后就是互补滤波了，首先得到二者的偏差：

Ang_Error=Ang_Acc-Ang_IGyro;

这句没什么说的，就是两个的差。

最后把反馈量给过去就是整个的滤波算法：

Ang_IGyro=Ang_IGyro+Ang_Error*0.002;

Ang_Error前的参数越大曲线跟随效应越好但是越大毛刺越多需要调，越小则曲线越平滑，相对迟缓，最后得到的角度效果如图：

黄色是加速度计输出，红色是滤波后输出(静止状态下)。

运动状态下的输出今后肯定要记录一下的啦，之前好乱，写到这发现好多东西没有一个记录，谈何交流，谈何进步！从现在开始吧！

2：直立部分，PD就直立的很棒了，但有一个很大的问题！1ms控制一次

先上代码，这块很少，就一套PD：

Speed_L = (0-Ang)*135 + GYRO_Y_Fliter*0.02;//直立PID

Speed_L就是最终给到电机的那个值(别吐槽我的变量名啊^_^)

然后说一下调试时候的现象，D先给0，加大P，P很小很小其实就可以直立了，但用手一按，恢复的力度很小，所以就加大P，结果就是恢复快一些了，但会很大幅度的晃动(挺稳定的晃动，肯定不会倒)，最终还是会直立，目前我的状态是后者，调到大幅度摆动后加D，然后就可以非常稳定的直立了，只要轮子在地面不可能倒的

问题来了，不清楚直立究竟该软还是该硬！

软了吧车子肯定不抖，硬了的话如果E车车板不加固的话会发生剧烈抖动，之前以为是共振，也以为是陀螺仪位置不对，但都解决不了，应该就是参数问题(也不一定，新车的抖动较之前的抖动要剧烈的多，基本和手机震动感觉一样，所以还是很奇怪，虽然改参数可以解决，希望有车友试试他的车把D加的很大是不是一个现象，如果是那肯定没问题了要不是我这车还真是有大问题。)，之前调有个神论就是参数越大越稳定，其实不对啊当时脑子进水，这里就有个矛盾了，直立到底应该尽可能硬甚至加固车板，还是相对疲软？

3：速度控制，最坑爹的3个参数来了！官方方案看不懂，只能跟着框图写程序，研究过很久那个图，究竟怎么化简到那去的还是不太清楚。1ms控制一次，但速度是50ms测一次(这里可以优化一下代码)。

上代码，此处：

Speed_L=Speed_L-7.2*(float)(Check_Speed_Left)-0.12*(float)D_Check_Speed_Left-(0.09)*I_Check_Speed_Left;

直接增量式PID，正负号考虑到采集得到的值，所以不太统一(不规范啊！自己都吐槽自己)，Speed_L就是给过去的PWM

Check_Speed_Left=(LeftWheel_Count+RightWheel_Count)/2-Speed_Set_Left; //平均一下

然后Check_Speed_Left是这么来的，后边减去的Speed_Set_Left是速度设定值，和官方方案一致。

代码也不是很难懂，其实直立这部分真的很快就可以立起来，但当初调了好久啊，可能应为那时候还小吧^_^，下面说说这些参数的影响

首先是P，就是上边的7.2，这个值小了加速特别慢，而且加减速，P越小，加速越慢，加减速周期越长，P大了有一个值可以使之匀速运行，再大就开始晃动了，而且这回是越晃越大直到摔倒！我是调到正好匀速那里，一点不晃动，也不加减速(可能机械比较稳定，所以一个P可以匀速走的)

D的话加上一点可能抗干扰？尝试过改变它但总觉得效果很乱，最后实验出了个范围来，真不懂D改怎么给

I比较奇怪，I大了开始起步过冲很明显！！！I小了有时候上下午速度不一样，而且有一段时间发现I的大小会影响速度！(当时根本也没管卧槽这是怎么调的车！骂自己)。

然后就是过坡悲剧了，隔壁学校大牛建议我直接调参数冲坡，但我就是上坡倒，总觉得控制很滞后。不知道是不是程序的原因

最后是转弯：

FTM_PWM_Duty(FTM0,FTM_CH2,(uint32)Speed+2.3*Turning-1*Diff+0); //左拐

FTM_PWM_Duty(FTM0,FTM_CH4,(uint32)Speed-3.3*Turning+1*Diff+0);

很简单，官网方案，有一个陀螺仪的反馈，或者是电机差速(不知道为什么换了个电机驱动，刚起步一旦倒车车子就会转动很大，顺便说下我这换电机驱动把信号线序弄错了，于是乎调整了下线，就裸调，调的正确了为止，之前没这个问题的，然后改反馈方向，各种重新弄都有问题，打算恢复陀螺仪反馈了，还没有尝试)

上边就是直立控制的代码了，然后再分享一下图像处理的思路，从大一开始就搞图像，今年大创项目也是图像处理，小车这边却那么屌丝的处理，所有的算法都是裸想的，没用啥matlab，特点就是简单粗暴，这块自己没调明白，线很差，没有过滤均匀，也总是串，就简单贴出来给大家看看吧，有些地方说明一下，等我调的超神了再详细的说

for(Line_Num=119;Line_Num>12; Line_Num--)

{//此处搜线，共120行舍去11行

if(j<30) //前几个循环中线固定，防止一开始就疯狗mode

{

midlast=79;

leftlast=0;

rightlast=159;

j++;

}

else //储存上一次的值

{

midlast=Midline_Array[Line_Num];

midlastline=mid;

leftlast=Leftside;

rightlast=Rightside;

}

for(i=0;i<=160; i++)//寻找左边界

{

if(0 ==img[midlast-i+Line_Num*160]) //摄像头滤波效果足够好所以敢这样写

{

Leftside=midlast-i;

Leftside_Scr=Leftside;

D_Left=Leftside-leftlast;

ifLost=1; //在这个语句中说明没有丢线

break; //从中间开始检测到黑点就立刻取出此值

}

if(0==(midlast-i)%160) //如果是160的倍数说明扫描到了图像的边缘丢了补线

{

if(74 == i)

{

if_R=1;

}

else

{

if_R=0;

}

Leftside=Rightside-Track_Lenth[Line_Num];

Leftside_Scr=0;//这个是不补线的赛道宽度，赛道屏幕宽度

break;

}

for(i=0;i<=160; i++)//寻找右边界

{

if(0 ==img[midlast+i+Line_Num*160])

{

Rightside=midlast+i;

Rightside_Scr=Rightside;

D_Right=Rightside-rightlast;

if(ifLost ==1)//如果没有丢失赛道则更新用于弯道补线的赛道宽度

{

ifLost=0;

Track_Lenth[Line_Num]=Rightside-Leftside;

}

break;

}

if(0==(midlast+i+1)%160) //如果是160的倍数说明扫描到了图像的边缘

{

Rightside=Leftside+Track_Lenth[Line_Num];

Rightside_Scr=160;

break;

}

Track_Lenth_Scr[Line_Num]=Rightside_Scr-Leftside_Scr;

mid=(Leftside+Rightside)/2;

if(mid>=160) //此处防止中心线值溢出

{

mid=159;

}

if(mid<=0)

{

mid=1;

}

//将mid值存入数组

Midline_Array[Line_Num]=mid;

if(Track_Lenth_Scr[Line_Num]>158) //赛道宽度超级宽就停止更新中线

{

Lost_Count[lostcount]=Line_Num;//丢失的这一段记录下来，十字补线

lostcount++;

}

//下边都是滤线了

for(i=0;i<lostcount+40;i++)//十字补线，效果明显，这里是亮点，就是把全白那一块(上边搜线程序得到)的最上和最下延伸，延伸到有线(必须有线！！bug处理不到位了又)的地方，把这两个地方的中点链接起来，我现在只是让丢了的地方都取近处的线了，都没连起来十字就没啥压力了，这个还有bug的，比如往最上最下延伸，最上最下没有线怎么办，不过想法自我感觉很不错的^_^，说好的最小二乘法延伸跪了，就想了这么个法子，调试会简单一些，方便一些。

{

if((Lost_Count[0]+20)<119)

{

Midline_Array[Lost_Count[0]-i+20]=Midline_Array[Lost_Count[0]+20];

}

else

{

Midline_Array[Lost_Count[0]-i+20]=79;

}

lostcount=0;

for(Line_Num=12;Line_Num<119; Line_Num++)

{