10自由度陀螺总体测试程序.docVIP

#include Wire.h // I2C library, gyroscope #include bma180.h #define GYRO_ADDR 0x68 // 陀螺仪地址gyro address, binary =when AD0 is connected to Vcc (see schematics of your breakout board) #define SMPLRT_DIV 0x15 #define DLPF_FS 0x16 #define INT_CFG 0x17 #define PWR_MGM 0x3E #define TO_READ 6 // 2 bytes for each axis x, y, z #define COMPASS_ADDR 0x1E //电子罗盘地址0011110b, I2C 7bit address of HMC5883 BMA180 bma180 ;//定义变量bm180加速度传感器 struct{ int x_alpha; //方向position int x_omiga; //角速度speed int x_acc; //加速度acceleration int y_alpha; int y_omiga; int y_acc; int z_alpha; int z_omiga; int z_acc; }DOF; //initializes the compass HMC5883 void initCompass() // 初始化电子罗盘 { //Put the HMC5883 IC into the correct operating mode //select mode register //continuous measurement mode writeTo(COMPASS_ADDR, 0x02, 0x00); } void getCompassData(int *x, int *y, int *z) //获得电子罗盘的测量数据 { int regAddress = 0x03; //double x, y, z; byte buff[TO_READ]; char str[50]; // 50 should be enough to store all the data from the readFrom(COMPASS_ADDR, regAddress, TO_READ, buff); //read the gyro data from the ITG3200 *x = ((buff[0] 8) | buff[1]);// *y = ((buff[2] 8) | buff[3]);// *z = ((buff[4] 8) | buff[5]);// } //initializes the gyroscope void initGyro() //陀螺 { /***************************************** * ITG 3200 * power management set to: * clock select = internal oscillator * no reset, no sleep mode * no standby mode * sample rate to = 3Hz * parameter to +/- 2000 degrees/sec * low pass filter = 5Hz * no interrupt ******************************************/ writeTo(GYRO_ADDR, PWR_MGM, 0x00); // writeTo(GYRO_ADDR, SMPLRT_DIV, 0xFF); // EB, 50, 80, 7F, DE, 23, 20, FF writeTo(GYRO_ADDR, SMPLRT_DIV, 00); // EB, 50, 80, 7F, DE, 23, 20, FF // writeTo(GYRO_ADDR, DLPF_FS, 0x1E); // +/- 2000 dgrs/sec, 1KHz, 1E, 19 writeTo(GYRO_ADDR, DLPF_FS, B11001); // +/- 2000 dgrs/sec, 1KHz, 1E, 19 writeTo(GYRO