-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathgyro2.py
169 lines (127 loc) · 4.39 KB
/
gyro2.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
import smbus #import SMBus module of I2C
from time import sleep #import
#some MPU6050 Registers and their Address
PWR_MGMT_1 = 0x6B
SMPLRT_DIV = 0x19
CONFIG = 0x1A
GYRO_CONFIG = 0x1B
INT_ENABLE = 0x38
ACCEL_XOUT_H = 0x3B
ACCEL_YOUT_H = 0x3D
ACCEL_ZOUT_H = 0x3F
GYRO_XOUT_H = 0x43
GYRO_YOUT_H = 0x45
GYRO_ZOUT_H = 0x47
def MPU_Init():
#write to sample rate register
bus.write_byte_data(Device_Address, SMPLRT_DIV, 7)
#Write to power management register
bus.write_byte_data(Device_Address, PWR_MGMT_1, 1)
#Write to Configuration register
bus.write_byte_data(Device_Address, CONFIG, 0)
#Write to Gyro configuration register
bus.write_byte_data(Device_Address, GYRO_CONFIG, 24)
#Write to interrupt enable register
bus.write_byte_data(Device_Address, INT_ENABLE, 1)
def read_raw_data(addr):
#Accelero and Gyro value are 16-bit
high = bus.read_byte_data(Device_Address, addr)
low = bus.read_byte_data(Device_Address, addr+1)
#concatenate higher and lower value
value = ((high << 8) | low)
#to get signed value from mpu6050
if(value > 32768):
value = value - 65536
return value
bus = smbus.SMBus(1) # or bus = smbus.SMBus(0) for older version boards
Device_Address = 0x68 # MPU6050 device address
MPU_Init()
print (" Reading Data of Gyroscope and Accelerometer")
while True:
#Read Accelerometer raw value
acc_x = read_raw_data(ACCEL_XOUT_H)
acc_y = read_raw_data(ACCEL_YOUT_H)
acc_z = read_raw_data(ACCEL_ZOUT_H)
#Read Gyroscope raw value
gyro_x = read_raw_data(GYRO_XOUT_H)
gyro_y = read_raw_data(GYRO_YOUT_H)
gyro_z = read_raw_data(GYRO_ZOUT_H)
#Full scale range +/- 250 degree/C as per sensitivity scale factor
Ax = acc_x/16384.0
Ay = acc_y/16384.0
Az = acc_z/16384.0
Gx = gyro_x/131.0
Gy = gyro_y/131.0
Gz = gyro_z/131.0
print ("Gx=%.2f" %Gx, u'\u00b0'+ "/s", "\tGy=%.2f" %Gy, u'\u00b0'+ "/s", "\tGz=%.2f" %Gz, u'\u00b0'+ "/s", "\tAx=%.2f g" %Ax, "\tAy=%.2f g" %Ay, "\tAz=%.2f g" %Az)
sleep(1)
C Program
Here, we are using WiringPi C library to read data from MPU6050 module.
/*
MPU6050 Interfacing with Raspberry Pi
http://www.electronicwings.com
*/
#include <wiringPiI2C.h>
#include <stdlib.h>
#include <stdio.h>
#include <wiringPi.h>
#define Device_Address 0x68 /*Device Address/Identifier for MPU6050*/
#define PWR_MGMT_1 0x6B
#define SMPLRT_DIV 0x19
#define CONFIG 0x1A
#define GYRO_CONFIG 0x1B
#define INT_ENABLE 0x38
#define ACCEL_XOUT_H 0x3B
#define ACCEL_YOUT_H 0x3D
#define ACCEL_ZOUT_H 0x3F
#define GYRO_XOUT_H 0x43
#define GYRO_YOUT_H 0x45
#define GYRO_ZOUT_H 0x47
int fd;
void MPU6050_Init(){
wiringPiI2CWriteReg8 (fd, SMPLRT_DIV, 0x07); /* Write to sample rate register */
wiringPiI2CWriteReg8 (fd, PWR_MGMT_1, 0x01); /* Write to power management register */
wiringPiI2CWriteReg8 (fd, CONFIG, 0); /* Write to Configuration register */
wiringPiI2CWriteReg8 (fd, GYRO_CONFIG, 24); /* Write to Gyro Configuration register */
wiringPiI2CWriteReg8 (fd, INT_ENABLE, 0x01); /*Write to interrupt enable register */
}
short read_raw_data(int addr){
short high_byte,low_byte,value;
high_byte = wiringPiI2CReadReg8(fd, addr);
low_byte = wiringPiI2CReadReg8(fd, addr+1);
value = (high_byte << 8) | low_byte;
return value;
}
void ms_delay(int val){
int i,j;
for(i=0;i<=val;i++)
for(j=0;j<1200;j++);
}
int main(){
float Acc_x,Acc_y,Acc_z;
float Gyro_x,Gyro_y,Gyro_z;
float Ax=0, Ay=0, Az=0;
float Gx=0, Gy=0, Gz=0;
fd = wiringPiI2CSetup(Device_Address); /*Initializes I2C with device Address*/
MPU6050_Init(); /* Initializes MPU6050 */
while(1)
{
/*Read raw value of Accelerometer and gyroscope from MPU6050*/
Acc_x = read_raw_data(ACCEL_XOUT_H);
Acc_y = read_raw_data(ACCEL_YOUT_H);
Acc_z = read_raw_data(ACCEL_ZOUT_H);
Gyro_x = read_raw_data(GYRO_XOUT_H);
Gyro_y = read_raw_data(GYRO_YOUT_H);
Gyro_z = read_raw_data(GYRO_ZOUT_H);
/* Divide raw value by sensitivity scale factor */
Ax = Acc_x/16384.0;
Ay = Acc_y/16384.0;
Az = Acc_z/16384.0;
Gx = Gyro_x/131;
Gy = Gyro_y/131;
Gz = Gyro_z/131;
printf("\n Gx=%.3f °/s\tGy=%.3f °/s\tGz=%.3f °/s\tAx=%.3f g\tAy=%.3f g\tAz=%.3f g\n",Gx,Gy,Gz,Ax,Ay,Az);
delay(500);
}
return 0;
}