Je n'ai pas acces a mes composantes pour tester mon programme.
A votre avis, il semble bon ou pas...
#include "Wire.h" // Arduino Wire library
#include "I2Cdev.h" //Installer ces 2 librairies
#include "MPU6050.h"
#include "math.h"
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
int16_t ax, ay, az; //mesures brutes
int16_t gx, gy, gz;
uint8_t Accel_range;
uint8_t Gyro_range;
float angle=0;
int right_a = 6;
int right_b = 5;
int right_c = 4;
int left_a = 9;
int left_b = 8;
int left_c = 7;
void setup() {
pinMode(right_a, OUTPUT);
pinMode(right_b, OUTPUT);
pinMode(right_c, OUTPUT);
pinMode(left_a, OUTPUT);
pinMode(left_b, OUTPUT);
pinMode(left_c, OUTPUT);
Wire.begin(); //I2C bus
Serial.begin(9600);
while (!Serial) {
; // wait for serial port to connect. Needed for native USB (LEONARDO)
}
// initialize device
Serial.println("Initialisation I2C...");
accelgyro.initialize();
// verify connection
Serial.println("Test de la conection du dispositif ...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection reussie" : "MPU6050 connection echec");
delay(1000);
}
void loop() {
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
angle=0.98*(angle+float(gy)*0.01/131) + 0.02*atan2((double)ax,(double)az)*180/PI;
Serial.println(angle);
delay(10);
if ( angle <= -10 && angle >= -20 ) {
digitalWrite(right_a, LOW);
digitalWrite(right_b, HIGH);
digitalWrite(right_c, HIGH);
digitalWrite(left_a, HIGH);
digitalWrite(left_b, HIGH);
digitalWrite(left_c, HIGH);
}
else if (angle <= -20 && angle >= -30 ) {
digitalWrite(right_a, LOW);
digitalWrite(right_b, LOW);
digitalWrite(right_c, HIGH);
digitalWrite(left_a, HIGH);
digitalWrite(left_b, HIGH);
digitalWrite(left_c, HIGH);
}
else if (angle <= -30 && angle >= -50) {
digitalWrite(right_a, LOW);
digitalWrite(right_b, LOW);
digitalWrite(right_c, LOW);
digitalWrite(left_a, HIGH);
digitalWrite(left_b, HIGH);
digitalWrite(left_c, HIGH);
}
else if ( angle >= 10 && angle <= 20 ) {
digitalWrite(right_a, HIGH);
digitalWrite(right_b, HIGH);
digitalWrite(right_c, HIGH);
digitalWrite(left_a, LOW);
digitalWrite(left_b, HIGH);
digitalWrite(left_c, HIGH);
}
else if (angle >= 20 && angle <= 30 ) {
digitalWrite(right_a, HIGH);
digitalWrite(right_b, HIGH);
digitalWrite(right_c, HIGH);
digitalWrite(left_a, LOW);
digitalWrite(left_b, LOW);
digitalWrite(left_c, HIGH);
}
else if (angle >= 30 && angle <= 60) {
digitalWrite(right_a, HIGH);
digitalWrite(right_b, HIGH);
digitalWrite(right_c, HIGH);
digitalWrite(left_a, LOW);
digitalWrite(left_b, LOW);
digitalWrite(left_c, LOW);
}
else {
digitalWrite(right_a, HIGH);
digitalWrite(right_b, HIGH);
digitalWrite(right_c, HIGH);
digitalWrite(left_a, HIGH);
digitalWrite(left_b, HIGH);
digitalWrite(left_c, HIGH);
}
}