2015年全国电子设计竞赛B题-风力摆控制系统

1、赛题回顾

2、系统简介

• 风力摆是通过六轴传感器（MPU6050）检测平台的水平角度进行反馈，控制四个8520空心杯电机的转动，带动螺旋桨转动实现激光点按照特定轨迹运动的机构。

• 产品尺寸：7474518mm

• 产品重量：0.68kg

• 杆长：φ4*340mm

• 标靶：1m*1m/

• 型材支撑架：4001040740mm固定后万向节节点距地高度675mm

3、硬件说明

• 整个模块分上下三层，最下层为亚克力板固定四个8520电机，中间层为电机驱动电路板、各个模块接口等，上层为主控核心板
• 电机两个正转，两个反转，分别对应X、Y的+/-两个方向；螺旋桨两正两反，在这里螺旋桨做推力方向，可以良好的避免异常情况下的射桨等极端情况
• 电机使用8520空心杯电机

3.1、stm32单片机最小系统

3.2、电机驱动

4、风力摆程序设计

4.1、MPU6050驱动

/**************************实现函数********************************************
*函数原型:		void MPU6050_setClockSource(uint8_t source)
*功　　能:	    设置  MPU6050 的时钟源
 * CLK_SEL | Clock Source
 * --------+--------------------------------------
 * 0       | Internal oscillator
 * 1       | PLL with X Gyro reference
 * 2       | PLL with Y Gyro reference
 * 3       | PLL with Z Gyro reference
 * 4       | PLL with external 32.768kHz reference
 * 5       | PLL with external 19.2MHz reference
 * 6       | Reserved
 * 7       | Stops the clock and keeps the timing generator in reset
*******************************************************************************/
void MPU6050_setClockSource(uint8_t source){
    IICwriteBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);

}

/** Set full-scale gyroscope range.
 * @param range New full-scale gyroscope range value
 * @see getFullScaleRange()
 * @see MPU6050_GYRO_FS_250
 * @see MPU6050_RA_GYRO_CONFIG
 * @see MPU6050_GCONFIG_FS_SEL_BIT
 * @see MPU6050_GCONFIG_FS_SEL_LENGTH
 */
void MPU6050_setFullScaleGyroRange(uint8_t range) {
    IICwriteBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
}

/**************************实现函数********************************************
*函数原型:		void MPU6050_setFullScaleAccelRange(uint8_t range)
*功　　能:	    设置  MPU6050 加速度计的最大量程
*******************************************************************************/
void MPU6050_setFullScaleAccelRange(uint8_t range) {
    IICwriteBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
}

/**************************实现函数********************************************
*函数原型:		void MPU6050_setSleepEnabled(uint8_t enabled)
*功　　能:	    设置  MPU6050 是否进入睡眠模式
				enabled =1   睡觉
			    enabled =0   工作
*******************************************************************************/
void MPU6050_setSleepEnabled(uint8_t enabled) {
    IICwriteBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
}

/**************************实现函数********************************************
*函数原型:		uint8_t MPU6050_getDeviceID(void)
*功　　能:	    读取  MPU6050 WHO_AM_I 标识	 将返回 0x68
*******************************************************************************/
uint8_t MPU6050_getDeviceID(void) {

    IICreadBytes(devAddr, MPU6050_RA_WHO_AM_I, 1, buffer);
    return buffer[0];
}

/**************************实现函数********************************************
*函数原型:		uint8_t MPU6050_testConnection(void)
*功　　能:	    检测MPU6050 是否已经连接
*******************************************************************************/
uint8_t MPU6050_testConnection(void) {
   if(MPU6050_getDeviceID() == 0x68)  //0b01101000;
   return 1;
   	else return 0;
}

/**************************实现函数********************************************
*函数原型:		void MPU6050_setI2CMasterModeEnabled(uint8_t enabled)
*功　　能:	    设置 MPU6050 是否为AUX I2C线的主机
*******************************************************************************/
void MPU6050_setI2CMasterModeEnabled(uint8_t enabled) {
    IICwriteBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled);
}

/**************************实现函数********************************************
*函数原型:		void MPU6050_setI2CBypassEnabled(uint8_t enabled)
*功　　能:	    设置 MPU6050 是否为AUX I2C线的主机
*******************************************************************************/
void MPU6050_setI2CBypassEnabled(uint8_t enabled) {
    IICwriteBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled);
}

/**************************实现函数********************************************
*函数原型:		void MPU6050_initialize(void)
*功　　能:	    初始化 	MPU6050 以进入可用状态。
*******************************************************************************/
void MPU6050_initialize(void) {
    MPU6050_setClockSource(MPU6050_CLOCK_PLL_YGYRO); //设置时钟
    MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_2000);//陀螺仪最大量程 +-1000度每秒
    MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2);	//加速度度最大量程 +-2G
    MPU6050_setSleepEnabled(0); //进入工作状态
	 MPU6050_setI2CMasterModeEnabled(0);	 //不让MPU6050 控制AUXI2C
	 MPU6050_setI2CBypassEnabled(0);	 //主控制器的I2C与	MPU6050的AUXI2C	直通。控制器可以直接访问HMC5883L
}

4.2、读取MPU6050内置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
       Yaw = 	atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;//yaw
    }
}

4.3、电机控制程序

void Motor_Init(void )
{
  GPIO_InitTypeDef         GPIO_InitStrucutre;
  TIM_TimeBaseInitTypeDef  TIM_TimeBaseInitStruct;
  TIM_OCInitTypeDef		TIM_OCInitStruct;

  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB,ENABLE);	
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);


  /***************************************************
  ***配置PWM输出的IO口
  *********************************************/	
  GPIO_InitStrucutre.GPIO_Mode=GPIO_Mode_AF_PP;	   //GPIO模式为复用推挽输出
  GPIO_InitStrucutre.GPIO_Pin=GPIO_Pin_6|GPIO_Pin_7; //Pin_6----TIM3_CH1,Pin_7----TIM3_CH2
  GPIO_InitStrucutre.GPIO_Speed=GPIO_Speed_50MHz;	  
  GPIO_Init(GPIOA,&GPIO_InitStrucutre);              //初始化GPIOA_6,GPIOA_7;
  GPIO_InitStrucutre.GPIO_Pin=GPIO_Pin_0|GPIO_Pin_1; //Pin_0----TIM3_CH3,Pin_1----TIM3_CH4
  GPIO_Init(GPIOB,&GPIO_InitStrucutre);              //初始化GPIOA_6,GPIOA_7;


  /***************************************************
  ***激光控制IO口也在这里配置
  *********************************************/	
  GPIO_InitStrucutre.GPIO_Mode=GPIO_Mode_Out_PP;	   //GPIO模式为推挽输出
  GPIO_InitStrucutre.GPIO_Pin=GPIO_Pin_12;           //控制激光的IO口为GPIOA_12;
  GPIO_InitStrucutre.GPIO_Speed=GPIO_Speed_50MHz;	  
  GPIO_Init(GPIOA,&GPIO_InitStrucutre);  
  GPIO_ResetBits(GPIOA,GPIO_Pin_12);                 //初始化后先关闭激光

  /***************************************************
  ***定时器3相关配置
  *********************************************/
  TIM_TimeBaseInitStruct.TIM_Period=7200-1;			//***装载值7200，预分频系数1，
  TIM_TimeBaseInitStruct.TIM_Prescaler=1-1; 			//***PWM输出频率 f=72000 000/7200/1=10Khz
  TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;
  TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式	
  TIM_TimeBaseInit(TIM3,&TIM_TimeBaseInitStruct);            //初始化TIM3


  /***************************************************
  ***PWM输出相关配置
  *********************************************/
  TIM_OCInitStruct.TIM_OCMode=TIM_OCMode_PWM1;             //定时器模式:PWM１模式
  TIM_OCInitStruct.TIM_OCPolarity=TIM_OCPolarity_High;     //输出极性高（占空比大，高电平时间长）
  TIM_OCInitStruct.TIM_OutputState=TIM_OutputState_Enable; //输出比较使能
  TIM_OCInitStruct.TIM_Pulse=0;	                
  TIM_OC1Init(TIM3,&TIM_OCInitStruct);                     //初始化TIM3_CH1  
  TIM_OC2Init(TIM3,&TIM_OCInitStruct);                     //初始化TIM3_CH2  
  TIM_OC3Init(TIM3,&TIM_OCInitStruct); 					 //初始化TIM3_CH3  
  TIM_OC4Init(TIM3,&TIM_OCInitStruct);					 //初始化TIM3_CH4  

  TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);  		 //CH1预装载使能	 
  TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable); 		 //CH4预装载使能	 
  TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable); 		 //CH1预装载使能	 
  TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable);  		 //CH4预装载使能	 
  TIM_ARRPreloadConfig(TIM3, ENABLE);                		 //使能TIM3ARR上的预装载寄存器
  TIM_Cmd(TIM3, ENABLE);  //开启定时器3
  {MOTORA=0;MOTORB=0;MOTORC=0;MOTORD=0;}                   //初始化后先关闭四个电机，防止意外
}


void Set_PWM(int PWM_X,int PWM_Y)
{
	if(PWM_X>0)
		  MOTORA=PWM_X,MOTORC=0;
	  else
		  MOTORC=-PWM_X,MOTORA=0;
	if(PWM_Y>0)
		  MOTORD=PWM_Y,MOTORB=0;
	  else
		  MOTORB=-PWM_Y,MOTORD=0;
}

5、资料下载

• https://download.csdn.net/download/m0_51061483/89852081