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
| #include <LiquidCrystal.h>
#include <Wire.h>
#include "fonctions.h"
#define X 0 // X axis
#define Y 1 // Y axis
#define Z 2 // Z axis
#define MPU_ADDRESS 0x68 // I2C address of the MPU-6050
#define FREQ 260 // Sampling frequency
#define SSF_GYRO 65.5 // Sensitivity Scale Factor of the gyro from datasheet
const int rs = 12, en = 11, d4 = 5, d5 = 4, d6 = 3, d7 = 2;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);
int gyro_raw[3] = {0, 0, 0};
// Average gyro offsets of each axis in that order: X, Y, Z
long gyro_offset[3] = {0, 0, 0};
// Calculated angles from gyro's values in that order: X, Y, Z
float gyro_angle[3] = {0, 0, 0};
// The RAW values got from accelerometer (in m/sec²) in that order: X, Y, Z
int acc_raw[3] = {0 , 0 , 0};
// Calculated angles from accelerometer's values in that order: X, Y, Z
float acc_angle[3] = {0, 0, 0};
int moy;
boolean initialise;
int temperature;
unsigned int period; // Sampling period
unsigned long loop_timer;
float fspeed;
void setup() {
Serial.begin(9600);
lcd.begin(16, 2);
Wire.begin();
TWBR = 12; // Set the I2C clock speed to 400kHz.
period = (1000000 / FREQ);
loop_timer = micros();
setupMpu6050Registers();
CalibrerMPU();
Serial.print("moyenne : ");
Serial.println(moy);
Serial.print(period);
initialise = true;
}
void loop() {
fspeed += vitesse();
lcd.setCursor(0, 0);
lcd.print("vitesse: ");
lcd.setCursor(8, 0);
lcd.print(fspeed);
lcd.setCursor(0, 1);
lcd.print(fspeed*18/5);
while (micros() - loop_timer < period);
loop_timer = micros();
}
void setupMpu6050Registers() {
// Configure power management
Wire.beginTransmission(MPU_ADDRESS); // Start communication with MPU
Wire.write(0x6B); // Request the PWR_MGMT_1 register
Wire.write(0x00); // Apply the desired configuration to the register
Wire.endTransmission(); // End the transmission
// Configure the gyro's sensitivity
Wire.beginTransmission(MPU_ADDRESS); // Start communication with MPU
Wire.write(0x1B); // Request the GYRO_CONFIG register
Wire.write(0x08); // Apply the desired configuration to the register : ±500°/s
Wire.endTransmission(); // End the transmission
// Configure the acceleromter's sensitivity
Wire.beginTransmission(MPU_ADDRESS); // Start communication with MPU
Wire.write(0x1C); // Request the ACCEL_CONFIG register
Wire.write(0x10); // Apply the desired configuration to the register : ±8g
Wire.endTransmission(); // End the transmission
Wire.beginTransmission(MPU_ADDRESS); // Start communication with MPU
Wire.write(0x1A); // Request the CONFIG register
Wire.write(0x03); // Set Digital Low Pass Filter about ~43Hz
Wire.endTransmission(); // End the transmission
}
void readSensor() {
Wire.beginTransmission(MPU_ADDRESS); // Start communicating with the MPU-6050
Wire.write(0x3B); // Send the requested starting register
Wire.endTransmission(); // End the transmission
Wire.requestFrom(MPU_ADDRESS, 14); // Request 14 bytes from the MPU-6050
// Wait until all the bytes are received
while (Wire.available() < 14);
acc_raw[X] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the acc_raw[X] variable
acc_raw[Y] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the acc_raw[Y] variable
acc_raw[Z] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the acc_raw[Z] variable
temperature = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the temperature variable
gyro_raw[X] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the gyro_raw[X] variable
gyro_raw[Y] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the gyro_raw[Y] variable
gyro_raw[Z] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the gyro_raw[Z] variable
}
float vitesse(){
float vitesse;
readSensor();
/*acc_raw[X] = sqrt(pow(acc_raw[X],2)+pow(acc_raw[Y], 2)+ pow(acc_raw[Z], 2));*/
vitesse = ((9.81*(acc_raw[X]))/4.096)*0.004; // 0.004 corespond à la période en secondes
Serial.print("x: ");
Serial.println(acc_raw[X]-104);
if (initialise){
return round(vitesse-moy);
}
else{
return vitesse;
}
}
void CalibrerMPU(){
int tours;
float Vitesse;
for(tours = 0 ; tours <= 300 ; tours ++){
Vitesse += vitesse();
delay(3);
}
moy = Vitesse/300;
} |
