手中有些关于双轮平衡小车的资料，特与大家分享，一同学习进步。
一、平衡小车原理
平衡原理
平衡小车是通过两个电机运动下实现小车不倒下直立行走的多功能智能小车，在外力的推拉下，小车依然保持不倒下。这么一说可能还没有很直观的了解 究竟什么是平衡小车，不过这个平衡小车实现的原理其实是在人们生活中的经验 得来的。如果通过简单的练习，一般人可以通过自己的手指把木棒直立而不倒的 放在指尖上，所以练习的时候，需要学会的两个条件：一是放在指尖上可以移动， 二是通过眼睛观察木棒的倾斜角度和倾斜趋势（角速度）。通过手指的移动去抵 消木棒倾斜的角度和趋势，使得木棒能直立不倒。这样的条件是不可以缺一的， 实际上加入这两个条件，控制过程中就是负反馈机制。
而世界上没有任何一个人可以蒙眼不看，就可以直立木棒的，因为没有眼睛的负反馈，就不知道笔的倾斜角度和趋势。这整个过程可以用一个执行式表达：
平衡小车也是这样的过程，通过负反馈实现平衡。与上面保持木棒直立比较则相对简单，因为小车有两个轮子着地，车体只会在轮子滚动的方向上发生 倾斜。控制轮子转动，抵消在一个维度上倾斜的趋势便可以保持车体平衡了。
所以根据上述的原理，通过测量小车的倾角和倾角速度控制小车车轮的加速度来消除小车的倾角。因此，小车倾角以及倾角速度的测量成为控制小车直立 的关键。我们的亚博智能平衡小车使用了测量倾角和倾角速度的集成传感器陀螺 仪-MPU6050（模块详细介绍在亚博智能平衡小车光盘资料3.硬件资料中）。
二、角度（物理分析 PD 算法）
控制平衡小车，使得它作加速运动。这样站在小车上（非惯性系，以车轮 作为坐标原点）分析倒立摆受力，它就会受到额外的惯性力，该力与车轮的加 速度方向相反，大小成正比。这样倒立摆(如图 2)所受到的回复力为：公式 1
F = mg sin θ-ma cos θ≈mg θ-mk1θ 式1 中，由于θ很小，所以进行了线 性化。假设负反馈控制是车轮加速度 a 与偏角θ成正比，比例为 k1。如果比例k1>g，（g 是重力加速度）那么回复力的方向便于位移方向相反了。
而为了让倒立摆能够尽快回到垂直位置稳定下来，还需要增加阻尼力。增 加的阻尼力与偏角的速度成正比，方向相反，因此公式1可改为：F = mg θ-mk1 θ -mk2 θ` 按照上述倒立摆的模型，可得出控制小车车轮加速度的算法：
a =k1θ+k2θ` 式中θ为小车角度,θ`为角速度。k1 k2都是比例系数 根据上述内容，建立速度的比例微分负反馈控制，根据基本控制理论讨论小车通过闭环控制保持稳定的条件（这里需要对控制理论有基本了解）。假设外 力干扰引起车模产生角加速度x（t）。沿着垂直于车模地盘方向进行受力分析，可 以得到车模倾角与车轮运动加速度以及外力干扰加速度a（t）x（t）之间的运动方 程。如图3所示。
图3
在角度反馈控制中，与角度成比例的控制量是称为比例控制；与 角速度成比例的控制量称为微分控制（角速度是角度的微分）。因此 上面系数 k1,k2 分别称为比例和微分控制参数。其中微分参数相当于 阻尼力，可以有效抑制车模震荡。通过微分抑制控制震荡的思想在后 面的速度和方向控制中也同样适用。 总结控制车模直立稳定的条件如下：
（1）能够精确测量车模倾角θ的大小和角速度θ'的大小；
（2）可以控制车轮的加速度。
上述控制实际结果是小车与地面不是严格垂直，而是存在一个对 应的倾角。在重力的作用下，小车会朝着一个方面加速前进。为了保 持小车的静止或者匀速运动需要消除这个安装误差。在实际小车制作 过程中需要进行机械调整和软件参数设置。另外需要通过软件中的速 度控制来实现速度的稳定性。在小车角度控制中出现的小车倾角偏 差，使得小车在倾斜的方向上产生加速。这个结果可以用来进行小车 的速度控制。下面将利用这个原理来调节小车的速度。
三、测速（物理模型 建立数学模型 传递函数 PD 算法） 假设小车在上面直立控制调节下已经能够保持平衡了，但是由于
安装误差，传感器实际测量的角度与车模角度有偏差，因此小车实际 不是保持与地面垂直，而是存在一个倾角。在重力的作用下，小车就 会朝倾斜的方向加速前进。控制速度只要通过控制小车的倾角就可以 实现了。具体实现需要解决三个问题：
（1）如何测量小车速度？
（2）如何通过小车直立控制实现小车倾角的改变？
（3）如何根据速度误差控制小车倾角？ 第一个问题可以通过安装在电机输出轴上的光码盘来测量得到
小车的车轮速度。如图 4 所示。利用控制单片机的计数器测量在固定 时间间隔内速度脉冲信号的个数可以反映电机的转速。
图4
第二个问题可以通过角度控制给定值来解决。给定小车直立控制 的设定值，在角度控制调节下，小车将会自动维持在一个角度。通过 前面小车直立控制算法可以知道，小车倾角最终是跟踪重力加速度Z 轴的角度。因此小车的倾角给定值与重力加速度Z轴角度相减，便可 以最终决定小车的倾角
第三个问题分析起来相对比较困难，远比直观进行速度负反馈分 析复杂。首先对一个简单例子进行分析。假设小车开始保持静止，然 后增加给定速度，为此需要小车往前倾斜以便获得加速度。在小车直 立控制下，为了能够有一个往前的倾斜角度，车轮需要往后运动，这 样会引起车轮速度下降（因为车轮往负方向运动了）。由于负反馈， 使得小车往前倾角需要更大。如此循环，小车很快就会倾倒。原本利 用负反馈进行速度控制反而成了“正”反馈。
为什么负反馈控制在这儿失灵了呢？原来在直立控制下的小车 速度与小车倾角之间传递函数具有非最小相位特性（在此省略了分 析），在反馈控制下容易造成系统的不稳定性。
为了保证系统稳定，往往取的小车倾角控制时间常数Tz很大。这 样便会引起系统产生两个共轭极点，而且极点的实部变得很小，使得系统的速度控制会产生的震荡现象。这个现象在实际参数整定的时候可以观察到。那么如何消除速度控制过程中的震荡呢？ 要解决控制震荡问题，在前面的小车角度控制中已经有了经验，那就是在控制反馈中增加速度微分控制。但由于车轮的速度反馈信号 中往往存在着噪声，对速度进行微分运算会进一步加大噪声的影响。 为此需要对上面控制方法进行改进。原系统中倾角调整过程时间常数 往往很大，因此可以将该系统近似为一个积分环节。将原来的微分环 节和这个积分环节合并，形成一个比例控制环节。这样可以保持系统 控制传递函数不变，同时避免了微分计算。
但在控制反馈中，只是使用反馈信号的比例和微分，没有利误差 积分，所以最终这个速度控制是有残差的控制。但是直接引入误差积 分控制环节，会增加系统的复杂度，为此就不再增加积分控制，而是 通过与角度控制相结合后在进行改进。
要求小车在原地停止，速度为0。但是由于采用的是比例控制， 如果此时陀螺仪有漂移，或者加速度传感器安装有误差，最终小车倾 角不会最终调整到0，小车会朝着倾斜的方向恒速运行下去。注意此 时车模不会像没有速度控制那样加速运行了，但是速度不会最终为0。 为了消除这个误差，可以将小车倾角设定量直接积分补偿在角度控制 输出中，这样就会彻底消除速度控制误差。第二点，由于加入了速度 控制，它可以补偿陀螺仪和重力加速度的漂移和误差。所以此时重力 加速度传感器实际上没有必要了。
此时小车在控制启动的时候，需要保持小车的垂直状态。此时陀螺仪的积分角度也初始化为0。当然如果电路中已经包括了重力加速度传感器，也可以保留这部分，从而提高小车的稳定性。在后面的 最终给定的控制方案中，保留了这部分的控制回路。
四、转向控制（PD 算法） 通过左右电机速度差驱动小车转向消除小车距离道路中心的偏
差。通过调整小车的方向，再加上车前行运动，可以逐步消除小车距 离中心线的距离差别。这个过程是一个积分过程，因此小车差动控制 一般只需要进行简单的比例控制就可以完成小车方向控制。但是由于 小车本身安装有电池等比较重的物体，具有很大的转动惯量，在调整 过程中会出现小车转向过冲现象，如果不加以抑制，会使得小车过度 转向而倒下。根据前面角度和速度控制的经验，为了消除小车方向控 制中的过冲，需要增加微分控制。
五、全方案整合 通过上面介绍，将车模直立行走主要的控制算法集中起来，如图5
图5
为了实现小车直立行走，需要采集如下信号：
（1）小车倾角速度陀螺仪信号，获得小车的倾角和角速度。
（2） 重力加速度信号
（z轴信号），补偿陀螺仪的漂移。该信号可以省略，有速度控制替 代。
（3） 小车电机转速脉冲信号，获得小车运动速度，进行速度控制。
（4） 小车转动速度陀螺仪信号，获得小车转向角速度，进行方向控 制。
在小车控制中的直立、速度和方向控制三个环节中，都使用了比 例微分（PD）控制，这三种控制算法的输出量最终通过叠加通过电机 运动来完成。
（1）小车直立控制：使用小车倾角的PD（比例、微分）控制；
g_fAngleControlOut = g_fCarAngle * g_fCarAngle_P + \
gyro[0] * g_fCarAngle_D ;
（2）小车速度控制：使用PD（比例、微分）控制；
g_fSpeedControlOutNew = (CAR_SPEED_SET - g_fCarSpeed) *
g_fCarSpeed_P +\
(CAR_POSITION_SET - g_fCarPosition) * g_fCarSpeed_I;
（3）小车方向控制：使用PD（比例、微分）控制。
speednow=-speedtarget*3.4 -gyro[2]*0.0015 ;
可通过单片机软件实现上述控制算法。 在上面控制过程中，车模的角度控制和方向控制都是直接将输出电压 叠加后控制电机的转速实现的。而车模的速度控制本质上是通过调节 车模的倾角实现的，由于车模是一个非最小相位系统，因此该反馈控 制如果比例和速度过大，很容易形成正反馈，使得车模失控，造成系 统的不稳定性。因此速度的调节过程需要非常缓慢和平滑。
六、PID 算法
图 6
控制相关的软件函数包括：
1.AngleCalculate：小车倾角计算函数。根据采集到的陀螺仪和重力加速度传感器的数值 计算小车角度和角速度。如果这部分的算法由外部一个运放实现，那么采集得到的直接是小车的角度和角速度，这部分算法可以省略。该函数是每 5 毫秒调用一次。
2.AngelControl：小车直立控制函数。根据小车角度和角速度计算小车电机的控制量。直 立控制是 5 毫秒调用一次。
3.SpeedControl：小车速度控制函数。根据小车采集到的电机转速和速度设定值，计算电 机的控制量。该函数是 100 毫秒调用一次。
4.SpeedControlOutput：速度输出平滑函数。由于速度是每 100 毫秒进行一次计算。为 了使得速度控制更加平滑，该函数将速度输出变化量平均分配到 20步 5 毫秒的控制周期中。
5.DirectionControlOutput：方向控制函数输出平滑函数。将方向控制的输出变化量平 均分配到 2 步 5 毫秒的控制周期中。
6.MotorOutput：电机输出量汇集函数。根据前面的直立控制、速度控制和方向控制所得 到的控制量进行叠加，分别得到左右两个电极的输出电压控制量。对叠加后的输出量进行饱
和处理。函数调用周期 5 毫秒。在此请大家注意速度控制量叠加的极性是负。
7.MotorSpeedOut：电机 PWM 输出计算函数。根据左右两个电极的输出控制量的正负 极性，叠加上一个小的死区数值，克服车模机械静态摩擦力。函数调用周期 5 毫秒。
8.SetMotorVoltage：PWM 输出函数：根据两个电机的输出量，计算出 PWM 控制寄存 器的数值，设置四个 PWM 控制寄存器的数值。函数调用周期 1 毫秒。
以上 9 个函数都是在 1 毫秒中断服务中进行被相互调用的。下图显示了这些函数之间的调用
与参数传递关系。在个函数附近也表明了调用的周期。
9.Chaoshengbo：加入超声波壁障模块：根据前方障碍物的距离检测，一旦检测到后， 通过直接 PWM 值输出（ g_fchaoshengbooutput），相障碍物反方上运动，无需算法实现。每
30 毫秒调用一次。
图 7
（注：以上函数框图为未添加旋转及超声波，添加方法与融合速度方式一样）
七、程序（只给出一部分内容）
（1） 时序总算法
1
单片机源程序如下:
- #include "upstandingcar.h"
- #include "I2C_MPU6050.h"
- #include "MOTOR.h"
- #include "led.h"
- #include "USART.H"
- #include "MPU6050.H"
- #include "UltrasonicWave.h"
- #include "stm32f10x_gpio.h"
- #include "math.h"
- #include <string.h>
- #include <stdlib.h>
- #include <stdio.h>
- /*****************************************/
- u8 newLineReceived = 0;
- u8 inputString[80] = {0};
- u8 ProtocolString[80] = {0};
- /*小车运行状态枚举*/
- enum{
- enSTOP = 0,
- enRUN,
- enBACK,
- enLEFT,
- enRIGHT,
- enTLEFT,
- enTRIGHT
- }enCarState;
- #define run_car '1'//按键前
- #define back_car '2'//按键后
- #define left_car '3'//按键左
- #define right_car '4'//按键右
- #define stop_car '0'//按键停
- int g_newcarstate = enSTOP; // 1前2后3左4右0停止
- char returntemp[] = "$0,0,0,0,0,0,0,0,0,0,0,0cm,8.2V#";
- char piddisplay[50] ="$AP";
- char manydisplay[80] ={0};
- char updata[80] ={0};
- /*****************多数据************************/
- u8 BST_u8MainEventCount; //主循环判断计数 在SysTick_Handler(void)中使用 每1ms加1
- u8 BST_u8SpeedControlCount; //速度控制循环计数 在SysTick_Handler(void)中使用 每5ms加1
- u8 BST_u8SpeedControlPeriod;
- u8 BST_u8DirectionControlPeriod;
- u8 BST_u8DirectionControlCount; //转向控制循环计数 在SysTick_Handler(void)中使用 每5ms加1
- u8 BST_u8trig;
- u8 ucBluetoothValue; //蓝牙接收数据
- float volt = 12.0;
- /******电机控制参数******/
- float BST_fSpeedControlOut; //速度控制PWM
- float BST_fSpeedControlOutOld;
- float BST_fSpeedControlOutNew;
- float BST_fAngleControlOut;
- float BST_fLeftMotorOut;
- float BST_fRightMotorOut;
- float BST_fCarAngle; //角度控制PWM
- float gyro_z;
- float gyrx;
- float gy0;
- /*-----角度环和速度环PID控制参数-----*///以下参考为重点调试参考，同电池电压有关，建议充好电再调试
- float BST_fCarAngle_P =91.3;// 91.3 //调大小时会左右摆，调大时会振动 请调到基本能够站立 P=91.3是用于给小车在运动过程使用
- float BST_fCarAngle_D =0.21; // 0.001 0.002 0.004 0.008 0.0010 0.011 调小时反应慢，调大时会干扰
- float BST_fCarSpeed_P=5.1;
- float BST_fCarSpeed_I=0.10;
- const double PID_Original[4] ={91.3, 0.21, 5.1, 0.10};
- char alldata[80];
- char *iap;
- /******速度控制参数******/
- s16 BST_s16LeftMotorPulse; //左电机脉冲数
- s16 BST_s16RightMotorPulse; //右电机脉冲数
- s32 BST_s32LeftMotorPulseOld;
- s32 BST_s32RightMotorPulseOld;
- s32 BST_s32LeftMotorPulseSigma; //50ms左电机叠加值
- s32 BST_s32RightMotorPulseSigma; //50ms右电机叠加值
- float BST_fCarSpeed; //测速码盘得出的车速
- float BST_fCarSpeedOld;
- float BST_fCarPosition; //测速码盘通过计算得到的小车位移
- /*-----悬停参数-----*/
- int leftstop=0;
- int rightstop=0;
- int stopflag=0;
- /******超声波********/
- float fchaoshengbo = 0; //超声波输出量
- float juli = 0; //超声波距离
- /******蓝牙控制参数******/
- float BST_fBluetoothSpeed; //蓝牙控制车速
- float BST_fBluetoothDirectionNew; //用于平缓输出车速使用
- float BST_fBluetoothDirectionSL; //左转标志位 由于PWM转向输出使用判断输出方式 固需要标志位
- float BST_fBluetoothDirectionSR; //右转标志位 由于PWM转向输出使用判断输出方式 固需要标志位
- int chaoflag=0;
- int x,y1,z1,y2,z2,flagbt;
- int g_autoup = 0;
- int g_uptimes = 5000; //自动上报间隔
- char charkp[10],charkd[10],charksp[10],charksi[10];
- char lspeed[10],rspeed[10],daccel[10],dgyro[10],csb[10],vi[10];
- char kp,kd,ksp,ksi;
- float dac = 0,dgy = 0;
- /************旋转*****************/
- float BST_fBluetoothDirectionL; //左旋转标志位 由于PWM旋转输出使用判断输出方式 固需要标志位
- float BST_fBluetoothDirectionR; //右旋转标志位 由于PWM旋转输出使用判断输出方式 固需要标志位
- int driectionxco=800;
- //******卡尔曼参数************
-
- float Q_angle=0.001;
- float Q_gyro=0.003;
- float R_angle=0.5;
- float dt=0.005; //dt为kalman滤波器采样时间;
- char C_0 = 1;
- float Q_bias, Angle_err;
- float PCt_0=0, PCt_1=0, E=0;
- float K_0=0, K_1=0, t_0=0, t_1=0;
- float Pdot[4] ={0,0,0,0};
- float PP[2][2] = { { 1, 0 },{ 0, 1 } };
- void SendAutoUp(void);
- /**********延时子函数****************/
- void delay_nms(u16 time)
- {
- u16 i=0;
- while(time--)
- {
- i=12000; //自己定义
- while(i--) ;
- }
- }
- /***********************************/
- /***************************************************************
- ** 函数名称: CarUpstandInit
- ** 功能描述: 全局变量初始化函数
- ***************************************************************/
- void CarUpstandInit(void)
- {
-
-
- BST_s16LeftMotorPulse = BST_s16RightMotorPulse = 0; //左右脉冲值 初始化
- BST_s32LeftMotorPulseSigma = BST_s32RightMotorPulseSigma = 0; //叠加脉冲数 初始化
- BST_fCarSpeed = BST_fCarSpeedOld = 0; //平衡小车车速 初始化
- BST_fCarPosition = 0; //平衡小车位移量 初始化
- BST_fCarAngle = 0; //平衡小车车速 初始化
- BST_fAngleControlOut = BST_fSpeedControlOut = BST_fBluetoothDirectionNew = 0; //角度PWM、车速PWM、蓝牙控制PWM 初始化
- BST_fLeftMotorOut = BST_fRightMotorOut = 0; //左右车轮PWM输出值 初始化
- BST_fBluetoothSpeed = 0; //蓝牙控制车速值 初始化
- BST_fBluetoothDirectionL =BST_fBluetoothDirectionR= 0; //蓝牙控制左右旋转标志位 初始化
- BST_fBluetoothDirectionSL =BST_fBluetoothDirectionSR= 0; //蓝牙控制左右转向标志位 初始化
-
- BST_u8MainEventCount=0; //用于5ms定时器子程序SysTick_Handler(void)中总中断计数位
- BST_u8SpeedControlCount=0; //用于5ms定时器子程序SysTick_Handler(void)中50ms速度平衡融入计数位
- BST_u8SpeedControlPeriod=0; //用于5ms定时器子程序SysTick_Handler(void)中50ms速度平衡融入计数位
- fchaoshengbo=0; //用于5ms定时器子程序SysTick_Handler(void)中超声波平衡融入计数位
-
-
- }
- void ResetPID()
- {
- if(BST_fCarAngle_P != PID_Original[0])
- {
- BST_fCarAngle_P = PID_Original[0];
- }
- if(BST_fCarAngle_D != PID_Original[1])
- {
- BST_fCarAngle_D = PID_Original[1];
- }
- if(BST_fCarSpeed_P != PID_Original[2])
- {
- BST_fCarSpeed_P = PID_Original[2];
- }
- if(BST_fCarSpeed_I != PID_Original[3])
- {
- BST_fCarSpeed_I = PID_Original[3];
- }
- }
- /***************************************************************
- ** 函数名称: AngleControl
- ** 功能描述: 角度环控制函数
- ***************************************************************/
- void AngleControl(void)
- {
- if(flagbt==1)
- {
- BST_fCarAngle_P=0;
- BST_fCarAngle_P=y1*1.71875;
- }
- if(flagbt==2)
- {
- BST_fCarAngle_D=0;
- BST_fCarAngle_D=(z1-64)*0.15625;
- }
- dac=accel[2];
- dgy=gyro[2];
-
- BST_fCarAngle = Roll - CAR_ZERO_ANGLE; //DMP ROLL滚动方向角度与预设小车倾斜角度值的差得出角度
- BST_fAngleControlOut = BST_fCarAngle * BST_fCarAngle_P + gyro[0] * BST_fCarAngle_D ; //角度PD控制
- }
- /***************************************************************
- ** 函数名称: SetMotorVoltageAndDirection
- ** 功能描述: 电机转速
- ***************************************************************/
- void SetMotorVoltageAndDirection(s16 s16LeftVoltage,s16 s16RightVoltage)
- {
- u16 u16LeftMotorValue;
- u16 u16RightMotorValue;
-
- if(s16LeftVoltage<0) //当左电机PWM输出为负时 PB14设为正 PB15设为负 （PB14 15 分别控制TB6612fng驱动芯片，逻辑0 1可控制左电机正转反转）
- {
- GPIO_SetBits(GPIOB, GPIO_Pin_14 );
- GPIO_ResetBits(GPIOB, GPIO_Pin_15 );
- s16LeftVoltage = (-s16LeftVoltage);
- }
- else
- {
- GPIO_SetBits(GPIOB, GPIO_Pin_15 ); //当左电机PWM输出为正时 PB14设为负 PB15设为正 （PB14 15 分别控制TB6612fng驱动芯片，逻辑0 1可控制左电机正转反转）
- GPIO_ResetBits(GPIOB, GPIO_Pin_14 );
- s16LeftVoltage = s16LeftVoltage;
- }
- if(s16RightVoltage<0)
- { //当右电机PWM输出为负时 PB12设为正 PB13设为负 （PB12 13 分别控制TB6612fng驱动芯片，逻辑0 1可控制左电机正转反转）
- GPIO_SetBits(GPIOB, GPIO_Pin_13 );
- GPIO_ResetBits(GPIOB, GPIO_Pin_12 );
- s16RightVoltage = (-s16RightVoltage);
- }
- else //当右电机PWM输出为正时 PB12设为负 PB13设为正 （PB12 13 分别控制TB6612fng驱动芯片，逻辑0 1可控制左电机正转反转）
- {
- GPIO_SetBits(GPIOB, GPIO_Pin_12 );
- GPIO_ResetBits(GPIOB, GPIO_Pin_13 );
-
- s16RightVoltage = s16RightVoltage;
- }
-
- u16RightMotorValue= (u16)s16RightVoltage;
- u16LeftMotorValue = (u16)s16LeftVoltage;
- TIM_SetCompare3(TIM2,u16LeftMotorValue); //TIM2与 u16RightMotorValue对比，不相同则翻转波形，调节PWM占空比
- TIM_SetCompare4(TIM2,u16RightMotorValue); //TIM3与 u16LeftMotorValue对比，不相同则翻转波形，调节PWM占空比
- #if 1 /*判断车辆 是否悬停或跌倒，调试用*/
-
- if(Pitch>10||Pitch<-10&BST_fBluetoothDirectionSR==0&BST_fBluetoothDirectionSL==0)
- {
- TIM_SetCompare3(TIM2,0);
- TIM_SetCompare4(TIM2,0);
- stopflag=1;
- }
- else stopflag=0;
-
- if(BST_fCarAngle > 50 || BST_fCarAngle < (-50))
- {
- TIM_SetCompare3(TIM2,0);
- TIM_SetCompare4(TIM2,0);
- stopflag=1;
- }
- else stopflag=0;
- #endif
- }
- /***************************************************************
- ** 函数名称: MotorOutput
- ** 功能描述: 电机输出函数
- 将直立控制、速度控制、方向控制的输出量进行叠加,并加
- 入死区常量，对输出饱和作出处理。
- ***************************************************************/
- void MotorOutput(void) //电机PWM输出函数
- {
- //右电机转向PWM控制融合平衡角度、速度输出
- BST_fLeftMotorOut = BST_fAngleControlOut +BST_fSpeedControlOutNew + BST_fBluetoothDirectionNew;//+directionl - BST_fBluetoothDirectionNew; //左电机转向PWM控制融合平衡角度、速度输出
- BST_fRightMotorOut = BST_fAngleControlOut +BST_fSpeedControlOutNew - BST_fBluetoothDirectionNew;//-directionl+ BST_fBluetoothDirectionNew; //右电机转向PWM控制融合平衡角度、速度输出
-
- if((s16)BST_fLeftMotorOut > MOTOR_OUT_MAX) BST_fLeftMotorOut = MOTOR_OUT_MAX;
- if((s16)BST_fLeftMotorOut < MOTOR_OUT_MIN) BST_fLeftMotorOut = MOTOR_OUT_MIN;
- if((s16)BST_fRightMotorOut > MOTOR_OUT_MAX) BST_fRightMotorOut = MOTOR_OUT_MAX;
- if((s16)BST_fRightMotorOut < MOTOR_OUT_MIN) BST_fRightMotorOut = MOTOR_OUT_MIN;
-
- SetMotorVoltageAndDirection((s16)BST_fLeftMotorOut,(s16)BST_fRightMotorOut);
-
- }
- void GetMotorPulse(void) //采集电机速度脉冲
- {
- //////////////////////////////////此部分为外部中断计算脉冲/////////////////////////////////////
- uint16_t u16TempLeft;
- uint16_t u16TempRight;
-
- u16TempLeft = TIM_GetCounter(TIM3); // TIM3定时器计算调用
- u16TempRight= TIM_GetCounter(TIM4); // TIM4定时器计算调用
- leftstop=u16TempLeft;
- rightstop=u16TempRight;
- TIM_SetCounter(TIM3,0);//TIM3->CNT = 0;
- TIM_SetCounter(TIM4,0);//TIM4->CNT = 0; //清零
- BST_s16LeftMotorPulse=u16TempLeft;
- BST_s16RightMotorPulse=(-u16TempRight);
-
- BST_s32LeftMotorPulseSigma +=BST_s16LeftMotorPulse; //脉冲值叠加 40ms叠加值
- BST_s32RightMotorPulseSigma +=BST_s16RightMotorPulse; //脉冲值叠加 40ms叠加值
- }
- /***************************************************************
- ** 函数名称: SpeedControl
- ** 功能描述: 速度环控制函数
- ***************************************************************/
- void SpeedControl(void)
- {
-
-
- BST_fCarSpeed = (BST_s32LeftMotorPulseSigma + BST_s32RightMotorPulseSigma );// * 0.5 ; //左右电机脉冲数平均值作为小车当前车速
- BST_s32LeftMotorPulseSigma =BST_s32RightMotorPulseSigma = 0; //全局变量 注意及时清零
- BST_fCarSpeedOld *= 0.7;
- BST_fCarSpeedOld +=BST_fCarSpeed*0.3;
-
- BST_fCarPosition += BST_fCarSpeedOld; //路程 即速度积分 1/11 3:03
- BST_fCarPosition += BST_fBluetoothSpeed; //融合蓝牙给定速度
- BST_fCarPosition += fchaoshengbo; //融合超声波给定速度
- if(stopflag==1)
- {
- BST_fCarPosition=0;
-
- }
-
- //积分上限设限//
- if((s32)BST_fCarPosition > CAR_POSITION_MAX) BST_fCarPosition = CAR_POSITION_MAX;
- if((s32)BST_fCarPosition < CAR_POSITION_MIN) BST_fCarPosition = CAR_POSITION_MIN;
-
- if(flagbt==3)
- {
- BST_fCarSpeed_P=0;
- BST_fCarSpeed_P=(y2-128)*0.46875;
- }
- if(flagbt==4)
- {
- BST_fCarSpeed_I=0;
- BST_fCarSpeed_I=(z2-192)*0.15625;
- }
-
-
- BST_fSpeedControlOutNew = (BST_fCarSpeedOld -CAR_SPEED_SET ) * BST_fCarSpeed_P + (BST_fCarPosition - CAR_POSITION_SET ) * BST_fCarSpeed_I; //速度PI算法 速度*P +位移*I=速度PWM输出
-
- }
- /***************************************************************
- ** 函数名称: BluetoothControl
- ** 功能描述: 蓝牙控制函数
- 手机发送内容
- 前：0x01 后：0x02
- 左：0x04 右：0x03
- 停止：0x07
- 功能键：（旋转）
- 左旋转:0x05 右旋转：0x06
- 停转：0x07
- ** 输 入:
- ** 输 出:
- switch (x)
- {
- case 0x00 : BST_fCarAngle_P=BST_fCarAngle_D=BST_fCarSpeed_P=BST_fCarSpeed_I=0;
- case 0x01 : BST_fBluetoothSpeed = 3000 ;chaoflag=1; break; //向前速度 250
- case 0x02 : BST_fBluetoothSpeed = (-3000);chaoflag=1; break; //后退速度 -250
- case 0x03 : BST_fBluetoothDirectionNew= -300; chaoflag=1;break ;//左旋
- case 0x04 : BST_fBluetoothDirectionNew= 300; chaoflag=1;break ;//右旋转
- case 0x05 : BST_fBluetoothDirectionNew= driectionxco; chaoflag=1;break ;//左旋
- case 0x06 : BST_fBluetoothDirectionNew= -driectionxco; chaoflag=1;break ;//右旋转
- case 0x07 : BST_fBluetoothDirectionL =0; BST_fBluetoothDirectionR = 0; BST_fBluetoothDirectionSL =0; BST_fBluetoothDirectionSR = 0;fchaoshengbo=0;BST_fBluetoothDirectionNew=0;chaoflag=0; break; //停
- case 0x08 : BST_fBluetoothDirectionSL =0; BST_fBluetoothDirectionSR = 0; fchaoshengbo=0;BST_fBluetoothDirectionNew=0;chaoflag=0;break; //停旋转
- case 0x09 : BST_fBluetoothSpeed = 0 ; break;
- case 0x0A : flagbt=1;break;
- case 0x0B : flagbt=2;break;
- case 0x0C : flagbt=3;break;
- case 0x0D : flagbt=4;break;
- default : BST_fBluetoothSpeed = 0;flagbt=0; BST_fBluetoothDirectionL=BST_fBluetoothDirectionR = 0;BST_fBluetoothDirectionSR=BST_fBluetoothDirectionSL=0;chaoflag=0;break;
- }
- ***************************************************************/
- int num = 0;
- u8 startBit = 0;
- int int9num =0;
- void USART3_IRQHandler(void)
- {
- u8 uartvalue = 0;
- if (USART_GetFlagStatus(USART3, USART_FLAG_ORE) != RESET)//注意！不能使用if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)来判断
- {
- USART_ClearFlag(USART3, USART_FLAG_ORE); //读SR其实就是清除标志
- USART_ReceiveData(USART3);
- }
-
- if(USART_GetFlagStatus(USART3,USART_FLAG_RXNE)!=RESET)
- {
- USART_ClearITPendingBit(USART3, USART_IT_RXNE);
- uartvalue = USART3->DR;
- if(uartvalue == '
- )
- {
- startBit = 1;
- num = 0;
- }
- if(startBit == 1)
- {
- inputString[num] = uartvalue;
- }
- if (startBit == 1 && uartvalue == '#')
- {
-
- newLineReceived = 1;
- startBit = 0;
- int9num = num;
-
- }
- num++;
- if(num >= 80)
- {
- num = 0;
- startBit = 0;
- newLineReceived = 0;
- }
-
- }
-
-
- }
- /**********************超声波距离计算***************************/
- void chaoshengbo(void)
- {
- if(chaoflag==0)
- {
-
- juli=TIM_GetCounter(TIM1)*5*34/200.0;
-
- if(juli <= 4.00) //判断若距离小于8cm，小车输出向后PWM值。
- {
- fchaoshengbo= (-300);
- }
- else if(juli >= 5 & juli <= 8)
- {
- fchaoshengbo=500;
- }
- else
- {
- fchaoshengbo=0; //距离大于8cm ，超声波PWM输出为0
- }
- }
-
- //寄生此上报数据
- //增加自动上报 10ms进一次，故10*50ms = 500ms
- //SendAutoUp();
- }
- /*计算上报的数据*/
- void CalcUpData()
- {
- float ls, rs, sLence;
- short s_Acc, s_Gyro;
-
- if(g_autoup == 1)
- {
- //CLI();
- ls = BST_fLeftMotorOut;
- rs = BST_fRightMotorOut;
- s_Acc = accel[1];
- s_Gyro = gyro[0];
- sLence = juli;
- //SEI();
-
- dac=(s_Acc/16384.0f)*9.8f;
- dgy=((s_Gyro-128.1f)/131.0f);
-
- ls=ls/3.91;
- rs=rs/3.91;
-
- memset(manydisplay, 0x00, 80);
- memcpy(manydisplay, "$LV", 4);
-
- memset(lspeed, 0x00, sizeof(lspeed));
- memset(rspeed, 0x00, sizeof(rspeed));
- memset(daccel, 0x00, sizeof(daccel));
- memset(dgyro, 0x00, sizeof(dgyro));
- memset(csb, 0x00, sizeof(csb));
- memset(vi, 0x00, sizeof(vi));
-
- if((ls <= 1000) && (ls >= -1000))
- sprintf(lspeed,"%3.2f",ls);
- else
- {
- //sprintf(str,"$AutoUpError!ls=%3.2f#",ls);
- //UART3_Send_Char(str); //返回协议数据包
- return;
- }
-
-
- if((rs <= 1000) && (rs >= -1000))
- sprintf(rspeed,"%3.2f",rs);
- else
- {
- //sprintf(str,"$AutoUpError!rs=%3.2f#",rs);
- //UART3_Send_Char(str); //返回协议数据包
- return;
- }
-
- if((dac > -20) && (dac < 20))
- sprintf(daccel,"%3.2f",dac);
- else
- {
- //sprintf(str,"$AutoUpError!dac=%3.2f#",dac);
- //UART3_Send_Char(str); //返回协议数据包
- return;
- }
-
- if((dgy > -3000) && (dgy < 3000))
- sprintf(dgyro,"%3.2f",dgy);
- else
- {
- //sprintf(str,"$AutoUpError!dgy=%3.2f#",dgy);
- //UART3_Send_Char(str); //返回协议数据包
- return;
- }
-
- if((sLence >= 0) && (sLence < 10000))
- sprintf(csb,"%3.2f",sLence);
- else
- {
- //sprintf(str,"$AutoUpError!juli=%3.2f#",juli);
- //UART3_Send_Char(str); //返回协议数据包
- return;
- }
-
- if((volt >= 0) && (volt < 20))
- sprintf(vi,"%3.2f",volt);
- else
- {
- //sprintf(str,"$AutoUpError!volt=%3.2f#",volt);
- //UART3_Send_Char(str); //返回协议数据包
- return;
- }
-
- strcat(manydisplay,lspeed);
- strcat(manydisplay,",RV");
- strcat(manydisplay,rspeed);
- strcat(manydisplay,",AC");
- strcat(manydisplay,daccel);
- strcat(manydisplay,",GY");
- strcat(manydisplay,dgyro);
- strcat(manydisplay,",CSB");
- strcat(manydisplay,csb);
- strcat(manydisplay,",VT");
- strcat(manydisplay,vi);
- strcat(manydisplay,"#");
- memset(updata, 0x00, 80);
- memcpy(updata, manydisplay, 80);
- }
-
- }
- /*
- 自动上报
- */
- void SendAutoUp(void)
- {
- g_uptimes --;
- if ((g_autoup == 1) && (g_uptimes == 0))
- {
- CalcUpData();
- UART3_Send_Char(updata); //返回协议数据包
- }
- if(g_uptimes == 0)
- g_uptimes = 5000;
- }
- int StringFind(const char *pSrc, const char *pDst)
- {
- int i, j;
- for (i=0; pSrc[i]!='\0'; i++)
- {
- if(pSrc[i]!=pDst[0])
- continue;
- j = 0;
- while(pDst[j]!='\0' && pSrc[i+j]!='\0')
- {
- j++;
- if(pDst[j]!=pSrc[i+j])
- break;
- }
- if(pDst[j]=='\0')
- return i;
- }
- return -1;
- }
- void CarStateOut(void)
- {
- switch (g_newcarstate)
- {
- case enSTOP: //停止
- {
- BST_fBluetoothSpeed = 0;
- fchaoshengbo=0;
- BST_fBluetoothDirectionNew=0;
- chaoflag=0;
- } break;
- case enRUN: //向前速度 800
- {
- BST_fBluetoothDirectionNew= 0;
- //BST_fSpeedControlOutNew=0;
- BST_fBluetoothSpeed = 800 ;
- chaoflag=1;
- }break;
- case enLEFT://左转
- {
- BST_fBluetoothDirectionNew= -300;
- chaoflag=1;
- }break;
-
- case enRIGHT: //右转
- {
- BST_fBluetoothDirectionNew= 300;
- chaoflag=1;
- }break;
-
- case enBACK: //后退速度 -800
- {
- BST_fBluetoothDirectionNew= 0;
- //BST_fSpeedControlOutNew=0;
- BST_fBluetoothSpeed = (-800);
- chaoflag=1;
- }break;
-
- case enTLEFT: //左旋
- {
- BST_fBluetoothDirectionNew = -driectionxco;
- chaoflag=1;
- }break;
- case enTRIGHT: //右旋
- {
- BST_fBluetoothDirectionNew = driectionxco;
- chaoflag=1;
- }break;
-
- default: BST_fBluetoothSpeed = 0; break; //停止
- }
- }
- void ProtocolGetPID(void)
- {
- memset(piddisplay, 0x00, sizeof(piddisplay));
- memcpy(piddisplay, "$AP", 4);
- if(BST_fCarAngle_P >= 0 && BST_fCarAngle_P <= 100)
- {
- sprintf(charkp,"%3.2f",BST_fCarAngle_P);
- }
- else
- {
- UART3_Send_Char("$GetPIDError#"); //返回协议数据包
- return;
- }
-
- if(BST_fCarAngle_D >= 0 && BST_fCarAngle_D <= 100)
- {
- sprintf(charkd,"%3.2f",BST_fCarAngle_D);
- }
- else
- {
- UART3_Send_Char("$GetPIDError#"); //返回协议数据包
- return;
- }
-
- if(BST_fCarSpeed_P >= 0 && BST_fCarSpeed_P <= 100)
- {
- sprintf(charksp,"%3.2f",BST_fCarSpeed_P);
- }
- else
- {
- UART3_Send_Char("$GetPIDError#"); //返回协议数据包
- return;
- }
- if(BST_fCarSpeed_I >= 0 && BST_fCarSpeed_I <= 100)
- {
- sprintf(charksi,"%3.2f",BST_fCarSpeed_I);
- }
- else
- {
- UART3_Send_Char("$GetPIDError#"); //返回协议数据包
- return;
- }
-
-
- strcat(piddisplay,charkp);
- strcat(piddisplay,",AD");
- strcat(piddisplay,charkd);
- strcat(piddisplay,",VP");
- strcat(piddisplay,charksp);
- strcat(piddisplay,",VI");
- strcat(piddisplay,charksi);
- strcat(piddisplay,"#");
-
- UART3_Send_Char(piddisplay); //返回协议数据包
- }
- void ProtocolCpyData(void)
- {
- memcpy(ProtocolString, inputString, num+1);
- memset(inputString, 0x00, sizeof(inputString));
- }
- /***************************************************************************
- 串口协议数据解析
- ***************************************************************************/
- void Protocol(void)
- {
- //USART_ITConfig(USART3, USART_IT_RXNE, DISABLE);//禁能接收中断
- //判断数据包有效性
-
-
- switch (ProtocolString[1])
- {
- case run_car: g_newcarstate = enRUN; UART3_Send_Char(returntemp);break;
- case back_car: g_newcarstate = enBACK; UART3_Send_Char(returntemp);break;
- case left_car: g_newcarstate = enLEFT; UART3_Send_Char(returntemp);break;
- case right_car: g_newcarstate = enRIGHT; UART3_Send_Char(returntemp);break;
- case stop_car: g_newcarstate = enSTOP; UART3_Send_Char(returntemp);break;
- default: g_newcarstate = enSTOP; break;
-
- }
- /*防止数据丢包*/
- if(strlen((const char *)ProtocolString)<21)
- {
- newLineReceived = 0;
- memset(ProtocolString, 0x00, sizeof(ProtocolString));
- //USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);//使能接收中断
- UART3_Send_Char("$ReceivePackError#"); //返回协议数据包
- return;
- }
- if (ProtocolString[3] == '1') //左摇
- {
- g_newcarstate = enTLEFT;
- UART3_Send_Char(returntemp); //返回协议数据包
-
- }
-
-
- if (ProtocolString[3] == '2') //右摇
- {
- g_newcarstate = enTRIGHT;
- UART3_Send_Char(returntemp); //返回协议数据包
- ……………………
- …………限于本文篇幅 余下代码请从51黑下载附件…………
所有资料51hei提供下载:
源码：
STM32平衡车.rar
(697.14 KB, 下载次数: 1254)
|
[复制链接]
顶上，谢谢楼主分享
