MPU-6050 Accelerometer/Gyroscope with 16F877A-CCS C,Picbasic Pro and Proton Code

[intel]

MPU-6050 6-Axis Accelerometer/Gyroscope with 16F877A - CCS C Code

Related to the acceleration sensors, many questions unanswered in the forums and saw the unfinished work. Codes associated with acceleration sensors, I'll be posting the help of the codes used in the forums.
Some mathematical formulas taken from their pdf files of the acceleration sensor. Some numbers, because it does not allow the processor bits I wrote using the appropriate numbers. CCS C is very efficient in this regard. 8-bit inefficiency is able to absorb ("math.h"). Proton and Picbasic Pro may be errors. I'm glad you publish corrections.
We may in the future use of these codes to balance the robot and share code.
Good work everyone.









Links:
http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf
http://www.i2cdevlib.com/devices/mpu6050#source
http://www.i2cdevlib.com/devices/mpu6050#registers
https://www.youtube.com/watch?v=lwlOZzNJqJc



This codes works fine for me.

Main Code:

Code:

#include <16f877A.h>
#FUSES NOWDT                    //No Watch Dog Timer
#FUSES NOBROWNOUT               //No brownout reset
#FUSES NOLVP                    //No low voltage prgming, B3(PIC16) or B5(PIC18) used for I/O
#FUSES HS                       //Dont change
#use    delay(clock=20MHz)  
#use I2C(master, sda=PIN_d0, scl=PIN_d1, slow)  //You can change, according your board ports
#use rs232(baud=9600,xmit=PIN_C6,rcv=PIN_C7)    //You can change, according your board ports
#include "EXLCD.C"       //You can change, according your board ports
//#include "EXLCD416.C"  //You can change, according your board ports
#include "MPU6050.C"
#include <math.h>

signed int8 A_data[6];
signed int8 temp_data[2];
signed int8 G_data[6];
signed int16 Xa=0,Ya=0,Za=0;
signed int16 temp=0;
signed int16 Xg=0,Yg=0,Zg=0;

void main()
{
delay_ms(2);
lcd_init();
mpu6050_init();    
   
printf(lcd_putc,"\f");
lcd_gotoxy(1,1);
printf(lcd_putc," MPU6050 Gyro   ");
lcd_gotoxy(1,2);
printf(lcd_putc," Accelerometer  ");
delay_ms(1000);
printf(lcd_putc,"\f");

   while(TRUE)
   {
   A_data[0]=mpu6050_read(0x3B); //Read X axis(LSB)
   A_data[1]=mpu6050_read(0x3C); //Read X axis(MSB)
   A_data[2]=mpu6050_read(0x3D); //Read Y axis(LSB)
   A_data[3]=mpu6050_read(0x3E); //Read Y axis(MSB)
   A_data[4]=mpu6050_read(0x3F); //Read Z axis(LSB)
   A_data[5]=mpu6050_read(0x40); //Read Z axis(MSB)
     
   temp_data[0]=mpu6050_read(0x41);
   temp_data[1]=mpu6050_read(0x42);
     
   G_data[0]=mpu6050_read(0x43); //Read X axis(LSB)
   G_data[1]=mpu6050_read(0x44); //Read X axis(MSB)
   G_data[2]=mpu6050_read(0x45); //Read Y axis(LSB)
   G_data[3]=mpu6050_read(0x46); //Read Y axis(MSB)
   G_data[4]=mpu6050_read(0x47); //Read Z axis(LSB)
   G_data[5]=mpu6050_read(0x48); //Read Z axis(MSB)
    
   Xa=make16(A_data[0],A_data[1]);
   Ya=make16(A_data[2],A_data[3]);
   Za=make16(A_data[4],A_data[5]);
    
   temp=make16(temp_data[0],temp_data[1]);
   temp=temp/340 + 36.53;
   
   Xg=make16(G_data[0],G_data[1]);
   Yg=make16(G_data[2],G_data[3]);
   Zg=make16(G_data[4],G_data[5]);
   
   float Heading = atan2((signed int16)Ya,(signed int16)Xa)* 180 / pi + 180;
     
   lcd_gotoxy(1,1);
   printf(lcd_putc,"X:%ld  ",Xa);
   lcd_gotoxy(10,1);
   printf(lcd_putc,"Y:%ld  ",Ya);
   lcd_gotoxy(1,2);
   printf(lcd_putc,"Z:%ld  ",Za);
   lcd_gotoxy(10,2);
   printf(lcd_putc,"H:%f  ",heading);
/* For  4x16 LCD
   lcd_gotoxy(1,3);
   printf(lcd_putc,"Xg:%ld  ",Xg);
   lcd_gotoxy(9,3);
   printf(lcd_putc,"Yg:%ld  ",Yg);
   lcd_gotoxy(1,4);
   printf(lcd_putc,"Zg:%ld  ",Zg);
   lcd_gotoxy(9,4);
   printf(lcd_putc,"T :%ld  ",temp);
*/
   printf("\n\r H: %f X: %ld  Y: %ld  Z: %ld  T: %ld  \n\r",Heading,Xg,Yg,Zg,temp);
   delay_ms(100);
   }
}

MPU-6050 Library:

Code:

// MPU6050 required Registers

#define W_DATA         0xD0
#define R_DATA         0xD1
#define PWR_MGMT_1     0x6B
#define PWR_MGMT_2     0x6C
#define SMPRT_DIV      0x19
#define CONFIG_R       0x1A
#define GYRO_CONFIG    0x1B
#define ACCEL_CONFIG   0x1C
#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

void mpu6050_write(int add, int data)
{
         i2c_start();
         i2c_write(W_DATA);
         i2c_write(add);
         i2c_write(data);
         i2c_stop();
 
}
      
int16 mpu6050_read(int add){
         int retval;
         i2c_start();
         i2c_write(W_DATA);
         i2c_write(add);
         i2c_start();
         i2c_write(R_DATA);
         retval=i2c_read(0);
         i2c_stop();
         return retval;
}
 
void mpu6050_init(){
         mpu6050_write(PWR_MGMT_1,  0x80);
         delay_ms(100);
         mpu6050_write(PWR_MGMT_1,  0x00);
         delay_ms(100);
         mpu6050_write(CONFIG_R,    0x01);
         delay_ms(10);
         mpu6050_write(GYRO_CONFIG, 0x00);

In future times, I will add Picbasic Pro and Protonbasic codes. With good wishes.

[intel]

Picbasic Pro code for MPU6050:

This codes works fine for me.

Code:

'****************************************************************
'*  Name    : MPU6050 PBP 16F877A.BAS                           *
'*  Author  : YUKSEL                                            *
'*  Notice  : Copyright (c) 2015 YUKSEL                         *
'*          : All Rights Reserved                               *
'*  Date    : 15.3.2015                                         *
'*  Version : 1.0                                               *
'*  Notes   :                                                   *
'*          :                                                   *
'****************************************************************
'Include "MODEDEFS.BAS" ' Include serial modes
adcon1 = 7     ' Turns Analogs off
'For 20 MHz config     
#config
 __config _HS_OSC & _LVP_OFF   
#ENDCONFIG

DEFINE OSC 20

TRISA = %00000000
TRISB = %00000000
PORTC = %01000000
PORTD = %00000011
PORTE = %00000000

DEFINE LCD_DREG	PORTB	
DEFINE LCD_DBIT		0		
DEFINE LCD_EREG	PORTB	
DEFINE LCD_EBIT		5		     		
DEFINE LCD_RSREG	PORTB	
DEFINE LCD_RSBIT	4		
DEFINE LCD_BITS		4		
DEFINE LCD_LINES	2	
READAX  VAR WORD
READAY  VAR WORD
READAZ  VAR WORD
READX   VAR Word 
READY   VAR Word
READZ   VAR Word
READT   VAR WORD
READT1  VAR word
'AZ VAR WORD
'X VAR BYTE
'Y VAR BYTE
'RAX VAR WORD
'RAY VAR WORD
'RX VAR WORD
'RY VAR WORD

Symbol SDA = PORTD.0        'I2C data pin. Pullup connection is required.
Symbol SCL = PORTD.1        'I2C clock pin. Pullup connection is required.
RW_DATA       CON $D0
PWR_MGMT_1    CON $6B
PWR_MGMT_2    CON $6C
SMPRT_DIV     CON $19
CONFIG_R      CON $1A
GYRO_CONFIG   CON $1B
ACCEL_CONFIG  CON $1C
ACCEL_XOUT_H  CON $3B
ACCEL_XOUT_L  CON $3C
ACCEL_YOUT_H  CON $3D
ACCEL_YOUT_L  CON $3E
ACCEL_ZOUT_H  CON $3F
ACCEL_ZOUT_L  CON $40
TEMP_OUT_H    CON $41
TEMP_OUT_L    CON $42
GYRO_XOUT_H   CON $43
GYRO_XOUT_L   CON $44
GYRO_YOUT_H   CON $45
GYRO_YOUT_L   CON $46
GYRO_ZOUT_H   CON $47
GYRO_ZOUT_L   CON $48
PAUSE 100 ' WAIT STARTUP MPU6050

I2CWRITE SDA,SCL,RW_DATA,PWR_MGMT_1,  [%10000000]  ' 'RESITER 6B CONFIG (PWR MANAGEMENT)  MPU6050 reset
pause 10
I2CWRITE SDA,SCL,RW_DATA,PWR_MGMT_1,  [%00000000]  ' 'RESITER 6B CONFIG (PWR MANAGEMENT)  MPU6050 START
PAUSE 10
I2CWRITE SDA,SCL,RW_DATA,CONFIG_R,    [%00000001]    'RESITER 1A CONFIG (FILTER CONFIG)
pause 10
I2CWRITE SDA,SCL,RW_DATA,GYRO_CONFIG, [%00000000]  ' 'RESITER 1B CONFIG (GYRO CONFIG)
pause 10
I2CWRITE SDA,SCL,RW_DATA,ACCEL_CONFIG,[%00010000]  ' 'RESITER 1C CONFIG (ACC CONFIG)
pause 10


READI2C:
I2CREAD SDA,SCL,RW_DATA,ACCEL_XOUT_H,[READAX.HighByte]  'Read the data starting at x_msb
I2CREAD SDA,SCL,RW_DATA,ACCEL_XOUT_L,[READAX.LowByte ]

I2CREAD SDA,SCL,RW_DATA,ACCEL_YOUT_H,[READAY.HighByte]
I2CREAD SDA,SCL,RW_DATA,ACCEL_YOUT_L,[READAY.LowByte ]

I2CREAD SDA,SCL,RW_DATA,ACCEL_ZOUT_H,[READAZ.HighByte]
I2CREAD SDA,SCL,RW_DATA,ACCEL_ZOUT_L,[READAZ.LowByte ]

I2CREAD SDA,SCL,RW_DATA,TEMP_OUT_H,  [READT.HighByte ]
I2CREAD SDA,SCL,RW_DATA,TEMP_OUT_L,  [READT.LowByte  ]

I2CREAD SDA,SCL,RW_DATA,GYRO_XOUT_H, [READX.HighByte]  'Read the data starting at x_msb
I2CREAD SDA,SCL,RW_DATA,GYRO_XOUT_L, [READX.LowByte ]

I2CREAD SDA,SCL,RW_DATA,GYRO_YOUT_H, [READY.HighByte]
I2CREAD SDA,SCL,RW_DATA,GYRO_YOUT_L, [READY.LowByte ]

I2CREAD SDA,SCL,RW_DATA,GYRO_ZOUT_H, [READZ.HighByte]
I2CREAD SDA,SCL,RW_DATA,GYRO_ZOUT_L, [READZ.LowByte ]

READT1 = (READT / 340 + 37)-192
'X = READAX>>2  
'Y = READAY>>2
'AZ = Y ATN X
PAUSE 50

LCDOUT $FE,1, "X=",SDec READAX,$FE,2,$FE,$14,$FE,$14,$FE,$14,$FE,$14,$FE,$14,$FE,$14,$FE,$14,$FE,$14,"Y=",SDec READAY,"   " 
LCDOUT $FE,$C0,"Z=",SDec READAZ,$FE,$C0+7,$FE,$C0+8,"T=",Sdec  READT1'/100,".",dec2 READT1/100, "   "
serout2 portC.6,84,[" AX= ",SDec READAX ," AY= ",SDec READAY ," X= ",SDec READX ," Y= ",SDec READY," Z= ",SDec READZ," T= ",Dec READT1,13,10]        
GoTo READI2C:

End

With good wishes.

[intel]

Proton code for MPU6050:

This codes works fine for me.

Code:

'****************************************************************
'*  Name    : MPU6050 PROTON 16F877A.BAS                        *
'*  Author  : YUKSEL                                            *
'*  Notice  : Copyright (c) 2015 YUKSEL                         *
'*          : All Rights Reserved                               *
'*  Date    : 9.3.2015                                          *
'*  Version : 1.0                                               *
'*  Notes   :                                                   *
'*          :                                                   *
'****************************************************************
Device = 16F877A
Declare Xtal = 20            ' Kristal frekansı: 20 MHz
CMCON = 7                    ' Tüm comparatorlar devre dışı
Declare All_Digital = True   ' 16f877A nın tüm uçları Dijital
TRISA = %00000000
TRISB = %00000000
TRISC = %01000000
TRISD = %00000011
TRISE = %00000000

Declare Rsout_Pin = PORTC.6  ' RSOut komutuyla gönderilecek veri için çıkış pinini tayin eder.
Declare Serial_Baud = 9600   ' 0 dan 65535 bps e kadar
Declare Rsout_Mode 0         ' Data 1= True Normal modda gönderilir. (Diğeri 0=Inverted modu)

Declare LCD_Type         0   ' Standart 2x16 LCD
Declare LCD_DTPort PORTB     ' LCD data portu
Declare LCD_DTPin  PORTB.0	 ' DATA girişi PORTB'nin B0 ucundan başlayacak
Declare LCD_ENPin  PORTB.5   ' Enable (EN) pini E2
Declare LCD_RSPin  PORTB.4   ' Register Select (RS) pini B4
Declare LCD_Interface    4   ' 4 bit LCD arayüzü
Declare LCD_Lines        2	 ' 2 satırlık LCD 	

Dim READAX  As Word
Dim READAY  As Word
Dim READAZ  As Word
Dim READX   As Word 
Dim READY   As Word
Dim READZ   As Word
Dim READT   As Word
Dim READT1  As Word
        
Symbol SDA = PORTD.0  ' I2C data pin. Pullup connection is required. You can change, according your board ports
Symbol SCL = PORTD.1  ' I2C clock pin. Pullup connection is required. You can change, according your board ports
Symbol W_DATA = $D0   'Used to perform a Write operation
Symbol R_DATA = $D1   'Used to perform a Read operation
Symbol PWR_M1 = $6B   'Power Management Register 1.This register allows the user to configure the power mode and clock source. It also provides a bit for resetting the entire device, and a bit for disabling the temperature sensor.
Symbol PWR_M2 = $6C   'Power Management Register 2.This register allows the user to configure the frequency of wake-ups in Accelerometer Only Low Power Mode. This register also allows the user to put individual axes of the accelerometer and gyroscope into standby mode.
Symbol CONF_R = $1A   'Congiguration Register.This register configures the external Frame Synchronization (FSYNC) pin sampling and the Digital Low Pass Filter (DLPF) setting for both the gyroscopes and accelerometers.
Symbol G_CONF = $1B   'Gyro Configuration Register.This register is used to trigger gyroscope self-test and configure the gyroscope full scale range.
Symbol A_CONF = $1C   'Accelerometer Congiguration Register. This register is used to trigger accelerometer self test and configure the accelerometer full scale range. This register also configures the Digital High Pass Filter (DHPF).
Symbol XA_MSB = $3B   'Read Register, Output of accelerometer X MSB 8-bit value.
Symbol XA_LSB = $3C   'Read Register, Output of accelerometer X LSB 8-bit value.
Symbol YA_MSB = $3D   'Read Register, Output of accelerometer Y MSB 8-bit value.
Symbol YA_LSB = $3E   'Read Register, Output of accelerometer Y LSB 8-bit value.
Symbol ZA_MSB = $3F   'Read Register, Output of accelerometer Z MSB 8-bit value.
Symbol ZA_LSB = $40   'Read Register, Output of accelerometer Z LSB 8-bit value.
Symbol TEMP_M = $41   'Read Register, Output of temperature MSB 8-bit value.
Symbol TEMP_L = $42   'Read Register, Output of temperature LSB 8-bit value.
Symbol XG_MSB = $43   'Read Register, Output of gyro X MSB 8-bit value.
Symbol XG_LSB = $44   'Read Register, Output of gyro X LSB 8-bit value.
Symbol YG_MSB = $45   'Read Register, Output of gyro Y MSB 8-bit value.
Symbol YG_LSB = $46   'Read Register, Output of gyro Y LSB 8-bit value.
Symbol ZG_MSB = $47   'Read Register, Output of gyro Z MSB 8-bit value.
Symbol ZG_LSB = $48   'Read Register, Output of gyro Z LSB 8-bit value.

I2COut SDA,SCL,W_DATA,PWR_M1,[%10000000]  'RESITER 6B CONFIG (PWR MANAGEMENT)  MPU6050 reset
DelayMS 100
I2COut SDA,SCL,W_DATA,PWR_M1,[%00000000]  'RESITER 6B CONFIG (PWR MANAGEMENT)  MPU6050 START
DelayMS 100
I2COut SDA,SCL,W_DATA,CONF_R,[%00000001]  'RESITER 1A CONFIG (FILTER CONFIG)
DelayMS 10
I2COut SDA,SCL,W_DATA,G_CONF,[%00000000]  'RESITER 1B CONFIG (GYRO CONFIG)
DelayMS 10
I2COut SDA,SCL,W_DATA,A_CONF,[%00010000]  'RESITER 1C CONFIG (ACCELERATOR CONFIG)
DelayMS 10

READI2C:
I2CIn SDA,SCL,R_DATA,XA_MSB,[READAX.HighByte]  'The first 8 bits of 16-bit X Axis
I2CIn SDA,SCL,R_DATA,XA_LSB,[READAX.LowByte ]  'The second 8 bits of 16-bit X Axis
I2CIn SDA,SCL,R_DATA,YA_MSB,[READAY.HighByte]
I2CIn SDA,SCL,R_DATA,YA_LSB,[READAY.LowByte ]
I2CIn SDA,SCL,R_DATA,ZA_MSB,[READAZ.HighByte]
I2CIn SDA,SCL,R_DATA,ZA_LSB,[READAZ.LowByte ]
I2CIn SDA,SCL,R_DATA,TEMP_M,[READT.HighByte ]
I2CIn SDA,SCL,R_DATA,TEMP_L,[READT.LowByte  ]
I2CIn SDA,SCL,R_DATA,XG_MSB,[READX.HighByte]  
I2CIn SDA,SCL,R_DATA,XG_LSB,[READX.LowByte ]
I2CIn SDA,SCL,R_DATA,YG_MSB,[READY.HighByte]
I2CIn SDA,SCL,R_DATA,YG_LSB,[READY.LowByte ]
I2CIn SDA,SCL,R_DATA,ZG_MSB,[READZ.HighByte]
I2CIn SDA,SCL,R_DATA,ZG_LSB,[READZ.LowByte ]

READT1 = (READT / 340 + 37)-192

Print At 1,1, "X=",SDec READAX,"     "
Print At 1,9, "Y=",SDec READAY,"     " 
Print At 2,1, "Z=",SDec READAZ,"     "
Print At 2,9, "T=",SDec READT1,"     "
RSOut " AX= ",SDec READAX ," AY= ",SDec READAY ," AZ= ",SDec READAZ," X= ",SDec READX ," Y= ",SDec READY," Z= ",SDec READZ," T= ",Dec READT1,13,10
DelayMS 100        
GoTo READI2C:

End

With good wishes.