CubeMX编写风力摆控制系统[HAL库]

由于项目需求，需要使用6050制作风力摆，因此写此博客给大家提供一个快速搭建的方法，通过cubemx快速生成。

控制方案设计

按照题目的任务要求，需要设计一个风力摆控制系统，在具有一定抗干扰的能力的情况下完成自由摆动、指定长度摆动、快速制动以及半径可设置的圆周运动等任务。我们采用STM32F103VET6单片机做为主控芯片，用6位姿态传感模块MPU6050来计算角度和风机状态，用直流风机带动摆杆运动。当MPU6050检测到摆杆的角度时，可根据三角函数公式计算出摆杆现在距离中心的具体位置（方向、距离），单片机控制PWM波的输出大小来控制风机的风速与方向，使摆杆达到在特定位置静止或按照一定的轨迹运动。当摆杆处于自然下垂状态时，给四个风机同时上电且风向都向外，此时摆杆仍处于受力平衡——静止状态。此时降低X轴上一个风机的转速，摆杆将会带动激光头在X轴上画一条直线，当达到一定的倾斜角度时，单片机可根据角度计算出此时距离中心的距离是否>=25cm，若达到要求后，此风机减速，X轴反方向上电机逐渐加速，恢复到初始速度，反方向做相同的运动。在此过程中，单片机做出A/D采样，Y轴方向方向风机随时做出矫正，防止发生轨迹偏移。控制系统框图如图1.1所示。

机械结构方案设计

由于摆杆长度（60cm_{70cm）较长，且要求激光头在地面画出15cm}35cm的圆，所以要求横杆的距离要足够长。横杆长度较长加之摆杆重量较大，所以要求底座要有足够的重量来支撑整个系统。如果结构不稳或者重量不够，摆杆运动过程中将会产生倾倒或者抖动等现象，影响测试结果。于是，底座采用了“工”型结构，保证了整个系统的稳定性。摆杆材料方面，我们选用轻便的硬质铁质材料与风机进行刚性连接，既能保证连接处的稳定，又可达到减轻底座负担的目的。风机选择方面，既要保证风力够大，能够实现题目基本要求中的自由摆运动、快速制动静止。又要有一定的重量，质量越大，惯性越大，越不容易改变原来的运动状态，在实现发挥部分（2）时，受到外界干扰，不容易改变原来的圆周运动状态机械结构如图1.2

1.底座；2.竖杆；3.横杆；4.螺栓；5.万向节；6.摆杆；7.风机（1）；8.风机（2）；9.风机（3）；10.风机（4）

电路与程序设计

1. 处理器：STM32F103VET6,72MHz主频，256KB内存，满足数据处理和运算的要求。

2. 传感器：MPU6050六轴姿态传感，3.3-5V供电电压，IIC通讯，最高速率可达400KHz，100Hz有效数据输出速率，陀螺仪范围±250 500 1000 2000°/s，多维度高精度测量。
   

3. 电机：716空心杯电机，规格直径7mm长16mm轴径0.8mm,额定电压3.4V，额定转速34000，额定电流0.8A，反应灵敏，风力大。

4. 电机驱动：L298N模块,驱动部分端子供电范围Vs：＋5V～＋35V；如需要板内取电，则供电范围Vs：+7V～+35V；驱动部分峰值电流Io：2A；逻辑部分端子供电范围Vss：＋5V～＋7V（可板内取电＋5V；逻辑部分工作电流范围:0～36mA；控制信号输入电压范围：低电平：－0.3V≤Vin≤1.5V，高电平：2.3V≤Vin≤Vss；使能信号输入电压范围：低电平：－0.3≤Vin≤1.5V（控制信号无效），高电平：2.3V≤Vin≤Vss（控制信号有效）；最大功耗：20W（温度T＝75℃时）

5）按键模式选择部分
按键名称 S1 S2 S3 S4 S5
功能 + - 确认 退出当前模式 参数设置确定

6. 显示与提示部分
   LED1为黄灯，LED2为红灯。S1和S2每按下一次，黄灯亮一次表示模式切换或参数修改成功；S3、S4和S5每按下一次，红灯亮3S表示按键有效。
   

程序功能描述与设计思路

系统采用独立按键控制进行模式选择，设置有5个按键，按键1和2代表模式加减（或参数值加减），按键3表示模式确认，按键5代表参数设置确认，按键4表示模式退出，使用两个LED灯做显示提示。系统开机后进入初始化状态两个LED灯常亮四个风机以相同的速度转动整个摆杆系统处于垂直平衡状态。然后通过按键1、2进行模式选择，按键3按下锁定当前模式，进入参数设置环节。参数设置时按键1、2表示参数加减，按下按键5表示锁定当前参数，系统开始执行任务。
2、程序设计思路
基本要求（1）可用闭环控制，设定倾角阈值，从低到高不断增加X风机转速，直到倾角超过阈值，记录此时PWM脉宽级作用时间。
基本要求（2）需要采用闭环控制算法，计算公式与（1）相同，设定好范围后，可与（1）使用相同的方法调试。
基本要求（3）要采用受力分析，使用力的合成。
基本要求（4）拉起一定角度后，如果不提供动力，摆杆将逐渐衰减，但是速度缓慢，因此需要提供与运动方向相反的力，阻碍摆杆运动。这个过程需要注意，实时采集摆杆的角度（比例项 P），进行微分从而确定角速度（陀螺仪直接测量亦可），为微分项D，采用类似于自平衡的PD控制算法即可。

程序流程图

PID算法框图

CUBEMX配置

代码部分

• 6050初始化

void DMP_Init(void)
{
      
   uint8_t temp[1]={
     0};
   IIC_ReadBytes(devAddr,0x75,1,&temp[0]);//0x68
	 //printf("mpu_set_sensor complete ......\r\n");
	if(temp[0]!=0x68)NVIC_SystemReset();
	if(!mpu_init())
  {
     
	  if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))
	  	 //printf("mpu_set_sensor complete ......\r\n");
	  if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))
	  	 //printf("mpu_configure_fifo complete ......\r\n");
	  if(!mpu_set_sample_rate(DEFAULT_MPU_HZ))
	  	 //printf("mpu_set_sample_rate complete ......\r\n");
	  if(!dmp_load_motion_driver_firmware())
	  	//printf("dmp_load_motion_driver_firmware complete ......\r\n");
	  if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))
	  	 //printf("dmp_set_orientation complete ......\r\n");
	  if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
	        DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
	        DMP_FEATURE_GYRO_CAL))
	  	 //printf("dmp_enable_feature complete ......\r\n");
	  if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ))
	  	 temp[0] = 1;//printf("dmp_set_fifo_rate complete ......\r\n");
	  run_self_test();
	  if(!mpu_set_dmp_state(1))
		  temp[0] = 1;
	  	 //printf("mpu_set_dmp_state complete ......\r\n");
  }
}

• DMP读取角度数据

void Read_DMP(void)
{
     	
	  unsigned long sensor_timestamp;
		unsigned char more;
		long quat[4];

				dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);		
				if (sensors & INV_WXYZ_QUAT )
				{
         
					 q0=quat[0] / q30;
					 q1=quat[1] / q30;
					 q2=quat[2] / q30;
					 q3=quat[3] / q30;
					 Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; 	
				     Roll= atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
				}

}

• 通过6050返回角度值进行电机打开与关闭

	
		if(Pitch>20){
     
			
			MOT1 = MOT1-Pitch;
			
			__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,MOT1);
			
			
		}else if(Pitch<-20){
     
			
				MOT3 = MOT3-Pitch;
			
				__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,MOT3);
		}else{
     
			
				__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,0);
				__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,0);
		}
		
		if(Roll>40){
     
			__HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1,800);
			
			
		}else if(Roll<-40){
     
		
				__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_1,800);
			
		}else {
     
			
			__HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1,0);
				__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_1,0);
		}
		
		

最终实验结果

根据代码，该风力摆控制系统能达到基本部分和发挥部分的全部要求和性能指标，由此可以得出以下结论：
1.风机的性能是决定系统能否完成题目要求的关键，风机性能由其本身决定，包括重量、产生的最大风速、以及所需的电流。任何一个达不到要求，将会给程序调控大大增加难度，所以硬件搭建很重要。
2.好的算法，也起到至关重要的作用，为了能达到题目要求，四个风机的速度分配比例很重要，这就需要良好的算法加合适的参数。
3.此控制系统，需要时间上的配合，时间参数同样重要，要能保证在规定时间内完成任务。
综上所述，本设计达到设计要求。

总结

这次比赛我们收获颇丰，除了学到硬件搭建、电路调试、软件测试相关的知识和技能，我们还亲身体验了团队协作合理安排时间的重要性。开始的时候，由于我们对电机驱动模块、716空心杯风机和供电导线了解不够，驱动板并联供电，使用延长杜邦线给电机供电，导致我们调试的时候，总达不到题目的要求还烧坏了不少电机。我们分头查阅大量资料讨论后，决定更换供电导线，并将两个驱动模块的供电提高分开，以减少干扰。更换之后，线路更可靠供电稳定了不少。从这一点来看，团队之间沟通很重要；最重要的是遇到困难要不要被得失所困扰，要一心一意去解决这个问题。