ECE 791/792 Final Report Project Title Quadcopterunh.edu/ece/Department/Senior...
Transcript of 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.
ECE 791/792: Final Report on Quadcopter
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
ECE 791/792: Final Report on Quadcopter
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
ECE 791/792: Final Report on Quadcopter
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.
ECE 791/792: Final Report on Quadcopter
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.
ECE 791/792: Final Report on Quadcopter
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.
ECE 791/792: Final Report on Quadcopter
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
ECE 791/792: Final Report on Quadcopter
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
ECE 791/792: Final Report on Quadcopter
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,
ECE 791/792: Final Report on 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.
ECE 791/792: Final Report on Quadcopter
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.
ECE 791/792: Final Report on Quadcopter
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
ECE 791/792: Final Report on Quadcopter
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.
ECE 791/792: Final Report on Quadcopter
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
ECE 791/792: Final Report on Quadcopter
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
ECE 791/792: Final Report on Quadcopter
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
ECE 791/792: Final Report on Quadcopter
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
ECE 791/792: Final Report on Quadcopter
19
Figure 10: Program Flow
ECE 791/792: Final Report on Quadcopter
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.
ECE 791/792: Final Report on Quadcopter
21
Figure 11: Roll and Pitch reading with motor off
Figure 12: Roll and Pitch reading with motor powered at minimum value
ECE 791/792: Final Report on Quadcopter
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.
ECE 791/792: Final Report on Quadcopter
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
ECE 791/792: Final Report on Quadcopter
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.
ECE 791/792: Final Report on Quadcopter
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
ECE 791/792: Final Report on Quadcopter
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
Servo servo2; //motor2
Servo servo3; //motor3
Servo servo4; //motor4
double val1; //value sent to motor 1
double val2; //value sent to motor 2
double val3; //value sent to motor 3
double val4; //value sent to motor 4
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();
ECE 791/792: Final Report on Quadcopter
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);
pinMode(led2, OUTPUT);
pinMode(led3, OUTPUT);
pinMode(led4, OUTPUT);
digitalWrite(led1, HIGH);
digitalWrite(led2, HIGH);
digitalWrite(led3, HIGH);
digitalWrite(led4, 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);
servo4.writeMicroseconds(val4);
sixDOF.init(); //begin the IMU
ECE 791/792: Final Report on Quadcopter
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
ECE 791/792: Final Report on Quadcopter
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;
}
servo1.writeMicroseconds(val1);
servo2.writeMicroseconds(val2);
servo3.writeMicroseconds(val3);
servo4.writeMicroseconds(val4);
if(inputr<0){//r
digitalWrite(led2, HIGH);
digitalWrite(led1, LOW);
}
else if(inputr>=0){
digitalWrite(led1, HIGH);
digitalWrite(led2, LOW);
}
if(inputp>0){//p
digitalWrite(led3, HIGH);
digitalWrite(led4, LOW);
}
else if(inputp<=0){
digitalWrite(led4, HIGH);
digitalWrite(led3, LOW);
}
// Serial.print(time);
// Serial.print("\t");
// Serial.print(time); //roll
Serial.print("roll");
Serial.print("\t");
ECE 791/792: Final Report on Quadcopter
30
Serial.print("pitch");
Serial.print("\t");
Serial.print("output1");
Serial.print("\t");
Serial.print("output2");
Serial.print("\t");
Serial.print("output3");
Serial.print("\t");
Serial.print("output4");
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("(2)");
Serial.print("\t");
Serial.print("(3)");
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(output2);
Serial.print("\t");
Serial.print(output3);
Serial.print("\t");
Serial.print(output4);
Serial.print("\t");
Serial.print(setpointp);
Serial.print("\t");
Serial.print(setpointr);
Serial.print("\t");
Serial.print(yaw);
Serial.print("\t");
Serial.print(val1);
ECE 791/792: Final Report on Quadcopter
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);
}