## Step 23: Code

Our code was written in a modified form of C++ that is described on the Arduino website.

Our code represents a feedback control system known as PID (proportional integral derivative). Currently, it only employs use of the proportional and derivative components. With our current code, the quadrotor self-stabilizes quite well in the air, but is a little unstable on takeoff. However, this instability can be mitigated by taking off quickly.

To find the current amounts of tilt on the X and Y axis from accelerometer and gyro data, we used an algorithm that would average previous accelerometer data and combine it with gyro data to reach an angle measurement that was fairly resilient to linear acceleration.

We only do 2 Pulsin commands per loop (instead of 4) to cut the loop time in half, which makes the quadrotor control system much more responsive.

//neutral accelerometer/gyro positions
#define X_ZERO 332
#define Y_ZERO 324
#define Z_ZERO 396
#define PITCH_ZERO 249
#define ROLL_ZERO 249
#define YAW_ZERO 248

#define GYRO_CON 1.47
#define ACCEL_CON 0.93

#define TIME_CON 0.02
#define SEN_CON 0.95

//motor speed vars
int speeds[4];

//gyro inputs - current tilt vars
float pitch, roll, yaw;
int pitchzero, rollzero;
//accelerometer inputs - current acceleration vars
float xin, yin, zin;

//human inputs - control info vars
float pitchin, rollin, yawin, zhuman;

//random other vars
float xaverage=0, yaverage=0;
int y=0;
int blah;

//proportionality constants
float p=2.5; // P proportionality constant
float d=0.5; // D proportionality constant

void setup() {
zhuman=0;
rollin=0;
Serial.begin(9600);
for(int x=6; x<10; x++) {
pinMode(x, OUTPUT);
}

//send upper bound for human inputs to the motor speed controllers
for(int x=6; x<10; x++) {
pulsout(x,2000);
}
delay(5000);

//get zeros for pitch and roll human inputs
for(int x=0; x<10; x++) {
}
pitchzero=y/10;
y=0;
for(int x=0; x<10; x++) {
}
rollzero=y/10;
}

void loop () {
//accelerometer and gyro inputs ranged -232 to 232?

//get human inputs through radio here range of -30 to 30 except for zhuman which has an ideal range of 1000-2000, only 2 pulses per loop
if(blah==0) {
yawin=0.06*((signed int) pulseIn(2,HIGH)-1500);
pitchin=0.06*((signed int) pulseIn(3,HIGH)-1500);
blah=1;
}
else {
zhuman=(signed int) pulseIn(4,HIGH);
rollin=0.06*((signed int) pulseIn(5,HIGH)-1400); //1400 instead of 1500 is to correct for the underpowered motor #4 by trimming it in code
blah=0;
}

//averaging, etc.
xaverage= SEN_CON *( xaverage + TIME_CON * pitch) + ( 1 - SEN_CON ) * xin;
yaverage= SEN_CON *( yaverage + TIME_CON * roll) + ( 1 - SEN_CON ) * yin;

//calculate the motor speeds
if(zhuman<1150) {
for(int x=0; x<4; x++) {
speeds[x]=zhuman;
}
}
else {
if(zhuman > 1450) {
zhuman = 1450;
}
speeds[0] = zhuman - p*(xaverage - pitchin) - p*(yawin) - d*pitch;
speeds[1] = zhuman - p*(pitchin - xaverage) - p*(yawin) + d*pitch;
speeds[2] = zhuman - p*(yaverage - rollin) + p*(yawin) - d*roll;
speeds[3] = zhuman - p*(rollin - yaverage) + p*(yawin) + d*roll;
}
//set the upper and lower bounds for motor speeds (1000=no speed, 1600=upper speed limit, 2000=maximum possible speed)
for(int x=0; x<4; x++) {
//speed limit between 1000 and 1600
if(speeds[x]<1000) {
speeds[x]=1000;
}
if(speeds[x]>1600) {
speeds[x]=1600;
}
}

//pulsouts to motor speed controllers
for(int x=0; x<4; x++) {
pulsout(x+6,speeds[x]);
}
}
void pulsout (int pin, int duration) {
digitalWrite(pin, HIGH);
delayMicroseconds(duration);
digitalWrite(pin, LOW);
}
I'm buying an Arduino Uno. I'm not sure if you know, but would you connect the same pins on the uno? Or do they have numbers and voltage correlations?
The Arduino Uno has a lightly different layout then the Pro Mini being used here. One plus is that you can skip that 3.3v regulator because there is one on the board already. You'll notice that there is a pin labelled &quot;3.3v&quot; to which you can connect the corresponding 3.3v port on your sensor board.<br><br>So the pins for the sensor board the line up as such;<br>3.3v = 3.3v<br>GND = GND<br>Gx4 Y = Analogue5<br>Gx4 X = Analogue4<br>Gx4 Z = Analogue3<br>A Z = Analogue2<br>A Y = Analogue1<br>A X = Analogue0<br><br>The rest of the pins should correspond plainly to the ones on your UNO <br>Digital pins 6,7,8,9 to the control pin on your ESCs. Human controls are on pins 2,3,4,5 to your receiver. Personally I am using an Xbee module instead so I will not be using that last set.
<p>Hi, I came across your project and I am doing almost the same. However, I am using and arduino UNO and an MPU6050. The Pin alignment that you mentioned above; I am a little confuse about it.</p><p>I can not find the Gx4 Y, Gx4 X, Gx4 Z or the A Z - X pins on my MPU6050 board. THe only way I connect it to it is by connecting SCL - Analog 5 and SDA - Analog 4. So can you please explain how can I implement it?</p>
<p>Hi, I am also using the same equipments : arduino uno and mpu 6050.</p><p>I have just interfaced mpu6050 with arduino and could read the yaw pitch roll values on serial monitor.</p><p>Instead of using RC Radio Controller, I want to use a bluetooth module and android app to control the flight controller.</p><p>Can you help me about how to use the onboaard sensors to give values and correspondingly how to make arduino give instructions to ESC and motors?</p><p>Really need help, reply asap.</p>
Can you send me all the code? I'm using arduino uno and mpu6050.. Please send all code and the Diagram wires esc to arduino and arduino to receiver... My email is rvalent86@gmail.com
<p>did u got the code?</p>
<p>What code do you want?</p>
<p>thank u so much for responding me. i'm making project on quadcopter and that should be controlled by using hand gestures. If u send me your email,I can tell u the details. Please help me on this. I want complete arduino code for quadcopter. I'm using arduino mega. </p>
<p>This is al the code...</p>
<p>Can you send me all the code? I'm using arduino uno and mpu6050.. Please send all code and the Diagram wires esc to arduino and arduino to receiver... My email is rvalent86@gmail.com</p>
<p>1. This is all the code for reading an MPU6050, which was the question. Download the MPU6050 library from <a href="http://www.varesano.net/files/FreeIMU-20121122_1126.zip" rel="nofollow">http://www.varesano.net/files/FreeIMU-20121122_112...</a> .</p><p>2. MPU:</p><p>Arduino 3.3v - MPU6050 Vcc</p><p>Arduino GND - MPU6050 GND</p><p>Arduino A5 - MPU6050 SCL</p><p>Arduino A4 - MPU6050 SDA</p><p>3. ESC</p><p>ESC-Signal wire - Arduino uno random programmed output port</p><p>ESC-Signal wire - Arduino uno random programmed output port</p><p>ESC-Signal wire - Arduino uno random programmed output port</p><p>ESC-Signal wire - Arduino uno random programmed output port</p><p>ESC-Ground wire - Arduino GND (Only do this with one ESC)</p><p>(Check if your ESC is a BEC or you have a 2nd battery)</p><p>ESC 5v (Only do this with one ESC!) - Arduino 5v (This ESC will power the Arduino and the Receiver)</p><p>4: Remote</p><p>Remote GND - Arduino GND</p><p>Remote 5v - Arduino 5v</p><p>Remote CH1 - Arduino input port</p><p>Remote CH2 - Arduino input port</p><p>Remote CH3 - Arduino input port</p><p>Remote CH4 - Arduino input port</p><p>I wish you good luck finding better sources with such an ungrateful attitude.</p>
I2C bus son
<p>I am also using xbee...can you please share the code with me...please</p><p>my email id is dhrvjsh102189@gmail.com</p>
What is a good way to get started with learning the Arduino?
Getting your hands dirty. Personally, I got myself the <a href="http://adafruit.com/products/68" rel="nofollow">starter kit</a> from Adafruit; and they also have an excellent set of <a href="http://learn.adafruit.com/category/learn-arduino" rel="nofollow">tutorials</a> .<br> <br> I had some programing experience at the time already, so for me it was an exercise in learning how to bring code into the physical realm. The Adafruit tutorial were good for me to shore up on the electronics side of things.<br> <br> Did you have a project in mind already? I find that having a specific goal when learning something new greatly increases your chances of success and I'm more than happy to take a stab at it if you have any specific questions.
Thank you. That's very useful. The project I want to build is a semi-autonomous ground rover with a remotely fired paintball gun.
<p>I want the complete code for quadcopter using arduino and also the parts list for my final year project. Can anyone help me?</p>
<p>nice work sir. sir i am using arduino uno and mu6050 </p><p>sir can u plz provide me the connections schematic and the code . my email</p><p>talhapervez317@gmail.com</p>
<p>Nice Article </p>
Can u jst tell me how much this project will cost....
<p>Genius. Thanks for sharing.</p>
<p>In esc has three wires which wire can i connect to arduino </p>
<p>hello friends, </p><p>can anyone help more about 'step19' please.I want more information about circuit diagram and how to wire Adurino and IMU ? and also I want to know about the first image at 'step 19', is this battrey ?!</p>
<p>Hi , we want design flight controller with arduino through model based design for a quad rotor , what sort of quadcopter is suitable for this aim ? a provided one or assembling separate parts ?</p>
Can you post a picture showing the position of the motors and to the pins they are connected to.I'm also on facebook as yawgyakobo
Thanks sanderv3. I will keep all that in mind. Be in touch. And update me if you utilize the android app and bluetooth.
Great job but I am having a hard time understanding the interface between the radio receiver and the Arduino. The individual channel outputs from the receiver are pulse width modulated-- usually about 1us for low, 1.5 us for neutral, 2 us for high. Yet you read in the channels using analogRead() so I take it that the Arduino is reading in the analog value of a pulse width modulated signal. I would have thought that would be a very inaccurate way of reading in the channels but you appear to have made it work. Is this by design, chance, or ?<br><br>Could this be contributing to the instability at takeoff since changing signal values might take more time to register through the pulse width to analog transition?
The analogReads are only used to read in the values from the accelerometers and gyros which are voltages.<br><br>The inputs from the radio receiver are read by the pulseIn commands a few lines of code below.
<p>hey, If I want to use a Bluetooth module instead of reciever and android app instead of transmitter, is it fine? I want less range.</p><p>As shown in step 19, if I use Bluetooth module, then I have to connect it pin 2,3,4,5(Human input)?</p>
<p>sir i m using arduinouno r3 for quadcopter and i have 10 dof plz tell me which pins should be connected between arduino and 10 dof. and tell me if there is a some need to modify above code. thank you</p>
<p>can you please share the code with me...please</p><p>my email id is <a href="mailto:jhongudyy@gmail.com" rel="nofollow">jhongudyy@gmail.com</a></p>
<p>sir i m making quad copter by using arduino uno r3, and i have 10 dof, i doesnt have idea how to connect them plz told me. and also tell me what is which i should use is the above code is useful for me? my email id is praveenkhatri831@gmail.com. </p>
Hello i want to ask you dose your code move the quad forward ,backward ,right and left where in the code dose it do this moving ??<br>And if it dose not how could i move it ??<br>Regards.
<p>Is this all of the code necessary to create a working quadrotor?</p>
<p>thanks this is a very good post! I just have a question: you said</p><p>&quot;To find the current amounts of tilt on the X and Y axis from <br>accelerometer and gyro data, we used an algorithm that would average <br>previous accelerometer data and combine it with gyro data to reach an <br>angle measurement that was fairly resilient to linear acceleration.&quot;</p><p>Would it be possible to know where we can find this code? </p>
<p>Hi, thanks so much for your post! I have a quick question. Do I need the transmitter and receiver? Is there anyway I can make it work without it? I'm a physics student getting ready for a final project- I have everything but those parts. So a quick reply would be ideal, thank you SO much!</p>
<p>i dont have motor, can it made by two different pair of motor</p>
<p>too little comments in your code such that it is difficult for me to understand your code. can you please explain me what does variables &quot;GYRO_CON ACCEL_CON TIME_CON SEN_CON zhuman xaverage yaverage blah&quot; mean? and how did you get the formulas like &quot;xaverage= SEN_CON *( xaverage + TIME_CON * pitch) + ( 1 - SEN_CON ) * xin;&quot; or &quot;speeds[0] = zhuman - p*(xaverage - pitchin) - p*(yawin) - d*pitch;&quot; ? Many thanks if you can explain me in detail!</p>
I'm missing something;two questions <br>1. is this in inches? <br>2. is it 2&quot; each side of the center line? <br>3. mark at points (.875,2.125), (1.375,.875), (2.125,.875), (2.125,0), (1.5, 1.5) from the origin or the point where the center lines intersect. <br>what is the two measurements?from center line i.e .875 from cl and 2.125 from cl?measured from which axis?
can i replace sparkfun mpu -6050 by inven sense mpu-6050? it costs less than sparkfun....
Hi, you can use whatever you like with MPU 6050. Sparkfun uses the MPU6050 IC manufactured from Inven Sense. From Inven Sense you can buy just the IC. From Sparkfun you can buy the chip soldered on a board and ready-to-use. The choice is yours :)
Hello, I was wondering how motor speed from the speeds[] array corresponds to the motors of the construction. Which equation is for the front, back, left and right motor. <br>Thank You in advance
i want all the details of this quadrotor plzzzzz help me futurescientist10@gmail.com
hi,i wanna build one,how can i estimate it's weight?
any one help? <br>
hello nice work. i have the data of the gyro x, gyro y and gyro z, and the inclination of pitch and roll too but i only want to make it stable in the air without using a remote control. ongly vertical takeoff and landing wo what do i have to change in the code?
sir, <br>the matter that u posted inspired me to make own quad rotor , <br>i am bech final year student of SKU , i am doing project on quad rotor and i want to make quad rotor integrated with camera to send a signal to laptop... <br> can u pls help me with some guide lines... <br> thank you sir..