Delta Thrust Vector Control System v8

1#include <SD.h>
2#include <SPI.h>
3#include <Servo.h>
4#include<Wire.h>
5//#include  <KalmanFilter.h>
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;    // LED connected to digital pin 9
16int ledGRN  = 16;    // LED connected to digital pin 9
17int ledRED = 15;    // LED connected  to digital pin 9
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//KalmanFilter kalman(0.001,  0.003, 0.03);
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  //120 minutes
73  //delay(7200000);
74  //digitalWrite(ledBLU,  HIGH);
75  //delay(5000);
76  //digitalWrite(ledBLU, LOW);
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; // AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more  details
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; // deg/s * s = deg
100  gyroAngleY = gyroAngleY + GyroY * elapsedTime;
101  //Serial.print(valueX);
102  
103  
104  //Serial.print("Accelerometer: ");
105  //Serial.print("X = "); Serial.print(AcX);
106  //Serial.print(" | Y = ");  Serial.print(AcY);
107 // Serial.print(" | Z = "); Serial.println(AcZ);
108  
109  //Serial.print("Gyroscope: ");
110 //Serial.print("X = "); Serial.print(GyX);
111  // Serial.print(" | Y = "); Serial.print(GyroY);
112  Serial.print(" | Z = ");  Serial.println(GyroZ);
113  //ignition();
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  //TVC Startup//
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