基于单片机的自平衡无人机的设计重873附件.docx

附录1mpu6050程序

#includeALL_DATA.h

#includempu6050.h

#includeI2C.h

#includefilter.h

#includestring.h

#includeLED.h

#includemyMath.h

#includekalman.h

#includeANO_Data_Transfer.h

#define SMPLRT_DIV 0x19

#define CONFIGL 0x1A

#define GYRO_CONFIG 0x1B

#define ACCEL_CONFIG 0x1C

#define ACCEL_ADDRESS 0x3B

#define ACCEL_XOUT_H 0x3B

#define ACCEL_XOUT_L 0x3C

#define ACCEL_YOUT_H 0x3D

#define ACCEL_YOUT_L 0x3E

#define ACCEL_ZOUT_H 0x3F

#define ACCEL_ZOUT_L 0x40

#define TEMP_OUT_H 0x41

#define TEMP_OUT_L 0x42

#define GYRO_XOUT_H 0x43

#defineGYRO_ADDRESS0x43

#define GYRO_XOUT_L 0x44

#define GYRO_YOUT_H 0x45

#define GYRO_YOUT_L 0x46

#define GYRO_ZOUT_H 0x47

#define GYRO_ZOUT_L 0x48

#define PWR_MGMT_1 0x6B

#define WHO_AM_I 0x75

#defineMPU6050_PRODUCT_ID0x68

#defineMPU6052C_PRODUCT_ID0x72

#ifdef USE_I2C_HARDWARE

#defineMPU6050_ADDRESS0xD0

#else

#defineMPU6050_ADDRESS0xD0

#endif

int16_tMpuOffset[6]={0};

staticvolatileint16_t*pMpu=(int16_t*)MPU6050;

int8_tmpu6050_rest(void)

{

if(IIC_Write_One_Byte(MPU6050_ADDRESS,PWR_MGMT_1,0x80)==FAILED)

returnFAILED;

delay_ms(20);

returnSUCCESS;

}

int8_tMpuInit(void)

{

uint8_tdate=SUCCESS;

bLED_L();

aLED_L();

fLED_H();

hLED_H();

do

{

date=IIC_Write_One_Byte(MPU6050_ADDRESS,PWR_MGMT_1,0x80);

delay_ms(30);

date+=IIC_Write_One_Byte(MPU6050_ADDRESS,SMPLRT_DIV,0x02);

date+=IIC_Write_One_Byte(MPU6050_ADDRESS,PWR_MGMT_1,0x03);

date+=IIC_Write_One_Byte(MPU6050_ADDRESS,CONFIGL,0x03);

date+=IIC_Write_One_Byte(MPU6050_ADDRESS,GYRO_CONFIG,0x18);

date+=IIC_Write_One_Byte(MPU6050_ADDRESS,ACCEL_CONFIG,0x09);

MPU_Err=1;

}

while(date!=SUCCESS);

date=IIC_Read_One_Byte(MPU6050_ADDRESS,0x75);

if(date!=MPU6050_PRODUCT_ID)

returnFAILED;

else

MpuGetOffset();

MPU_Err=0;

returnSUCCESS;

}

#defineGyro_Read()IIC_read_Bytes(MPU6050_ADDRESS,0X3B,buffer,6)

#defineAcc_Read()IIC_read_Bytes(MPU6050_ADDRESS,