 |
 |
| View previous topic :: View next topic |
| Author |
Message |
PCM programmer
Joined: 06 Sep 2003
Posts: 21708
|
|
 Posted: Thu Sep 25, 2014 12:20 pm |
 |
|
Here is the ported code from the links I posted. This code does not use
the internal DMP mpu6050 to do the calculations. The calculations are
done in the PIC with the following code.
Here's the sample output I got. As I lift up one side of the mpu6050
board, the Y angle increases:
| Quote: |
Gyro X offset sum: -234453 Gyro X offset: -234
Gyro Y offset sum: 138770 Gyro Y offset: 138
Gyro Z offset sum: -78797 Gyro Z offset: -78
accel_xangle = 0
accel_yangle = 3
accel_xangle = 0
accel_yangle = 3
accel_xangle = 0
accel_yangle = 3
accel_xangle = 0
accel_yangle = 3
accel_xangle = 0
accel_yangle = 16
accel_xangle = 0
accel_yangle = 36
accel_xangle = 0
accel_yangle = 84
|
Test program:
| Code: | #include <18F4620.h>
#fuses INTRC_IO, NOWDT, BROWNOUT, PUT, NOLVP
#use delay(internal=32M)
#use rs232(baud=9600, UART1, ERRORS)
// I'm using software i2c on these pins for the mpu6050
// because the hardware pins on C3 and C4 are already
// in use on my development board.
#use i2c(Master, scl=PIN_C0, sda=PIN_C1)
#include <math.h>
#define MPU6050_SLAVE_WRT 0xD0
#define MPU6050_SLAVE_RD 0xD1
// Register addresses
#define MPU6050_WHO_AM_I 0x05
#define MPU6050_SMPLRT_DIV 0x19
#define MPU6050_CONFIG 0x1A
#define MPU6050_GYRO_CONFIG 0x1B
#define MPU6050_PWR_MGMT_1 0x6B
#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 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
// In the +/- 500 degrees mode, the gyro sensitivity is 65.5 LSB/dps.
#define gyro_xsensitivity 66.5 //66.5 Dead on at last check
#define gyro_ysensitivity 66.5 //72.7 Dead on at last check
#define gyro_zsensitivity 65.5
#define a 0.01
#define dt 0.0025
// Global variables
float gyro_xrate;
float gyro_yrate;
float gyro_zrate;
//int16 gyro_xrateraw;
//int16 gyro_yrateraw;
//int16 gyro_zrateraw;
signed int16 gyro_xout;
signed int16 gyro_yout;
signed int16 gyro_zout;
signed int16 accel_xout;
signed int16 accel_yout;
signed int16 accel_zout;
signed int16 gyro_xout_offset;
signed int16 gyro_yout_offset;
signed int16 gyro_zout_offset;
signed int32 gyro_xout_offset_1000sum;
signed int32 gyro_yout_offset_1000sum;
signed int32 gyro_zout_offset_1000sum;
float gyro_xangle;
float gyro_yangle;
float gyro_zangle;
//int32 gyro_xangleraw;
//int32 gyro_yangleraw;
//int32 gyro_zangleraw;
float accel_xangle;
float accel_yangle;
//float accel_zangle;
//float kalman_xangle;
//float kalman_yangle;
//float kalman_zangle;
//--------------------------------------------------
int16 MPU6050_read_word(int8 reg_addr)
{
int8 lsb, msb;
int16 retval;
i2c_start();
i2c_write(MPU6050_SLAVE_WRT);
i2c_write(reg_addr);
i2c_start();
i2c_write(MPU6050_SLAVE_RD);
msb = i2c_read(); // Read MSB byte first.
lsb = i2c_read(0); // Do a NACK on the last read
i2c_stop();
retval = make16(msb, lsb);
return(retval);
}
//------------------------------------------------------
void Get_Accel_Values()
{
accel_xout = MPU6050_read_word(ACCEL_XOUT_H);
accel_yout = MPU6050_read_word(ACCEL_YOUT_H);
accel_zout = MPU6050_read_word(ACCEL_ZOUT_H);
}
//----------------------------
void Get_Gyro_Rates()
{
gyro_xout = MPU6050_read_word(GYRO_XOUT_H) - gyro_xout_offset;
gyro_yout = MPU6050_read_word(GYRO_YOUT_H) - gyro_yout_offset;
gyro_zout = MPU6050_read_word(GYRO_ZOUT_H) - gyro_zout_offset;
gyro_xrate = (float)gyro_xout/gyro_xsensitivity;
gyro_yrate = (float)gyro_yout/gyro_ysensitivity;
gyro_zrate = (float)gyro_zout/gyro_zsensitivity;
gyro_xangle += gyro_xrate * dt;
gyro_yangle += gyro_yrate * dt;
gyro_zangle += gyro_zrate * dt;
}
//-----------------------------------------
void Get_Accel_Angles()
{
accel_xangle = 57.295*atan((float)accel_yout/ sqrt(pow((float)accel_zout,2)+pow((float)accel_xout,2)));
accel_yangle = 57.295*atan((float)-accel_xout/ sqrt(pow((float)accel_zout,2)+pow((float)accel_yout,2)));
}
//--------------------------------------------------
void Calibrate_Gyros()
{
int16 x = 0;
gyro_xout_offset_1000sum = 0;
gyro_yout_offset_1000sum = 0;
gyro_zout_offset_1000sum = 0;
for(x=0; x<1000; x++)
{
gyro_xout = MPU6050_read_word(GYRO_XOUT_H);
gyro_yout = MPU6050_read_word(GYRO_YOUT_H);
gyro_zout = MPU6050_read_word(GYRO_ZOUT_H);
gyro_xout_offset_1000sum += gyro_xout;
gyro_yout_offset_1000sum += gyro_yout;
gyro_zout_offset_1000sum += gyro_zout;
delay_ms(1);
}
gyro_xout_offset = gyro_xout_offset_1000sum/1000;
gyro_yout_offset = gyro_yout_offset_1000sum/1000;
gyro_zout_offset = gyro_zout_offset_1000sum/1000;
printf("Gyro X offset sum: %ld Gyro X offset: %ld \n\r", gyro_xout_offset_1000sum, gyro_xout_offset);
printf("Gyro Y offset sum: %ld Gyro Y offset: %ld \n\r", gyro_yout_offset_1000sum, gyro_yout_offset);
printf("Gyro Z offset sum: %ld Gyro Z offset: %ld \n\r", gyro_zout_offset_1000sum, gyro_zout_offset);
}
//--------------------------------------------
#define BW_256_HZ 0
#define BW_10_HZ 5
#define BW_5_HZ 6
void set_DLPF(int8 bandwidth)
{
i2c_start();
i2c_write(MPU6050_SLAVE_WRT);
i2c_write(MPU6050_CONFIG);
i2c_write(bandwidth);
i2c_stop();
}
//--------------------------------------------
void Setup_mpu6050(void)
{
delay_ms(100);
i2c_start();
i2c_write(MPU6050_SLAVE_WRT);
i2c_write(MPU6050_PWR_MGMT_1);
i2c_write(0x00);
i2c_stop();
i2c_start();
i2c_write(MPU6050_SLAVE_WRT);
i2c_write(MPU6050_GYRO_CONFIG);
i2c_write(0x08);
i2c_stop();
i2c_start();
i2c_write(MPU6050_SLAVE_WRT);
i2c_write(MPU6050_SMPLRT_DIV);
i2c_write(0x07);
i2c_stop();
delay_ms(100); // Gyro start-up time is 30ms after power-up.
}
//------------------------------------------------
//===================================
void main()
{
Setup_mpu6050();
Calibrate_Gyros();
//set_DLPF(BW_5_HZ);
while(TRUE)
{
Get_Accel_Values();
Get_Gyro_Rates();
Get_Accel_Angles();
printf("accel_xangle = %7.0f \n\r", accel_xangle);
printf("accel_yangle = %7.0f \n\r", accel_yangle);
printf("\n\r");
delay_ms(2000);
}
while(1);
} |
This code is not mine. I just ported it to CCS. I don't want to trouble-
shoot the code. I don't want to explain it. I just wanted to get you going
on your project. |
|
 |
Ruby
Joined: 04 Jul 2014
Posts: 44
|
|
| Posted: Thu Sep 25, 2014 12:44 pm |
|
|
| Thank you. I will check the code. Thanks alot. |
|
|
Ruby
Joined: 04 Jul 2014
Posts: 44
|
|
| Posted: Fri Sep 26, 2014 3:55 pm |
|
|
| Hi guys. Code works perfect with 16f877a. I have also cleaned the code cause it uses only accel values. I am trying to find some examples about DMP. |
|
|
mohammad3d
Joined: 28 Mar 2009
Posts: 17
|
|
|
|
|
You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot vote in polls in this forum
|
Powered by phpBB © 2001, 2005 phpBB Group
|
FAQ 
Search
Register
Profile 
Log in to check your private messages 
Log in
