MPU6050 NXC测试程序

blackblue

刚想从NXC着手学习NXT的编程,不料I_ROBOT又出NXJ的诱惑，前二天又受IROBOT先生教诲，已经心生放弃NXC转投NXJ门下的意念！

好戏又是不断，ROBOT C又被不完全破解，好吧，正式放弃NXC了。。。。。。

在放弃之前，将自己曾经学习了一个月多一点的NXC小成果汇报出来一点（不对，其实我汇报过一次了，上次传过一个视频，用MPU6050做的自平衡车，不是太稳定，但能站上个几分钟的）。现在将MPU6050的测试程序发上，希望论坛的高手指正！

#define PORT S4
#define Adresse (0x68<<1)  //MPU6050 I2C-Adresse
#define Pi 3.14159
//#define gyro_gain 0.060975
//#define acc_gain  0.000061
byte init1[] = {Adresse, 0x19, 0x07};   //SMPLRT_DIV 0X07 (125HZ)
byte init2[] = {Adresse, 0x1A, 0x06};   //CONFIG 0X06 (5HZ)
byte init3[] = {Adresse, 0x1B, 0x18};   //GYRO_CONFIG  (2000dgs/s)
byte init4[] = {Adresse, 0x1C, 0x01};   //Accel CONFIG  (2g,5HZ)
byte init5[] = {Adresse, 0x6B, 0x00};   //上电唤醒
byte read_gyro[] = {Adresse, 0x43};     //GYRO数据寄存器始地址
byte read_acc[] = {Adresse, 0x3B};      //ACC数据寄存器始地址
byte nbytes;
byte data_Gyro[];
byte data_Acc[];
float Acc_Yspeed = 0;
float Acc_Zspeed = 0;
float Acc_Xspeed = 0;
float angle_Acc_x = 0;
float A = 0;
float gyro_raw = 0;  //X轴角速度
float dt = 0;        //积分时间
float angle_GA =0;   //融合角
float now = 0;
float preTime = 0;
float arctan = 0;
task main()
{
      // MPU6050初始化
      SetSensorLowspeed(PORT);
      I2CWrite(PORT, 0, init1);
      while(I2CStatus(PORT, nbytes)>0);
      Wait(5);
      I2CWrite(PORT, 0, init2);
      while(I2CStatus(PORT, nbytes)>0);
      
      Wait(5);
      I2CWrite(PORT, 0, init3);
      while(I2CStatus(PORT, nbytes)>0);
      
      Wait(5);
      I2CWrite(PORT, 0, init4);
      while(I2CStatus(PORT, nbytes)>0);
      
      Wait(5);
      I2CWrite(PORT, 0, init5);
      while(I2CStatus(PORT, nbytes)>0);
      
      Wait(100);
      while (1)
      {
              I2CWrite(PORT, 6, read_gyro);    //初始化GYRO寄存器
              while(I2CStatus(PORT, nbytes)>0);
              if (nbytes == 6)
              {
                  I2CRead(PORT, 6, data_Gyro); //读GYRO寄存器
                  gyro_raw = ((data_Gyro[0]<<8) + data_Gyro[1]) - 18;
                  gyro_raw /= 16.4;  // 2000dgs/s 量程角速度
               }
               
              I2CWrite(PORT, 6, read_acc);     //初始化ACC寄存器
              while(I2CStatus(PORT, nbytes)>0);
              if (nbytes == 6)
              {
                  I2CRead(PORT, 6, data_Acc);  //读ACC寄存器
                  Acc_Xspeed = ((data_Acc[0]<<8) + data_Acc[1]) / 16384.0;   //X轴加速度
                  Acc_Yspeed = ((data_Acc[2]<<8) + data_Acc[3]) / 16384.0;   //Y轴加速度
                  Acc_Zspeed = ((data_Acc[4]<<8) + data_Acc[5]) / 16384.0;   //Y轴加速度
                  
                  arctan = sqrt(Acc_Xspeed / (Acc_Yspeed * Acc_Yspeed + Acc_Zspeed * Acc_Zspeed));  //由加速度计算经X轴角度
                  angle_Acc_x = Atan(arctan) * 180 / Pi;
               }
            //积分时间
            now = CurrentTick();
            dt = (now - preTime) / 1000;
            preTime = now;
            int K = 0.8;
            A = K / (K + dt);
            
            angle_GA = A * (angle_GA + gyro_raw * dt) + (1-A) * angle_Acc_x;  //滤波角度(一阶互补滤波)
            NumOut(0,LCD_LINE1,angle_GA);
            NumOut(0,LCD_LINE3,angle_Acc_x);     // 平衡车只需要用到X角度和角速度 angle_GA ,gyro-raw
            Wait(200);
        }

}

----------------------------------------------------------------------------------
注：我在ARDUINO下使用DMP可以直接出四位元，有专门的库，但是NXT真心不知道怎么用DMP了，因为MPU6050要用DMP除了这个库，硬件连接也与普通I2C不一样，需要用到一个中断脚，ARDUINO上是将MPU6050的INT脚接到数字2脚上，NXT的这数字2脚对应的是什么脚?怎么接，真心一无所知！要是NXT也能用DMP，那么在NXT足球比赛这种玩法时，MPU6050（DMP）+HM5883+NXT将所向披靡，甚至能用NXT来控制飞行器将也不是问题！

iRobot

那个，我的名字是小驼峰命名方式，iRobot。

学任何一门语言，都有前途，C更是万物始祖，值得深入研究。

我打算等有时间了，买一套Arduino，互相学习。

Air

您好，你是用互补滤波器做的吧，调制的怎么样？

Air

您好，能留个联系方式吗，我最近也在用nxc研究互补滤波，求大神指教！ qq:379294443

blackblue

我那是什么大神！楼上的你加我吧：519980540！注明是“中文乐高”就行！

boylong

大牛，您知道怎么做NXC的蓝牙收发程序吗？知道函数名称也行啊

blackblue

http://robotics.benedettelli.com/BT_NXC.htm

去这地方，下个库吧！