2018华东赛区无线节能组—经验总结

一、基础知识

（1）总体设计

赛车可以选择性利用陀螺仪分别获取当前赛车的角度和角速度来观测车身姿态。在实际操作过程中为了简便，只通过车体前方的电磁传感器（经电磁板信号放大）采集赛道信息，经内部 AD 进行模数转换后，输入到控制核心（核心板KEA128等等），用于赛车的运动控制决策。通过编码器测速模块来检测车速（采用速度闭环则需要编码器），并采用 KEA128 的输入捕捉功能进行脉冲计数计算速度和路程；电机转速控制采用 PID 控制，通过 PWM 控制驱动电路调整电机的转速，完成智能车速度的闭环控制。此外，还可以增加按键、拨码开关等等作为输入输出设备，便于于小车的调试。

 

（2）模块化处理

①KEA128 主控模块：作为整个智能车的“大脑”，将接受电感传感器、陀螺仪、编码器等传感器的信号，根据控制算法做出控制决策，驱动直流电机完成对智能车的控制。

②传感器模块：是智能车的“眼睛”，可以通过一定的前瞻性，提前感知前方的赛道信息，为智能车的“大脑”做出决策提供必要的依据和充足的反应时间。

③电源模块：为整个系统提供合适而又稳定的电源。

④无刷电调电路控制模块：驱动电机完成智能车加速减速控制.比赛中不允许使用购买的外置电调，必须自己根据电路打印pcb。红树伟业有内置编码器的无刷电机，但是速度较慢。

⑤速度检测模块：检测反馈智能车后轮的转速，用于速度的闭环控制。

⑥LCD 显示模块：了解与掌握一些车的运行状态,便于对车的状态做出判断，正确的调试

⑦按键和开关：用于调试过程中的参数修改，减少调试时间，起到人机交互作用。

比赛时大概可以使用三种传感器：电磁传感器，姿态传感器（mpu6050），速度传感器（编码器）。我们只采用了电感。

 

（3）电磁感应原理

高中物理我们就学过这方面的知识，，我们知道在导线中通入变化的电流，变化电场产生变化磁场，且磁场与电流的变化规律具有一致性。变化的磁场会在在电感产生感应电动势，且该感应电动势的大小和通过线圈回路的磁通量的变化率成正比。在导线周围的不同位置的磁感应方向和大小都不一样，所以不同位置上的电感产生的感应电动势也应该是不同。据此，则可以确定电感的大致位置。从而确定小车的位置。

 

（4）电感的布局

   赛道主要由一个通电导线作为引导，磁场的分布是以通电直导线为圆心的一个个同心圆，越靠近通电直导线，磁性越强，磁场分布就越密。磁场的方向可以用用右手握住通电直导线，大拇指指向电流方向，四指所指的方向就是磁场方向。这些东西我们都已经学过。在这里我们终于可以体会到它的应用了。而那个电感是一个绕着线圈的小柱子，法拉第电磁感应原理告诉我们，磁通率变化越快，产生的感应电动势越大，那么这就决定了我们将电感与导线垂直放置的时候，可以取得最大电感。并且越靠近导线，这个感应电动势E越大。在实际的布局中，我坚持的原则是，边测边布局，如何放置能够识别特征，就怎么放置，不一定要追求对称。

 

（5）信号处理

1.将电感的感应电动势经过电磁板放大，经过导线传递给主控板的KEA128，程序里可以进行进一步的滤波。

2.将编码器的脉冲脉冲信号（表征转速）传递给核心板，在程序里可以自己确定多久采集一次脉冲，从而确定自己组别的脉冲范围。

3.mpu6050可以获取xyz方向的加速度信息，从而确定小车的位置，直立组用得比较多，在这里我就不多做解释。

 

（6）关于无刷电机

无刷直流电机与普通直流电机的区别是：无刷直流电机是用过霍尔传感器进行换向，传统的直流有刷电机是通过碳刷和整流子进行机械换向。无刷直流电机的优势是无刷直流电机在换向的过程中没有火花产生，而普通的有刷电机在换向的过程中有少量的火花产生；寿命长，

效率率高。当然，这是最理想的效果。我们是实际调车的过程中发现，无刷电机的带负载能力比较低，拐弯的时候如果摩檫力比较大可能卡死，车身很重的时候，跑得相当的慢。如果可能的话，可以使用正常的有刷电机，其实不会产生多少的电量消耗。

 

（7）车模、电机、舵机的选择

比赛过程中绝大多数车模的都是红树伟业的车模，但是节能组并没有车模要求，卓大大也有批评大家使用雷同的车模的意思。实际上，选用整套车模的队伍基本上没有取得好成绩。

节能组对车身的要求比其他组要求比较高，重量往往决定了速度和省电能力。厉害的组别往往是自己设计的，搭建的十分简单，几根棒加上电路板车身就构成了整辆车，虽然样子比较丑，但实际跑起来具有绝对的优势。电机和舵机是最主要耗能原件，比赛对节能组同样是没有型号要求，比赛的时候卓大大也跟我们说过，选用小型的舵机和电机其实会更省电。而且还十分便宜。

 

（8）2018年预赛赛道元素

 

①颠簸路面：速度不够容易卡死，速度过快容易甩出赛道。

②起跑线：有时间检车装置和停车用的磁铁。我们利用干簧管可以检测到磁铁，从而实现停车。

③欧姆环：新增元素，在程序部分我有专门介绍。

④坡道：在决赛才有，检测到坡道应该减速，不然可能冲出赛道

⑤路颠：放置在赛道两旁的凸起，撵路颠，速度就会降下来

⑥双车会车区域：应该是两辆车到达该区域不能的间隔不能超过5s，难度比较大

 

 

 

 

二、电路部分

（1）5V稳压电路

5v稳压电路是一个关键的电路，因为它是电机的驱动电路，主要的耗能都在这里，但是LM2940是线性稳压的，降压损耗能量以热量形式散发，所以会负载较大的时候这个芯片会严重发烫！

（2）3v3稳压电路

3v3稳压电路比较简单，一般不会出现什么问题。

 

 

（3）6v5稳压电路（舵机供电电路）

6v5稳压电路是也一个关键的电路，因为它是舵机的驱动电路，主要的耗能都在这里，但是LM2941是线性稳压的，降压损耗能量以热量形式散发，所以会负载较大的时候这个芯片也会严重发烫！

（4）拨码开关电路

（5）lcd引脚分布

 

（6）停车电路

    干簧管是一个机械传感器，它在磁铁的作用下会导通，这个时候12口连通，那么INT的电压就是3v3，此时通过芯片的引脚状态检测，并书写相关程序，就可以实现停车功能。

（7）3v3滤波电路

（8）独立按键

（9）无线充电接收电路

    此电路是逐飞设计的无线充电接收电路，今年的无线节能组最关键的一个电路，然而该电路接收效率低，我们也因此没有取得很好的成绩。此电路有多种设计方案，可在网上或着卓大大微信公众号上阅读相关资料，自行设计，原理也十分的简单，在你们学的电工上会有所涉及。

（10）电磁信号放大电路

   该电路是红树伟业所设计的电路，可以检测到二倍信号，往届没有欧姆环，使用的放大的电路与该电路有所不同。放大电路中的滑动变阻器可以调节信号放大倍数。

（11）12V升压模块

    这个电路是我们为了提高无刷电机转速而采用的电路，它可以将低至2.7V的电压升到12v，充分里用电容的剩余电量，否则，电容电压低于7V无刷电机就基本停止了。不过芯片比较小，比较难焊接。考验你们的焊工喔！

（12）无线充电

这个电路是我们为了解决LM2941在舵机工作的时候严重发烫时而添加的，AS1015是开关型稳压芯片，这种降压模式不会产生过多的热量，自然也就更省电了。不过芯片也比较小，比较难焊接

三、程序部分

（1）程序框架

    所谓的程序框架其实就是你程序是怎么流动的而已，主要包括硬件端口的初始化，中断优先级（如果需要用到多个中断），定时中断时间，while里写什么（while也相当于一个中断，只不过它的优先级是最低的）。一般来说只需要一个中断就够，多个中断产生冲突的时候，对于编码器这种时间性强的传感器来说，就会产生一定的影响。

（2）主程序

    在主程序里一般会首先初始化一下我们需要用到的功能，如果不初始化，程序可能会出错，也可能会出现无法使用该功能的现象，比如lcd不初始化，那么在调用的时候它可能是不亮屏幕的，具体情况我记得也不是很清楚了。需要初始化的可能会包括：led初始化，adc初始化，按键初始化，lcd初始化，pwm频率、占空比范围初始化，定时中断定义、使能，全局中断使能等等。具体还得看需要用到什么功能。

（3）算法

所谓的算法就是公式，利用一个公式可以起到什么作用。

①滤波：采集到的电感值可能会波动很大，怎么办呢，一般人会想到，取平均值。这就是一种算法。我们对采集的电感值做了以下几方面的处理，首先每个端口采集5次信号取平均，打包成函数。再用该函数采集5次信号，去掉一个最大去掉一个最小取平均。接着，为了方便计算，我们采用的归一化处理，电感采集的值一般在零到两千五百不等，具体与电磁板和采集精度有关，这里就先不用纠结了。所谓的归一化，就是等比例缩放到一个范围内，我们将其缩放到0-100。其实很简单，归一化的公式有很多，就一个简单的公式。

也可以采用加权递推平均滤波法，定义一个长度为 N 的数组，依次存取采集数据，越近采集的值，权重越大。进行加权运算得到滤波结果 。

②PID算法：关于这方面的解释在网上有很多，解释起来也比较麻烦，但可以遵循以下经验，下面介绍两种简化后的pid算法

M_PWM=err*Kp+derr*Kd;   //注意这里是非增量式PID

非增量式pid也叫做位置式pid，它去掉了KI项。与上一次的信息无关，这个公式主要用于控制舵机，从名字上也可以知道，这个公式就是现在需要转到某个角度，那么它就输出某个pwm，这就是所谓的位置。

temp=err*Kp+derr*Kd;

M_PWM+=temp; //注意这里是增量式PID

我们注意到增量式pid前面有个+号，这个加号说明，这一次的值与上一次是有关的。这个公式主要用来控制电机的。由于电机的转速主要是通过电压来决定的，如果上一次没有达到预定的转速，那么下一次的功率应该加大，直到我们的要求，这就是增量，也就是不断的加，功率大了就减小，功率大了就增加。

我们注意到，两个公式都只使用了Kp和Kd,一般来说KI在直立组用的比较多，我尝试了用KI，结果发现比之前跑的更抖了，就没有继续使用了。在学习过程中，我们鼓励自己去尝试，不要死死的保持这个公式的原样。

下面介绍一下Kp和Kd在实际调试过程中的作用:Kp的具体表现是速度，代表着上升的斜率，这个值越大，就越快达到那个值，但太大了会刹不住车。在调试舵机的时候，我们会发现，这个值调大了后，舵机转向速度明显加快。Kd的具体表现为阻力，可以这样来说在Kp条件下加以一定的压制。在实际调车过程中，这两个值取得比较合适的话，就会出现直到走得很直，速度上去了，电量省了。弯道拐得很顺，也有同样效果。

查看前几届学长学姐的程序的Kp和Kd其实都是相对固定的，但实际上，不同的偏差是需要对应不同的参数的，这就是所谓的动态调参。我们组最终采用了指数增长的形式来确定Kp和Kd的大小，一般来说，弯道偏差越大Kp应该越大，Kd应该越小。所以可以更具这个规律，自己去选择模型。望你们不要受到约束，去创新，去尝试！

（4）直到定位处理

跑直到和弯道其实只需要两个电感就足够了，某一时刻，采两个“一”字排布的电感的感应电动势，其中一个感应电动势最大的电感必然会离导线最近。这个时候不管是用差速也好舵机也好，都需要往电感大的方向转弯，拐多少就是PID计算的问题了，在调车的时候，自己根据左右电感差值来确定转角或者左右轮的速度差值。一般来说，采用最简单的方式就行，左右电感相减，把这个偏差送进pid程序里计算，然后会得出一个pwm，把这个pwm给相应的电机，舵机赋值就行。差速的话，左边慢右边快自然向左转。舵机的话，首先要确定中值，pwm大于小于这个值就会产生转向，这个需要根据实际情况而定。

（5）欧姆环处理

欧姆环是今年新增加的一个赛道元素，可以采用多种进环的方案，这个时候，比较关键的还是电感的布局，这就是我在前面谈到的，不要拘泥于是否对称，要重要识别特征。我们直接采用了两个竖直电感作为判断依据，开始的时候还增加了一个斜电感（特征比较明显），最后两个竖直电感已经完全能够实现进环了，就没有再使用斜电感。当然，其它组别也有采用内八外八等等。我想再说一遍，这些东西，不要拘泥于过去的比赛，一定能要自己不断的尝试，这样你会收获更多的东西。

竖直电感在直道上都是与导线平行的，值相对来说比较小，但是到了欧姆环的时候会出现磁场叠加和交叉的情况，我们可以根据这个来实现进环。具体操作如下，直到的时候确定几个变量当作标志位。识别到环的特征后（这个特征会持续一段距离），可以定角度进环，也可以根据偏差来进环。一旦进环后就可以按正常的弯道行走了，这个时候确定进环标志位用来防止下一次入环，也就是跑一圈回来的时候还是会检测到这个特征，有这个标志位后，就可以确定已经在环内了，下一步就可以书写程序，让它不在拐进去。出环后，我们就可以把标志位清空，这样就可以进下一个环了。当然这个程序会遇到很多问题，特征值的测量非常重要，这就得靠你们自己了，加油！

（6）限位处理

由于机械结构和弯道的问题，必须对拐弯进行限位，具体表现为：我们人为规定pwm最大最小只能输出多少。换一句话来说就是，转大了会卡死，或者赛道本根本就不需要拐那么大角度，拐大了就出界了。这个需要视情况而定，我在这里也不好说明。

（7）丢线处理

所谓的丢线就是所有的电感都在导线的一边了，这样一来，就会出现很多问题，尽管这时需要拐很大的弯，但是电感的差值可能很小，甚至为0。一旦出现这种情况，如果不及时拐回来，肯定就出界了。所以我们就想出一招。两个电感都很小的时候，给一个固定的转角。比如给限位的最大值。（实际上我们也好像没有给最大值，看能不能拐过去吧！）。但你们肯定会有疑问了，既然差值都为0了，我到底向哪边拐呀？这个时候，你们聪明的晶晶学姐就想出了一个办法：防微杜渐！！！，“我让它还没达到0那个程度的时候就拐回来了不久行了嘛！”。嗯！事实上就是这么做的，可以拐任何弯！其中欧姆环其实弯度也很大，里面会多次利用到这个丢线程序！

当然，我们在pid里增加了指数调节模型，实际上，去掉了丢线处理，他还是可以拐任何弯！希望你们可以想到一些更好的方法。

四、总结

飞思卡尔比赛是机器人俱乐部到目前为止最为重要的一个比赛，在这个比赛中需要花费很多的时间和精力，当然也能够学到很多的东西。在组别的选择上其实没有太大的区别，并不存在某一个组别特别简单，某一个组别特别困难。它们的难点不一样而已。在调试的过程中掌握的了规律，这个比赛其实也就很简单了。我的观点是，不一定要熬很多夜你才能掌握某个东西，但你一定要不断的去尝试自己的想法，去积累经验，不要以花哨为目标，要为了效果而努力。更为重要的一点的是，在这个比赛中，我们会收获很多不一样的东西。组员，组别之间的争吵猜忌埋怨最终都会转化成最浓厚的友谊。

愿诸位在明年的比赛中取得好成绩！

开荒者路途遥远，耕耘者仍在前行！

#include "common.h"
#include "include.h"//关于电感
//adc经过判断和计算后的偏差
uint16 AD[8], AD_v[8];
uint16  max_v[8] = { 2516,2619,0,2573,2573,0,0,0 }, min_v[8] = { 39,50,50,42,50,50,50,50 };                                 //找到6个电感中的最大最小值，电感标定采集值
int16 cline_err;
uint16 adc_av(ADCn_Ch_e adcn_ch, ADC_nbit bit);
void Read_ADC(void);
void adc_max(void);
void to_one(void);
void adc_init1(void);
uint8 stop = 3;
uint8 ms = 0;
//关于编码器
//记录编码器采集的脉冲数
//设定速度
//pid计算后应该给予的速度电压
//当前速度获取程序 //编码器初始化
float djc;
uint8 count = 0;
uint16  cspeed;
uint16  speed;
uint16  uspeed;
uint16  speedh = 0;
uint32  angle;
uint32  lastangle;
void bmq_get(void);
//虚拟示波器程序
void vcan_sendware1(uint8 *wareaddr, uint32 waresize);
//以下为多种pid的运算方案
float Moto_PIDyou(float Target_Speed, float System_Feed_Speed);
float duoji_PID3(float cline_err);              //变系数PD舵机控制(位置式) KD可变，控制参数 
int  DuoP = 24;   //
int  DuoD = 2;
int  timecount;
int  speedcnt;
int  speedkp;
int  dianjispeed;
int errz[8];
int flag = 0;
unsigned char  Dir_last = 0;
int  dir_error = 0, dis_error;
int  Calculate_Length;
int  DirectionErr[9] = { 0 };
int disgy_AD_val_1, disgy_AD_val_2;
float Error_Delta;
unsigned short servPWMDuty; //当前舵机值
int8 flag1 = 0;
int8 flag2 = 0;
int8 flag3 = 0;
int8 flags = 0;
int8 diuxian = 0;
int8 zhidao = 0;void Push_And_Pull(int *buff, int len, int newdata);
float Slope_Calculate(uint8 begin, uint8 end, int *p);
void adjduoji();
void adjspeed(uint16 speed, uint16 cspeed);
///以下是正式进入调车程序/////清除中断标志位,一定要清除，不然就一直在中断
void pit_ch1_irq(void)
{ftm_pwm_duty(FTM2, FTM_CH5, 0);float o_pwm;Read_ADC();adc_max();to_one();//AD[0]=AD[0];djc = (float)(AD[4] - AD[0] - 3);angle = duoji_PID3(djc) + 4800;/*******正常道路*********/if (abs(djc)<12)uspeed = 300;else if (25>abs(djc) >= 12)uspeed = 150;elseuspeed = 0;while (flag1 == 0 && AD_v[3]>1450 && AD_v[1]<1150){Read_ADC();// adc_max();to_one();if (AD_v[3]>1450 && AD_v[1]<1150){// djc=16*(float)(AD[3]-AD[1]);angle = 5200;ftm_pwm_duty(FTM2, FTM_CH4, angle);flags = 1;led(LED0, LED_ON);}else{djc = (float)(AD[4] - AD[0]);ftm_pwm_duty(FTM2, FTM_CH4, angle);led(LED0, LED_OFF);//                  flags=2;//          flag1=1;//          led(LED0,LED_OFF);//          led (LED1,LED_ON);}if (70>AD_v[0]){flags = 2;flag1 = 1;led(LED0, LED_OFF);led(LED1, LED_ON);ftm_pwm_duty(FTM2, FTM_CH4, 4800);////        djc=(float)(AD[4]-AD[0]+3);//        angle=duoji_PID3(djc)+4800;//        ftm_pwm_duty(FTM2,FTM_CH4,angle);  //正常走}}ftm_pwm_duty(FTM2, FTM_CH4, 4800);while (flag2 == 0 && AD_v[1]>1450 && AD_v[3]<1250){Read_ADC();// adc_max();to_one();ftm_pwm_duty(FTM2, FTM_CH4, 4250);if (AD_v[1]>1450 && AD_v[3]<1250){ //led(LED1,LED_OFF);led(LED2, LED_ON);// djc=16*(float)(AD[3]-AD[1]);angle = 4250;ftm_pwm_duty(FTM2, FTM_CH4, angle);flags = 3;}else{djc = (AD[4] - AD[0]);ftm_pwm_duty(FTM2, FTM_CH4, angle);led(LED2, LED_OFF);//           led (LED3,LED_ON);//          flags=4;//           flag2=1;}if (110>AD_v[4]){led(LED2, LED_OFF);led(LED3, LED_ON);flags = 4;flag2 = 1;ftm_pwm_duty(FTM2, FTM_CH4, 4800);//          djc=(float)(AD[4]-AD[0]+3);//        angle=duoji_PID3(djc)+4800;//        ftm_pwm_duty(FTM2,FTM_CH4,angle);  //正常走}}ftm_pwm_duty(FTM2, FTM_CH4, 4800);//led (LED3,LED_OFF);flags = 0;ftm_pwm_duty(FTM2, FTM_CH4, angle);  //正常走if (abs(djc)<10 && 400<AD_v[0]<600 && 400<AD_v[4]<600 && 100<AD_v[1]<200 && 100<AD_v[3]<200 && 4850<angle<4750 && abs(AD_v[5] - 1100)<100){flag2 = 0; flag1 = 0;led(LED3, LED_OFF);led(LED1, LED_OFF);}//      if(400<AD_v[1]<600&&400<AD_v[4]<600&&4900<angle<4700)//      {//        flag1=0;//      }flag3 = PTD0_IN;if (PTD0_IN == 0){ftm_pwm_duty(FTM2, FTM_CH5, 0);}else{while (PTD0_IN);stop = stop - 1;if (stop == 0){while (1){ftm_pwm_duty(FTM2, FTM_CH4, 4800);ftm_pwm_duty(FTM2, FTM_CH5, 10000);}}}PIT_Flag_Clear(PIT1);}void main(void)
{uint8 del = 0, i;//以下是开关，显示类初始化///四个led灯初始化，用于指示当前进入的函数，虽然可能没什么用led_init(LED0);led_init(LED1);led_init(LED2);led_init(LED3);gpio_init(PTD0, GPI, 0);/*************************拨码开关选择延迟时间***************************///       gpio_init (PTD1, GPI,1);//       gpio_init (PTH6, GPI,1);//       gpio_init (PTH7, GPI,1);//       gpio_init (PTE7, GPI,1);//       gpio_init (PTB2, GPI,0);//     while(PTB2_IN==0);//    led (LED2,LED_ON);////       if(PTD1_IN==0)//       {//       led (LED0,LED_ON);//       del=1;//       }//       if(PTH6_IN==0)//       {//       led (LED1,LED_ON);//       del=2;//       }//       if(PTH7_IN==0)//       {//       led (LED2,LED_ON);//      // del=3;//       }//       if(PTE7_IN==0)//       {//        led (LED3,LED_ON);//        del=4;//       }/*************************延迟设定***************************///      for(i=0;i<60;i++)//      {//      systick_delay_ms(5000);//      led (LED0,LED_ON);//     systick_delay_ms(5000);//     led (LED0,LED_OFF);//     }systick_delay_ms(4000);led (LED1,LED_ON);systick_delay_ms(4000);led (LED2,LED_ON);systick_delay_ms(4000);led (LED3,LED_ON);systick_delay_ms(4000);led (LED1,LED_OFF);led (LED2,LED_OFF);led (LED3,LED_OFF);//   }///以下是与中断定义有关的初始化pit_init_ms(PIT1, 5);                                   //  定时 500msenable_irq(PIT_CH1_IRQn);                             // 使能PIT_CH1中断//   pit_init_ms(PIT0, 3);//   enable_irq(PIT_CH0_IRQn);//   NVIC_SetPriority(PIT_CH1_IRQn,0);//   NVIC_SetPriority(PIT_CH0_IRQn,1);//以下是adc初始化程序///adc_init1();//以下是lcd初始化程序//////以下是与电机和舵机有关的初始化/////     ftm_pulse_init(FTM1, FTM_PS_1, TCLK1);ftm_pwm_init(FTM2, FTM_CH5, 1000, 10000);     //PTB5    //FTM0_PRECISON  精度入口ftm_pwm_init(FTM2, FTM_CH4, 300, 4800);      //PTB4     //4254基本上是90度角EnableInterrupts;while (1){/*****************端口状态测试***************************///            if(PTH7_IN==1)//       {//       led (LED2,LED_ON);//       del=3;//       }//         if(PTH7_IN==0)//       {//       led (LED2,LED_OFF);//       del=3;//       }}}
/以上是主要程序//以下进入函数定义区
虚拟示波器核心函数
void vcan_sendware1(uint8 *wareaddr, uint32 waresize)
//示波器核心函数   前面表示数组，长度必须与通道对应，avr[5] 必须对应通道5，
//类型必须大致对应，char 对应 char 或者unsigned char。才能显示
{
#define CMD_WARE     3uint8 cmdf[2] = { CMD_WARE, ~CMD_WARE };    //串口调试 使用的前命令uint8 cmdr[2] = { ~CMD_WARE, CMD_WARE };    //串口调试 使用的后命令uart_putbuff(VCAN_PORT, cmdf, sizeof(cmdf));    //先发送前命令uart_putbuff(VCAN_PORT, wareaddr, waresize);    //发送数据uart_putbuff(VCAN_PORT, cmdr, sizeof(cmdr));    //发送后命令}/编码器脉冲获取
void bmq_get(void)
{//con=PTE5_IN;if (count == 4){count = 0;cspeed = ftm_pulse_get(FTM0);ftm_pulse_clean(FTM0);}//cspeed是当前速度，全局变量
}/adc采集和处理程序///void adc_init1(void)                          //电感的定义和获取
{adc_init(ADC0_SE0);      //PTA0               //必须初始化一个值，否则不显示状态，情况目前未知adc_init(ADC0_SE1);     //PTA1adc_init(ADC0_SE2);     //PTA6adc_init(ADC0_SE3);     //PTA7adc_init(ADC0_SE12);     //PTF4               //警告是由于没有用ADC0_SE0之类的名称，没用已定义的宏定义adc_init(ADC0_SE13);     //PTF5adc_init(ADC0_SE14);    // PTF6adc_init(ADC0_SE15);     //PTA7}/取平均函数///
//端口名，精度，取5次做平均
uint16 adc_av(ADCn_Ch_e adcn_ch, ADC_nbit bit)
{int i;float AD_av[5];uint16 sum = 0;for (i = 0; i<5; i++){AD_av[i] = adc_once(adcn_ch, bit);}for (i = 0; i<5; i++){sum += AD_av[i];}sum = sum / 4;return sum;
}/三个数组分别表示多次采集数组，平均值数值，采集之和数组
//连续采集5X4次电感值
void Read_ADC(void)
{int16  i, j, k;uint16  AD_duo[8][5], ad_sum[6], temp;for (i = 0; i<5; i++){AD_duo[0][i] = adc_av(ADC0_SE0, ADC_12bit);AD_duo[1][i] = adc_av(ADC0_SE1, ADC_12bit);AD_duo[2][i] = adc_av(ADC0_SE2, ADC_12bit);AD_duo[3][i] = adc_av(ADC0_SE3, ADC_12bit);AD_duo[4][i] = adc_av(ADC0_SE12, ADC_12bit);AD_duo[5][i] = adc_av(ADC0_SE13, ADC_12bit);AD_duo[6][i] = adc_av(ADC0_SE14, ADC_12bit);AD_duo[7][i] = adc_av(ADC0_SE15, ADC_12bit);}//下面进行冒泡排序，为的是取掉一个最大值，去掉一个最小值//第一个循环中，i=0到5，代表了6个电感//j=0到4，五个数据排序for (i = 0; i<8; i++){for (j = 0; j<5; j++){for (k = 0; k<5 - j; k++){if (AD_duo[i][k] > AD_duo[i][k + 1])    //前面的比后面的大  则进行交换{temp = AD_duo[i][k + 1];AD_duo[i][k + 1] = AD_duo[i][k];AD_duo[i][k] = temp;}}}}for (i = 0; i<8; i++)    //求中间三项的和{ad_sum[i] = AD_duo[i][1] + AD_duo[i][2] + AD_duo[i][3];AD_v[i] = ad_sum[i] / 3;}}void adc_max(void)
{uint8  j;for (j = 0; j<8; j++){if (AD_v[j] > max_v[j] + 20){max_v[j] = AD_v[j];}if (AD_v[j]<min_v[j]){min_v[j] = AD_v[j];}}
}///对平均值进行归一化处理
void to_one(void)
{uint8  i;float sensor_to_one[8];for (i = 0; i<8; i++){sensor_to_one[i] = (float)(AD_v[i] - min_v[i]) / (float)(max_v[i] - min_v[i]);if (sensor_to_one[i] <= 0.0)  sensor_to_one[i] = 0.001;if (sensor_to_one[i]>1.0)  sensor_to_one[i] = 1.0;AD[i] = (uint16)(100 * sensor_to_one[i]);     //AD[i]为归一化后的值  范围为0-100}}/**************************pid运算方案*************************************/
float duoji_PID3(float cline_err)                //变系数PD舵机控制(位置式) KD可变，
{volatile static float err = 0, last_err = 0, derr = 0, l_last_err, dderr;volatile static float M_PWM = 0;volatile static float Kd, Kp, Ki;err = cline_err;// Push_And_Pull(errz,8,err);//derr=100*Slope_Calculate(0,8,errz);//偏差50代表中间derr = err - last_err;                         //derr dderr赋初值dderr = err - 2 * last_err + l_last_err;/******DIU XIAN*******/if (AD[0]>19 && AD[4]>19)    //直道{if (err<0)Kp = 1.8;else  if (err>0)Kp = 1.6;zhidao = 1;diuxian = 0;}// if(abs(err)>35)Kp = 2 + exp(0.025*abs(err));if (flag1 == 1 || flag2 == 1)Kp = 2 + exp(0.05*abs(err));//  if(abs(err)<800)Kd = 110 - exp(0.045*abs(err));// else Kd++;Ki = 0;M_PWM = err * Kp + derr * Kd + Ki * dderr;;                     //注意这里是非增量式PID//  }l_last_err = last_err;                      //记录上上次偏差last_err-err;/7记录上次偏差值servo_control (M_PWM);最后赋值给舵机函数last_err = err;/*************XDIU IAN***********/if (AD_v[0]<100 && AD[4]<100 || AD_v[0]<80 || AD_v[4]<80){if (AD[0]<AD[4])M_PWM = 420;if (AD[0]>AD[4])M_PWM = -500;diuxian = 1;zhidao = 0;}/*************XIAN WEI***********/if (M_PWM>450)M_PWM = 450;if (M_PWM<-510)M_PWM = -510;return M_PWM;
}float Moto_PIDyou(float Target_Speed, float System_Feed_Speed)                   //电机的pid函数
{static float err = 0, last_err = 0, derr = 0;static long int M_PWM = 0;static float Kd;float Kp;float temp;err = Target_Speed - System_Feed_Speed;derr = err - last_err;if (fabs(err)>10)Kp = 200;elseKp = 10;if (fabs(err)<10)Kd = 600;elseKd++;temp = err * Kp + derr * Kd;M_PWM += temp;last_err = err;if (M_PWM>9500)M_PWM = 9500;if (M_PWM<-9500)M_PWM = -9500;return(M_PWM);                   //返回PWM的值}/最小二乘法拟合斜率，求电感插值导数/
float Slope_Calculate(uint8 begin, uint8 end, int *p)
{int xsum = 0, ysum = 0, xysum = 0, x2sum = 0;uint8 i = 0;float result = 0;static float resultlast;p = p + begin;for (i = begin; i<end; i++){xsum += i;ysum += *p;xysum += i * (*p);x2sum += i * i;p = p + 1;}if ((end - begin)*x2sum - xsum * xsum) //判断除数是否为零{result = ((end - begin)*xysum*1.0 - xsum * ysum*1.0) / ((end - begin)*x2sum*1.0 - xsum * xsum*1.0);resultlast = result;}else{result = resultlast;}return result;
}///退栈函数///
void Push_And_Pull(int *buff, int len, int newdata)
{int i;for (i = len - 1; i>0; i--){*(buff + i) = *(buff + i - 1);}*buff = newdata;
}