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初始化：
//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，带两电机导致功率不够。

搜索

复制