/
MPU6050.c
81 lines (66 loc) · 2.08 KB
/
MPU6050.c
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
/*
MPU6050 Interfacing with Raspberry Pi
http://www.electronicwings.com
*/
#include <wiringPiI2C.h>
#include <stdlib.h>
#include <stdio.h>
#include <wiringPi.h>
#include <xdo.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(){
xdo_t *xdo = xdo_new(NULL);
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);
/* subtract raw value the calibration offsets and divide by sensitivity scale factor*/
Gx = (Gyro_x - 118)/40;
Gy = (Gyro_y - 45)/40;
xdo_move_mouse_relative( xdo ,-(int)Gy , (int)Gx);
//delay(500);
}
return 0;
}