1#include <SD.h>
2#include <SPI.h>
3#include <Servo.h>
4#include<Wire.h>
5
6const int MPU=0x68;
7int16_t AcX,AcY,AcZ,Tmp,GyX,GyroY,GyroZ;
8int pitch;
9int accAngleX;
10int accAngleY;
11int yaw;
12int GyroX;
13int gyroAngleX;
14int gyroAngleY;
15int ledBLU = 14;
16int ledGRN = 16;
17int ledRED = 15;
18int mosfet = 24;
19int valueX = 110;
20int valueY = 150;
21Servo servoX;
22Servo servoY;
23
24
25float elapsedTime, currentTime, previousTime;
26float kp = 1;
27float ki = 1;
28float kd = 1;
29int pos;
30
31
32void setup(){
33 Wire.begin();
34 Wire.beginTransmission(MPU);
35 Wire.write(0x6B);
36 Wire.write(0);
37 Wire.endTransmission(true);
38 Serial.begin(9600);
39 servoX.attach(5);
40 servoY.attach(6);
41 pinMode(mosfet, OUTPUT);
42 pinMode(ledBLU, OUTPUT);
43 pinMode(ledGRN, OUTPUT);
44 pinMode(ledRED, OUTPUT);
45
46 digitalWrite(ledBLU, HIGH);
47 delay(500);
48 digitalWrite(ledBLU, LOW);
49
50 digitalWrite(ledGRN, HIGH);
51 delay(500);
52 digitalWrite(ledGRN, LOW);
53
54 digitalWrite(ledRED, HIGH);
55 delay(500);
56 digitalWrite(ledRED, LOW);
57
58
59
60 digitalWrite(ledBLU, HIGH);
61 delay(500);
62 digitalWrite(ledBLU, LOW);
63
64 digitalWrite(ledGRN, HIGH);
65 delay(500);
66 digitalWrite(ledGRN, LOW);
67
68 digitalWrite(ledRED, HIGH);
69 delay(500);
70 digitalWrite(ledRED, LOW);
71 Serial.begin(9600);
72
73
74
75
76
77}
78void loop() {
79
80 filter();
81 map();
82 previousTime = currentTime;
83 currentTime = millis();
84 elapsedTime = (currentTime - previousTime) / 1000;
85 accAngleX = (atan(AcY / sqrt(pow(AcX, 2) + pow(AcZ, 2))) * 180 / PI) - 0.58;
86 accAngleY = (atan(-1 * AcX / sqrt(pow(AcY, 2) + pow(AcZ, 2))) * 180 / PI) + 1.58;
87
88 Wire.beginTransmission(MPU);
89 Wire.write(0x3B);
90 Wire.endTransmission(false);
91 Wire.requestFrom(MPU,12,true);
92 AcX=Wire.read()<<8|Wire.read();
93 AcY=Wire.read()<<8|Wire.read();
94 AcZ=Wire.read()<<8|Wire.read();
95 GyroX=Wire.read()<<8|Wire.read();
96 GyroY=Wire.read()<<8|Wire.read();
97 GyroZ=Wire.read()<<8|Wire.read();
98
99 gyroAngleX = gyroAngleX + GyroZ * elapsedTime;
100 gyroAngleY = gyroAngleY + GyroY * elapsedTime;
101
102
103
104
105
106
107
108
109
110
111
112 Serial.print(" | Z = "); Serial.println(GyroZ);
113
114 delay(5);
115
116}
117void filter () {
118 Serial.print(GyroX);
119 pitch = 0.9 * gyroAngleX + 0.1 * accAngleX;
120 yaw = 0.9 * gyroAngleY + 0.1 * accAngleY;
121}
122
123void map () {
124
125
126
127 digitalWrite(ledBLU, HIGH);
128 valueY = map(yaw, -2000, 2000, 90, 130);
129 valueX = map(pitch, -2000, 2000, 105, 145);
130 servoX.write(valueX);
131 servoY.write(valueY);
132
133
134
135
136}
137void ignition () {
138
139
140 digitalWrite(mosfet, HIGH);
141 Serial.println("mosfetHIGH");
142 delay(3000);
143
144
145
146
147}
148