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;
} |
Partager