1char t;
2#include <Wire.h>
3#include <MPU6050.h>
4#include <Servo.h>
5MPU6050 sensor ;
6Servo sg90y;
7Servo sg90x;
8int servo_pinX = 2;
9int servo_pinY = 3;
10int motor1A = 4;
11int motor1B = 5;
12int motor2A = 6;
13int motor2B = 7;
14
15int16_t ax, ay, az ;
16int16_t gx, gy, gz ;
17
18void setup() {
19 pinMode(servo_pinX, OUTPUT);
20 pinMode(servo_pinY, OUTPUT);
21 pinMode(motor1A, OUTPUT);
22 pinMode(motor1B, OUTPUT);
23 pinMode(motor2A, OUTPUT);
24 pinMode(motor2B, OUTPUT);
25 pinMode(A5, INPUT);
26 pinMode(A4, INPUT);
27 sg90x.attach ( servo_pinX );
28 sg90y.attach ( servo_pinY );
29 Wire.begin ( );
30 sensor.initialize ( );
31 Serial.begin(9600);
32 delay(400);
33}
34void forward() {
35 digitalWrite(motor1A, LOW);
36 digitalWrite(motor1B, HIGH);
37 digitalWrite(motor2A, LOW);
38 digitalWrite(motor2B, HIGH);
39}
40
41void reverse() {
42 digitalWrite(motor1A, HIGH);
43 digitalWrite(motor1B, LOW);
44 digitalWrite(motor2B, LOW);
45 digitalWrite(motor2A, HIGH);
46}
47void right() {
48 digitalWrite(motor1B, HIGH);
49 digitalWrite(motor1A, LOW);
50 digitalWrite(motor2B, LOW);
51 digitalWrite(motor2A, HIGH);
52}
53void left() {
54 digitalWrite(motor1B, LOW);
55 digitalWrite(motor1A, HIGH);
56 digitalWrite(motor2B, HIGH);
57 digitalWrite(motor2A, LOW);
58}
59void nomove() {
60 digitalWrite(motor1B, LOW);
61 digitalWrite(motor1A, LOW);
62 digitalWrite(motor2B, LOW);
63 digitalWrite(motor2A, LOW);
64}
65void loop() {
66 sensor.getMotion6 (&ax, &ay, &az, &gx, &gy, &gz);
67 ay = map (ay, -17000, 17000, 0, 180) ;
68 sg90y.write (ay);
69 ax = map (ax, -17000, 17000, 0, 180) ;
70 sg90x.write (ax);
71 if (Serial.available()) {
72
73 t = Serial.read();
74 Serial.println(t);
75 if (t == 'F') {
76 forward();
77 }
78 else if (t == 'B') {
79 reverse();
80 }
81 else if (t == 'L') {
82 left();
83 }
84 else if (t == 'R') {
85 right();
86 }
87
88 else if (t == 'S') {
89 nomove();
90 }
91 }
92}
93