A

PRACTICAL TRAINING

REPORT

ON

Three-Axis Auto Stabilizing Video Camera Platform

Submitted for the partial fulfillment of Degree

Bachelor of Technology

Electrical Engineering

Submitted By: - Submitted To:-

Eklavya Sharma Dr. L. Solanki

12EBKEE031 Principal (Academics)

DEPARTMENT OF ELECTRICAL ENGINEERING

B. K. BIRLA INSTITUTE OF ENGINEERING & TECHNOLOGY PILANI (RAJ.)

(Affiliated to Rajasthan Technical University, Kota)

2014-2015

II

III

ACKNOWLEDGEMENT

I would like to express my sincere gratitude indebtedness to Mr. Amrish Bhatnagar for allowing

me to undergo the summer training of 45/60 days at Central Electronics Engineering Research

Institute (CSIR-CEERI), Pilani.

I am grateful to Mr. Anil Kumar Saini for the help provided in completion of the training, which

was assigned to me. Without his friendly help and guidance it was difficult to develop this project.

I am also thankful to Dr. P.S. Bhatnagar (Director BKBIET, Pilani) his true help & providing the

resources on completion of Training. Last but not least, I pay my sincere thanks and gratitude Dr.

L. Solanki (Principal, Academics, BKBIET Pilani)) for his valuable suggestions &

encouragement.

I would also like to thanks Mr. Santosh Jangid (Assistant Professor BKBIET, Pilani) and Mr.

Rajesh Singh Shekhawat (Assistant Professor BKBIET, Pilani) for timely giving the support and

suggestion during the training period.

Eklavya Sharma Date: 04/08/2015

12EBKEE031

IV

TABLE OF FIGURES

S. NO. FIGURES PAGE NO.

Fig. 1.1. Arduino Prototyping Board (Arduino Uno) 4

Fig. 1.2. Arduino Integrated Development Environment 7

Fig. 2.1. Full Wave Bridge Rectifier 8

Fig. 2.2 Full Wave Bridge Rectifier 9

Fig. 2.3. Pin configuration of LM78XX series 10

Fig. 2.4. LM7805 Connections 11

Fig. 2.5. Model Prototype 12

Fig. 2.6. Auto Stabilizing Platform Design 13

Fig. 2.7. Servo Motor Sg90 14

Fig. 2.8. Wire configuration of servo motor 16

Fig. 3.1. MPU-6050 18

Fig. 3.2. Arduino Uno connection 20

Fig. 4.1. Yaw, Pitch and Roll 23

Fig. 5.1. Final Prototype Of Self-Balancing Platform 29

V

TABLE OF CONTENTS

S. NO. CONTENTS PAGE NO.

Certificate II

Acknowledgement VI

Table Of Figures IV

Organizational Profile VI

1. Abstract 1

1.1. Motivation 2

1.2. Objectives And Goals 3

1.3. Initial Research 4

A. The Arduino Platform 4

B. Arduino IDE 6

2. Power Supply 10

2.1. Prototype 11

2.2. Methodology And Implementation 12

2.3. Dc Servo Motors (Sg90) 14

3. Coding Implementation 16

3.1. MPU-6050 17

4. Motion Processing Unit 20

4.1. Gy-521 21

4.2. Tilt Sensing Using A Three-Axis Accelerometer 22

4.3. Tilt Sensing Using A Three-Axis Gyroscope 25

4.4. Kalman Filter 27

5. Free Mpu-6050 Libraries 28

5.1. Controlled Platform Sketch Flow Chart 29

6. Program Code 30

6.1. Testing And Results 37

6.2. Conclusion 38

References 39

VI

ORGANIZATIONAL PROFILE

CSIR-Central Electronics Engineering Research Institute, popularly known as

CSIR-CEERI, is a constituent establishment of the Council of Scientific and

Industrial Research (CSIR), New Delhi. The foundation stone of the institute was

laid on 21st September, 1953 by the then Prime Minister Pt. Jawaharlal Nehru. The

actual R&D work started towards the end of 1958. The Institute (CSIR-CEERI) has

since blossomed into a center of excellence for development of technology and for

advanced research in electronics. Over the years the institute has developed a

number of products and processes and has established facilities to meet the emerging

needs of electronics industry.

Week-I

1

ABSTRACT

Basically auto-stabilizing platform consists of platform which is balanced by movement of three

servo motors in opposite direction to the movement of the platform. Arduino Uno process the tilt

angles obtained from MPU-6050 and give instruction to the respective servo motors to rotate by

certain angle depending on its previous position to balance or control the platform. The intent of

the platform design is to maintain the platform at an initially selected angle while the support

structure orientation changes. The software was written with logic to convert the digital data from

the accelerometer and gyroscope to an acceleration and gyro magnitude vector and then converted

in degrees. The InvenSense MPU-6050 sensor contains a 3axis MEMS accelerometer and a 3axis

MEMS gyro in a single chip whose outputs are calibrated properly by using KALMAN FILTER

to give the precise angle. The magnitude of the angle was then compared to a predetermined

mathematical function to infer the angle of tilt of the platform. The angle of tilt is then converted

to angle of rotation for the servos to adjust their current position and bring the platform in balanced

condition. Testing showed the platform to perform as expected. Although some error on the final

angle was expected, the magnitude of the error observed indicated the platform design has a high

sensitivity to low tolerance mechanical joints (slop). Overall the platform design was validated

based on the positional accuracy of the platform given the low quality components used to create

it. In other words, the platform performed greater than the sum of its parts.

2

1.1. MOTIVATION

There are many benefits to an electro-mechanically controlled platform. This purpose of this

project work is to design and build the auto stabilizing platform as a surface whose angle in relation

to a user specified plane is controlled autonomously by electronic means. And maintaining a

budget of less than ₹1500 for all the hardware and software. With advent of self-balancing devices,

be it Segway, DIY, or TIPI, it fascinated me with its futuristic scope of self-balancing devices, like

security camera pan/tilt/roll mechanisms, gimbaled nautical compasses, and scissor lifts. And a

simple self-stabilizing skateboard, controlled by your gestures (leg movement) fascinated me the

most. Before starting the project I have gained the firsthand knowledge of the sensors(MPU-6050)

, Arduino UNO, Filters, robust mechanical system and henceforth able to conclude a auto

balancing platform with three axis of freedom.

3

1.2. OBJECTIVES AND GOALS

To demonstrate the techniques involved in balancing a platform.

To work on precise movements and accurate control of platform using filtering process.

To understand the working of MPU-6050. MPU-6050 work involves understanding the pin

configurations of the measuring unit and configuring the correct libraries for it.

To identify the correct connections needed for all the peripheral hardware to communicate

with the microcontroller.

Establishing lines of communication with the correct hardware pin addresses will allow for

easy identification on how each individual piece of hardware will transmit and process data

to and from the MPU.

To establish the power supply to each electronic components.

4

1.3. INITIAL RESEARCH

Without any prior knowledge of how an auto-balancing cum camera controlled platform would

work, further research was required to get an idea of what was possible for this project.

Supplementary investigation was necessary such as to power the system, how to approach and

which direction to take towards designing the software, how the platform would accomplish its

main task of balancing.

a. THE ARDUINO PLATFORM

The Arduino platform is an open source electronic prototyping system. It is composed of two parts,

the Arduino Uno board and the Arduino IDE (Integrated Development Environment). The Uno

board is designed to provide an easy to use human changeable pin interface to the Atmel AVR AT

mega microcontroller, the heart of the Arduino hardware. The AVR microcontrollers from Atmel

use the C language for programming and are commercially available and very popular with

hobbyists and electronics enthusiasts. Arduino builds on this by adding simplicity to the hardware

interface and an easy to use software package.

Fig. 1.1. - Arduino Prototyping Board (Arduino Uno)

5

Arduino is meant to be used as a physical computing platform. That is, to use the electronic

hardware to interface with humans using sensors and actuators controlled by software executed by

a computer. The groundwork of this paper focuses on the physical computing aspect of Arduino.

The internal functionality and architecture of the ATmega328 microcontroller and the Arduino

board is outside the scope of this project and as such will not be discussed further.

Microcontroller ATmega328

Operating Voltage 5 volt

Input Voltage (recommended) 7-12 volt

Input Voltage (limits) 6-20 volt

Digital I/O Pins 14 (of which 6 provide PWM output)

Analog Input Pins 6

DC Current per I/O Pin 40 mA

DC Current for 3.3V Pin 50 mA

Flash Memory 32 KB (ATmega328) of which 0.5 KB used

by bootloader

SRAM 2 KB (ATmega328)

EEPROM 1 KB (ATmega328)

Clock Speed 16MHz

Table 1.1. - Arduino Specifications

6

b. ARDUINO IDE

The open-source Arduino Software (also known as the Arduino IDE - short for Integrated

Development Environment) makes it easy to write code and upload it to the board. It runs on

Windows, Mac OS X, and Linux. The environment is written in Java and based on Processing and

other open-source software. This software can be used with any Arduino board.

This software can be used with any Arduino board.The Arduino IDE is the software environment

used to create the programs, called “sketches,” that will be executed by the Arduino hardware. The

IDE uses a modified C language compiler to build, translate, and transmit the code to the

microcontroller board. The AVR microcontrollers are typically programmed using the C language.

Since C can be somewhat difficult to learn for the typical hobbyist, the IDE makes a combination

of modified C and Arduino specific commands available to the user. The IDE provides the

simplicity for less proficient users but supports advanced users who are knowledgeable in C. The

IDE also has a set of additional code functions called libraries. Libraries extend the commands

available to provide capabilities not available in the core Arduino language. In essence, a library

allows the user to perform seemingly complex functions using a small set of commands.

For example, this paper will focus on controlling servo motors. The SERVO library is called within

the sketch to simplify the code. All libraries used are provided in the appendix along with the

complete servo sketch source code.

Installing Arduino IDE

Windows 8, 7, Vista, and XP

Go to the Arduino download page and download the latest version of the Arduino software

for Windows.

When the download is finished, un-zip it and open up the Arduino folder to confirm that yes,

there are indeed some files and sub-folders inside. The file structure is important so don’t be

moving any files around unless you really know what you are doing.

7

Power up your Arduino by connecting your Arduino board to your computer with a USB

cable (or FTDI connector if you are using an Arduino pro).

Fig. 1.2. - Arduino Integrated Development Environment

Week-II

8

POWER SUPPLY

The input voltage of an Arduino can vary from 7 -12 Volts. For the 12 V power supply design I

have used a simple full wave bridge rectifier circuit. A Full Wave Bridge Rectifier is a circuit,

which converts an ac voltage into a pulsating dc voltage using both half cycles of the applied ac

voltage. The full wave bridge rectifier circuit is very reliable, cheaper and the resulting output is

much easier to smooth. The designing of the circuit would cost you less than ₹150(less than $5).

Fig. 2.1. - Full Wave Bridge Rectifier

1. Step Down Transformer – A single phase 230:18 V step down transformer is required for

12v dc power supply. Likewise, to design a 5 V power supply you may use the 230:10. There

are three wire in the transformer and 9-0-9 (considering the 18 v) is written on it. So, you are

needed to connect the outermost wires of the transformer (which are of same colors).

2. Diode – Four Diodes are used in the circuit for rectification and a single diode is used as

freewheeling diode. All diodes are 1N4007.

3. Voltage Regulator IC– LM 7812 a voltage regulator IC is used in the 12V DC Power supply.

Similarly, for 5 volt power supply just replace the LM 7812 with LM 7805.

9

4. Capacitances– capacitances are used for reducing ripples in DC power supply as per

specifications. A 1000 µF and a 10 µF capacitances. All these items are easily available at

electronics shop.

5. Copper wires– For AC mains they should have the current capacity of more than 1 A.

6. Jumper wires– For connection purposes.

7. Soldering iron

8. Soldering wire

9. General Purpose PCB

10. Two pin plug

Optional components

1. Adapter Jack- The adapter jack is used to connect the power supply with the Arduino board.

2. Heat sink (optional) – For the high current outputs from the power supply and for proper heat

dissipation from the voltage regulator. The heat sink may be connected to the Voltage regulator

IC.

3. LED- You can also connect the led with a limiting resistor for the indication of power supply.

4. Limiting resistor- Limiting resistors are used to limit the current flow through the LED.

Fig. 2.2. - Full Wave Bridge rectifier

10

A LM7805 Voltage Regulator is a voltage regulator that outputs +5 volts and the current up to

750mA. The output of LM7812 is given to the LM7805. The LM7805 is sufficient to drive the

three servo motors.

An easy way to remember the voltage output by a LM78XX series of voltage regulators is the last

two digits of the number. A LM7805 ends with 05, thus it outputs 5 volts. The 78 part is just the

convention that the chip makers use to denote the series of regulators that output voltage is positive.

The other series of regulators, the LM79XX, is the series that output voltage is negative. After you

have all the above required items, plan the layout of the power supply and carefully solder all the

components. The resulting system will be a 12 volt power supply with 700mA and check the output

of the power supply with a multimeter.

Fig. 2.3. - Pin configuration of LM78XX series

The LM7805, like most other regulators, is a three-pin IC.

Pin 1(input pin): The input pin is the pin that accepts the incoming DC voltage, which the voltage

regulator will eventually regulate down to 5volts.

Pin 2(ground): Ground pin establishes the ground for the regulator.

11

Pin3 (output Pin): The output pin is the regulated 5 volts DC.

Fig. 2.4. - LM7805 Connections

12

2.1. PROTOTYPE

In order to answer some questions about designing the self-balancing platform, it was decided to

first design a small scale prototype. This section details on findings about hardware selection, and

software approaches to solving the problem. In addition to the control scheme, the hardware

selection was re-evaluated and re-imagined at almost every aspect of the hardware selection.

The servos supporting and controlling the top platform are arranged to control the pitch and roll.

Note that pitch and roll are only subjective directions and are used to describe the motion. Since

the platform has no “front” or “back” the terms pitch and roll are meaningless descriptions of the

rotations about the X and Y axis. A close up view of the servo arrangement and axis is illustrated

in Figure.

Fig. 2.5. - Model Prototype

13

2.2. METHODOLOGY AND IMPLEMENTATION

The auto-stabilizing platform consists of three platforms, the “top” platform (smaller aluminum

piece), which is autonomously controlled, based on initial user input, a “bottom” platform (large

Perspex piece) and intermediate servo lying on the top of the bottom servo. The MPU-6050 is

installed just below the top platform through which the tilt angle is being measured.

Fig. 2.6. - Auto Stabilizing Platform Design

This servo arrangement is a more efficient use of space and limits the amount of slop compared to

linkages and hinges as seen in other designs. The Y axis servo is controlled by the Y axis data

stream from the accelerometer and vice versa for the X axis. Note that the X axis servo motion is

in-plane with the photo while the Y axis would rotate out of plane.

14

The servos are installed into aluminum brackets. These brackets allow the servo to be installed

onto the bottom and top platforms. The servo “C” brackets are then used to attach the servo bodies,

via the body brackets, to themselves and the platforms. The “C” brackets attach to the body and to

the rotating Servo horn.

Based upon rethinking over several prototypes I finally agreed upon two of them which seemed

most suitable for our project considering mainly the form factor and feasibility. One of them is

having the base and the platform on the same level and the other having a more conventional type

of design having platform vertically above the main base. Considering aesthetics and being popular

I finally agreed on to the second design. Placing motors perpendicular to each other gives us the

advantage of having three degrees of rotation. I will be using self-made Aluminums brackets to

hold the motors in their place.

15

2.3. DC SERVO MOTORS (SG90)

There are three servo motors in the auto stabilizing platform which are used to balance each axes

namely yaw, pitch and roll.

These servo motors would provide the proper amount of power required to maintain its own

weight as well as the weight of all the electrical and physical hardware while at the same time

still maintaining balanced equilibrium.

Fig. 2.7. - Servo Motor Sg90

Tiny and lightweight with high output power. Servo can rotate approximately 180 degrees (90 in

each direction), and works just like the standard kinds but smaller. You can use any servo code,

hardware or library to control these servos. Good for beginners who want to make stuff move

without building a motor controller with feedback & gear box, especially since it will fit in small

places. It comes with a 3 horns (arms) and hardware.

Second Week

16

The shaft can be easily angled between 0 to 180 degrees. This wire is given a pulse application for

a specified duration, which in turn controls the angle of the shaft in a particular position for a

certain point of time. This modulation is famously referred to as the PWM (Pulse Width

Modulation). The servomotor expects a coded signal every few seconds. The duration of the pulse

determines the angular degree of the shaft.

Servo Specification

• Stall Torque: 1.6kg/cm @ 6.0V

• Speed: 0.12 seconds/60deg. @ 4.8V

• Dimensions: 21x12x22 mm

• Temperature Range: 0c - 55c

• Operating Voltage: 3.0V - 6.0V

• Weight: 9 grams

• Modulation: Analog

Fig. 2.8. - Wire configuration of servo motor

Week-III

17

CODING IMPLEMENTATION

The software development really boils down to the programming techniques used on this project.

Creating a balancing platform with the use of a gyroscope requires that the program keep track of

the gyroscope’s orientation and attempt to keep the gyroscope level. Kalman filtering is used as

the filter which combines both the values of accelerometer and gyroscope for two degrees of

freedom as the accelerometer reading are unable to give the yaw of the module. The purpose to

use the filter is due to drift observed in the readings of MPU-6050. Actually, the auto-stabilizing

platform need only two servos but using three servos for yaw, pitch and roll it can be used as self-

stabilizing camera also.

Analog output accelerometers and gyroscopes communicate with a Pulse-Width Modulation

(PWM) signal, which most microcontrollers support. Digital output accelerometers and

gyroscopes, such as the ones found on sparkfun.com, communicate using standard I2C protocol.

There are a few different ways to communicate with motor controllers, which can be categorized

by either analog or digital input. Analog input is done via PWM. Digital input can be done in a

couple ways. The first is by simulating an R/C signal that sets the speed and direction of the motor

until specified at another time.

The second is to use serial data to communicate the speed and direction of the motors. The main

advantage to using serial data is that the microcontroller can communicate with the motor

controller with just one serial port. Arduino obtains tilt angle from MPU by using I2C Interface

and then control servo motors using its PWM pins and the frequency of operation of servo motors

can be made synonymous with that of MPU.

18

3.1. MPU-6050

The InvenSense MPU-6050 sensor contains a MEMS accelerometer and a MEMS gyro in a single

chip. It is very accurate, as it contains 16-bits analog to digital conversion hardware for each

channel. Therefor it captures the x, y, and z channel at the same time. The sensor uses the I2C-bus

to interface with the Arduino. The MPU-6050 is not expensive, especially given the fact that it

combines both an accelerometer and a gyro.

Fig. 3.1. - MPU-6050

Reading the raw values for the accelerometer and gyro is easy. The sleep mode has to be disabled,

and then the registers for the accelerometer and gyro can be read. But the sensor also contains a

1024 byte FIFO buffer. The sensor values can be programmed to be placed in the FIFO buffer.

And the buffer can be read by the Arduino.

The FIFO buffer is used together with the interrupt signal. If the MPU-6050 places data in the

FIFO buffer, it signals the Arduino with the interrupt signal so the Arduino knows that there is

data in the FIFO buffer waiting to be read. A little more complicated is the ability to control a

second I2C-device.

The MPU-6050 always acts as a slave to the Arduino with the SDA and SCL pins connected to

the I2C-bus. But beside the normal I2C-bus, it has its own I2C controller to be a master on a second

(sub)-I2C-bus. It uses the pins AUX_DA and AUX_CL for that second (sub)-I2C-bus.

19

It can control, for example, a magnetometer. The values of the magnetometer can be passed on to

the Arduino.

Things get really complex with the "DMP". The sensor has a "Digital Motion Processor" (DMP),

also called a "Digital Motion Processing Unit". This DMP can be programmed with firmware and

is able to do complex calculations with the sensor values.

For this DMP, InvenSense has a discouragement policy, by not supplying enough information how

to program the DMP. However, some have used reverse engineering to capture firmware.

The DMP ("Digital Motion Processor") can do fast calculations directly on the chip. This reduces

the load for the microcontroller (like the Arduino). The DMP is even able to do calculations with

the sensor values of another chip, for example a magnetometer connected to the second (sub)-I2C-

bus.

MPU6050 / GY-521 Arduino UNO Pin

VCC 5V (the GY-521 has a voltage regulator)

GND GND

SDA A4 (I2C SDA)

SCL A5 (I2C SLC)

INT D2 (interrupt #0)

Table No. 3.1. - Arduino Uno and MPU-6050 Connections

20

Fig. 3.2. - Arduino Uno connection

Week-IV

21

MOTION PROCESSING UNIT

The MPU-60X0 Motion Processing Unit is the world’s first motion processing solution with

integrated 9-Axis sensor fusion for handset and tablet applications, game controllers, motion

pointer remote controls, and other consumer devices. The MPU-60X0 has an embedded 3-axis

MEMS gyroscope, a 3-axis MEMS accelerometer, and a Digital Motion Processor™ (DMP™)

hardware accelerator engine with an auxiliary I2C port that interfaces to 3rd party digital sensors

such as magnetometers.

The MPU-60X0 features three 16-bit analog-to-digital converters (ADCs) for digitizing the

gyroscope outputs and three 16-bit ADCs for digitizing the accelerometer outputs. For precision

tracking of both fast and slow motions, the parts feature a user-programmable gyroscope full-scale

range of ±250, ±500, ±1000, and ±2000°/sec (dps) and a user-programmable accelerometer full-

scale range of ±2g, ±4g, ±8g, and ±16g.

The MPU-6050 provides a VLOGIC reference pin (in addition to its analog supply pin: VDD),

which sets the logic levels of its I2C interface. The VLOGIC voltage may be 1.8V±5% or VDD.

VDD power supply voltage range of 2.375V-3.46V.

22

4.1. GY-521

This sensor board has a voltage regulator. When using 3.3V to the VCC the resulting voltage (after

the onboard voltage regulator) might be too low for a good working I2C bus. It is preferred to

apply 5V to the VCC pin of the sensor board. The board has pull-up resistors on the I2C-bus. The

value of those pull-up resistors are sometimes 10k and sometimes 2k2. The 2k2 is rather low. If it

is combined with other sensor board which have also pull-up resistors, the total pull-up impedance

might be too low.

23

4.2. TILT SENSING USING A THREE-AXIS ACCELEROMETER

An accelerometer measures the acceleration in specific directions. We can measure the direction

of gravity w.r.t the accelerometer and get the tilt angle w.r.t the vertical. It is free from drifts and

other errors.

The angle of tilt can be measured from the acceleration along x-direction as follows:

𝑎𝑐𝑐𝑋 = 𝑔 sin 𝜑 (Angles φ in roll, θ in pitch and ψ in yaw about the x, y and z axes respectively)

𝑎𝑐𝑐𝑋 = 𝑔 cos 𝜑 (Ax, Ay are accelerations in x and y directions)

𝑟𝑜𝑙𝑙 = atan 2(𝑎𝑐𝑐𝑌, 𝑎𝑐𝑐𝑍) × 𝑅𝐴𝐷_𝑇𝑂_𝐷𝐸𝐺;

𝑝𝑖𝑡𝑐ℎ = atan(−𝑎𝑐𝑐𝑋/𝑠𝑞𝑟𝑡(𝑎𝑐𝑐𝑌 × 𝑎𝑐𝑐𝑌 + 𝑎𝑐𝑐𝑍 × 𝑎𝑐𝑐𝑍)) × 𝑅𝐴𝐷_𝑇𝑂_𝐷𝐸𝐺;

Fig. 4.1. - Yaw, Pitch and Roll

24

Atan2 has an output range from -π to π I simply add π, so the range it converted to 0 to 2π. To

convert it from radians to degrees we simply multiply the result by 57.29577. This is predefined

in the Arduino IDE as RAD_TO_DEG.

For small angles

𝐴𝑥 = 𝑔𝜃

𝜃 = 𝐾 × 𝐴𝑥 (K is a constant)

25

4.3. TILT SENSING USING A THREE-AXIS GYROSCOPE

This problem faced in accelerometer is solved by using a Gyroscope. It would measure angular

velocity and angle can be found by integrating. The rate gyro measures angular velocity and

outputs a voltage Vg:

𝑉𝑔 = ω + f(T) + 𝑒𝑔

Where f(T) represents the effect of temperature and eg represents error, which is not known. So the

correct formula is:

𝐴𝑛𝑔𝑢𝑙𝑎𝑟 𝑉𝑒𝑙𝑜𝑐𝑖𝑡𝑦 𝜔 =(𝑉𝑔 − 𝑉𝑧)

𝑆𝑒𝑛𝑠𝑖𝑡𝑖𝑣𝑖𝑡𝑦 [𝑖𝑛 𝑉/(𝑑𝑒𝑔/𝑠𝑒𝑐)]

First you have to translate futile (a number from 0-1023) into something useful (this is for ADC

with a 10 bit resolution, for example this should be 4095 (212-1=4095) for 12 bit ADC). To do this

I just use this simple equation:

𝑔𝑦𝑟𝑜𝑅𝑎𝑡𝑒 =𝑔𝑦𝑟𝑜𝐴𝐷𝐶 − 𝑔𝑦𝑟𝑜𝑧𝑒𝑟𝑜

𝑠𝑒𝑛𝑠𝑖𝑡𝑖𝑣𝑖𝑡𝑦

Where gyroADC are the values read from our sensor, gyrozero is the value when it is stationary while

sensitivity is the sensitivity found in the datasheet.

After looking into the gyros datasheets you will see that the sensitivity is 3.33mV/deg/s .To

translate these into futile values is pretty easy:

𝑆𝑒𝑛𝑠𝑖𝑡𝑖𝑣𝑖𝑡𝑦

3.3×1023

So in this example I get:

0.00333

3.3× 1023 = 1.0323.

The final equation will look like this:

26

𝑔𝑦𝑟𝑜𝑅𝑎𝑡𝑒 =(𝑔𝑦𝑟𝑜𝐴𝐷𝐶 − 𝑔𝑦𝑟𝑜𝑧𝑒𝑟𝑜)

1.0323

The result will come out as degrees per second. To translate this into degrees you have to know

the exact time since the last loop. Fortunately, the Arduino got a simple command to do so: millis().

By using that, one can calculate the time difference (delta time) and thereby calculate the angle of

the gyro. The final equation will look like this:

𝑔𝑦𝑟𝑜𝐴𝑛𝑔𝑙𝑒 += 𝑔𝑦𝑟𝑜𝑅𝑎𝑡𝑒 ×𝑑𝑡𝑖𝑚𝑒

1000

But this approach fails at slow angular velocities unfortunately, the gyro drifts (due to small errors

in slow velocities integrate and accumulate into a big error resulting in drift) over time. That means

it cannot be trusted for a longer time span, but it is very precise for a short time. This is when the

accelerometer comes in handy. It does not have any drift, but it is too unstable for shorter time

span.

In reality, we do not have static conditions, but dynamic conditions. So in case of dynamic

accelerations, the accelerometer and gyroscope outputs are combined together using the Kalman

Filtering Method.

27

4.4. KALMAN FILTER

Kalman filter is, it is an algorithm which uses a series of measurements observed over time, in this

context an accelerometer and a gyroscope. These measurements will contain noise that will

contribute to the error of the measurement. The Kalman filter will then try to estimate the state of

the system, based on the current and previous states, that tend to be more precise that than the

measurements alone. The Kalman filter operates recursively on streams of noisy input data to

produce a statistically optimal estimate of the underlying system state.

The algorithm works in a two-step process:

In the prediction step, the Kalman filter produces estimates of the current state variables, along

with their uncertainties. Once the outcome of the next measurement (necessarily corrupted with

some amount of error, including random noise) is observed, these estimates are updated using a

weighted average, with more weight being given to estimates with higher certainty. Because of the

algorithm's recursive nature, it can run in real time using only the present input measurements and

the previously calculated state; no additional past information is required.

The Kalman filter uses a system's dynamics model (e.g., physical laws of motion), known control

inputs to that system, and multiple sequential measurements (such as from sensors) to form an

estimate of the system's varying quantities (its state) that is better than the estimate obtained by

using any one measurement alone. As such, it is a common sensor fusion and data fusion algorithm.

In easy way we can say that the accelerometer is in general very noise when it is used to measure

the gravitational acceleration since the platform is moving in three axes. The problem with the

gyro is that it drifts over time – just like a spinning wheel-gyro will start to fall down when it is

losing speed.

In short you can say that you can only trust the gyroscope on a short term while you can only trust

the accelerometer on a long term.

Week-V

28

FREE MPU-6050 LIBRARIES

MPU6050 can be easily used on Arduino compatible boards using the Arduino MPU-6050 library

which implements sensor fusion very efficient filter enabling you to do easy and straightforward

orientation sensing using tri axis accelerometer and gyroscope. It is very easy to implement and

the functions used in this library is easily comprehended. We found it easily compatible with our

6DOF MPU. And free Kalman filter libraries also being used enabling us to combine the values

of accelerometer and gyroscope calculate the precise angle.

28

5.1. MECHANICAL MODEL

Based upon discussion over several prototypes I finally agreed upon two of them which seemed

most suitable for my project considering mainly the form factor and feasibility. One of them is

having the base and the platform on the same level and the other having a more conventional type

of design having platform vertically above the main base. Placing motors perpendicular to each

other gives us the advantage of having three degrees of rotation. I will be using self-made

Aluminums brackets to hold the motors in their place.

Fig. 5.1. – Final Prototype of Self-Balancing Platform

30

5.1. CONTROLLED PLATFORM SKETCH FLOW CHART

Main loop

Fig. 5.2. - Controlled Platform Sketch Flow Chart

The sketch begins with the administrative tasks of calling the servo library, initializing MPU-6050,

initializing variables, and assigning the Arduino board input and output pins. These are initialized

only once within the sketch and are not part of the main program loop. The main program loop

contains all the commands for reading the accelerometer and gyroscope data applying Kalman

Filter and converting the data to useful information for the servos. And continuously maintaining

the servo angle to balance the platform.

Read MPU-6050

Convert Radian to Degrees

Kalman FilterTransform

Angle of Tilt to Servo Angle

Transform Servo Angle to

Servo Pulse

Transmit the Angle Signal to

Servo

Initialize Servo Libraries

Initialize MPU-6050

Initialize All Variables

Assign Hardware Pins

Week-VI

31

PROGRAM CODE

The program code for self-stabilizing platform:

#include <Servo.h>

#include <Wire.h>

#include <Kalman.h>

#include <MPU6050.h>

#include <I2Cdev.h>

MPU6050 mpu;

Servo myservoX, myservoY;

int valX, valY;

int cposX=95;

int cposY=95;

int i=1;

int prevalX,prevalY;

Kalman kalmanX; // Create the Kalman instances

Kalman kalmanY;

/* IMU Data */

double accX, accY, accZ;

double gyroX, gyroY, gyroZ;

double gyroXangle, gyroYangle; // Angle calculate using the gyro

only

double kalAngleX, kalAngleY; // Calculated angle using a Kalman

filter

double pkalAngleX,pkalAngleY;

32

uint32_t timer;

uint8_t i2cData[14]; // Buffer for I2C data

// TODO: Make calibration routine

void setup() {

Serial.begin(115200);

Wire.begin();

myservoX.attach(9);

myservoX.write(cposX);

myservoY.attach(10);

myservoY.write(cposY);

Serial.println("Initialize MPU");

mpu.initialize();

Serial.println(mpu.testConnection() ? "Connected" :

"Connection failed");

#if ARDUINO >= 157

Wire.setClock(400000UL); // Set I2C frequency to 400kHz

#else

TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to

400kHz

#endif

i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1)

= 1000Hz

i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc

filtering, 256 Hz Gyro filtering, 8 KHz sampling

i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s

i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to

±2g

33

while (i2cWrite(0x19, i2cData, 4, false)); // Write to all

four registers at once

while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis

gyroscope reference and disable sleep mode

while (i2cRead(0x75, i2cData, 1));

if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register

Serial.print(F("Error reading sensor"));

while (1);

}

delay(100); // Wait for sensor to stabilize

/* Set kalman and gyro starting angle */

while (i2cRead(0x3B, i2cData, 6));

accX = (i2cData[0] << 8) | i2cData[1];

accY = (i2cData[2] << 8) | i2cData[3];

accZ = (i2cData[4] << 8) | i2cData[5];

double roll = atan2(accY, accZ) * RAD_TO_DEG;

double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) *

RAD_TO_DEG;

kalmanX.setAngle(roll); // Set starting angle

kalmanY.setAngle(pitch);

gyroXangle = roll;

gyroYangle = pitch;

timer = micros();

/* mpu.setXGyroOffset(220);

mpu.setYGyroOffset(76);

mpu.setZGyroOffset(-85);

34

mpu.setZAccelOffset(1788);*/

}

void loop() {

/* Update all the values */

while (i2cRead(0x3B, i2cData, 14));

accX = ((i2cData[0] << 8) | i2cData[1]);

accY = ((i2cData[2] << 8) | i2cData[3]);

accZ = ((i2cData[4] << 8) | i2cData[5]);

gyroX = (i2cData[8] << 8) | i2cData[9];

gyroY = (i2cData[10] << 8) | i2cData[11];

gyroZ = (i2cData[12] << 8) | i2cData[13];

double dt = (double)(micros() - timer) / 1000000; // Calculate

delta time

timer = micros();

double roll = atan2(accY, accZ) * RAD_TO_DEG;

double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) *

RAD_TO_DEG;

double gyroXrate = gyroX / 131.0; // Convert to deg/s

double gyroYrate = gyroY / 131.0; // Convert to deg/s

double gyroZrate = gyroZ / 131.0; // Convert to deg/s

// This fixes the transition problem when the accelerometer

angle jumps between -180 and 180 degrees

if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX

< -90)) {

kalmanX.setAngle(roll);

kalAngleX = roll;

35

gyroXangle = roll;

} else

kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); //

Calculate the angle using a Kalman filter

//if (abs(kalAngleX) > 90)

//gyroYrate = -gyroYrate; // Invert rate, so it fits the

restriced accelerometer reading

kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);

if (gyroXangle < -180 || gyroXangle > 180)

gyroXangle = kalAngleX;

if (gyroYangle < -180 || gyroYangle > 180)

gyroYangle = kalAngleY;

/* Print Data */

Serial.print("kal\t");

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

Serial.print("\t");

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

Serial.print("\r\n");

delay(37);

/*logic 1 for servomotor 1 */

if(kalAngleX>=10&&kalAngleX<=171)

{

do

{

i=1;

myservoX.write(cposX-i);

cposX=cposX-i;

}while((kalAngleX>=175&&kalAngleX<=179)||(kalAngleX>=-

179&&kalAngleX<=-175)||(kalAngleX<-179&&kalAngleX>-190));

36

Serial.print("\tkkkkalAngleX-pkalAngleX");

pkalAngleX=kalAngleX;

}

/*logic 2 for servo motor 1 */

if(kalAngleX>=-171&&kalAngleX<-10)

{

do

{

i=1;

myservoX.write(cposX+i);

cposX=cposX+i;

}while((kalAngleX>=175&&kalAngleX<=178)||(kalAngleX>=-

179&&kalAngleX<=-175)||(kalAngleX<-179&&kalAngleX>-190));

}

/*Logic for servo motor 2*/

if(kalAngleY>=-80&&kalAngleY<=-18)

{

do

{

i=1 ;

myservoY.write(cposY-i);

cposY=cposY-i;

}while((kalAngleY>=-17&&kalAngleY<=-

1)||(kalAngleY>=0&&kalAngleY<=10)||(kalAngleY>-1&&kalAngleY<0));

Serial.print("\tkkkkalAngleY-pkalAngleY");

pkalAngleY=kalAngleY;

}

/*logic 2 for servo motor 2 */

if(kalAngleY>=13&&kalAngleY<=80)

{

do

37

{

i=1;

myservoY.write(cposY+i);

cposY=cposY+i;

}while((kalAngleY>=-10&&kalAngleY<=-

1)||(kalAngleY>=0&&kalAngleY<=10)||(kalAngleY>-1&&kalAngleY<0));

Serial.print("\tkkkkalAngleY-pkalAngleY");

pkalAngleY=kalAngleY;

}

}

38

6.1. TESTING AND RESULTS

The testing is meant to determine the accuracy of the leveling capability of the platform when

aligned parallel to the surface of the earth. The actual transit time is not a success criterion. The

platform functionality was tested using a bubble level. The testing process began by aligning the

upper platform parallel to the earth (or perpendicular to the earths acceleration due to gravity

vector) using a specified level. The lower platform was then rotated in one and then both axis while

observing the specified level. The testing was completed with a low mass object, the specified,

and a higher mass object.

In both test cases the platform performed as designed. A quantitative measurement was completed

and the testing results showed the platform’s accuracy to be approximately 95%. When a heavier

object was placed on the level, the accuracy improved to about 98%. Some extreme angles (within

the mechanical limits) were tested and the platform maintained consistent behavior. It was also

observed that the platform was more accurate when correcting in one direction versus another on

the same rotation axis. This phenomenon is likely due to the flexibility of the aluminum

components and the mechanical slop of the joints the servos act on. The results of the testing

proved that the platform design functioned as designed and intended.

39

6.2. CONCLUSION

This paper demonstrated the feasibility of the platform design using inexpensive hardware and

software; total design under ₹1500. The platform was designed using inexpensive materials,

Perspex and aluminum sheet metal, controlled by an open source Arduino microcontroller, MPU-

6050, and three servos. Software was written with logic to convert the digital data from an

accelerometer to an acceleration magnitude vector. The magnitude was then compared to a

predetermined function to infer the angle of tilt of the platform. The angle of tilt is then converted

to angle of rotation for the servos to act on. Testing showed the platform to perform as expected.

Although some error on the final angle was expected, the magnitude of the error observed indicated

the platform design has a high sensitivity to low tolerance mechanical joints (slop). Overall the

platform design was validated based on the positional accuracy of the platform given the low

quality components used to create it. A few unintended behaviors were discovered during the

testing phase that limits the functionality of the platform. The platform exhibits a phenomenon the

author calls “motion tilt”. This phenomenon occurs when the platform is exposed to acceleration

other than gravity. This behavior can be remedied by implementing other accelerometers to detect

such movements and a software update with logic used to ignore “in plane” motion.

The platform was also found to have some lag between the readings and the tilt correction. This

behavior is not easily corrected. It is due a number of variables like the latency of the servos and

the accelerometer reading averaging function. However, the success criterion for the platform

design was the actual correction of the tilt angle, not the speed at which it was corrected. There are

many performance improvement opportunities that can be implemented with simple modifications

to hardware and software. Further development opportunities include the control of other degrees

of freedom, such as yaw, cancellation of all non-gravitational acceleration to reduce the effects of

motion tilt, and use of digital servos with metal gears will improve the latency and reaction time

of the platform.

40

REFERENCES

Books

1. McRoberts, Michael. Beginning Arduino. New York: Apress, 2010. Print.

2. Banzi, Massimo. Getting Started with Arduino. Sebastopol, CA: O'Reilly, 2009.

3. Margolis, Michael. Arduino Cookbook. Sebastopol, CA: O'Reilly, 2011.

4. Analog Devices Inc. "ADXL322 Datasheet." Small and Thin +- 2g Accelerometer.

5. Norwood: Analog Devices, 2007.

Website Links

1. Auto-Leveling Platform – St. Mary’s University

http://engineering.stmarytx.edu/~nechon/

2. Devry New Brunswick

http://www.youtube.com/watch?v=f9ALAvE3gBQ

3. Self -leveling surface with Arduino

http://www.youtube.com/watch?v=cTUBDagKdbA&feature=related

4. Self-Leveling Platform

http://www.me.berkeley.edu/ME102/Past_Proj/f08/group_09/intro_objective.html

5. Self-leveling platform

http://www.youtube.com/watch?v=TiTRUwU7kRs

6. ECE572SelfLevelingPlatform

http://www.youtube.com/watch?v=CuN_ZkLK0gM&NR=1

7. Stewart Platform University of Adger, Norway

http://www.youtube.com/watch?v=WmKnnp1xTPg&NR=1

8. Self-leveling platform for control of autonomous lawn mower

http://www.dspace.de/ftp/papers/uc/cheok.pdf

9. Proposed self-leveling controlled platform

http://www.youtube.com/watch?v=vmf1ThwrNM0

10. Hitec Servo Manual. Tech. Hitec RCD. Web. 10 June 2011.

http://www.hitecrcd.com/files/Servomanual.pdf

11. Arduino Uno. May 2011. July 2011

http://arduino.cc/en/Main/ArduinoBoardUno

41

12. "Atmel ATMega328 Datasheet." Atmel Inc. Web. 06 May 2011.

http://www.atmel.com/dyn/resources/prod_documents/doc8161.pdf

13. "Reference Section." Arduino - HomePage. 11 Mar. 2011. Web. 26 May 2011.

http://www.arduino.cc

14. "Accelerometer and Gyro Buying Guide." SparkFun Electronics. 12 July 2010.

http://www.sparkfun.com

15. Lynx Motion Inc.

http://www.lynxmotion.com