EEPW论坛

昨天换了一个MZ48，回去测试了一下，编码器OK了，采回来的数据比较稳定，应该能保证0.05°，主要由于自己做的车架固定的传感器安装精度不太高；

下面是采的数据：

板子上设计了四个灯，全闪是待机，点一下按键全灭进入平衡状态，单个等常亮是表示方向，两个灯亮是原地转圈

下面插一下主要代码：

主程序-测试版

s16 Gyro_x,Gyro_y,Gyro_z;
	u8 i;
	u16 Voltage = 0;
	Init_All();													//³õʼ»¯ËùÓÐÓ²¼þ
	while(1)
	{
		Voltage = 0.4032 * Get_Adc();			//µç³Øµçѹ£¨µ¥Î»0.01V£© 
		SPI1_CS = 0;
		for(i=0;i<SPI_Mz48;i++)
		{
			Mz48[i] = (SPI1_ReadWriteByte(0xFFFF) & 0x3FFF);	
			delay_us(1);
		}	
		SPI1_CS = 1;		
		printf("\n MZ48_1Êý¾Ý£º%d  MZ48_2Êý¾Ý£º%d  µç³Øµçѹ£º%d \n",Mz48[0],Mz48[1],Voltage);			// Ïò´®¿Ú´òÓ¡
//	/*
//		Gyro_x=getGyroX();  	   //XÖáÊý¾Ý
//		Gyro_y=getGyroY();  	   //YÖáÊý¾Ý
//		Gyro_z=getGyroZ();  	   //ZÖáÊý¾Ý
//	*/
//		Gyro_x=getAccX();  	   //XÖáÊý¾Ý
//		Gyro_y=getAccY();  	   //YÖáÊý¾Ý
//		Gyro_z=getAccZ();  	   //ZÖáÊý¾Ý
////		printf("\n xÖáÊý¾Ý£º%d  yÖáÊý¾Ý£º%d  zÖáÊý¾Ý£º%d \n",Gyro_x,Gyro_y,Gyro_z);			   		// Ïò´®¿Ú´òÓ¡	
		KEY_Scan();
		if(Mini_Balance)
		{
			LED1 = 1;
			LED2 = 1;
			LED3 = 1;
			LED4 = 1;
		}
		else
		{
			LED1 = ~LED1;
			LED2 = ~LED2;
			LED3 = ~LED3;
			LED4 = ~LED4;
		}
		delay_ms(500);		//ÉÔ×÷ÑÓʱ
	}	

	SPI初始化：



	
 	//SPI1¿Ú³õʼ»¯,·ÃÎÊMZ48
	RCC_APB2PeriphClockCmd(	RCC_APB2Periph_GPIOA | RCC_APB2Periph_AFIO | RCC_APB2Periph_SPI1, ENABLE );	
	/* Configure SPI1 pins: SCK, MISO and MOSI */
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_6 | GPIO_Pin_7;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;  	//¸´ÓÃÍÆÍìÊä³ö
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOA, &GPIO_InitStructure);

	/* Configure I/O for MZ48 Chip select */
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;  			//SPI1_CS
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;  	//¸´ÓÃÍÆÍìÊä³ö
	GPIO_Init(GPIOA, &GPIO_InitStructure);

	GPIO_SetBits(GPIOA,GPIO_Pin_4|GPIO_Pin_5|GPIO_Pin_6|GPIO_Pin_7);

	/* SPI1 configuration */
	SPI_Cmd(SPI1, DISABLE);
	SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;  	//ÉèÖÃSPIµ¥Ïò»òÕßË«ÏòµÄÊý¾Ýģʽ:SPIÉèÖÃΪ˫ÏßË«ÏòÈ«Ë«¹¤
	SPI_InitStructure.SPI_Mode = SPI_Mode_Master;		//ÉèÖÃSPI¹¤×÷ģʽ:ÉèÖÃΪÖ÷SPI
	SPI_InitStructure.SPI_DataSize = SPI_DataSize_16b;	//ÉèÖÃSPIµÄÊý¾Ý´óС:SPI·¢ËͽÓÊÕ16λ֡½á¹¹
	SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low;			//Ñ¡ÔñÁË´®ÐÐʱÖÓµÄÎÈ̬:ʱÖÓÐü¿ÕµÍ
	SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge;		//Êý¾Ý²¶»ñÓÚµÚ¶þ¸öʱÖÓÑØ£¿£¿£¿£¨SPI_CPHA_1Edge£©
	SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;			//NSSÐźÅÓÉÓ²¼þ£¨NSS¹Ü½Å£©»¹ÊÇÈí¼þ£¨Ê¹ÓÃSSI룩¹ÜÀí:ÄÚ²¿NSSÐźÅÓÐSSIλ¿ØÖÆ
	SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_256;		//¶¨Ò岨ÌØÂÊÔ¤·ÖƵµÄÖµ:²¨ÌØÂÊÔ¤·ÖƵֵΪ2¼´@72MHzʱΪ36M
	SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;	//Ö¸¶¨Êý¾Ý´«Êä´ÓMSBλ»¹ÊÇLSBλ¿ªÊ¼:Êý¾Ý´«Êä´ÓMSBλ¿ªÊ¼
	SPI_InitStructure.SPI_CRCPolynomial = 7;			//CRCÖµ¼ÆËãµÄ¶àÏîʽ
	SPI_Init(SPI1, &SPI_InitStructure);  				//¸ù¾ÝSPI_InitStructÖÐÖ¸¶¨µÄ²ÎÊý³õʼ»¯ÍâÉèSPIx¼Ä´æÆ÷
	/* Enable SPI1  */
	SPI_Cmd(SPI1, ENABLE); 								//ʹÄÜSPIÍâÉè

搜索

复制

板子截的真不错

晚上睡觉前简单测了一下倾角的滤波融合，加速度不好控制，测试的效果一般，没感觉出不同滤波的差别：

我的代码注视部分贴上去老是乱码呢！

//*********************************************************
// 倾角计算
//*********************************************************
void Angle_Calcu(void)	 
{
	float Accel_x;	     		// X轴加速度值暂存 
	float Accel_z;	     		// Z轴加速度值暂存
	float Angle_ax;      		// 由加速度计算的倾斜角度
	float K,A,x1,x2,y1;
	float Vs[4];	
	//------加速度--------------------------

	Accel_x  = getAccX();	  																// 读取X轴加速度
	Accel_z  = getAccZ();	  																// 读取Z轴加速度	
	Angle_ax = atan((Accel_x * 1.0f) / (Accel_z * 1.0f)); 			// 去除零点偏移,计算得到角度（弧度）反正切  
	Angle_ax = (Angle_ax * 180 ) / 3.14 ;   								// 弧度转换为度
	Vs[0] = Angle_ax;
	//-------角速度-------------------------

	//范围为2000deg/s时，换算关系：16.4 LSB/(deg/s)

	Gyro_y = getGyroY();	      					//静止时角速度Y轴输出为-1左右
	Gyro_y = -(Gyro_y + 1)/16.4;         	//去除零点偏移，计算角速度值,负号为方向处理 


	//-------卡尔曼滤波融合-----------------------

	Kalman_Filter(Angle_ax,Gyro_y);       //卡尔曼滤波计算倾角
	Vs[1] = Angle;

	//-------一阶互补滤波-----------------------	
	K = 0.075;                          //对加速度计取值的权重
	A = K / (K + dt);
	Angle = A * (Angle + Gyro_y * dt) + (1-A) * Angle_ax;
	Vs[2] = Angle;

	//-------二阶互补滤波-----------------------		
	K = 0.5;
	x1 = (Angle_ax - Angle) * K * K;
	y1 = y1 + x1 * dt;
	x2 = y1 + 2 * K *(Angle_ax - Angle) + Gyro_y;
	Angle = Angle + x2 * dt;
	Vs[3] = Angle;
	printf("%f \t %f \t %f \t %f \t %f \n",Gyro_y,Vs[0],Vs[1],Vs[2],Vs[3]);			// 向串口打印

}

搜索

复制

搜索

复制

今天发现一奇怪现象供电不足，导致系统全部复位，之后MPU6050一直不能工作了，断电重新上电控制板也不行，然后重新下载了一次程序又可以工作了，反复试了好多次都是这样的，求解释，谢谢了

补充供电不足是由于我用的路由器的9V适配器，输出0.6A，带两电机导致功率不够。

搜索

复制