Prepared by: Faculty Advisersmontoya.sdsmt.edu/reu/2014/Del_Grosso_Final_Report_2014.pdfposition of the pendulum and accept motor commands from a PC running a controller. While that

Implementation of a Linear Inverted Pendulum System for University Laboratory

Instruction and System Identification Research

Prepared by:

Adrian Del Grosso

Faculty Advisers:

Dr. Charles Tolle

Associate Professor, Department of Electrical and Computer Engineering

Dr. Alfred Boysen

Professor, Department of Humanities

Research Experience for Undergraduates

Summer 2014

South Dakota School of Mines and Technology

501 E Saint Joseph Street

Rapid City, SD 57701

1

Table of Contents

Abstract . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.0 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.0 Broader Impact . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

3.0 Procedure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

3.1 Adapt Board Stack to Drive Motor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

3.2 Write Software to Drive the Motor and Provide Location and Rotational Feedback . . . . . . 7

3.3 Write Software for PC Controller for Motor Commands . . . . . . . . . . . . . . . . . . . . . 10

3.4 Determine Linear Approximation of the System Using System Identification . . . . . . . . . 11

4.0 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

4.1 Embedded Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

4.2 PC Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

4.3 System Identification . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

5.0 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

6.0 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

6.1 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

6.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

7.0 Appendix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

7.1 Hardware Configuration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

7.2 Block Settings . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

7.2.1 USB Transmit UART Settings . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

7.2.2 USB UART6 Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

7.2.3 Motor Controller Serial UART3 Setup . . . . . . . . . . . . . . . . . . . . . . . 17

7.3 Code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

7.3.1 Decoder Read Code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

Acknowledgments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

2

Abstract

Linear inverted pendulum systems are often used for controls research because they are well understood, easy

to take measurements from, and are very nonlinear from a mathematics perspective. In addition, they can

represent other systems, which allows new control techniques, such as nonlinear control theories, to be tested on

such systems before moving to larger and more complex systems.

In addition, examples with linear inverted pendulums can be found in any controls engineering or control

theory textbook. To allow the study this classic example both at the university level, and for the Office of Naval

Research, a belt-driven linear inverted pendulum system will be implemented. The system will use hardware-

in-the loop and will use full-state feedback. It will be capable of reading position, angle, how they change with

respect to time, and will apply corrections to itself according to that data. The system dynamics will be identified

using system identification, and tested using a linear approximation of the system’s dynamics. This will prove

the functionality of the software and hardware that comprise the system.

1.0 Introduction

1.1 Background

Inverted pendulums can be found as an example in nearly every controls engineering textbook. The common

example features the inverted pendulum under a moving cart similar to the example shown below (figure 1).

Figure 1: An Inverted Pendulum Example

In controls engineering, control solutions are often compared to inverted pendulum systems for efficiency due

to their relatively simple and well known characteristics, and small number of states. Therefore, since much of the

3

field of controls engineering uses linear pendulums as a comparison for other systems, students going into a field

that requires controls should be familiar with these systems. Unfortunately, the costs of well characterized linear

pendulum systems for educational purposes are restrictive for many institutions. This is one of the principle

reasons why this project was undertaken. Being able to make a linear inverted pendulum for an extremely small

fraction of their commercial cost would benefit SDSM&T’s students, and possibly many students from other

universities as well. In addition, the system could be used as a platform for cutting-edge controls research on

non-linear systems for use by graduate students and professors. Some of the research being done by professors like

SDSM&T’s Dr. Charles Tolle studies nonlinear system identification. An inverted linear pendulum provides an

excellent platform on which experiments can be performed for this research due to the nonlinear characteristics

of the system.

The physical frame of the linear inverted pendulum and the control board stack that were used in this project

were already mostly completed when this project started. The physical design of this frame includes an 80/20

structure, a 12-volt Pittman motor, a 4:1 planetary gearbox, a moving 80/20 carriage, an all-thread pendulum

arm, two quadrature encoders, 3D-printed end-stops with motor disconnects, and a belt that is driven by two

plastic pulley wheels and attached to the cart. Because much of this was already in place at the start of the

project, the main goal of the project is software oriented: perfecting both the embedded software that drives the

pendulum, and perfecting the software for a PC to communicate with the embedded system and take data.

Figure 2: The Inverted Pendulum System

4

2.0 Broader Impact

Inverted pendulum systems can be representative of other, more complex systems, such as rockets trying

to stay vertical during their ascent. Some institutions, such as MIT, have even done research on how inverted

pendulums can be used to model how people walk (Kuo, Donelan, Ruina). Because of this, human beings are

arguably the most common example of inverted pendulums. By being able to model the process by which people

walk, it becomes possible to mimic it using software and machines. Understanding inverted pendulums could

allow us to create robots or prosthetic devices that can walk like humans do. Additionally, technology like this

can be found being used in products such as the Segway, where the person riding the Segway and the control

arm act as the pendulum, and the base acts as the cart. See example in figure 1.

3.0 Procedure

3.1 Adapt Board Stack to Drive Motor

The most important part of getting the pendulum system working is the control board stack. The stack

being used for this project was designed originally by Dr. Tolle, and has gone through several iterations over

several years. It includes an ARM powered STM32f4 Discover board for processing, a quadrature decoder, USB

connectivity, a motor controller, common power bus system, and power supply. The eventual goal of the motor

board stack is to run not just this experiment, but many others in the future without having to manufacture

unique boards for each experiment. Similar systems can cost several thousand dollars, but this custom stack can

be built for less than a thousand dollars. This is the first experiment to utilize the board stack however, so the

software to run the board stack had to be mostly pioneered from little or no existing software. In addition, the

subtleties of sampling at the correct rates for the hardware and the optimal way to connect all the devices had

to be determined as the project progressed.

The first thing that had to be done to get the motor stack to read data from the pendulum via its quadrature

encoders was getting USB communication working. The board had to be able to send the data back to a PC that

could interpret it. This is done through a daughter board with an FTDI chip on it. Since I was new to Matlab

at the outset of the program, this and other basic concepts for the Discover board were figured out by trial and

5

Figure 3: The Custom Board Stack for the Project

error more than anything else. Through the resolution of various bugs, it was found that USB communication

needs binary headers and footers to terminate a set of data. Without them, data has a tendency to get lost or

misinterpreted. In addition, the USB serial port latency needs to be changed from the computer’s default setting

to as low as possible to enable high-speed data transferal by giving the port a higher CPU priority.

After USB communication was established and the latency issues were fixed, the next priority for the board

was the encoder system. The pendulum has two encoders. One is on the motor, and provides location information.

The other is on the pendulum itself, and provides angle (rotational) information. The decoder in the board stack

is not a daughter board to the Discovery board, and was not made to work directly with the Discovery board.

Therefore, a custom code block was used in Simulink which was written in C, and handles the actual low-level

reading of the decoder. It was written by Dr. Tolle, and required minimal editing to function. The block works

by taking two inputs. The first input, specifies which encoder to read from, and the second input is an active

high reset. Additionally, the encoders have an index channel that allows the decoder to know when they have

made a full rotation, and subsequently resets the encoder to zero. Because the project requires absolute encoding

where the encoder counts are consistent around multiple revolutions, it was found that the index channel of the

encoders needed to be connected to ground to prevent the encoder from being reset. The datasheets allow the

inputs to float, but in practice they experience unpredictable behavior if left un-grounded.

The last part of the software portion’s initial setup was figuring out how to interface with the motor driver.

6

The board stack uses a RoboClaw 2x30a motor driver to drive the motor. It uses packet serial commands that

include the address of the motor, the motor command, a scaling value for the command, and a checksum. Much

of the logic behind programming this functionality in Simulink had already been done by a graduate student,

and I was able to use much of that code as an example.

It was decided to implement a solenoid at the top of the pendulum’s swing. This was done so that the

pendulum could be held at different angles for varied sets of data. In system identification, the initial conditions

have to be varied, and optimally, only one at a time. This solenoid was implemented to allow that to happen

easily while still being allowed to zero the encoder values with the pendulum straight downwards. The solenoid

consists of 14 gauge magnet wire wrapped around an iron core. The unit is encased in a 3d-printed holder, and

was attached to the 80/20 frame of the pendulum system. Tests performed on the solenoid for heat production

and magnetic field strength yielded that 2.5 amps of continuous current through the solenoid was safe, and would

not melt the coating on the wires, but would maintain enough field strength to hold the pendulum at various

angles. After all these modifications and initial setup tasks were completed, the majority of the work became

writing the actual software for the board.

3.2 Write Software to Drive the Motor and Provide Location and Rotational Feedback

The overall goal of the software that would be running on the Discover board was to take data on the angle and

position of the pendulum and accept motor commands from a PC running a controller. While that sounds fairly

simple, it involves time synchronization, custom C code, unit conversions, and gets complicated fairly quickly.

This section will walk though how this software was developed and how it works.

As alluded to in section 3.1, the manufacturer of the STM32f4 Discover board makes a blockset for Matlab

and Simulink. It allows programmers to use a graphical method for programming the board stack. The resulting

program then compiles down to C code for the ARM processor on the Discover board. In addition, the graphical

nature of Simulink allows a program to have some degree of self-documentation. The program follows an organized

flow chart of operations, which can be easily understood when compared to the C code the flow chart represents.

While some portions of the code still had to be written in C (see section 3.1), most of the program was done

using the blockset.

For the the first main goal, taking data, the program required two decoder read blocks. These were the

blocks that had custom C code in them. I decided I should implement these first. I placed one block for each

encoder and decided to set the sample rate of the blocks to 100Hz originally and I could adjust as needed. The

quadrature decoder chip sends back a 32 bit value for each encoder as four unsigned 8 bit numbers. The four 8

7

bit outputs of those blocks then had to be combined into a single value. Using bit-wise operators, several blocks

shifted the 32 bits into a single signed word.

Figure 4: Decoder-reading Simulink Code

After the values were in a single word, it became much easier to translate that value in to a number with

meaningful units. The encoders measured in raw counts. Those values ranged from 0 to 2048 for the rotational

encoder, and 0 to 1024 for the motor encoder. These were converted into radians and inches respectively.

In addition, the numbers are converted into doubles, that way the fractional portion of the new units aren’t

truncated. A unit-delay was also instituted to provide a rough derivative approximation for both encoders (θ

and x). It works by taking the new double, storing it for one sample, and subtracting it from the new value.

Then, it’s divided by the time-step. This forms the equations: θ = ∆θ∆t and x = ∆x

∆t . This was intended to provide

rough data that could be used to compare other derivative methods against, or could be smoothed and used in

system identification. These four values were then passed to the transmit block for USART6 on the Discover

board, which corresponds to the USB daughter card (Figure 4).

The portion of the code that interfaces with the motor controller consists of a block to receive a motor

command from the computer via USB. This signed 16 bit command is then split into two 8 bit commands to

be compatible with the motor controller’s protocol. These 8 bit commands, preceded by the controller’s address

and drive command, are then sent through a USART using packet serial to the motor controller board. These

8

Figure 5: Data Conversion and Derivative Approximation Code

commands are then succeeded by a checksum, which consists of all the commands added together, and masked

with 0x7F using a bitwise AND function. The USART Tx block is set for blocking transfer in binary mode, and

uses binary headers and footers to separate different sets of data. Without these headers and footers, data has

a tendency to get lost. This was the cause of many bugs early on in the project, as data would seem to ”float”

between the correct value and zero resulting in unpredictable behavior.

For data-taking and debugging purposes, code was needed to control the solenoid, and to synchronize the

pendulum’s release with the start of the motor commands. While not included in the final program, it proved

invaluable for getting data. Figure 6 shows the code used to control the solenoid. The solenoid was connected

to the second motor’s outputs on the motor controller board, allowing for very accurate control and the reuse of

the motor-driving code. The difference in the code is that the solenoid uses a clock and switch to compare the

current up-time of the board to a set delay value. It turns on and remains active until the current time on the

9

board exceeds the set delay, after which the solenoid deactivates. Also, the command telling the motor controller

board which command to execute is also different to indicate motor 2 instead of motor 1.

Figure 6: Motor Driving Code

Figure 7: Sollenoid Code

3.3 Write Software for PC Controller for Motor Commands

The software that runs on the PC for hardware-in-the loop implementation has to be able to communicate

with the system before anything can be accomplished. This communication was established using the same USB

connection that data gets read from.

10

3.4 Determine Linear Approximation of the System Using System Identification

To make sure the system functions well, it was decided to test the system using a linear approximation of the

system’s dynamics, linearized about the topmost point of the pendulum’s swing (pi radians). With the linearized

dynamics, we could then get the pendulum to balance. To obtain the dynamics of the system, the following

formula was used: X = A+B, where A+ is the Moore-Penrose pseudoinverse of A.

In order to gather the information required to populate the A and B matrices, multiple sets of data were

taken. The data consisted of the pendulum swinging through pi radians ± 10 percent of the circumference of

the pendulum. The code for taking this data is shown in figure 8.

Figure 8: Simulink code for Reading Data

In order to test the approximation that was determined, the system was simulated in MATLAB using a

state-space equation (see figure 11). Values were driven into the system to determine if the output in the region

the system was linearized about was similar to the actual response of the system.

11

4.0 Results

4.1 Embedded Software

The embedded software has been fully implemented, and can be sent motor commands from a PC, and responds

accordingly. The system communicates successfully using two-way USB communication, and communicates with

the RoboClaw motor controller board with packet serial commands. Additionally, the encoders can be successfully

read, converted into meaningful units, and be transmitted. In addition, the embedded software sends back all

states of the system for use in full state feedback control systems. The data that the pendulum system sends to

the PC can be seen in figure 9.

Figure 9: Data From System When Driven by Sine Wave

4.2 PC Software

While the actual PC software will eventually be the responsibility of whomever designs the labs and experiments,

test programs such as the one in Figure 7 reveal the data that the boards are sending back can be successfully

12

read by the PC, and can be stored. Additionally, The PC can issue motor commands over the USB connection.

4.3 System Identification

Using data collected from the system within ten percent of the top of the pendulum’s swing, a preliminary

model of the system’s dynamics was created. This model can be found in figure 10 (equation 1). While simulation

of the pendulum system found in figure 11 found the model to have some inaccuracies, these errors stemmed

from the system identification procedures and not the system itself. It can be concluded that because system

identification could be performed on the system that it is operating correctly.

−0.0379 −6.0406 0.0001 −0.01550.5991 −28.7087 −0.0028 −0.69400.0369 5.1840 0.9997 −0.01880.0163 6.5953 0.1096 10.2341

x+

−0.0080−0.5150−0.0001−0.0140

u (1)

Figure 10: Preliminary System Model

Figure 11: Simulink Simulation of System

5.0 Discussion

In addition to the results of the project, the total cost of the system was around $1k. This is well below

the costs of some of the commercially available inverted pendulum systems. This could be a major selling point

for using these systems in laboratory settings.

13

6.0 Conclusion

6.1 Summary

At the conclusion of this program, the system has been implemented. The total cost of the parts for the project

totaled less than $2k, meeting the cost goal of the project. Additionally, the board stack has been proven as a

viable platform for other experiments based on its success in this project.

6.2 Future Work

This experiment was designed with future work in mind. With the board stack working and a program to base

future work off of, this inverted pendulum system will become a platform for research and student laboratory

assignments. As part of the funding from the Office of Naval Research, Dr. Tolle plans to use this device, as well

as others, to do controls experiments and system identification experiments on a nonlinear system. Nonlinear

system identification is cutting edge research at the time this paper is being written, and has huge potential to

revolutionize control systems. Understanding nonlinear systems could eventually lead to controls that can adapt

to changing dynamics, and correct for problems that the designers of a plant had never even begun to consider.

7.0 Appendix

This appendix contains reference material for additional information on the code and processes in this project

so that it may be reproduced for future projects.

14

7.1 Hardware Configuration

7.2 Block Settings

7.2.1 USB Transmit UART Settings

Figure 12: Settings for USB Transmit Block on Discovery Board

15

7.2.2 USB UART6 Setup

Figure 13: Configuration of UART Setup Block for USB

16

7.2.3 Motor Controller Serial UART3 Setup

Figure 14: Data From System When Driven by Sine Wave

7.3 Code

7.3.1 Decoder Read Code

#include"stm32f4_io_decread.h"

/*

Supported signal data types are;

- double

- single

- uint32

- int32

- uint16

- int16

- uint8

- int8

- boolean

*/

17

void enable_decread(void)

{

GPIO_InitTypeDef GPIO_InitStruct;

RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);

GPIO_InitStruct.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_10 | GPIO_Pin_11| GPIO_Pin_12|

GPIO_Pin_13 | GPIO_Pin_14;

GPIO_InitStruct.GPIO_Mode = GPIO_Mode_OUT;

GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;

GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;

GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;

GPIO_Init(GPIOD, &GPIO_InitStruct);

RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);

GPIO_InitStruct.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_9|GPIO_Pin_10 | GPIO_Pin_11| GPIO_Pin_12|

GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;

GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN;




GPIO_Init(GPIOE, &GPIO_InitStruct);

RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);

GPIO_InitStruct.GPIO_Pin = GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;

GPIO_InitStruct.GPIO_Mode = GPIO_Mode_OUT;




GPIO_Init(GPIOC, &GPIO_InitStruct);

void disable_decread(void)

{

18

// do nothing

}

void output_decread(double in1,double in2 ,uint8_t *out1, uint8_t *out2, uint8_t *out3, uint8_t

*out4)

{

#define d 1

uint16_t *rbyte1, *rbyte2;

// measure the length of our subroutine

GPIO_SetBits(GPIOD, GPIO_Pin_14);

// set the board address and turn on the output enable:

GPIO_ResetBits(GPIOC, GPIO_Pin_13|GPIO_Pin_14|GPIO_Pin_15); //select address

GPIO_SetBits(GPIOD, GPIO_Pin_12); //set D12(high) ..enable 3 to 8 decoder

SysTimer_delay_us(d);

/******select encoder x or y********/

if(in1 == 0)

{

GPIO_ResetBits(GPIOD, GPIO_Pin_13); //select encoder x

}

else

{

GPIO_SetBits(GPIOD, GPIO_Pin_13); //select encoder y

}


//reset decoder

if(in2 == 1)

{

GPIO_ResetBits(GPIOD, GPIO_Pin_0|GPIO_Pin_1);

SysTimer_delay_us(1);

19

GPIO_SetBits(GPIOD, GPIO_Pin_0|GPIO_Pin_1);


GPIO_ResetBits(GPIOD, GPIO_Pin_0|GPIO_Pin_1);


}

//Select MSB

GPIO_ResetBits(GPIOD, GPIO_Pin_10);


*rbyte1 = GPIOE->IDR;



while (*rbyte1 != *rbyte2)

{




}

*out1 = (uint8_t) (*rbyte1 >> 8);

//select 3rd byte


//GPIO_ResetBits(GPIOD, GPIO_Pin_10); // test reload top byte with a different code style


*rbyte1 = ((GPIOE->IDR & 0xFF00) >> 8);




{




}

20

*out2 = *rbyte1;

//select 2nd byte







{




}

*out3 = *rbyte1;

//select LSB







{




}

*out4 = *rbyte1;//*out4 = 8;

GPIO_ResetBits(GPIOD, GPIO_Pin_12); //set D12(high) ..enable 3 to 8 decoder


}

21

Acknowledgments

The funding for this project came from the Office of Naval Research grant number N00014-12-1-0347. Thanks

to the National Science Foundation for allowing this research to happen jointly with both organizations under

grant number EEC-1359476, and for welcoming me into the REU program. Special thanks to Dr. Charles

Tolle for his guidance and direction during this project, as well as Dr. Alfred Boysen for his help with writing,

speaking, and professional development. Lastly, thanks to SDSM&T for hosting this REU program, as well as

this project. Special thanks to the other REU students who shared the controls lab with this project.

References

Burden, R. L., & Faires, J. D. (2005). Numerical analysis (8th ed.). Boston: PWS-Kent Pub. Co.

Friedland, B. (1996). Advanced control system design. Englewood Cliffs, N.J.: Prentice Hall.

Ljung, L. (2009). System identification: theory for the user (2. Aufl. 11. print. ed.). Upper Saddle River, NJ:

Prentice-Hall.

Boubaker, O. (2012). The inverted Pendulum: A fundamental Benchmark in Control Theory and Robotics.

Tunis Cedex, Tunisia: National Institute of Applied Sciences and Technology.

Kuo, A., Donelan, M., & Ruina, A. Energetic Consequences of Walking Like an Inverted Pendulum:

Step-to-Step Transitions. : Departments of Mechanical Engineering & Biomedical Engineering, University of

Michigan, Ann Arbor, MI; 2 School of Kinesiology, Simon Fraser University, Burnaby, BC, Canada; and 3

Department of Theoretical & Applied Mechanics, Cornell University, Ithaca, NY.

22

ST. (n.d.). M32F4 Series. Retrieved July 28, 2014, from

http://www.st.com/web/en/catalog/mmc/FM141/SC1169/SS1577?sc=stm32f4

Lima, Jose, & Gon calves, Jose. (2006, January 1). Inverted Pendulum Virtual Control Laboratory. .

Retrieved July 29, 2014, from https://bibliotecadigital.ipb.pt/bitstream/10198/1907/1/inverted

23

Documents

Prepared by: Faculty Advisersmontoya.sdsmt.edu/reu/2014/Del_Grosso_Final_Report_2014.pdfposition of the pendulum and accept motor commands from a PC running a controller. While that