ece 791/792 final report project title quadcopterunh.edu/ece/department/senior...

ECE 791/792 Final Report

Project Title: Quadcopter

Team Members: Rudra Timsina, Micah Lucas, Marc Salas

Advisor: Professor Richard A. Messner

ECE Courses Involved: 651,617,618,649,633,634,602,603,772

ECE 791/792: Final Report on Quadcopter

2

Abstract

A quadcopter is a four rotor helicopter. New technologies are constantly being

implemented on quadcopters and other multi-rotor aerial vehicles. When trying to implement

new control technologies onto quadcopters, typically a quadcopter has to be built from

scratch. When starting this project our group realized there was a need for a quadcopter that

can not only be physically accommodating to new technologies, but also be able to interface

them directly to the control software dictating the vehicle’s movement. We go through the

process of designing and implementing a quadcopter based on our own desired

specifications. This includes using the system identification tool in MATLAB to predict the

quadcopter’s physical model, as well as assisting in control scheme implementation. The end

result is a working quadcopter, however better control schemes need to be implemented in

order to achieve adequate stability.


3

Tables of contents

Project Summary 5

Project Objective 5

Method and Materials 6

Arduino Mega 7

Battery 7

Electronic Speed Controller 8

Inertial Measurement Unit 8

Motors 9

Propellers 10

Receiver Module 10

Specific task to achieve goal 11

Build Frame 11

Motor control using ESC 12

Flight Stabilization using IMU 13

Stability 13

Experimental setup 14

Model Prediction 14

Interfacing GPS and range finders with Arduino 17

Programming 17

Problems Encountered 20

Results 21

Conclusion and Future work 22

References 23

Appendix A 24

Appendix B 25


4

Tables

Table 1: Lifting capability with different motors and propellers 10

Table 2: Budget 24

Figures

Figure 1: System block diagram 7

Figure 2: Arduino Mega 7

Figure 3: LiPo Battery 8

Figure 4: Electronic Speed Controller 8

Figure 5: Digital IMU 9

Figure 6: Roll, Pitch and Yaw axis 9

Figure 7: Motor 10

Figure 8: Receiver Module 11

Figure 9: Frame 12

Figure 10: The change in the orientation of a quadcopter with a step response 14

Figure 11: Simulink Diagram 15

Figure 12: Step response of the system after PID was implemented 15

Figure 13: Frequency Response of the predicted model 16

Figure 14: Nyquist Plot 16

Figure 15: Quadcopter axis and PID output assignment 18

Figure 16: Program Flow 19

Figure 17: Roll and Pitch reading with motor off 21

Figure 18: Roll and Pitch reading with motor powered at minimum value 21


5

Project Summary

Drone technologies have wide and varying uses in today’s market economy; from military

surveillance to potential package delivery for online distributors. These drones allow us to have

an aerial view of our surroundings and an ability to deliver physical media in a fast, efficient and

autonomous manner.

When trying to implement new technologies on quadcopters, you typically have to build a

quadcopter from the ground up. There are no current market options for modular quadcopters,

where one can easily add devices and edit the software. Building a quadcopter from scratch

simply so you can prototype a new sensor or control scheme is a daunting task.

Our goal in this project is to design an easily modifiable quadcopter. Additionally it should

be able to fly on its own to a given location and come back to the user’s station. It will use the

GPS address provided by the user to fly to that location.

Project Objectives:

The main objective of the Quadcopter project was to build a modular quadcopter to

allow for easy prototyping of sensors and control schemes. Our desired specifications

to achieve this are as follows:

Large lifting capacity: the quadcopter should be able to carry a payload of at

least one pound. This allows for a greater flexibility in what can be added. We

based this number off of what we thought might be reasonable things to add to

a quadcopter: bigger battery, camera, radio transmitter etc.

Fight time: 10 minutes is about the typical flight time of most custom built

quadcopters. We aimed for this length of time because it seemed like a nice

place to start and it allows time to do most normal quadcopter operations.

Long distance flight: our goals for flying distance were to be able to fly far

enough away that we wouldn’t be able to see it but not so far that the battery

wouldn’t be able to power it long enough to get back to us. We decided a flight

distance of about one kilometer would be sufficient for this project.


6

Methods and Materials

We used an Arduino Mega as the brain to control the quadcopter. The Arduino outputs

pulse width modulated (PWM) signals which control the motors. We chose transmitter and

receiver modules that are compatible with Arduino. Four PWM pins were used as inputs from

the controller and four pins were used to output PWM signals to the motors. The motor control

signals are determined using a Proportional-Integral-Derivative (PID) feedback gain control

scheme and are transmitted to the ESC’s and then to the motors. The inertial measurement unit

provides the orientation of the quadcopter and sends the information to the Arduino so the motor

outputs can be adjusted.

We used the following parts to meet the objectives of our project:

1. Arduino Mega

2. Inertial Measurement Unit

3. Electronic Speed Controllers

4. Motors

5. Receiver Module

6. Propellers

7. Frame Kit

8. Battery

9. Controller

For full product information see Table 2 in Appendix A.


7

Figure 1: System block diagram

Arduino Mega

The Arduino Mega processes all the data from other components in the quadcopter. This

microcontroller is based on ATmega2560 chip and has 54 digital I/O pins, out of which 14 can

be used for pulse width modulation. It also has 16 analog input pins which is suitable for

multiple sensor integration. Arduino microcontroller boards like UNO and Duemilanove only

support six analog input pins and are not suitable for projects that use more than six sensors. It

runs with 5V operating voltage and takes input voltage of 5V-12V.

Figure 2: Arduino Mega

Battery

This project uses a three cell lithium polymer (LiPo) battery with 2200 mAh at 11.1 V,

giving the quadcopter a flight time of about 10 minutes. This battery can source a maximum of

66A to the motors (when hovering the quad pulls about 10-15 Amps). Lithium polymer batteries

provide a very high discharge rate that is perfect for high current motor applications.


8

Figure 3: Lipo Battery

Electronic Speed Controller (ESC’s)

The electronic speed controllers are key components of the quadcopter. They take the low

power 5 volt PWM signal from the Arduino and amplify it and convert it to the three wire

configuration used by the dc motors. Each of the motors requires an ESC which is controlled by

the pulse width modulation pin from the Arduino Mega. The ESC’s run on 5.6-16.8 volts which

is compatible with the battery that we use. The ESC’s also supply 5v power to other electronic

devices used on the quadcopter such as the Arduino and the IMU.

Figure 4: Electronic Speed Controller

Inertial Measurement Unit (IMU)

The inertial measurement unit (IMU) is responsible for the orientation and acceleration of

the quadcopter. It uses a combination of gyroscope and three dimensional accelerometers to

provide six degrees of freedom. The accelerometer provides the magnitude and direction of

acceleration which is useful for maneuvering the quadcopter in a desired direction. This device

interfaces with the Arduino by using a two wire interface known as I2C. Communication is done


9

using a data line and a clock line, which are specified on the Arduino (pin 20 for data and pin 21

for the clock).

An IMU library for Arduino (FreeSix IMU) was used to get Roll and Pitch angles. Pitch

is the angle offset on front and back of the quadcopter and roll is sidewise angle offset. Yaw is

the rotational angle offset. This angle was not implemented in our project due to gyro drift, and

was compensated from the controller. Figure 6 below shows roll, pitch and yaw orientations.

Motors

The motors are the muscles of the quadcopter, they provides the thrust needed to lift the

craft. For this project we use (4) DC brushless motors. Four of these motors have enough power

to lift between 3.5 and 7 lbs depending on the propeller used. This gave us a good working

range. The motors were chosen considering the specification in table 1 below. The general rule

is that a quadcopter should weigh about half of the motors max lifting capability to allow for

good performance, stability, and efficiency. The total weight of the quadcopter was measured to

be about 2 lbs.

Figure 5: Digital IMU Figure 6: Roll, Pitch and Yaw axis


10

Table 1: Lifting capability with different motors and propellers

Figure 7: Motor

Propellers

Propellers were selected based on the table 1 above. We used both 10 inch and 11 inch

propellers. The 10 inch propellers give less lift but are more resilient than the 11 inch propellers,

while the 11 inch propellers are more efficient they tended to break very easily.

Receiver Module

This receiver is standard with most R/C devices. It uses 2.4GHz FM to communicate with

the remote controller and has 6 channels. Four channels are used to control the quadcopter,


11

leaving 2 extra channels to use for additional functions. Two of the channels were used to set

desired roll or pitch angles, one of them was used to compensate for the yaw movement and one

of them was used for throttle.

Figure 8: Receiver Module

Specific tasks to achieve goal

1. Build Frame

We planned to build the frame using Lexan but later decided go with a prebuilt frame kit.

We would have had to learn how to use design software if Lexan was going to be used for the

frame. We decided to buy the frame because it was cheaper, less time consuming to build, and

would weigh less. The frame we bought was 26 inches long which supported bigger propellers

and a bigger payload. The idea behind the bigger frame was that it can support multiple

accessories for a modular quadcopter. The frame is made up of aluminum tubes and is very light.

Springs were mounted to the legs of the frame to protect the quadcopter from crashing hard

while landing. Designing such would be difficult with no previous knowledge of software like

SolidWorks. Figure 9 shows the frame we built using the kit.


12

Figure 9 : Frame

2. Motor control using an Electronic Speed Controller (ESC’s)

Electronic speed controllers use weak PWM signals from the receiver and amplify

it to provide enough power for the motors. The signal coming to the ESC was controlled

from the Arduino. We read the input PWM signal from the R/C controller. The Arduino

read this signal as a pulse width value in microseconds. The input pulse width values

were then adjusted with PID gain for the desired output. The outputs for each motor were

then passed into a servo library for Arduino. The servo library controls the motors based

on the pulse width values. The servo library takes minimum pulse width of about 1150

microseconds and maximum of about 2500 microseconds. The details of this are

explained in the programming section.

The ESCs have three output terminals that are connected to the motors. The inputs

to the ESCs contain two wires for power (directly from the battery) and ground and a data

line connected to the Arduino PWM output. They also have voltage regulators built in

which we use to power the Arduino.


13

3. Flight stabilization with Inertial Measurement Unit (IMU)

Stability

The stability of the quadcopter is maintained using a proportional-integral-derivative

(PID) feedback controller that is implemented in the Arduino code. The PID controller

minimizes the output error by adjusting the control input.1 The proportional gain affects the

system based on the present error, integral gain, on past error, and derivative gain, on predicted

future error. Higher proportional gain increases the input and produces higher output. Lower

values result in slower system response. Integral gain adjusts the input and produces output

based on the past error. Higher integral gain often results in the quadcopter drastically

overshooting the desired angle which can lead to an oscillatory response. The derivative gain

helps to maintain steady state response of the system by changing the output based on the current

rate at which the system is changing. The equations of outputs for each gain elements are shown

below.

∫

It was very difficult to get appropriate PID values that can make our system stable. For a

known system, PID values can be calculated or tuned using MATLAB or other tuning software.

The system’s transfer function was not known to us and therefore we were not able to tune gain

elements without identifying our system first. We use the system identification tools in

MATLAB that takes time domain input and output data and estimates a desired output model.

We performed several experiments to obtain these input and output data to get a better estimation

of our system model.

1 http://en.wikipedia.org/wiki/PID_controller


14

Experimental setup

We provided a step input (pulse width of 1600 millisecond), just enough for the

quadcopter to lift off the ground and measured the output data for the time it was in air. The

output data represents the change in roll and pitch axis. We obtained about one hundred data

points in about 600 milliseconds. The data was then imported into the system identification tool

in MATALB to predict the model for the quadcopter. The roll and pitch angle goes from 0 to 90

and 0 to -90 on each side. For the system identification purpose we use positive angles. The

figure 10 below shows the experimental result.

Figure 10: The change in the orientation of a quadcopter with a step response

Model Prediction

We used the imported data to predict a state space model for the quadcopter using the

system identification tool. The experimental data best fit with the model output for a fifth order

state space model. The transfer function was derived from the state space equations. The

equation below shows the transfer function for the system.


15

This transfer function was then implemented in simulink to see the system response. The figure

11 below is the simulink diagram for the system.

Figure 11: Simulink Diagram

We then used the PID tuner in MATLAB to tune the PID gain values to get a better response.

Figure 12 below is the step response of the system after PID was realized into the system.

Figure 12: step response of the system after PID was implemented


16

Figure 13 and 14 are the frequency response and the Nyquist plot of the predicted model. We

have a gain margin of about 55 dB and infinite phase margin. Due to smaller gain margin we had

smaller PID values to minimize the overall gain of the system.

Figure 13: Frequency Response of the predicted model

Figure 14: Niquist Plot


17

4. Interfacing quadcopter with remote control

The receiver module has multiple inputs that receive radio signals from the

controller. Each of the channels on controller corresponds to the inputs on the receiver.

“THRO, AILE, ELEV and RUDD”, are the inputs in receiver that we used. “THRO” was

used to receive the throttle signal to power the motors. “AILE” and “ELEV” receives

signal for desired roll and pitch angle respectively. These angles are the inputs to the PID

that sets the desired orientation of the quadcopter. The yaw reading from the IMU was

not good enough to use for yaw control, therefore, we use ‘RUDD’ to receive yaw

control signal from the controller. Each of these input variables were assigned an input

pin to the Arduino so that the amplitude of the signal can be set to a desired level.

5. Interfacing GPS and obstacle avoidance sensors with Arduino

We aimed to implement GPS and range finders for autonomous flight but due to

limited time and stability issues we were not able to add this feature to our quadcopter.

Programming

All code is implemented on the Arduino Mega. Figure 16 is a code flow diagram for the

code listed in Appendix B. The initializing step is used to set up the Arduino to run the PID

functions, read Gyroscope data and set up the input and output pins for the receiver and motors.

Initializing takes about 10 seconds to complete and has a forced time delay to allow the

gyroscope enough time to start giving accurate data. After the delay the system enters the main

loop.

Once in the main loop we read all four channels of the receiver which are PWM signals.

Next we read gyroscope values off the IMU sensor. At this point we are done reading input pins.

The next part of the code is a necessary safety feature, where the Arduino has to determine if it’s

receiving any data from the controller. We do this by checking if the throttle values we just read

from the receiver are above a certain value. If the throttle is below some threshold we set all the

outputs to zero. If there is an acceptable throttle value we use the pitch value we received from


18

the controller and apply an offset to the angle value we got from the gyroscope. We then do the

same with roll values. Next we use the built in PID library.

The PID function uses the current angle value to adjust the output to achieve stability.

Basically it adjusts the outputs in such a way that the quadcopter’s gyroscope readings will have

zero degrees of error. Looking at Figure 15(motor/pid outputs), we use two PID’s. One PID

controls output 1. The other controls output 3. The outputs range from positive 100 to negative

100. To calculate output 2 we set it to negative output 1. The same is done for output 4.

Once the outputs have been assigned a value we run a quick algorithm to determine the

values that need to be sent to each motor. To do this we add the two output values that are

adjacent to each motor and then include the throttle and yaw values. So, for example, the value

for motor 1 would be:

Once all the motor values are calculated we call a servo function to output a PWM signal for

each motor, then we start the loop over again.

Figure15: Quadcopter axis and PID output assignment


19

Figure 10: Program Flow


20

Problems Encountered

We ran into a lot of issues throughout the course of this project. We had problems with

multiple pieces of hardware, software and problems with our PID values. We also encountered

problems due to wrong interpretation of the simulation result.

Throughout the project we had several issues with some of the physical components. One

of the first problems we encountered was when we first started to code. We would set our motor

outputs to some PWM value and immediately our ESC’s would start beeping and the motors

wouldn’t move. Fortunately we were able to troubleshoot the problem after a couple hours. We

realized that the motors had to be set within a specific range of values (between 1000 and 1150)

for the first 5 seconds, when the ESC’s first get powered.

By far the worst hardware problem encountered with this project was the gyroscope.

There is a huge issue with drift. Looking at figure 17, it shows that even when sitting completely

still the gyroscope values drift by about 1 degree per minute, but when the Arduino first turns on

there is a huge discrepancy. Looking at figure 18, we had the quadcopter sitting on the table but

with the motors turned on low. The angles are drifting over 5 degrees in fractions of a second

with lots of noise. When the quadcopter is actually flying the effect is much worse. Another

problem encountered with the gyroscope is that sometimes the angle values will switch. The roll

values take on the values of the pitch and the pitch takes on the values of the yaw. The only way

to fix the angle switching is to reset the power to the Arduino and gyroscope.

We were fortunate in that we were able to find drivers for the gyroscope, so

implementing it was relatively easy. When it came to the accelerometer, however, there was no

support. The group made a decision fairly early on to just use the gyroscope for stability to save

time and effort. This later turned out to be a mistake as we figured out, much later on, that a PD

controller for position is necessary to be able to hover.

When PID values were tuned with MATLAB we did not take into account the gain

margin of the system. We forced the simulation to produce a better response without considering

the overall systems gain. As a result the simulation values did not work with the real system and

we had to spend lot of time tuning the values manually. Tuning the PID values took us about 100

hours of just changing parameters in the gain, checking if stability was better or worse than

changing it again, over and over.


21

Figure 11: Roll and Pitch reading with motor off

Figure 12: Roll and Pitch reading with motor powered at minimum value


22

Results

In the end we were able to get the quadcopter flying. Due to issues with the gyroscope

and the fact that none of us are competent pilots we were unable to do continuous flight for more

than 30 seconds. The quad is, however, able to lift over a pound and a half with ease. In theory

with the controller that was used we could control the quadcopter from about a kilometer away.

The battery life is a little lower than was desired but a larger capacity battery can easily be

implemented with little to no effect on stability.

In the end although it can fly, the quadcopter is not as stable as it needs to be. Trying to

stabilize a quadcopter with just a gyroscope means that although it is stable enough to fly it will

not be able to hover in one spot. For better stability the use of some form of position tracking is

needed.

Conclusion and Future Work

The project taught us how a product is built going through different phases within a fixed

time constraint and limited budget. The project offered us hands on product design experience

from proposal writing to final presentation.

This project involved system modeling to better apply control strategies. We learned how

a model for a system can be predicted using the system identification tool in MATLAB. The

project taught us how a simulation model can be interpreted to implement a real system. It was a

great experience learning to control four motors to stabilize a complex system. This project

provided us with an opportunity to learn about feedback control system. We learned how PID’s

can be implemented to obtain stability in a quadcopter. We experimented with the effects of each

gain element on the system. We learned how these gain elements can be tuned using MATLAB’s

PID tuner to adjust the input control signal for a desired output.

Future work involves applying a better control technique for better stability. GPS will be

realized for autonomous flight. Features like obstacle avoidance will be added using range

finders. Given the quadcopter’s ability to lift large payloads, live first person video surveillance

would be a good feature to add on.


23

References

http://www.electroschematics.com/7963/arduino-mega-2560-pinout/

http://www.motionrc.com/admiral-2200mah-3s-11-1v-30c-lipo-

battery/?gclid=CJbf1Km6lb4CFa_m7AodvAMAWg

https://www.hobbyking.com/hobbyking/store/__4312__TURNIGY_Plush_18amp_Speed_Contr

oller.html

http://theboredengineers.com/2012/05/the-quadcopter-basics/

http://www.horizonhobby.com/products/ar6210-6-channel-dsmx-receiver-SPMAR6210

https://www.sparkfun.com/products/10121

http://www.hobbyking.com/hobbyking/store/__17922__nx_4005_650kv_brushless_quadcopter_

motor.html

http://www.electroschematics.com/7963/arduino-mega-2560-pinout/

http://www.motionrc.com/admiral-2200mah-3s-11-1v-30c-lipo-battery/?gclid=CJbf1Km6lb4CFa_m7AodvAMAWg

http://www.motionrc.com/admiral-2200mah-3s-11-1v-30c-lipo-battery/?gclid=CJbf1Km6lb4CFa_m7AodvAMAWg

https://www.hobbyking.com/hobbyking/store/__4312__TURNIGY_Plush_18amp_Speed_Controller.html

https://www.hobbyking.com/hobbyking/store/__4312__TURNIGY_Plush_18amp_Speed_Controller.html

http://theboredengineers.com/2012/05/the-quadcopter-basics/

http://www.horizonhobby.com/products/ar6210-6-channel-dsmx-receiver-SPMAR6210

https://www.sparkfun.com/products/10121

http://www.hobbyking.com/hobbyking/store/__17922__nx_4005_650kv_brushless_quadcopter_motor.html

http://www.hobbyking.com/hobbyking/store/__17922__nx_4005_650kv_brushless_quadcopter_motor.html


24

Appendix A

Parts Model #/Item ID Quan

tity

Total Cost

Arduino Mega Arduino Mega 2560 R3 1 58.95

IMU ITG3200/ADXL345 1 64.95

GPS EM-506 (48 channel) 1 39.95

Receiver AR6210 (6 channel DSMX) 1 69.99

Motors NX-4005-650kv 4 123.24

ESCs Turnigy Plush 18 amp speed controller 4 48.12

Range Finders Maxbotix HRLV-EX1 2 69.90

Frame Kits HobbyKing X666 (glass Fiber) 1 19.60

Propellers 10x4.5 SF 2 sets 8.40

Battery Admiral 3S 11.1V 2200mAh LiPo 30C 1 17.99

Shipping -- 68.22

Total -- 589.31

Table 2: Budget

Due to limited time and issues with stability, some of the parts above were not implemented in

the project.


25

Appendix B

/*

PWM PINS

THRO -> 5

AILE -> 6

ELEV -> 7

RUDD -> 8

motor1 -> 9

motor2 -> 10

motor3 -> 11

motor4 -> 12

(4) OUT1 (1)

\ /

O \ / O

U \ / U

T \ / T

4 / \ 3

/ \

/ \

/ \

(3) OUT 2 (2)

changing pids so we only have 2 PID's, one for each axis "//m"

*/

#include <PID_v1.h>

#include <FreeSixIMU.h>

#include <Servo.h>

#include <Wire.h>

int led1 = 22; // these are set to go high as the increased output is running. ie the light indicates

the motor that will be physically higher than the other

int led2 = 24; //

int led3 = 26;

int led4 = 28;

int bstat = 1150; //pwm zero value

double time1 = 0; // used for outputting millis(if needed)

double time2 = 0;

int pin5 = 5; // input THRO on pin 5


26

int pin6 = 6; // input AILE on pin 6

int pin7 = 7; // input ELEV on pin 7

int pin8 = 8; // input RUDD on pin 8

int yaw = 0; //yaw controll

double output1 = 0; //this is the output of PID //roll

double output2 = 0; // second PID output //roll

double output3 = 0; //output of pitch pid //pitch

double output4 = 0; //output of second pitch pid //pitch

double setpointr = 0; //PID roll setpoint angle

double setpointp = 0; //PID pitch setpoint angle

// most stable so far at values

float Kp =0.4;//0.3;//0.4;//2.5;//5;//2;//22.0491210055433; //1.75 // 0.05

float Ki =0.0000001;//0.002;//0.035;//0.15; //0.08;//4.71276400378977; //0.875 //0.02

float Kd =0.05;//0.1;//0.01;//0.06; //0.1;

int damp = 1; //ratio of error comp to "off" comp FOR DAMPING, smaller numbers make quad

respond slower and much more stable

int offsetr = 7; // gyro roll offset as needed

int offsetp = -3; // gyro pitch offset as needed

Servo servo1; //motor1




double val1; //value sent to motor 1




float angles[3]; // yaw pitch roll

double inputr; //roll input = angles[1]

double inputra; //absolute roll input

double inputp; //pitch input = angles[2

double inputpa; //absolute pitch input

//set up pid controlls

PID myPID1(&inputr, &output1, &setpointr,Kp,Ki,Kd, DIRECT);

//mPID myPID2(&inputra, &output2, &setpointr,Kp,Ki,Kd,REVERSE);

PID myPID3(&inputp, &output3, &setpointp,Kp,Ki,Kd, DIRECT);

//mPID myPID4(&inputpa, &output4, &setpointp,Kp,Ki,Kd,reVERSE);

FreeSixIMU sixDOF = FreeSixIMU();


27

void setup()

{

myPID1.SetOutputLimits(-100,100);

myPID3.SetOutputLimits(-100,100);

//setup here

Serial.begin(115200);

Serial.println("Lets be Friends");

Wire.begin();

myPID1.SetMode(AUTOMATIC);

//m myPID2.SetMode(AUTOMATIC);

myPID3.SetMode(AUTOMATIC);

//m myPID4.SetMode(AUTOMATIC);

// SetoutputLimits(0, 50); //limit pid output

servo1.attach(9);

servo2.attach(10);

servo3.attach(11);

servo4.attach(12);

pinMode (pin5,INPUT); // set pin 2 to input pwm //thro

pinMode (pin6,INPUT); // set pin 3 to input pwm //aile

pinMode (pin7,INPUT); //elev

pinMode (pin8,INPUT); //rudd

// led pin pinmodes

pinMode(led1, OUTPUT);




digitalWrite(led1, HIGH);




int val1=bstat;

int val2=bstat;

int val3=bstat;

int val4=bstat;

servo1.writeMicroseconds(val1); // write all motors to zero

servo2.writeMicroseconds(val2); // avoids esc program mode

servo3.writeMicroseconds(val3);


sixDOF.init(); //begin the IMU


28

Serial.println("JELLY!!!!!");

delay(5000); // wait for motors to setup

Serial.println("PrePare to bE TERMINATED");

time1 = millis();

}

void loop()

{

//PID_ATune(&inputr,&output1);

//time = time in millis

int thro = pulseIn(5, HIGH); // reads pin and saves as "thro" from reciever

int aile = pulseIn(6, HIGH); // reads pin and saves as "aile" from reciever

int elev = pulseIn(7, HIGH);

int rudd = pulseIn(8, HIGH);

setpointr =0;// ((aile-1500)/30); //set roll setpoint off aile channel

setpointp =0;// ((elev-1500)/30); //set pitch setpoint off elev channel

yaw = (rudd-1500);

//angles

sixDOF.getEuler(angles);

//angles[1]=angles[1]+90;

//ngles[2]=angles[2]+90;

inputr = -angles[1] +offsetr-((aile-1500)/15); //roll

//m inputra = abs(inputr); // absolute roll value

inputp = -angles[2]+offsetp+((elev-1500)/15);//angles[2]+4; // pitch with no offset... yet

//m inputpa =abs(inputp); //abs(inputp); // absolute pitch

//---------------------------------check for throttle

if (thro <=1170){

val1=bstat;

val2=bstat;

val3=bstat;

val4=bstat;

output1 = output2 = output3 = output4 =0;

//Serial.print("safe");

//Serial.print("\t");

thro = pulseIn(5, HIGH);

}//if


29

else{

//---------------------roll start-------------------------

myPID1.Compute(); // increase output1

myPID3.Compute();//output 3

output2=-output1;

output4=-output3;

val4 = thro+((output1)+(output4))+yaw;//- //-- for cw

val2 = thro+((output2)+(output3))+yaw; //++ for cw

val3 = thro+((output2)+(output4))-yaw;//-

val1 = thro+((output1)+(output3))-yaw;

}





if(inputr<0){//r


digitalWrite(led1, LOW);

}

else if(inputr>=0){



}

if(inputp>0){//p



}

else if(inputp<=0){



}

// Serial.print(time);

// Serial.print("\t");

// Serial.print(time); //roll

Serial.print("roll");

Serial.print("\t");


30

Serial.print("pitch");

Serial.print("\t");

Serial.print("output1");

Serial.print("\t");


Serial.print("\t");


Serial.print("\t");


Serial.print("\t");

Serial.print("setP");

Serial.print("\t");

Serial.print("setr");

Serial.print("\t");

Serial.print("yaw");

Serial.print("\t");

Serial.print("(1)");

Serial.print("\t");


Serial.print("\t");


Serial.print("\t");

Serial.println("(4)");

Serial.print(inputr); //roll

Serial.print("\t");

Serial.print(inputp); //pitch

Serial.print("\t");

Serial.print(output1);

Serial.print("\t");


Serial.print("\t");


Serial.print("\t");


Serial.print("\t");

Serial.print(setpointp);

Serial.print("\t");

Serial.print(setpointr);

Serial.print("\t");

Serial.print(yaw);

Serial.print("\t");

Serial.print(val1);


31

Serial.print("\t");

Serial.print(val2);

Serial.print("\t");

Serial.print(val3);

Serial.print("\t");

Serial.print(val4);

Serial.print("\t");

Serial.println(aile);

delay(50);

time2=millis();

double tim=time2-time1;

time1=time2;

myPID1.SetSampleTime(tim);

myPID3.SetSampleTime(tim);

}

ece 791/792 final report project title quadcopterunh.edu/ece/department/senior...

Documents