Cảm biến gia tốc, góc MPU6050 _ Pic16F887

D

Dương Đức Lộc

Trứng gà
Không biết lỗi chỗ nào chương trình chưa chạy. Mong các bạn giúp. Cám ơn.

Code: 
#include <htc.h>
#include <stdio.h>
#include "uart.h"
#include "i2c.h"
//#include "lcd.h"
 
__CONFIG(INTIO & WDTDIS & PWRTEN & MCLREN & UNPROTECT & DUNPROTECT & BORDIS & IESODIS & FCMDIS & LVPDIS);
 
//baudrate 19200
 
//+++++++++++++++++++++++++DEFINE MPU6050+++++++++++++++++++++++++++
#define SMPLRT_DIV 0x19 //0x07(125Hz)
#define CONFIG 0x1A //0x06(5Hz)
#define GYRO_CONFIG 0x1B //0x18(2000deg/s)
#define ACCEL_CONFIG 0x1C //0x01(2G£¬5Hz)
#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
#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 //
#define MPU6050_ADD 0x68
#define MPU6050_SLAVE_WRT 0xD0 //dia chi cua cam bien mpu6050
#define MPU6050_SLAVE_RD 0xD1
 
 
int Mpu6050_Read(unsigned char address){
char lsb, msb;
 
i2c_start();
i2c_write(MPU6050_SLAVE_WRT);
i2c_write(address);
i2c_start();
i2c_write(MPU6050_SLAVE_RD);
msb = i2c_read(1);
lsb = i2c_read(0);
i2c_stop();
return ((msb<<8) | lsb);
}
void Mpu6050_Write(unsigned char address,unsigned char Data) {
i2c_start();
i2c_write(MPU6050_SLAVE_WRT);
i2c_write(address);
i2c_write(Data);
i2c_stop();
}
int GetData(unsigned char address){
char H,L;
H=Mpu6050_Read(address);
L=Mpu6050_Read(address+1);
return (H<<8)+L;
}
 
void Mpu6050_init(){
Mpu6050_Write(PWR_MGMT_1, 0x00); // internal 8MHz, disabled SLEEP mode, disable CYCLE mode
Mpu6050_Write(SMPLRT_DIV, 0x07); //sample rate: 8khz
Mpu6050_Write(CONFIG, 0x06); //DLPF disable
Mpu6050_Write(GYRO_CONFIG, 0x18); //full scale range mode 3 +-2000do/s
Mpu6050_Write(ACCEL_CONFIG, 0x01); //full scale range mode 1 +-4g
 
// Acc_MPU6050_init
//0x00: +-2g 16384 LSB/g
//0x08: +-4g 8192 LSB/g
//0x10: +-8g 4096 LSB/g
//0x18: +-16g 2048 LSB/g
 
}
 
//===================================
void main(){
int x_accel,y_accel,z_accel,x_gyro,y_gyro,z_gyro;
 
//lcd_init();
uart_init();
i2c_init();
Mpu6050_init();
 
while(1){
x_accel = GetData(ACCEL_XOUT_H);
//// y_accel = GetData(ACCEL_YOUT_H);
//// z_accel = GetData(ACCEL_ZOUT_H);
// x_gyro = GetData(GYRO_XOUT_H);
//// y_gyro = GetData(GYRO_YOUT_H);
z_gyro = GetData(GYRO_ZOUT_H);
//
 
printf("Ax= %u\r\n",x_accel);
__delay_ms(100);
}
 
}
 
void putch(unsigned char byte){
// output one byte
while(!TXIF) // set when register is empty
continue;
TXREG = byte;
}
 
D

Dương Đức Lộc

Trứng gà
Đã fix lỗi, ct chạy tốt.


#include <htc.h>
#include <stdio.h>
#include "uart.h"
#include "i2c.h"
//#include "lcd.h"

__CONFIG(INTIO & WDTDIS & PWRTEN & MCLREN & UNPROTECT & DUNPROTECT & BORDIS & IESODIS & FCMDIS & LVPDIS);

//baudrate 19200

//+++++++++++++++++++++++++DEFINE MPU6050+++++++++++++++++++++++++++
#define SMPLRT_DIV 0x19 //0x07(125Hz)
#define CONFIG 0x1A //0x06(5Hz)
#define GYRO_CONFIG 0x1B //0x18(2000deg/s)
#define ACCEL_CONFIG 0x1C //0x01(2G£¬5Hz)
#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
#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 PWR_MGMT_2 0x6C //
#define WHO_AM_I 0x75 //
#define MPU6050_ADD 0x68
#define MPU6050_SLAVE_WRT 0xD0 //dia chi cua cam bien mpu6050
#define MPU6050_SLAVE_RD 0xD1

void Mpu6050_init();

int Mpu6050_Read(unsigned char address){
char msb;


i2c_start();
i2c_write(MPU6050_SLAVE_WRT);
i2c_write(address);
i2c_start();
i2c_write(MPU6050_SLAVE_RD);
msb = i2c_read(1);
// lsb = i2c_read(0);
i2c_stop();

return msb;

// return ((msb<<8)|lsb);
}
void Mpu6050_Write(unsigned char address,unsigned char Data) {
i2c_start();
i2c_write(MPU6050_SLAVE_WRT);
i2c_write(address);
i2c_write(Data);
i2c_stop();
}
int GetData(unsigned char address){
char H,L;
H=Mpu6050_Read(address);
L=Mpu6050_Read(address+1);
return (H<<8)+L;
}

void Mpu6050_init(){
Mpu6050_Write(PWR_MGMT_1,0x00); // internal 8MHz, disabled SLEEP mode, disable CYCLE mode
Mpu6050_Write(SMPLRT_DIV,0x07); //sample rate: 8khz
Mpu6050_Write(CONFIG,0x06); //DLPF disable
Mpu6050_Write(GYRO_CONFIG,0x18); //full scale range mode 3 +-2000do/s
Mpu6050_Write(ACCEL_CONFIG,0x01); //full scale range mode 1 +-4g

// Acc_MPU6050_init
//0x00: +-2g 16384 LSB/g
//0x08: +-4g 8192 LSB/g
//0x10: +-8g 4096 LSB/g
//0x18: +-16g 2048 LSB/g

}
char get_ack_status(unsigned char address)
{
unsigned char status;

i2c_start();
status = i2c_write(address); // Status = 0 if got an ACK
i2c_stop();

if(status == 0)
return(1);
else
return(0);
}

void check_i2c()
{
unsigned char i;
unsigned char status;
unsigned char count = 0;

printf("\n\rStart:\n\r");

__delay_ms(100);

// Try all slave addresses from 0x10 to 0xEF.
// See if we get a response from any slaves
// that may be on the i2c bus.
for(i=0x10; i < 0xF0; i+=2)
{
status = get_ack_status(i);
if(status == 1)
{
printf("ACK addr: 0x%x\n\r", i);
count++;
__delay_ms(100);
}
}

if(count == 0)
printf("\n\rNothing Found");
else
printf("\n\rNumber of i2c chips found: %u", count);

while(1);
}


void MPU6050_Test_I2C(){
unsigned char Data ;

Data = Mpu6050_Read(WHO_AM_I);

if(Data == 0x68)
printf("\n\rI2C Read Test Passed, MPU6050 Address: 0x%x",Data);
else
printf("\n\rERROR: I2C Read Test Failed, Stopping");

}


//===================================
void main(){
int x_accel,y_accel,z_accel,x_gyro,y_gyro,z_gyro;

// lcd_init();
uart_init();
i2c_init();
Mpu6050_init();
// check_i2c();
// MPU6050_Test_I2C();
while(1){
x_accel = GetData(ACCEL_XOUT_H);
y_accel = GetData(ACCEL_YOUT_H);
z_accel = GetData(ACCEL_ZOUT_H);
x_gyro = GetData(GYRO_XOUT_H);
y_gyro = GetData(GYRO_YOUT_H);
z_gyro = GetData(GYRO_ZOUT_H);

// printf("\f%d,%d,%d",x_accel,y_accel,z_accel);
// lcd_gotoxy(0,1);
// printf("%d,%d,%d",x_gyro,y_gyro,z_gyro);

// printf("\r\nAx , Ay, Az, Gx, Gy, Gz");
printf("\r\n a/g : %d,%d,%d, %d,%d,%d",x_accel,y_accel,z_accel,x_gyro,y_gyro,z_gyro);
// printf("%d,%d,%d",x_gyro,y_gyro,z_gyro);
__delay_ms(50);
}
//
}

void putch(unsigned char byte){
// output one byte
while(!TXIF) // set when register is empty
continue;
TXREG = byte;
}
 
You must log in or register to reply here.
Top