15年全国电赛风力摆控制系统（一点也不难）

说明：本文章适用于STM32初学者，想完成一个好玩且有深度的项目但不知道从何下手的同学。

任务需求

我们要做一个东西必须要清楚我们的目的是什么？这个东西要干什么，有什么作用，最终我们理想的效果是什么？在此我罗列一下我们做出来的风力摆所应达到的效果需求.如下所示：

一长约 60cm~70cm 的细管上端用万向节固定在支架上，下方悬挂一组（2~4 只）直流风机，构成一风力摆，如图1 所示。风力摆上安装一向下的激光笔，静止时，激光笔的下端距地面不超过 20cm。设计一测控系统，控制驱动各风机使风力摆按照一定规律运动，激光笔在地面画出要求的轨迹。

（1）从静止开始，15s 内控制风力摆做类似自由摆运动，使激光笔稳定地在地面画出一条长度不短于 50cm 的直线段，其线性度偏差不大于±2.5cm，并且具有较好的重复性；

（2）从静止开始，15s 内完成幅度可控的摆动，画出长度在30~60cm 间可设置，长度偏差不大于±2.5cm 的直线段，并且具有较好的重复性；

（3）可设定摆动方向，风力摆从静止开始，15s 内按照设置的方向（角度）摆动，画出不短于20cm 的直线段；

（4）将风力摆拉起一定角度（30°~45°）放开，5s 内使风力摆制动达到静止状态。

（5）以风力摆静止时激光笔的光点为圆心，驱动风力摆用激光笔在地面画圆，30s 内需重复3 次；圆半径可在15~35cm 范围内设置，激光笔画出的轨迹应落在指定半径±2.5cm 的圆环内；

（6）在要求（5）后继续作圆周运动，在距离风力摆 1~2m 距离内用一台 50~60W 台扇在水平方向吹向风力摆，台扇吹 5s 后停止，风力摆能够在5s 内恢复（5）规定的圆周运动，激光笔画出符合要求的轨迹；

任务分析

风力摆是通过六轴传感器（MPU6050）检测平台的水平角度进行反馈，
控制四个 8520 空心杯电机的转动，带动螺旋桨转动实现激光点按照特定
轨迹运动的机构。那么让他怎么摆呢？我们必须需要一个陀螺仪去测量我们的速度信息（角度，角速度，角加速度），对这些速度信息进行处理然后送入我们的PID控制，进行姿态的控制。
并且为避免硬件部分导致的效果不好，尽量避免在整个摆杆的外部有很多线去拉扯。所以我们制作PCB，并且电池装在摆杆上，仅需要两根电源线即可。一体化设计，尽可能的做到轻量化，小巧而精致；

材料（基础部分）

MPU6050陀螺仪：获取风力摆姿态信息。
HC-05蓝牙模块：无线切换风力摆运动状态。
空心杯电机：体积小，功率大，抗干扰较强
电机驱动(SI2302):驱动空心杯电机*4
点状激光头

硬件说明

点状激光头 –固定在底部主控板上

MPU6050 陀螺仪模块-焊接在中间板上

四个电机（ABCD）对应固定在主控板上，电机两个正转，两个反转，分别对应 X、Y 的+/-两个方向；螺旋桨两正两反，在这里螺旋桨做推力方向，如果上电没有推力可以将扇叶反着安装即可，这样可以良好的避免异常情况下的射桨等极端情况。

MOS（SI2302）原理图

MPU6050陀螺仪原理图

PCB 效果图

模块使用方法

HC-05蓝牙模块
使用前要先在AT模式进行配置。配置蓝牙名称、密码、波特率、主从机等（AT指令），蓝牙模块指示灯快闪是普通模式，慢闪是进入AT模式。
我们拿到蓝牙模块后
（1）通过杜邦线将蓝牙模块的VCC,GND,TX,RX分别接入USB TO TTL模块（串口）的VCC,GND,RX,TX引脚。

(2)打开串口上位机软件本人使用的是正点原子配套的XCOM,先按住蓝牙模块上的黑色按键在上电，我们可以看到蓝牙模块上的指示灯开始慢闪，这样我们就进入了AT模式。❗❗❗注意：以这种方式进入AT模式，我们在串口上位机中波特率要设置为38400才能保持正常通信。
（3）在上位机中发送AT注意要以回车结尾，若收到OK，说明通信成功之后就可以设置其他内容了。我们可以查询手册，去发送相关指令，我只提咱们常用的几个指令：以下若配置成功则返回OK；

设置/查询设备名称：

AT+NAME =Beijing\r\n ——设置模块设备名为：“Beijing”
AT+NAME?\r\n ——查询模块设备名

设置/查询-配对码：
AT+PSWD=Param\r\n Param：配对码（自己随意设置）
AT+ PSWD？\r\n 默认值：1234
设置/查询-串口参数：
设置串口波特率：115200，2 位停止位，Even 校验
AT+UART=115200，1,2,\r\n
AT+ UART？——查询波特率

（4）设置波特率为115200；密码，名称随便设随自己心意；这样我们就配置好了，到时候接单片机串口通信的管脚就好（随程序而定）

MPU6050陀螺仪

原理大家有兴趣的可以百度，我就不在此赘述。
简言之，这个模块自带DMP可以解算出欧拉角，MPU6050得出来的姿态角有三种：PITCH（俯仰角）、ROLL（横滚角）、YAW（航向角）。我们安装时，要平放着放在车上。那么我们的车要立起来，要么是俯仰角要么是横滚角，航向角是Z轴上的我们暂时不用。
SCL和SDA是连接MCU的IIC接口，MCU通过这个IIC接口来控制MPU6050,INT是中断触发接口，陀螺仪每成功捕捉到一次数据，INT引脚的电平就会拉低触发外部中断。如果接GND，则MPU6050的IIC地址是:0X68，如果接VDD，则是0X69，我们默认接地。
我们所用到的管脚：
VCC,GND,SCL,SDA,AD0,INT（关键）
❗❗❗陀螺仪要水平放置不然可能会自检过不了，从而无法使用。

原理分析（理清思路）

风力摆，我们可以理想化的看成一根长杆做单摆运动，而我们学过大学物理可以建立相应的物理模型（单摆运动模型）
我们以只有一个方向摆动，即x轴为例列出理想化的单摆模型

其中f(x)表示位移、A 表示振幅、表示角速度、φ 表示相位，周期 T 与之间的关系为：T=2π/w

根据公式我们可以估算出单摆运动的周期

取 L(摆杆长度)为1m
g=重力加速度，我们取 10m/s²
那么T=2*3.14 * (1/10)½=1.98s

我们可以取极限L=0.3或L=2得到大家的周期的取值范围有 T（1.09 ~2.81）s，然后我们就可以实测然后我们就可以根据单摆周期的实验法去测得我们的周期：这里我们测量的是十次单摆周期秒表计时为 11.5s。

对于f(x) = A* sin(ωt +φ)
A：振幅，是我们需要设定的摆动幅度；
φ：相位，在这里可以理解为开始时对应的位移，我们设为 0；
ω：2π/T (已知常数)
t：自变量，u32 TimeCnt;//运动时间计数器
在 5ms 定时器中断中 TimeCnt+=5；
显然公式可化为

显然我们采用的方法是将连续的函数用离散值去代替，类似于积分的思想，将一个完整周期分成一个个很短的时间，而我们这一个个很短的时间就使用我们MCU的计时器来完成，我们设定一个5ms的外部中断，每触发一次中断TimeCnt+=5，这里的TimeCnt就代表运动的时间，而每5ms取的这些点映射到2π的周期上，就记录对应点位移随着时间在 A 范围内正弦函数周期性变化情况；从而控制风力摆在X轴摆动的位移。其他方向类比即可，而方向的调整可利用xy方向振幅的变化来改变摆动的角度，以及xy方向相差的相位即可改变摆动的图案。

风力摆实际位移：
离地高度 float Height=675（单位 mm）；
角度由陀螺仪模块反馈获得，X 方向为 Roll，Y 方向为 Pitch
激光点位置 float Measure_X，可以通过测得的角度三角转换得出
测量值：Measure_X=tan(Roll)* Height;
加入初始值、弧度单位换算即：
Measure_X=(float)tan((Roll-ZHONGZHI_B)/1802PI)*Height;

风力摆理论位移：
我们要控制激光点的位置满足单摆运动因此有目标值 Target_X：
Alpha=(float)TimeCnt/Period2PI; //PI=π
Target_X=Amplitude_xsin(Alpha); //X 方向目标值函数
PID 公式：pwm=Kpe(k)+Ki*∑e(k)+Kd[e（k）-e(k-1)]
我们就可以通过控制 Amplitude_x 来控制摆动的幅度

之后将实际与理论值装载入PID闭环中，完成风力摆的控制。

李萨如图形

定义：李萨如图形由在互相垂直的方向上的两个频率成简单整数比的简谐振动所合成的规则的、稳定的闭合曲线；
要点：互相垂直 · 简谐运动 · 频率成整数比
风力摆系统满足要求，我们来看下李萨如图形的数学定义：

其中， n≥1 且 0≤φ≤π/2 , 设 n=q/p; n 称为曲线的参数，是两个正弦振动的频率比;

当风力摆同时参与两个互相垂直方向上的简谐运动，风力摆的位移是这两个振动的矢量和
又有李萨如图形特殊情形：
若 a=b，n=1，则曲线是椭圆
若 φ=π/2 ，则这椭圆其实是圆
若 φ=0 ，则这椭圆其实是线段

则可以引出我们的PID控制系统入口参数就是风力摆理论上在X/Y方向对应的位移，及实际上在X/Y方向的位移。我们只要将X、Y对应方向上的位移用代码表示出来，传入PID控制系统即可。我们只需要通过蓝牙去控制振幅比例，相位差即可实现直线，画圆甚至任意图案。

PID框架搭建

函数功能：位置式PID控制器
入口参数：风力摆实际位移，目标位移
返回值：电机PWM
根据位置式离散PID公式
pwm=Kpe(k)+Ki∑e(k)+Kd[e（k）-e(k-1)]
e(k)代表本次偏差
e(k-1)代表上一次的偏差
∑e(k)代表e(k)以及之前的偏差的累积和;其中k为1,2,k;
pwm代表输出

int Position_PID_X (float value,float Target)
{   static float Bias,Pwm,Integral_bias,Last_Bias;Bias=value-Target;                                    //计算偏差Integral_bias+=Bias;                                  //求出偏差的积分Pwm=Position_KP*Bias+                                 //PID控制器比例项Position_KI*Integral_bias+                        //PID控制器积分项Position_KD*(Bias-Last_Bias);                     //PID控制器微分项 Last_Bias=Bias;                                       //保存上一次偏差 return Pwm;                                           //增量输出
}int Position_PID_Y (float value,float Target)
{   static float Bias,Pwm,Integral_bias,Last_Bias;Bias=value-Target;                                    //计算偏差Integral_bias+=Bias;                                  //求出偏差的积分Pwm=Position_Kp*Bias+                                 //PID控制器比例项Position_Ki*Integral_bias+                        //PID控制器积分项Position_Kd*(Bias-Last_Bias);                     //PID控制器微分项 Last_Bias=Bias;                                       //保存上一次偏差 return Pwm;                                           //增量输出
}

Measure_X=(float)tan((Roll-ZHONGZHI_B)/1802PI)Height;
Target_X=Amplitude_xsin(Alpha); //X 方向目标值函数

则PID运算的函数为：
int Position_PID_X (Measure_X,Target_X)
Y方向同理。

系统初始参数

中断函数编写

我们采用外部中断，即通过陀螺仪的INT引脚进行中断的触发（不清楚的朋友可以去看我关于平衡车的文章），陀螺仪每成功采样一次数据即触发一次中断，相应 TimeCnt+=5，将数据压入PID控制器后，PID返回的PWM值加载到电机上。

void EXTI9_5_IRQHandler(void)
{if(EXTI_GetITStatus(EXTI_Line5)!=0)//一级判定{if(PBin(5)==0)//二级判定{EXTI_ClearITPendingBit(EXTI_Line5);//清除中断标志位//1.采集编码器数据&MPU6050角度信息。mpu_dmp_get_data(&Pitch,&Roll,&Yaw);         //角度MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);    //陀螺仪MPU_Get_Accelerometer(&aacx,&aacy,&aacz);  //加速度TimeCnt+=5;        //运行时间定时器    //2.将数据压入闭环控制中，计算出控制输出量。Get_RC();Alpha=(float)TimeCnt/Period*2*PI;      //float不可省略，单摆周期处理成三角函数2π周期Target_X=Amplitude_x*sin(Alpha);       //X方向目标值函数Target_Y=Amplitude_y*sin(Alpha+Phase); //Y方向目标值函数Measure_X=(float)tan((Roll-ZHONGZHI_B)/180*2*PI)*Height;Measure_Y=(float)tan((Pitch-ZHONGZHI_A)/180*2*PI)*Height;Motor_X=Position_PID_X(Measure_X,Target_X);Motor_Y=Position_PID_Y(Measure_Y ,Target_Y);Xianfu_Pwm(5800);//限幅5800，满占空比5800，避免出现异常   if(Turn_Off()==0) Set_Pwm(Motor_X,Motor_Y);}}
}

限幅函数

void Xianfu_Pwm(int amplitude)
{   if(Motor_X<-amplitude) Motor_X=-amplitude;  if(Motor_X>amplitude)  Motor_X=amplitude;   if(Motor_Y<-amplitude) Motor_Y=-amplitude;  if(Motor_Y>amplitude)  Motor_Y=amplitude;
}

赋值函数

void Set_Pwm(int Motor_X,int Motor_Y)
{if(Motor_X>0)           PWMA1=0,PWMC1=Motor_X;else                PWMA1=-Motor_X,PWMC1=0;if(Motor_Y>0)           PWMB1=0,PWMD1=Motor_Y;else                PWMB1=-Motor_Y,PWMD1=0;}

蓝牙串口指令

void Get_RC(void)
{float Xishu=1.4142;    //系统设定振幅与圆周运动时半径R相差√2倍static int flag_mode;flag_mode=Debug_key;   //通过调试界面下方的按键控制运动方式（0==失能,1==直线,2==圆周,3==稳态）if(flag_mode==1)       //摆动模式{Flag_Stop=0;       //开启输出Data_Amplitude=Basic_Amplitude;//初始振幅Data_Phase=0;      //直线运动相位差为0
//      Period=Data_Period;//周期==避免异常，周期一般不做修改，测试好后做全局变量宏定义switch(Flag_Direction)//方向按钮控制振幅比例系数(tan(Data_Period))，重力、摇杆、按键三种控制模式不做区分{case 1: Data_Gama=0;       break;case 2: Data_Gama=PI/4;    break;case 3: Data_Gama=PI/2;    break;case 4: Data_Gama=3*PI/4;   break;case 5: Data_Gama=-PI/3;   break;case 6: Data_Gama=-2*PI/3; break;case 7: Data_Gama=-5*PI/6; break;case 8: Data_Gama=-PI/6;   break;case 0: Data_Gama=0;    break;}}else if(flag_mode==2)  //圆周模式{Flag_Stop=0;       //开启输出Data_Amplitude=Basic_Amplitude;//初始振幅Data_Gama=PI/4;    //振幅比例系数(tan(PI/4)==1)
//      Period=Data_Period;//周期==避免异常，周期一般不做修改，测试好后做全局变量宏定义switch(Flag_gear)  //高低速挡位控制相位差==控制旋转方向{case 1: Data_Phase=PI/2;   break;case 2: Data_Phase=3*PI/2; break;default: Data_Phase=PI/2;  break;}}else if(flag_mode==3)  //稳定模式{Flag_Stop=0;       //开启输出Data_Amplitude=0;  //振幅为0==维持稳定Data_Gama=PI/4;    //振幅比例系数(tan(PI/4)==1)Phase=0;           //相位差为0
//      Period=Data_Period;//周期==避免异常，周期一般不做修改，测试好后做全局变量宏定义}else if(flag_mode==0)  //自由模式{Flag_Stop=1;       //关闭输出Data_Amplitude=0;  //振幅为0==维持稳定Data_Gama=PI/4;    //振幅比例系数(tan(PI/4)==1)Phase=0;           //相位差为0
//      Period=Data_Period;//周期==避免异常，周期一般不做修改，测试好后做全局变量宏定义}
/********根据模式对应的数据，计算得到所需要的参数：振幅、振幅比例系数、相位差**********/Amplitude_x=Data_Amplitude*sin(Data_Gama)*Xishu;//振幅XAmplitude_y=Data_Amplitude*cos(Data_Gama)*Xishu;//振幅YPhase=Data_Phase;  //相位差
//      Period=Data_Period;//周期==避免异常，周期一般不做修改，测试好后做全局变量宏定义}

其中
Data_Amplitude
Data_Gama
Data_Phase

题目要求中的自稳，我们可直接将振幅设置为0即可
要求画圆，我们就调整相位差即可。
这三个参数是我们蓝牙模式需要调整的参数，大家使用自己习惯的蓝牙控制指令去修改即可。

蓝牙串口USART

u8 flag_mode;
void USART3_IRQHandler(void)
{int Bluetooth_data;if(USART_GetITStatus(USART3,USART_IT_RXNE)!=RESET)//接收中断标志位拉高{Bluetooth_data=USART_ReceiveData(USART3);//保存接收的数据//*************************调试界面下方按键*************************************//if(Bluetooth_data>=97&&Bluetooth_data<=105)   //键值a-i  模式调节{switch(Bluetooth_data){case 0x61: Debug_key=1;  break;    //指令a==直线模式(修改振幅可修改角度振幅A/B=tan(a))case 0x62: Debug_key=2;  break;    //指令b==圆周模式case 0x63: Debug_key=3;  break;    //指令c==稳态模式case 0x64: Debug_key=4;  break;    //指令d==默认状态，电机失能case 0x65: Debug_key=0;  break;    //指令e==默认状态，电机失能case 0x66: Debug_key=0;  break;    //指令f==默认状态，电机失能case 0x67: Debug_key=7;  break;    //指令g==默认状态，电机失能case 0x68: Basic_Amplitude-=10;  break;    //指令h==默认状态case 0x69: Basic_Amplitude+=10;  break;    //指令i==默认状态，电机失能}} //*************************直线方向调节*************************************//if((Bluetooth_data>=65&&Bluetooth_data<=72)||(Bluetooth_data==90)) {switch(Bluetooth_data){case 0x41: Flag_Direction=1; break;    //指令A==向前case 0x42: Flag_Direction=2; break;    //指令B==右前case 0x43: Flag_Direction=3; break;    //指令C==向右case 0x44: Flag_Direction=4; break;    //指令D==右后case 0x45: Flag_Direction=5; break;    //指令E==向后case 0x46: Flag_Direction=6; break;    //指令F==左后case 0x47: Flag_Direction=7; break;    //指令G==向左case 0x48: Flag_Direction=8; break;    //指令H==左前default:   Flag_Direction=0; break;    //指令Z==停止}}
//****************************高低速档位*************************************************//if(Bluetooth_data>=88&&Bluetooth_data<=89) {switch(Bluetooth_data){case 0x58: Flag_gear=1; break;    //指令X==高速档case 0x59: Flag_gear=2; break;    //指令Y==低速档}}}}

我这里将平衡小车之家蓝牙APP指令代码作为举例供大家参考。大家可以多尝试去设置不同的值观察风力摆摆出的图案，加强学习，加强理解。

PID调参

对于一个控制系统，我们期望的响应结果是稳（系统稳定不震荡不发散）、快（系统响应快速）、准（系统静态误差小）。对PID控制器的调节结果评价也是如此。
PID系数的作用

比例系数Kp：

三个参数中的绝对主力，不可或缺。Kp增大可以加快系统响应，减小静差，但系统超调量会加大，稳定性变差。比例控制是一种立即控制，只要有偏差，就立即输出控制量。大部分系统只需要P控制即可实现基本的稳快准需求。

积分系数Ki：

三个参数中的一般主力，用于消除静差、Ki减小可以降低超调量，使系统的稳定性增强。积分控制是一种修复控制，只要有偏差，就会逐渐去往消除偏差的方向去控制。

微分系数Kd：

三个参数中的预备人员，一般不用，在反馈量噪声比较大时可能会使系统震荡。Kd增大可以加快系统响应，减小超调量，适用于迟滞系统或无阻尼系统。微分控制是一种提前控制，以偏差的变化率为基准进行控制。

在风力摆系统中我们需要风力摆有非常高的跟随性也就是单摆的位移图像能够尽可能的和我们理论的sin图像重合。所以很简单我们只需要尽可能的加大D微分项即可，微分控制是一种提前控制，以偏差的变化率为基准进行控制。我们要有良好的跟随性所以D一定要很大。所以像风力摆这种需要跟随性的系统，我们通常采用PD控制器。我们的系统PWM限幅是5800，所以我们可以取D项为1500，而P只需要一点点即可我们设置为10，积分项我们这里不需要，可以根据自己的硬件实际情况去修改PID的值，只要保证D项非常大即可，大家可以试出一个理想的参数。

总结

风力摆模型实际上就是一个物理中的单摆模型，我们在分析问题时要思考如何将一个物理模型用代码去表示出来，并且寻找到需要压入PID控制的真正参数。在这里我们实际稳定的是位移。我们可能会找成角度，但是静下心来想一想角度的稳定是很困难的。我们必须知道这道题真正的其实是李萨如图形，运动的合成。这样才能实现画出各种各样的图案。这道题其实和平衡车很像，用到的代码模块基本都是一样的，唯一就是PID的代码需要改变。但换汤不换药，只要我们能熟悉PID的编写，把每个模块都调熟，相信一定会学到很多东西，也肯定能够在之后的电赛中取得很好的成绩。本人通过平衡小车之家的代码例程学习而来，给大家作出分享。

如果觉得对你有帮助的话，欢迎一键三连哦！❤️