projet moto

International Français
15

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