├── connection.png
├── LICENSE
├── README.md
├── NRF24__transmitter_code
    └── NRF24__transmitter_code..ino
└── NRF24_receiver_code
    ├── NRF24_receiver_code(no_PID).ino
    └── PID_receiver.ino


/connection.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/manojsharma221/Airplane--Fixed-Wing-UAV/HEAD/connection.png


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2019 Manoj Sharma
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # Airplane--Fixed-Wing-UAV
 2 | An arduino based remote controlled airplane with PID control loop using MPU6050 <br />
 3 | 
 4 | It is a fully Arduino based RC plane.Flight controller, Transmitter all are made using Arduino.The range is more than 1KM. <br />
 5 | Transmitter and receiver system are made using arduino and NRF24 transreceiver and there is also a LCD display showing the pitch and roll trim values on the transmitter. There was also an MPU6050 gyro and accelerometer sensor installed on the plane for PID control, self-balancing the airplane in strong winds.
 6 | 
 7 | # DEMO: https://www.youtube.com/playlist?list=PLtB2X1q31Kq5JVEsOvBol5XOvkQDj3aM6 
 8 | # THINGS USED:
 9 | A2212 1000kv brushless motor with 10x4.5 propeller. <br />
10 | 30A simonk ESC. <br />
11 | One 9g plastic servo on the elevators. <br />
12 | One  MG90S metal gear servo for the aileron. <br />
13 | Battery meter. <br />
14 | 
15 | # FLIGHT CONTROLLERS/RECEIVER:
16 | Arduino Nano <br />
17 | NRF24l01(PCB Type) <br />
18 | 
19 | # TRANSMITTER:
20 | Arduino UNO <br />
21 | nRF24L01+ PA/LNA(antenna type) <br />
22 | MB102 3.3v regulator <br />
23 | 2 analog joysticks <br />
24 | 
25 | ## NOTE
26 | * The code is commented enough to understand everything. <br />
27 | 
28 | * If the transmitter doesn't transmit connect a 100 microfarad capacitor on the NRFantenna module.If it doesn't work even after that then using the non-antenna type PCB NRF24 module on the transmitter will be the only solution I guess.The reason is that the antenna type gives too much of issues regarding power, noise, etc.
29 | 


--------------------------------------------------------------------------------
/NRF24__transmitter_code/NRF24__transmitter_code..ino:
--------------------------------------------------------------------------------
 1 | /*A basic 4 channel transmitter using the nRF24L01 module.*/
 2 | /* Like, share and subscribe, https://www.youtube.com/channel/UCNMdGm2BwTYoPvY7Yus1QpA  */
 3 | /* Channel Name  Manoj Sharma...(yes, I know I need to change it. */
 4 | 
 5 | /* First include the libraries.Google and  Download it. 
 6 | *  Install the NRF24 library to your IDE
 7 |  * Upload this code to the Arduino UNO
 8 |  * Connect a NRF24 module to it:
 9 |  
10 |     Module // Arduino UNO
11 |     
12 |     GND    ->   GND
13 |     Vcc    ->   3.3V
14 |     CE     ->   D9
15 |     CSN    ->   D10
16 |     CLK    ->   D13
17 |     MOSI   ->   D11
18 |     MISO   ->   D12 
19 |  */
20 |  
21 | #include <SPI.h>
22 | #include <nRF24L01.h>
23 | #include <RF24.h>
24 | 
25 | /*Create a unique pipe out. The receiver has to 
26 |   wear the same unique code*/
27 |   
28 | const uint64_t pipeOut = 0xE8E8F0F0E1LL; //IMPORTANT: The same as in the receiver
29 | 
30 | RF24 radio(9, 10); // select  CSN  pin
31 | 
32 | // The sizeof this struct should not exceed 32 bytes
33 | // This gives us up to 32 8 bits channals
34 | struct MyData {
35 |   byte throttle;
36 |   byte pitch;
37 |   byte roll;   // we will be only using 3 channels Throttle, one servo for roll and one for pitch control.
38 |   };
39 | 
40 | MyData data;
41 | 
42 | void resetData() 
43 | {
44 |   //This are the start values of each channal
45 |   // Throttle is 0 in order to stop the motors
46 |   //127 is the middle value of the 10ADC.
47 |     
48 |   data.throttle = 0;
49 |   data.pitch = 127;
50 |   data.roll = 127;
51 |    
52 |  }
53 | 
54 | void setup()
55 | {
56 |   //Start everything up
57 |   radio.begin();
58 |   radio.setAutoAck(false);
59 |   radio.setDataRate(RF24_250KBPS);
60 |   radio.openWritingPipe(pipeOut);
61 |   radio.setPALevel(RF24_PA_MAX); //if the connection does'nt work change it to LOW or MIN( options LOW,MIN,HIGH,MAX)
62 |   radio.stopListening();
63 |   resetData();
64 |  }
65 | 
66 | /**************************************************/
67 | 
68 | // Returns a corrected value for a joystick position that takes into account
69 | // the values of the outer extents and the middle of the joystick range.
70 | int mapJoystickValues(int val, int lower, int middle, int upper, bool reverse)
71 | {
72 |   val = constrain(val, lower, upper);
73 |   if ( val < middle )
74 |     val = map(val, lower, middle, 0, 128);
75 |   else
76 |     val = map(val, middle, upper, 128, 255);
77 |   return ( reverse ? 255 - val : val );
78 | }
79 | 
80 | void loop()
81 | {
82 |   // The calibration numbers used here should be measured 
83 |   // for your joysticks till they send the correct values.
84 |   data.throttle = mapJoystickValues( analogRead(A0), 13, 524, 1015, true );//connect the middle pin of the throttle potentionmeter to analog pin A0 and the other two pins to ground and +5v or arduino
85 |   data.roll      = mapJoystickValues( analogRead(A1), 12, 544, 1021, true );//do same procedure here also(anolog pin A1)
86 |   data.pitch    = mapJoystickValues( analogRead(A2), 34, 522, 1020, true );//and here too(Analog pin A2)
87 |   radio.write(&data, sizeof(MyData));
88 | }
89 | 


--------------------------------------------------------------------------------
/NRF24_receiver_code/NRF24_receiver_code(no_PID).ino:
--------------------------------------------------------------------------------
  1 | /* Receiver code for the Arduino Radio control with PWM output
  2 |  *  
  3 |  *  THIS ONLY WORKS WITH ATMEGA328p registers!!!!
  4 |  *  Install the NRF24 library to your IDE
  5 |  * Upload this code to the Arduino UNO
  6 |  * Connect a NRF24 module to it:
  7 |  
  8 |     Module // Arduino UNO
  9 |     
 10 |     GND    ->   GND
 11 |     Vcc    ->   3.3V
 12 |     CE     ->   D9
 13 |     CSN    ->   D10
 14 |     CLK    ->   D13
 15 |     MOSI   ->   D11
 16 |     MISO   ->   D12
 17 | 
 18 | This code receive 3 channels and create a PWM output for each one on D2, D3, D4
 19 | Please, like share and subscribe : https://www.youtube.com/channel/UCNMdGm2BwTYoPvY7Yus1QpA
 20 | */
 21 | 
 22 | #include<Servo.h>
 23 | int pin2;
 24 | int pin3;
 25 | int pin4;
 26 | Servo esc;
 27 | Servo servo1;
 28 | Servo servo2;
 29 | unsigned long previousMillis = 0;                            //Set initial timer value used for the PWM signal
 30 | 
 31 | #include <SPI.h>
 32 | #include <nRF24L01.h>
 33 | #include <RF24.h>
 34 | const uint64_t pipeIn = 0xE8E8F0F0E1LL;     //Remember that this code is the same as in the transmitter
 35 | 
 36 | RF24 radio(9, 10); 
 37 | 
 38 |                     //We could use up to 32 channels
 39 | struct MyData {
 40 | byte throttle;      //We define each byte of data input, in this case just 3 channels
 41 | byte pitch;
 42 | byte roll;
 43 | };
 44 | 
 45 | MyData data;
 46 | 
 47 | void resetData()
 48 | {
 49 | //We define the inicial value of each data input
 50 | //3 potenciometers will be in the middle position so 127 is the middle from 254
 51 | data.throttle = 0;
 52 | data.pitch = 130;
 53 | data.roll = 130;
 54 | }
 55 | 
 56 | /**************************************************/
 57 | 
 58 | void setup()
 59 | {Serial.begin(250000);
 60 |   esc.attach(2); // connect ESC signal pin to digital pin D2
 61 |  servo1.attach(3);//Aileron or roll servo to pin D3
 62 | servo2.attach(4);//Pitch servo to pin D4
 63 | esc.writeMicroseconds(1000);
 64 | resetData();
 65 | radio.begin();
 66 | radio.setAutoAck(false);
 67 | 
 68 | radio.setDataRate(RF24_250KBPS);
 69 | radio.openReadingPipe(1,pipeIn);
 70 | //we start the radio comunication
 71 | radio.startListening();
 72 | }
 73 | 
 74 | /**************************************************/
 75 | 
 76 | unsigned long lastRecvTime = 0;
 77 | 
 78 | void recvData()
 79 | {
 80 | while ( radio.available() ) {
 81 | radio.read(&data, sizeof(MyData));
 82 | lastRecvTime = millis(); //here we receive the data
 83 | }
 84 | }
 85 | 
 86 | /**************************************************/
 87 | 
 88 | void loop()
 89 | {
 90 | recvData();
 91 | unsigned long now = millis();
 92 | //Here we check if we've lost signal, if we did we reset the values 
 93 | if ( now - lastRecvTime > 1000 ) {
 94 | // Signal lost?
 95 | resetData();
 96 | }
 97 | 
 98 | pin2 = map(data.throttle, 0, 255, 1000, 2000);     //PWM value on digital pin D2
 99 | pin3 = map(data.roll,    0, 255, 30, 130);     //(between 0 to 180(in my case i have set it to vary between 30 to 130)PWM value on digital pin D3
100 | pin4 = map(data.pitch,     0, 255, 30, 130);     //PWM value on digital pin D4
101 | 
102 |   esc.writeMicroseconds(pin2);
103 |   delay(15);
104 |   servo1.write(pin3);
105 |   delay(15);
106 |   servo2.write(pin4);
107 |   delay(15);
108 |   
109 |   
110 |   Serial.print("Throttle: "); Serial.print(pin2);  Serial.print("    ");
111 |   Serial.print("Roll: ");     Serial.print(data.roll);      Serial.print("    ");
112 |   Serial.print("Pitch: ");     Serial.print(data.pitch);      Serial.print("    ");
113 |   Serial.print("\n");
114 | 
115 | /*To test if the wireless system is working coneect the receiver module to your PC,
116 |  open the serial monitor and 
117 |  set baud rate to 250000 
118 |  Now move the joysticks on your transmitter 
119 |  you should see the values changing
120 |  */
121 |  }
122 | 
123 | 
124 | 


--------------------------------------------------------------------------------
/NRF24_receiver_code/PID_receiver.ino:
--------------------------------------------------------------------------------
  1 | #include <Wire.h>
  2 | #include <Servo.h>
  3 | //#include<EEPROM.h>
  4 |           //RFID
  5 | Servo esc;
  6 | Servo pitch_servo;
  7 | Servo yaw_servo;
  8 | Servo roll_servoL;
  9 | Servo roll_servoR;
 10 | 
 11 | unsigned long previousMillis = 0;
 12 | #include <SPI.h>
 13 | #include <nRF24L01.h>
 14 | #include <RF24.h>
 15 | const uint64_t pipeIn = 0xE8E8F0F0E1LL;     //Remember that this code is the same as in the transmitter
 16 | 
 17 | RF24 radio(9, 10); 
 18 | 
 19 | struct MyData {
 20 | byte throttle;      //We define each byte of data input, in this case just 3 channels
 21 | byte pitch;
 22 | byte roll;
 23 | byte yaw;
 24 | byte TPU;
 25 | byte TPD;
 26 | byte TRR;
 27 | byte TRL;
 28 | };
 29 | MyData data;
 30 | 
 31 | void resetData()
 32 | {
 33 | //We define the inicial value of each data input
 34 | //3 potenciometers will be in the middle position so 127 is the middle from 254
 35 | data.throttle = 0;
 36 | data.pitch = 130;
 37 | data.roll = 130;
 38 | data.yaw=130;
 39 | data.TPU=0;
 40 | data.TPD=0;
 41 | data.TRR=0;
 42 | data.TRL=0;
 43 | }
 44 | 
 45 | 
 46 | 
 47 | 
 48 | 
 49 | 
 50 | /*MPU-6050 gives you 16 bits data so you have to create some 16int constants
 51 |  * to store the data for accelerations and gyro*/
 52 | 
 53 | int16_t Acc_rawX, Acc_rawY, Acc_rawZ,Gyr_rawX, Gyr_rawY, Gyr_rawZ;
 54 |  
 55 | 
 56 | float Acceleration_angle[2];
 57 | float Gyro_angle[2];
 58 | float Total_angle[2];
 59 | 
 60 | 
 61 | 
 62 | float elapsedTime, time, timePrev;
 63 | float rad_to_deg = 180/3.141592654;
 64 | 
 65 | float errorpitch,errorroll,pwmpitch, pwmroll,PIDpitch,PIDroll, previous_errorpitch,previous_errorroll;
 66 | float pidpitch_p=0;
 67 | float pidpitch_i=0;
 68 | float pidpitch_d=0;
 69 | 
 70 | float pidroll_p=0;
 71 | float pidroll_i=0;
 72 | float pidroll_d=0;
 73 | 
 74 | /////////////////PID CONSTANTS/////////////////
 75 | double kp=1.5;//3.55
 76 | double ki=0;//0.003
 77 | double kd=0;;//2.05
 78 | ///////////////////////////////////////////////
 79 | 
 80 | int throttle,yaw;
 81 | double pitch;
 82 | double roll;
 83 | /* TRIM CONSTANTS*/
 84 | float tpu,tpd,trr,trl,pitchTrim,rollTrim;
 85 | float desired_angle = 0; //This is the angle in which we whant the
 86 |                          //balance to stay steady
 87 | 
 88 | 
 89 | 
 90 | void setup() {
 91 |   Wire.begin(); //begin the wire comunication
 92 |   Wire.beginTransmission(0x68);
 93 |   Wire.write(0x6B);
 94 |   Wire.write(0);
 95 |   Wire.endTransmission(true);
 96 |   Serial.begin(250000);
 97 |   esc.attach(2);
 98 |   roll_servoL.attach(3);
 99 |   roll_servoR.attach(4);
100 |    pitch_servo.attach(5);
101 |    yaw_servo.attach(6);
102 |   esc.writeMicroseconds(1000);
103 |   delay(2000);
104 |    //pitchTrim=EEPROM.read(2);
105 |    //yawTrim=EEPROM.read(100);
106 |  resetData();
107 | radio.begin();
108 | radio.setAutoAck(false);
109 | 
110 | radio.setDataRate(RF24_250KBPS);
111 | radio.openReadingPipe(1,pipeIn);
112 | //we start the radio comunication
113 | radio.startListening();
114 |   time = millis(); //Start counting time in milliseconds
115 | }//end of setup void
116 | 
117 | unsigned long lastRecvTime = 0;
118 | 
119 | void recvData()
120 | {
121 | while ( radio.available() ) {
122 | radio.read(&data, sizeof(MyData));
123 | lastRecvTime = millis();
124 | }
125 | }
126 | 
127 | 
128 | void loop() {
129 |   recvData();
130 | 
131 | /////////////////////////////I M U/////////////////////////////////////
132 |       timePrev = time;  // the previous time is stored before the actual time read
133 |     time = millis();  // actual time read
134 |     elapsedTime = (time - timePrev) / 1000; 
135 | 
136 |     if ( time - lastRecvTime > 1000 ) {
137 | // Signal lost?
138 | resetData();
139 | }
140 | 
141 | //Here we check if we've lost signal, if we did we reset the values 
142 | 
143 | throttle = map(data.throttle, 0, 255, 1000, 2000);     //PWM value on digital pin D2
144 | pitch = map(data.pitch,    0, 255, 30, 140);     //PWM value on digital pin D3
145 | roll = map(data.roll,     0, 255, 30, 140);     //PWM value on digital pin D5
146 | yaw = map(data.yaw,     0, 255, 30, 140);
147 | tpu=data.TPU;
148 | tpd=data.TPD;
149 | trr=data.TRR;
150 | trl=data.TRL;
151 | 
152 | /*TRIMMING*/
153 | if (tpu==1)
154 | {
155 |   pitchTrim=pitchTrim+0.1;
156 |   if(pitchTrim>50)
157 |   {
158 |     pitchTrim=50;
159 |   }
160 |  // EEPROM.write(2,pitchTrim);
161 | }
162 | if(tpd==1)
163 | {
164 |   pitchTrim=pitchTrim-0.1;
165 |  if(pitchTrim<-50)
166 |  {
167 |   pitchTrim=-50;
168 |  }
169 |  // EEPROM.write(2,pitchTrim);
170 | }
171 | if (tpu==0||tpd==0)
172 | {
173 |   pitchTrim=pitchTrim+0;
174 | }
175 | 
176 | if(trr==1)
177 | {
178 |   rollTrim=rollTrim+0.1;
179 |   if(rollTrim>50)
180 |   {
181 |     rollTrim=50;
182 |   }
183 |  // EEPROM.write(100,yawTrim);
184 | }
185 | if(trl==1)
186 | {
187 |   rollTrim=rollTrim-0.1;
188 |   if(rollTrim<-50)
189 |   {
190 |     rollTrim=-50;
191 |   }
192 | //  EEPROM.write(100,yawTrim);
193 | }
194 | if(trr==0||trl==0)
195 | {
196 |   rollTrim=rollTrim+0;
197 | }
198 | 
199 | //Serial.print(pitchTrim);Serial.print("    ");Serial.println(rollTrim);
200 |   
201 |   /*The tiemStep is the time that elapsed since the previous loop. 
202 |    * This is the value that we will use in the formulas as "elapsedTime" 
203 |    * in seconds. We work in ms so we haveto divide the value by 1000 
204 |    to obtain seconds*/
205 | 
206 |   /*Reed the values that the accelerometre gives.
207 |    * We know that the slave adress for this IMU is 0x68 in
208 |    * hexadecimal. For that in the RequestFrom and the 
209 |    * begin functions we have to put this value.*/
210 |    
211 |      Wire.beginTransmission(0x68);
212 |      Wire.write(0x3B); //Ask for the 0x3B register- correspond to AcX
213 |      Wire.endTransmission(false);
214 |      Wire.requestFrom(0x68,6,true); 
215 |    
216 |    /*We have asked for the 0x3B register. The IMU will send a brust of register.
217 |     * The amount of register to read is specify in the requestFrom function.
218 |     * In this case we request 6 registers. Each value of acceleration is made out of
219 |     * two 8bits registers, low values and high values. For that we request the 6 of them  
220 |     * and just make then sum of each pair. For that we shift to the left the high values 
221 |     * register (<<) and make an or (|) operation to add the low values.*/
222 |     
223 |      Acc_rawX=Wire.read()<<8|Wire.read(); //each value needs two registres
224 |      Acc_rawY=Wire.read()<<8|Wire.read();
225 |      Acc_rawZ=Wire.read()<<8|Wire.read();
226 | 
227 |  
228 |     /*///This is the part where you need to calculate the angles using Euler equations///*/
229 |     
230 |     /* - Now, to obtain the values of acceleration in "g" units we first have to divide the raw   
231 |      * values that we have just read by 16384.0 because that is the value that the MPU6050 
232 |      * datasheet gives us.*/
233 |     /* - Next we have to calculate the radian to degree value by dividing 180º by the PI number
234 |     * which is 3.141592654 and store this value in the rad_to_deg variable. In order to not have
235 |     * to calculate this value in each loop we have done that just once before the setup void.
236 |     */
237 | 
238 |     /* Now we can apply the Euler formula. The atan will calculate the arctangent. The
239 |      *  pow(a,b) will elevate the a value to the b power. And finnaly sqrt function
240 |      *  will calculate the rooth square.*/
241 |      /*---X---*/
242 |      Acceleration_angle[0] = atan((Acc_rawY/16384.0)/sqrt(pow((Acc_rawX/16384.0),2) + pow((Acc_rawZ/16384.0),2)))*rad_to_deg;
243 |      /*---Y---*/
244 |      Acceleration_angle[1] = atan(-1*(Acc_rawX/16384.0)/sqrt(pow((Acc_rawY/16384.0),2) + pow((Acc_rawZ/16384.0),2)))*rad_to_deg;
245 |  
246 |    /*Now we read the Gyro data in the same way as the Acc data. The adress for the
247 |     * gyro data starts at 0x43. We can see this adresses if we look at the register map
248 |     * of the MPU6050. In this case we request just 4 values. W don¡t want the gyro for 
249 |     * the Z axis (YAW).*/
250 |     
251 |    Wire.beginTransmission(0x68);
252 |    Wire.write(0x43); //Gyro data first adress
253 |    Wire.endTransmission(false);
254 |    Wire.requestFrom(0x68,4,true); //Just 4 registers
255 |    
256 |    Gyr_rawX=Wire.read()<<8|Wire.read(); //Once again we shif and sum
257 |    Gyr_rawY=Wire.read()<<8|Wire.read();
258 |  
259 |    /*Now in order to obtain the gyro data in degrees/seconda we have to divide first
260 |    the raw value by 131 because that's the value that the datasheet gives us*/
261 | 
262 |    /*---X---*/
263 |    Gyro_angle[0] = Gyr_rawX/131.0; 
264 |    /*---Y---*/
265 |    Gyro_angle[1] = Gyr_rawY/131.0;
266 | 
267 |    /*Now in order to obtain degrees we have to multiply the degree/seconds
268 |    *value by the elapsedTime.*/
269 |    /*Finnaly we can apply the final filter where we add the acceleration
270 |    *part that afects the angles and ofcourse multiply by 0.98 */
271 | 
272 |    /*---X axis angle---*/
273 |    Total_angle[0] = 0.98 *(Total_angle[0] + Gyro_angle[0]*elapsedTime) + 0.02*Acceleration_angle[0];
274 |    /*---Y axis angle---*/
275 |    Total_angle[1] = 0.98 *(Total_angle[1] + Gyro_angle[1]*elapsedTime) + 0.02*Acceleration_angle[1];
276 |    
277 |    /*Now we have our angles in degree and values from -100º to 100º aprox*/
278 |     //Serial.println(Total_angle[0])  ;   Serial.println(Total_angle[1]);
279 | 
280 |    
281 |   
282 | /*///////////////////////////P I D///////////////////////////////////*/
283 | /*Remember that for the balance we will use just one axis. I've choose the x angle
284 | to implement the PID with. That means that the x axis of the IMU has to be paralel to
285 | the balance*/
286 | 
287 | /*First calculate the error between the desired angle and 
288 | *the real measured angle*/
289 | errorpitch = Total_angle[1] - desired_angle;
290 | errorroll = Total_angle[0] - desired_angle;
291 | 
292 |     
293 | /*Next the proportional value of the PID is just a proportional constant
294 | *multiplied by the error*/
295 | 
296 | pidpitch_p = kp*errorpitch;
297 | pidroll_p=kp*errorroll;
298 | /*The integral part should only act if we are close to the
299 | desired position but we want to fine tune the error. That's
300 | why I've made a if operation for an error between -2 and 2 degree.
301 | To integrate we just sum the previous integral value with the
302 | error multiplied by  the integral constant. This will integrate (increase)
303 | the value each loop till we reach the 0 point*/
304 | if(-3 <errorpitch <3)
305 | {
306 |   pidpitch_i = pidpitch_i+(ki*errorpitch);  
307 | }
308 | 
309 | if(-3 <errorroll <3)
310 | {
311 |   pidroll_i = pidroll_i+(ki*errorroll);  
312 | }
313 | 
314 | /*The last part is the derivate. The derivate acts upon the speed of the error.
315 | As we know the speed is the amount of error that produced in a certain amount of
316 | time divided by that time. For taht we will use a variable called previous_error.
317 | We substract that value from the actual error and divide all by the elapsed time. 
318 | Finnaly we multiply the result by the derivate constant*/
319 | 
320 | pidpitch_d = kd*((errorpitch - previous_errorpitch)/elapsedTime);
321 | 
322 | pidroll_d = kd*((errorroll - previous_errorroll)/elapsedTime);
323 | 
324 | /*The final PID values is the sum of each of this 3 parts*/
325 | PIDpitch = pidpitch_p + pidpitch_i + pidpitch_d;
326 | 
327 | PIDroll = pidroll_p + pidroll_i + pidroll_d;
328 | 
329 | 
330 | /*We know taht the min value of PWM signal is 1000us and the max is 2000. So that
331 | tells us that the PID value can/s oscilate more than -1000 and 1000 because when we
332 | have a value of 2000us the maximum value taht we could sybstract is 1000 and when
333 | we have a value of 1000us for the PWM sihnal, the maximum value that we could add is 1000
334 | to reach the maximum 2000us*/
335 | if(PIDpitch < -140)
336 | {
337 |   PIDpitch=-140;
338 | }
339 | if(PIDpitch > 140)
340 | {
341 |   PIDpitch=140;
342 | }
343 | 
344 | if(PIDroll < -140)
345 | {
346 |   PIDroll=-140;
347 | }
348 | if(PIDroll > 140)
349 | {
350 |   PIDroll=140;
351 | }
352 | 
353 | /*Finnaly we calculate the PWM width. We sum the desired throttle and the PID value*/
354 | pwmpitch = pitch - PIDpitch-pitchTrim;
355 | pwmroll = roll - PIDroll+rollTrim;
356 | 
357 | 
358 | /*Once again we map the PWM values to be sure that we won't pass the min
359 | and max values. Yes, we've already maped the PID values. But for example, for 
360 | throttle value of 1300, if we sum the max PID value we would have 2300us and
361 | that will mess up the ESC.*/
362 | //Right
363 | if(pwmpitch < 30)
364 | {
365 |   pwmpitch= 30;
366 | }
367 | if(pwmpitch > 140)
368 | {
369 |   pwmpitch=140;
370 | }
371 | //Left
372 | if(pwmroll < 30)
373 | {
374 |   pwmroll= 30;
375 | }
376 | if(pwmroll > 140)
377 | {
378 |   pwmroll=140;
379 | }
380 | 
381 | /*Finnaly using the servo function we create the PWM pulses with the calculated
382 | width for each pulse*/
383 | esc.writeMicroseconds(throttle);
384 | pitch_servo.write(pwmpitch);
385 | yaw_servo.write(yaw);
386 | roll_servoL.write(140-pwmroll+30);
387 | roll_servoR.write(140-pwmroll+30);
388 | previous_errorpitch = errorpitch; //Remember to store the previous error.
389 | previous_errorroll = errorroll;
390 | Serial.print("PitchTrim: ");  Serial.print(pitchTrim);      Serial.print("    ");
391 | Serial.print("RollTrim: ");   Serial.print(rollTrim);       Serial.print("    ");
392 | Serial.print("AnglePitch");   Serial.print(errorpitch); Serial.print("    ");
393 | Serial.print("AngleRoll");    Serial.print(errorroll); Serial.print("    ");
394 | Serial.print("Pitch");    Serial.print(pitch);       Serial.print("    ");
395 | Serial.print("Roll");     Serial.print(roll);        Serial.print("    ");
396 | Serial.print("PWM Pitch");    Serial.print(pwmpitch);       Serial.print("    ");
397 | Serial.print("PWN Roll");     Serial.print(pwmroll);        Serial.print("    ");
398 | Serial.print("\n");
399 |   
400 | }
401 | //end of loop void
402 | 


--------------------------------------------------------------------------------