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
| #include "Wire.h" // Arduino Wire library
#include "I2Cdev.h" //Installer ces 2 librairies
#include "MPU6050.h"
#include "math.h"
#include <AFMotor.h>
AF_DCMotor motor(2);
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
int inputPin =A0;
int inputPin1 =A1;
int16_t ax, ay, az; //mesures brutes
int16_t gx, gy, gz;
uint8_t Accel_range;
uint8_t Gyro_range;
float angle=0;
void setup() {
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);
Serial.begin(9600); // set up Serial library at 9600 bps
Serial.println("Motor test!");
// turn on motor
motor.setSpeed(200); //definie la vitesse du moterur
motor.run(RELEASE);
pinMode(inputPin,INPUT_PULLUP);
pinMode(inputPin1,INPUT_PULLUP);
Serial.begin(9600);
}
void loop() {
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
angle=0.0*(angle+float(gy)*0.01/131) + 1*atan2((double)ax,(double)az)*180/PI;
Serial.println(angle);
controlemotV2(angle);
delay(10);
}
void controlemotV2(int alpha) {
int Val1=digitalRead(inputPin);
Serial.println(Val1);
int Val2=digitalRead(inputPin1);
Serial.println(Val2);
if ( Val1==0){
motor.run(BACKWARD);
}
else if ( Val2==0){
motor.run(FORWARD);
}
else if (int (alpha < -10)){
motor.run(FORWARD); //fait avancer le chariot vers la droite
}
else if (int (alpha > 10)){
motor.run(BACKWARD); //fait avancer le chariot vers la gauche
}
else
motor.run(RELEASE);
} |
