课程设计分析

题目：风力摆控制系统

一、 题目要求

1.基本要求

采用MPU6050和空心杯电机以及任意一款单片机，设计一风力摆控制系统（系统模型图见图1），能够在竖直平面（平面与杆1垂直。下文无特殊说明，所说的竖直平面均与此平面重合）内控制角β的值，即控制摆杆使摆杆的当前位置与初始位置之间的夹角（在竖直平面内）可控。
        
图1 风力摆控制系统

2.设计要求

（1） 模块必须有总开关，能够控制整个电路是否通电
（2） 主控部分具有复位电路，可采用学院发主控板，也可采用stc15f2k60s2、stm32、k60、plc等主控（要求必须为最小系统板，即除主控必要功能外不得有其他与本设计有关的功能）
（3） 显示部分，要求能够显示自己队伍队长的学号（如采用数码管进行显示，可显示学号后三位），能够显示设定的角度数值和当前角度数值，显示P、I、D的参数数值，其他功能可自行发挥
（4） 系统角度设置要求，基本要求：角度可自定义，最终效果，有一个稳定的摆动角度。设计要求：角度可通过外部输入设备任意给定，并在可容许时间内达到设定角度值。其他要求：其他实现功能可自行发挥
（4）电路板要求自制（可用AD软件自行绘制，如自己绘制需在电路板上注明日期队员姓名队长学号，或者用洞洞板面包板自行搭建），电源模块可用成品
（5）能够清楚知道PID系统中各个参数的意义，并能够实际调试PID参数
（6）如自己绘制电路板、采用高级单片机、或具有创新部分可根据实际情况加分

二.设计难点

（1）机械结构的设计与传感器放置位置选择
（2）角度传感器的数据读取与处理
（3）PID算法的代码实现与参数调试

三.控制方案示意

角度控制可通过角度传感器（MPU6050）反馈角度信息，使用PID算法实现系统的闭环控制。
动力系统可选用空心杯电机提供风力，吹动摆杆在竖直平面内运动，下图为摆杆实现方案示意。
                
图2 风力摆实现方案示意

思路分析

从“测”、“控”的角度分析，首先是“测”，即使用ＭＰＵ６０５０进行角度的测量，然后是“控”，采用闭环控制系统，系统的输入量是预设的角度，输出量是控制电机速度的ｐｗｍ波，控制器采用ｐｉｄ控制器。基本的工作流程为，设定角度，作为系统的目标值也即系统的参比变量、设定值、系统输入，ＰＩＤ控制器的输入为当前值测量值与设定值的差，将此数据给控制器即可输出ｐｗｍ数值用于执行器——电机的调速作用，由于两侧电机的转速不同则产生的吹力不同，即可使白干产生倾斜，使用测量元件反馈当前角度，从而形成一个单回路闭环控制系统。

硬件选择

一、主控

１．选择一　STC15W408AS

详细参数请参照此链接
相关资料请参照此链接

２．选择二　STM32F103C8T6

相关资料请参照此链接，提取码：a7w5
        

二、显示模块

１．选择一　LCD1602（含ｉｉｃ转接板）

相关资料请参照此链接
为了减少单片机Ｉ/Ｏ口的浪费，所以使用含有转接板的显示屏，该显示屏只需要两个Ｉ/Ｏ口以及一个ＶＣＣ一个ＧＮＤ即可驱动，，由于材料历程为Arduino程序，所以在下文会给出ｓｔｃ１５单片机程序及注释。

２．选择二　0.96寸OLED显示屏

相关资料请参照此链接，提取码：cvux
              

三、GY-521 MPU6050

相关资料请参照此链接，提取码：es9t
该模块采用ｉｉｃ与主控进行通讯，下文会给予程序和相关注释
            

四、2路直流电机驱动板模块

其驱动方式可参照L298N的驱动方式，相关资料请参照此链接，下问亦会给出相关代码与注释

五、716空心杯

机械结构

一、碳纤维管

选用８×６×１规格的碳纤维管，以配合轴承使用，也可作为摆杆使用
            

二、深沟球微型小轴承

采用８×２２×７mm规格的轴承，可替代于摆杆顶部的万向节使用
            

程序部分（软件）

程序设计思路（个人看法），首先测试每个模块是否可以正常工作，确定模块没有问题，将所有模块组合为一个系统，进行整体功能的测试，实现基本功能后搭建机械结构，进行参数调节，接下来介绍每个模块的程序及测试过程。此处仅针对部分函数进行解释，工程文件见文章末尾网盘链接。

一、LCD1602（含ｉｉｃ转接板）程序

以下代码均位于PCF8574.c文件中

void delay()　　//延时函数，ｉｉｃ延时使用
{ ;; }

void init()//IIC初始化
{
	sda=1;
	delay();
	scl=1;
	delay();
}

void start()  //IIC起始信号
{	
	sda=1;
	delay();
	scl=1;
	delay();
	sda=0;
	delay();
}

以下为信号的时序图：
            

void stop()   //IIC终止信号
{
	sda=0;
	delay();
	scl=1;
	delay();
	sda=1;
	delay();
}

以下为信号的时序图：
          
起始和终止信号：
      

void respons()  //等待IIC应答
{
	uchar i;
	scl=1;
	delay();
	while((sda==1)&&(i<250))i++;
	scl=0;
	delay();
}

以下为信号的时序图：

字节传送与应答：
    

void write_byte(uchar date)//IIC写数据zaixie
{
	uchar i,temp;
	temp=date;


	for(i=0;i<8;i++)　　//一位一位的写入
	{
		temp=temp<<1;　　//左移一位，末位补零，高位溢出到ＣＹ寄存器
		scl=0;
	  	delay();
		sda=CY;
		delay();
		scl=1;
		delay();
	}
	scl=0;
	delay();
	sda=1;
	delay();
}

uchar read_byte()//IIC读数据
{
	uchar i,k;
	scl=0;
	delay();
	sda=1;
	delay();
	for(i=0;i<8;i++)
	{
		scl=1;
		delay();	
		k=(k<<1)|sda;　　//现将ｋ左移一位，然后一位一位的进行赋值
		scl=0;
		delay();	
	}
	return k;
}

I2C总线进行数据传送时，时钟信号为高电平期间，数据线上的数据必须保持稳定，只有在时钟线上的信号为低电平期间，数据线上的高电平或低电平状态才允许变化。
      

void write_add(uchar date1)//写入数据到I/O
{
	start();
	write_byte(0x4e);   //写地址，此处A０ A１ A２ 均为１
	respons();
	write_byte(date1);　　//写入的数据
	respons();
	stop();
}

PCF8574写模式时序图，看前两行即可，下面的没研究！！！
    

uchar read_add()//读数据，从I/O口
{
	uchar date1;
	start();
	write_byte(0x4f); 　　//读地址，此处A０ A１ A２ 均为１
	respons();
	date1=read_byte();
	respons();
	stop();
	return date1;
}zaixie

同样前两行，后面看不懂！！！
      
下表为从机地址表，当ｉｉｃ总线上挂载多个设备时，每个设备都有自己的一个地址，方便主机进行分辨。有关ｉｉｃ总线的从机地址问题可以参考这个博客。
              
***LCD1602.c***文件中的函数都是对芯片寄存器地址的操作，没必要看的很明白，会用最后一个函数“ShowString”即可。

二、PWM波产生程序

以下程序片段均来自C51-6050-UART.C

sbit left_moto_pwm = P3^2;
sbit right_moto_pwm = P3^3;// i/o定义

unsigned char pwm_val_right = 0;      //电机pwm周期
unsigned char push_val_right = 0;     //电机pwm占空比

unsigned char pwm_val_left = 0;      //电机pwm周期
unsigned char push_val_left = 0;     //电机pwm占空比

/*********************定时器初始化************************/
void TimerInit()
{
        TMOD=0X11;                    //设置定时器工作方式
        TH0=(65536-100)/256;          //100US定时
        TL0=(65536-100)%256;
        TR0= 1;                       //打开定时器１
        ET0= 1;                       //打开定时器１中断
        EA = 1;                       //开总中断
}
/*********************左电机pwm输出***********************/
void pwm_out_lift_moto()
{

		if(pwm_val_left <= push_val_left)
			left_moto_pwm = 1;
		else
			left_moto_pwm = 0;
		
		if(pwm_val_left > 100)
			pwm_val_left = 0;
		
}

/*********************右电机pwm输出***********************/
void pwm_out_right_moto()
{

		if(pwm_val_right <= push_val_right)
			right_moto_pwm = 1;
		else
			right_moto_pwm = 0;
		
		if(pwm_val_right > 100)
			pwm_val_right = 0;

}

/***************定时器中断*************/
void time0() interrupt 1   using 3
{
	
	TH0=(65536-10)/256;          //装初值
  	TL0=(65536-10)%256;
	
	pwm_val_left++;               //电机pwm周期
  	pwm_val_right++;              //电机pwm周期
	
	pwm_out_lift_moto();          //左电机pwm输出
	pwm_out_right_moto();         //右电机pwm输出
	
}

程序就类似于下面这个坐标系，横轴是pwm_val_left，忘写了！！！
在一个周期内高电平占比越大，电机转的也就越快，这就是ｐｗｍ的意思了！！！！

就先到这吧，剩下的ｉｍｕ的应用及补充ｐｉｄ等开学在写！！！！
开学遥遥无期，剩下的硬件、imu应用、PID部分我会继续更新（2020年４月20）

IMU数据获取与应用

有关IMU的其他应用请参照我的这篇博客，现在补充一下imu的获取与应用的部分。

MPU6050在stm32上获取数据

MPU6050在stm32上获取数据相对简单因为都有现成的库，直接调用即可，下面直奔主题，看一下MPU6050在stm32上是如何获取数据的。
拿到相应的库函数，首先是imu的初始化，这个没有深究，只要能够正常初始化既可，然后就是对mpu6052的寄存器操作，这里使用的iic通信，单纯的应用的话也没有太大的必要去深究，因为这个需要研究mpu6050的数据手册，比较重要的一个函数就是获取pitch,roll,yaw;的欧拉角。我们直接上代码：

//µÃµ½dmp´¦ÀíºóµÄÊý¾Ý(×¢Òâ,±¾º¯ÊýÐèÒª±È½Ï¶à¶ÑÕ»,¾Ö²¿±äÁ¿Óеã¶à)
//pitch:¸©Ñö½Ç ¾«¶È:0.1¡ã   ·¶Î§:-90.0¡ã <---> +90.0¡ã
//roll:ºá¹ö½Ç  ¾«¶È:0.1¡ã   ·¶Î§:-180.0¡ã<---> +180.0¡ã
//yaw:º½Ïò½Ç   ¾«¶È:0.1¡ã   ·¶Î§:-180.0¡ã<---> +180.0¡ã
//·µ»ØÖµ:0,Õý³£
//    ÆäËû,ʧ°Ü
u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
{
	float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
	unsigned long sensor_timestamp;
	short gyro[3], accel[3], sensors;
	unsigned char more;
	long quat[4]; 
	if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;	 
	/* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
	 * This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
	**/
	/*if (sensors & INV_XYZ_GYRO )
	send_packet(PACKET_TYPE_GYRO, gyro);
	if (sensors & INV_XYZ_ACCEL)
	send_packet(PACKET_TYPE_ACCEL, accel); */
	/* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
	 * The orientation is set by the scalar passed to dmp_set_orientation during initialization. 
	**/
	if(sensors&INV_WXYZ_QUAT) 
	{
		q0 = quat[0] / q30;	//q30¸ñʽת»»Îª¸¡µãÊý
		q1 = quat[1] / q30;
		q2 = quat[2] / q30;
		q3 = quat[3] / q30; 
		//¼ÆËãµÃµ½¸©Ñö½Ç/ºá¹ö½Ç/º½Ïò½Ç
		*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;	// pitch
		*roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;	// roll
		*yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;	//yaw
	}else return 2;
	return 0;
}

在相应的库里我们可以找到这个函数，这个函数的作用就是获取pitch,roll,yaw;的欧拉角表示，那他是如何获取的呢：
MPU6050 DMP输出的是姿态解算后的四元数，采用q30格式，也就是放大了2的30次方，我们要得到欧拉角，就得做一个转换，代码如下：

q0 = quat[0] / q30;	//q30¸ñʽת»»Îª¸¡µãÊý
q1 = quat[1] / q30;
q2 = quat[2] / q30;
q3 = quat[3] / q30; 
//¼ÆËãµÃµ½¸©Ñö½Ç/ºá¹ö½Ç/º½Ïò½Ç
*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;	// pitch
*roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;	// roll
*yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;	//yaw

quat[0]~quat[3]：是MPU6050的DMP解算后的四元数，q30格式。
q30：是一个常量：1073741824，即2的30次方。
57.3：是弧度转换为角度，即180/π，这样结果就是以度（°）为单位的。

这里用到了四元数向欧拉角的转换，有关四元数与欧拉角的知识请参考我的这篇博客，有关公式的推导可以参考这篇博客，经过推导我们可以得到如下两个式子：

为了运算方便，只需比对两个矩阵的第三行与第一列即可得出结论。

在最终得到实际运算代码前，说明两个小细节：

1. 三个欧拉角𝜃𝜑𝛾最终在代码中体现时，会有正负号的区别，这个很好理解，四轴向上，向右，顺时针滚动为正还是负呢？你喜欢怎样，只要不离谱，，只要不离谱，代码是自己的，可以自行调整。可以看到正点原子的代码的方向选取与惯性导航这本书的方向选取，在pitch角与roll角上是相反的。
2. 计算横滚角𝜃时，上述计算式子分母和实际代码有些许差异，是因为进行了一次1=𝑞02+𝑞12+𝑞22+𝑞32的等价代换。
   下面，我们再来看看这段四元数转欧拉角的代码，是不是很清晰了呢？

*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;	// pitch
*roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;	// roll
*yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;	//yaw

至此我们就得到了pitch,roll,yaw;的欧拉角表示。

MPU6050在stc15单片机上的使用

MPU6050在stc15单片机上就没有像32上方便了，目前找到的资料都是读取的mpu6050的寄存器的数值，想要获取像32那样的数据就只能仿照32的库自己进行程序编写，这里提供一种较简单方法，仿照的是飞卡直立车的思想。
既然欧拉角不好获取，那我们就直接利用mpu6050寄存器的值，我们可以用加加速度计的数值，如果看过我关于imu的那篇博客就应该理解了加速度计数据的含义，这里就不在赘述了，直接讲应用：
以z轴为例，当我们的imu水平放在桌面静止不动时我们可以得到一个z轴方向的加速度，然后我们将imu反转180度，使z轴朝下，这时我们会得到第二个z轴方向的加速的，这两个加速度数值的特点是大小相差不大，符号是相反的，这时我们就可以将这两个数做差取绝对值然后除180度，就会得到一个系数，接着我们可以将imu所以摆放，读取z轴的加速的值，将得到的数值与之前计算得到的系数相乘就是此时我们imu，z轴的偏角。

至此我们知道如何获得imu的角度，然后我们就可利用角度值进行我们的算法设计了。

PID算法

pid算法大家都比较熟悉了，好几门课上都讲过了，也就不再赘述他的原理跟各个参数的作用了，就说一下我的一些想法吧。个人感觉课本上的pid算法讲的很精彩，这个参数什么作用，有什么整定参数的方法，但是那些只是理论，只是给我们的实践提供一种方法或者是方向，但是真正的应用就会感觉没有那么漂亮了。
你想要在一个系统上去实现你的算法，会遇到各种各样课本上没法说明的问题。首先想要在你的系统上实现你的算法，你就需要对你的系统有个充分的理解，比如说他的动力系统是什么？你将如何得到算法所需要的偏差？你的系统有哪些参数可以提供给你去计算偏差？你会选择哪些参数使用？你通过pid算法计算出来的结果是什么？怎么利用你计算的结果？我们通常会把计算所得结果直接当做pwm的占空比使用，但是使用的时候，你是把他当做当前pwm的占空比使用，还是叠加到之前的占空比上使用？等等，这些问题都是你需要考虑的，这就要求你对你的系统有足够的认识。通过前面知识的铺垫到这里你应该对我们要做的系统有个整体的理解了，下面我再细说一下。
我们的动力系统采用的是空心杯电机，在电机上装上叶片提供动能，然后我们用到的主要的传感器是mpu6050，他可以提供角速度和加速度，我们就可以利用这些数据得到我们最终需要的角度信息，通过设定目标角度与当前角度做差就得到了我们算法所需要的偏差，将偏差带入我们的pid算法中，就会输出一个数值，我们将这个数值作为我们pwm的占空比进行对电机调速，这样我们这个系统的流程就出来了，接下来就是优化了。
所谓的优化其实就是pid参数的整定，这里我们可以以课本为我们的理论基础，但是也是要灵活的处理，对于参数p我们可以先大体计算一个范围，这个范围怎么计算呢？我们可以这样算，输入的偏差是角度之间的差值，数值通常是几度，输出是我们的占空比，这就根据自己的系统来了，输出多大占空会对我们系统的运动产生影响，然后带入课本上的公式，我们就可以得到一个p的大体范围，然后就是在系统上实际的调试了。调试过程你会发现一些现象跟课本是一样的，比如当你的p过大了，你会发现你的系统会不停地震荡，而且震荡幅度越来越大，这时就可以适当的降低p的值。你也可能会发现当你的系统的溶洞过大时就算你的p很小，他也会震荡，这时就说明这个扰动已经超出你能够调节的范围了。然后d他会有一个提前的作用，确实但是你可这样想，把你的系统的运动过程慢镜头化，然后不断地想象把参数代入公式会对系统有什么样的影响，这时你可能会发现，把d变成负的要比正的效果好，这时就要灵活处理，不能死磕课本的公式了。再然后是i，他有消除净差的作用，但是也会让系统反映变慢，这时你就要去权衡这两个的关系了，对于一些自横系统，他本来自己就可以消除净差，这时就可以考虑不用i了，还有积分还要考虑限幅，如果不进行限幅，他会越积越大，最终整个系统就会跑飞。总之再调参数的时候需要灵活的处理。
对于pid参数的调节与系统参数的选择还是要去实践，就比如我们的体统也不一定只使用角度信息，我们也可使用角速度信息，将加速信息也带入pid算法，如果系统反映过快，此时他的某个轴的角速度会很大，那我们就可以利用角速度信息，将角速度带入pid算法给一个负的参数，缓冲一下，也是可达到比较好的效果的，这个还是需要一下实践才会感受到。
好了，扯了折磨多直接上代码 吧：

//Rol·½Ïòpid
float PID_Rol_PosCalc(float NextPoint)
{
  register float  iError,dError;
	
	iError = Rol_Set_Pos - NextPoint;        // Æ«²î
	
	Rol_SumError += iError;				    				// »ý·Ö
	
	if(Rol_SumError > 100.0)									//»ý·ÖÏÞ·ù100
		Rol_SumError = 100.0;
	
	else if(Rol_SumError < -100.0)
		Rol_SumError = -100.0;	
	
	dError = iError - Rol_LastError; 					// µ±Ç°Î¢·Ö
	
	Rol_LastError = iError;
	
	return(rol_p * iError - rol_i * Rol_SumError + rol_d * dError);//·µ»Ø¼ÆËãÖµ
}

这里我就尝试了一下把d变成负的，效果还是有的。

程序调试

然后再说一下程序调试的一些个人理解吧，交互式调试程序很重要，为什么呢？因为程序在单片机里，他跑成什么样你是不知道滴，他就需要让程告诉你他跑到哪了，这里就可使用一个led灯，当程序跑的这个地方就让他量一下，如果没亮那肯定就是跑飞了，然后把led等往前挪挪，看看问题出在了那里，然后就是显示屏，你可以输出数据，当你的系统运行跟你的预期不一样的时候，你就可以把程序跑的数据打到显示屏上，很容易就会发现哪个地方的程序出问题了。
然后参数的整定，参数整定有个很大的问题是什么呢，就是你每该一次参数你就得重新编译一遍程序然后在下载，这个过程很折磨人的，有时候你就会花费一天的时间在编译下载程序上，这个问题我们就可以通过无线模块，比如蓝牙、zigbee、wifi等，通过串口跟电脑连接，借助电脑的串口助手进行参数整定，这时我们只需要把要整定的参数设置成全局变量，利用单片机的串口进行参数赋值，这样就很方便的进行参数整定了。下面贴一下我写的串口参数整定：

//Timer2 ÖжϷþÎñº¯Êý
void TIM2_IRQHandler(void)
{		 		
//	LED=!LED;
	TIM2->SR &= ~(1<<0);			//Çå³ýÖжϱê־λ
	tests();
	
	switch(USART_RX_BUF[0])//¶¯Ì¬µ÷²Î
	{
		case 1: rol_p++; break;
		case 2: rol_p--; break;
		case 3: rol_i+=0.1; break;
		case 4: rol_i-=0.1; break;
		case 5: rol_d++; break;
		case 6: rol_d--; break;
		case 7: pit_p++; break;
		case 8: pit_p--; break;
		case 9: pit_i+=0.1; break;
		case 10: pit_i-=0.1; break;
		case 11: pit_d++; break;
		case 12: pit_d--; break;
		default:break;
	}
	USART_RX_BUF[0] = 0;
	
}

相关图片

系统样例图

大概就是这个呀子，材料有限，只能做到这样了。

角度波形图

采集到的角度的信息就是这个样子的，其实还是可以的，如果想要在平滑一点，可以计入卡尔曼滤波，这个在飞卡直立车上使用过，其他的一些直立车也都会用。
然后试验了一个现象：就是你的平面的一些图像都是可以以用xy轴坐标轴上的正弦波叠加而成的。

这是在做圆周运动的时候的图像。

这是在做45度斜直线使得图线。

这是在做正方形运动时的图像。

这是在做三角形运动的图像。

注：
IMU的其他应用请参照我的这篇博客

51部分测试代码我放在了网盘，有需要请下载。(提取码: 3ied )

相关硬件pcd材料我放在了网盘，有需要请下载（提取码：2437）

stm32工程我放在了网盘，有需要请下载（提取码：yjjs）

相关资料我放在了网盘，有需要请下载（提取码：g3ix）

声明：部分图片资料链接来自于淘宝店铺，如有侵权请告知修改！！！