
#include "Wire.h"
const int MPU_addr=0x68; int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
int minVal=265; int maxVal=402;
double x; double y; double z;
void setup(){
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
Serial.begin(9600);
}
void loop(){
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true);
AcX=Wire.read()<<8|Wire.read();
AcY=Wire.read()<<8|Wire.read();
AcZ=Wire.read()<<8|Wire.read();
int xAng = map(AcX,minVal,maxVal,-90,90);
int yAng = map(AcY,minVal,maxVal,-90,90);
int zAng = map(AcZ,minVal,maxVal,-90,90);
x= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
z= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);
Serial.print("AngleX= "); Serial.println(x);
Serial.print("AngleY= "); Serial.println(y);
Serial.print("AngleZ= "); Serial.println(z); Serial.println("-----------------------------------------");
delay(400);
}
#include "SPI.h"
#include "Wire.h"
#include "Adafruit_GFX.h"
#include "Adafruit_SSD1306.h"
#define SCREEN_WIDTH 128 // Largeur de l'écran OLED en pixels
#define SCREEN_HEIGHT 64 // Hauteur de l'écran OLED en pixels
// Declaration de l'écran SSD1306 connecté en I2C (pins SDA, SCL)
#define OLED_RESET 4
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
const int MPU_addr=0x68; // Adresse I2C du MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
double x; double y; double z;
int minVal=265; int maxVal=402;
void setup() {
Serial.begin(9600);
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B);
Wire.write(0); // Reveil du MPU-6050
Wire.endTransmission(true);
// accès à l'écran
if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // ou 0x3D
Serial.println(F("connection impossible"));
for(;;);
}
}
void loop() {
adxl();
delay(40);
}
void adxl(){
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true);
AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
Serial.print("AcX = "); Serial.print(AcX);
Serial.print(" | AcY = "); Serial.print(AcY);
Serial.print(" | AcZ = "); Serial.print(AcZ);
Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53); //Température en degrés celcius
Serial.print(" | GyX = "); Serial.print(GyX);
Serial.print(" | GyY = "); Serial.print(GyY);
Serial.print(" | GyZ = "); Serial.println(GyZ);
int xAng = map(AcX,minVal,maxVal,-90,90);
int yAng = map(AcY,minVal,maxVal,-90,90);
int zAng = map(AcZ,minVal,maxVal,-90,90);
x= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
z= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);
//Affichage du résultat sur l'écran 0.96 pouces
display.clearDisplay();
display.setTextSize(2);
display.setTextColor(SSD1306_WHITE);
display.setCursor(0,0);
display.println(F("X"));
display.setCursor(50,0);
display.println(x);
display.setCursor(0,20);
display.println(F("Y"));
display.setCursor(50,20);
display.println(y);
display.setCursor(0,40);
display.println(F("Z"));
display.setCursor(50,40);
display.println(z);
display.display();
delay(300);
}
Voici le résultat:
#include "SPI.h"
#include "Wire.h"
#include "Adafruit_GFX.h"
#include "Adafruit_SSD1306.h"
#include "SoftwareSerial.h"
#define SCREEN_WIDTH 128 // Largeur de l'écran OLED en pixels
#define SCREEN_HEIGHT 64 // Hauteur de l'écran OLED en pixels
// Déclaration de l'écran SSD1306 connecté en I2C (pins SDA, SCL)
#define OLED_RESET 4 //
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
SoftwareSerial BT (7,8);
const int MPU_addr=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
double x; double y; double z; double w;
int minVal=265; int maxVal=402;
char a; // Donnée si on veut recevoir de la donnée Bluetooth
void setup() {
BT.begin(9600); //Activation du Bluetooth
Serial.begin(9600); // Activation du Port série
Wire.begin(); // Activation du MPU6050
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
// Activation du Display
if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // ou Adresse 0x3D
Serial.println(F("Connection impossible"));
for(;;);
}
}
void loop() {
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true);
AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
int xAng = map(AcX,minVal,maxVal,-90,90);
int yAng = map(AcY,minVal,maxVal,-90,90);
int zAng = map(AcZ,minVal,maxVal,-90,90);
x= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
z= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);
display.clearDisplay();
display.setTextSize(2);
display.setTextColor(SSD1306_WHITE);
display.setCursor(0,0);
display.println(F("X"));
display.setCursor(50,0);
display.println(x);
display.setCursor(0,20);
display.println(F("Y"));
display.setCursor(50,20);
display.println(y);
display.setCursor(0,40);
display.println(F("Z"));
display.setCursor(50,40);
display.println(z);
display.display();
delay(300);
w = x + y + z;
if( w >= 50 && w <= 80){
Serial.print(w);
Serial.println("mode droite");
BT.write('r');
while(w >= 50 && w <= 80){
delay(1);
adxl();
}
Serial.println("exit de droite");
BT.write('s');
}
else if (w >= 190 && w <= 260){
Serial.print(w);
Serial.println("mode arriere");
BT.write('b');
while(w >= 190 && w <= 260){
delay(1);
adxl();
}
Serial.println("exit de arriere");
BT.write('s');
}
else if (w >= 280 && w <= 350){
Serial.print(w);
Serial.println("mode gauche");
BT.write('l');
while(w >= 280 && w <= 350){
delay(1);
adxl();
}
Serial.println("exit de gauche");
BT.write('s');
}
else if (w >= 680 && w <= 720){
Serial.print(w);
Serial.println("mode avant");
BT.write('f');
while(w >= 680 && w <= 720){
delay(1);
adxl();
}
Serial.println("exit de avant");
BT.write('s');
}
else {
Serial.print(w);
Serial.println("mode stop");
}
delay(40);
}
void adxl(){
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true);
AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
int xAng = map(AcX,minVal,maxVal,-90,90);
int yAng = map(AcY,minVal,maxVal,-90,90);
int zAng = map(AcZ,minVal,maxVal,-90,90);
x= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
z= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);
display.clearDisplay();
display.setTextSize(2);
display.setTextColor(SSD1306_WHITE);
display.setCursor(0,0);
display.println(F("X"));
display.setCursor(50,0);
display.println(x);
display.setCursor(0,20);
display.println(F("Y"));
display.setCursor(50,20);
display.println(y);
display.setCursor(0,40);
display.println(F("Z"));
display.setCursor(50,40);
display.println(z);
display.display();
delay(300);
w = x + y + z;
}