Soft Tools for Robotics and Controls Implementations

A Thesis

Submitted to the Faculty

Drexel University

Robert M. Sherbert

in partial fulﬁllment of the

requirements for the degree

Master of Science in Electrical Engineering

Dedications

To the parents who placed me on my path, to the mentors who guided me along its many turns, and to the friends who made the long journey swift. iii

Acknowledgments

There are a number of people to whom I owe a great deal of thanks in completing this document. While the labor has been my own, the inspiration for it and the support to ﬁnish it have come from the community around me. In creating this work I have taken on the role of toolsmith and, as tools are worthless without their users, it is to these individuals that I am especially indebted.

I would like to thank Dr. Oh for lending his vision of robotics testing and prototyping which inspired this work. You have taught me more than I realized there was to know about the modern practice of science. I would also like to thank Dr. Chmielewski for lending his experience, insight, and enthusiasm to the project.

Having these ideas weighed against and improved by your practical knowledge has provided a very important validation for me.

Above all I would like to thank my friends at DASL, without whom the entirety of this project would have been consigned to the dust bin long ago. You have given me not only critical feedback and suggestions but also the support and encouragement that has helped me carry this to completion. I could not have done this without you.

Lastly, I thank my parents. Without you I never would have started, much less ﬁnished.

I do not yet know where life will take me, but I do know that because of all of you I leave this place a better person than I came. Thank you. iv

Table of Contents

LIST OF FIGURES ...... vii

ABSTRACT ...... ix

1. INTRODUCTION ...... 1

1.1 Review of Literature ...... 3

1.2 ProposedSolution ...... 5

1.2.1 Conductor ...... 6

1.2.2 Score ...... 7

2. CONDUCTOR: DESIGN ...... 9

2.1 ConceptofOperations ...... 9

2.2 FrameworkOverview ...... 9

2.3 ControlComponents ...... 12

2.3.1 State ...... 12

2.3.2 Controller ...... 14

2.4 Communication Components ...... 15

2.4.1 Device ...... 15

2.4.2 Protocol ...... 16

2.4.3 Hardware ...... 18

2.5 Interfacingwith Conductor ...... 19

2.5.1 InternalAccess ...... 19

2.5.2 ExternalAccess ...... 20

3. CONDUCTOR: TESTING/RESULTS ...... 21

3.1 Stress Testing ...... 21

3.2 Example/Implementation ...... 22

3.2.1 HighDegreeofFreedomSystems ...... 23 v

3.2.2 Portability ...... 26

3.3 Summary ...... 26

4. SCORE: DESIGN ...... 29

4.1 ConceptofOperations ...... 29

4.2 Systems Architecture ...... 29

4.2.1 Overview ...... 29

4.2.2 HardwareAuto-Conﬁguration ...... 30

4.2.3 SymbolicExpressionManager ...... 34

4.2.4 SystemInput/Output ...... 37

5. SCORE: TESTING/RESULTS ...... 39

5.1 Hardware Auto Conﬁguration Testing ...... 39

5.1.1 ADCAuto-Conﬁguration ...... 39

5.1.2 PWMAuto-Conﬁguration ...... 39

5.2 Time Domain Controller Testing ...... 43

5.2.1 ProportionalResponseTest ...... 43

5.2.2 IntegralResponseTest ...... 46

5.2.3 WindupIntegrator ...... 47

5.2.4 Derivative Response Test ...... 49

5.2.5 Integrated System Test ...... 54

5.3 Summary ...... 61

6. CONCLUSIONS ...... 62

6.1 Results fromConductor ...... 62

6.2 ResultsfromScore ...... 63

6.3 Contributions ...... 64

6.4 Future Possibilities ...... 64

BIBLIOGRAPHY ...... 65

APPENDIX A: DCMOTOR MODEL PARAMETERS ...... 67

TABLE OF CONTENTS TABLE OF CONTENTS vi

APPENDIX B: PRIMER ON RT SYSTEMS & FREERTOS ...... 68

B.1 Function/Purpose ...... 68

B.2 Concepts/Terminology ...... 69

B.3 Types of RT Systems ...... 70

B.3.1 RTSystems onMicrocontrollers ...... 70

B.3.2 RTSystemsonx86Computers ...... 70

B.4 FreeRTOS ...... 71

B.4.1 Tasks ...... 71

B.4.2 Inter-Task Communication ...... 72 vii

List of Figures

2.1 Schematic diagram of data ﬂow through the Conductor system ...... 9

2.2 Schematic diagram of the State’s functionality ...... 12

2.3 Schematic diagram of the Device’s functionality ...... 15

2.4 Schematic diagram of the Protocol’s downstream functionality ...... 16

2.5 Schematic diagram of the Protocol’s upstream functionality ...... 17

2.6 Example of Conductor log ﬁles ...... 18

3.1 Conductor stress testing results ...... 22

3.2 Example Conductor node arrangement ...... 23

3.3 The code to realize the motor arrangement in Figure 3.2 ...... 24

3.4 Example Conductor/Orocos PID controller ...... 24

3.5 Example Conductor-Matlab interface code ...... 25

3.6 Walking trajectory time progression with virtual mini-Hubo ...... 26

3.7 The mini-Hubo climbing stairs while driven by Conductor ...... 27

3.8 The three test systems for Conductor performing the box task ...... 28

4.1 Score system architecture ...... 30

4.2 Testing ﬂowchart for score ...... 32

4.3 Data structure for the ADC auto-conﬁguration code ...... 33

4.4 A graphical depiction of an example system name table...... 37

4.5 Example score script ﬁle ...... 38

5.1 Initializing an ADC with Score ...... 40

5.2 PWM generator testing: conﬁg ﬁles ...... 41

5.3 PWM generator testing: osilliscope traces ...... 42

5.4 Score code for re-sampling ...... 43

5.5 Proportional test error ...... 44 viii

5.6 Session to conﬁgure proportional test ...... 45

5.7 Testing the proportional response ...... 46

5.8 Error between the Score integration of sint and the ideal − cos t ...... 47

5.9 Session to conﬁgure integral test ...... 48

5.10 Hardware testing of the integral feature ...... 49

5.11 Topology of a simple anti-windup technique attached to a PI controller ...... 49

5.12 Testing the anti-windup shown in Figure 5.11...... 50

5.13 Code for testing the anti-windup ...... 50

5.14 Error between the Score derivative of sint and the ideal − cos t ...... 51

5.15 Score code for integration and differentiation tests ...... 51

5.16 Session to conﬁgure derivative test ...... 52

5.17 Derivative response sample I/O, input is 120Hz 0.5V ramp ...... 52

5.18 Measured derivative (via PWM duty cycle) vs true ramp waveform derivative ...... 53

5.19 Simulink model of DC motor in servo conﬁguration ...... 54

5.20 Score code for PID system test ...... 55

5.21 Position error of the servo motor after addition of the PID controller ...... 56

5.22 Comparison of the Simulink and Score PID controllers ...... 57

5.23 Difference between Simulink and Score PID controllers ...... 57

5.24 System test circuit ...... 58

5.25 Code conﬁguring Score as a negative feedback controler ...... 59

5.26 Negative feedback controller testing ...... 60

LIST OF FIGURES LIST OF FIGURES ix

Abstract Soft Tools for Robotics and Controls Implementations Robert M. Sherbert Paul Y. Oh, Ph.D. and Thomas Chmielewski, Ph.D.

Engineers’ abilities to design and construct electro-mechanical systems have reached the point where humanoid, quadruped, hexapod, and other comparably complex robots are now physically realizable. This thesis argues that while the mathematical tools to control such systems exist, the software tools required to implement such controls are lacking. To fill this gap a state based software abstraction, in which the robot is conceptualized in terms of its mathematical states instead of in terms of the sensor/actuator properties, is proposed. To test the effectiveness of this representation, two programs have been created that adhere to its ideal. The first is a high level, system-of-systems approach that leverages existing robotics development tools to obtain a more intuitive interface through the state representation. The second is a low level tool that provides a similar representation for the peripheral collection on micro controllers. This document describes both the design of each system and the testing done to verify its technical correctness. Applications to large scale test systems such as the Hubo humanoid robot are also described. Additionally, scaling experiments between various sized humanoid robots are used to demonstrate the flexibility of the state based approach.

Pluralitas non est ponenda sine neccesitate. -Occam’s Razor

Chapter 1: Introduction

Capabilities for mechanical and electrical design in robotics have attained a level of mastery over physical principals. Likewise, the mathematics needed to realize controllers for increasingly complex systems have been developed. These advances have resulted in many impressive high degree of freedom robots including adult-sized humanoid robots such as the Hubo[1]. Equally complex are quadruped and hexapod designs such as Boston Dynamics’ Big Dog[2] and NASA’s ATHLETE[3]. Sadly, it is the case that the software systems driving all of these robots are point solutions. There is no software that is currently intuitive enough to drive high degree of freedom robots in the general case. This thesis examines a state based approach to robotic software design and demonstrates its applications to complex robotic systems.

The lack of capabilities for driving high degree of freedom machines is particularly obvious from a record of embarrassingly public controls and robotics failure that can be traced back to software. A simple Internet search reveals dozens of videos of the Asimo falling up and down stairs, off of platforms, and into crowds. A more high proﬁle failure involved the Mars Climate Orbiter, which disintegrated in the Martian atmosphere after a software unit mix-up led to incorrect course computations.

These failures are due in many respects to the lack of testing and prototyping abilities of the control- s/robotics community. In other engineering disciplines (aerospace, automotive, silicone wafer design) there is a division between the Design and Engineering (D&E), the small scale prototyping, Testing and Evaluation

(T&E), and Validation and Verification (V&V) of nearly finished prototypes. These distinct stages are the hallmark of an efficient and well tuned engineering establishment that can deliver products to market quickly and cheaply. In robotics, however, this cycle is not followed in part because of the lack of appropriate software tools. The V&V (scaled modeling) stage in particular is often ignored due difficulties associated with scaling. Robots are complex machines usually assembled from a variety of sensors and actuators from many different vendors. Switching between two hardware platforms (as is required by scaling) would necessitate a large deal of reprogramming to make the switch - and therefore is usually forgotten all-together.

This deﬁciency is clearly recognized by the community. A huge number of robotic development environ- 2 ments (RDEs) have been designed and implemented since the early 1990s. A trio of articles which survey the

field authored by Kramer[4], Mohamed[5], and Shakhimardanov [6] detail nine, fifteen, and eighteen tools respectively. A total of twenty-five unique RDEs and related tools are mentioned within the articles. Most of these are focused primarily on the creation of robotics middle-ware. These twenty-five are still only a small subset of the field with dozens more in existence. So far, no clearly dominant package has emerged and many offer similar features and benefits. This plurality makes it particularly difficult for the robotics community to develop standards and replicate results across platforms.

Despite the high level of development on these tools, it is important to note that most of these packagesare focused on alleviating strictly software concerns. The problems of interconnecting separate software modules, package management, and data organization are dealt with frequently. What is missing from most of these tools are convenience features for the controls engineer. Support for implementing the simplest control techniques (PID, state-feedback, etc) is rare. Support for modern techniques such as intelligent (neural networks, fuzzy logic, etc), optimal (linear quadratic regulators), or non-linear (feedback linearization) control is wholly absent.

This trend is contrary to the development of most complex technologies, where the usual pattern is that a capability and the tools used to build it move forward in tandem. Advances in the technology force the tools to increase in accuracy, speed, etc. These improvements to the tools then allow a better system to be built. Examples abound from the realm of software and computer technology. As the Internet has evolved, it has seen the development of things such as Structured Query Languages (SQL) and Ajax - tools highly specialized for web based operations. Likewise, as digital multimedia applications have become increasingly prevalent the development of Graphics Processing Units (GPUs) and Digital Signal Processors (DSPs) have greatly advanced. These processors are based on the single instruction multiple data (SIMD) paradigm that naturally expresses multimedia operations. The pattern to be drawn from these examples is that as the tools develop, facilities for performing the fundamental operations of that technology are integrated into them as atomic operations.

In controls, the fundamental operation one would expect to exist in a software tool is the ability to represent the controlled system in terms of states. State based modeling is the main mathematical technique used

CHAPTER 1: INTRODUCTION 3 by systems engineering, and it is reasonable to expect advanced software tools will possess the ability to use this representation. Unfortunately, there are no tools which can both a) use this form of representation and b) easily connect to hardware systems. It is the absence of this abstraction that deﬁnes the lag of software tools behind their mathematical and physical counterparts.

What a state representation would bring to a software system is best explained by example. Take the case of a servo motor - instead of interacting with the servo in terms of its provided Application Programming

Interface (API), the engineer will have access to a node θ within the software which provides ’get’ and ’set’ methods that control the peripheral device. By representing all sensors and actuators of the system in a similar fashion the engineer can focus on the controls equations while easily moving ideas between different platforms.

1.1 Review of Literature

Unfortunately, the state representation that would conveniently solve this problem is not available within the many tools that exist for robot design. This is true of both high level controllers in which the robot is treated as a system-of-systems (including Robotic Development Environments, LabVIEW, etc) and the embedded tools used to create individual sensors and actuators (real time kernels, embedded operating systems, etc).

The body of work that most readily identiﬁes itself with robotics software is that of the Robotics De- velopment Environments mentioned earlier. Among the numerous tools available are Player/Stage[7], the

Robot Operating System[8] (ROS), OpenRAVE[9], Microsoft Robotics Developer Studio (MRDS) [10], and

Webots. The tools in this category largely use what is called a ’component based representation’ in which a physical device such as a servo motor, distance sensor, laser scanner, etc is tied to a process or thread that exposes its important properties to the rest of the program. The form of this exposure is speciﬁc to the category of the device, though the speciﬁc categories change as a product of the tool. For example, Player/Stage groups sensors into categories such as ’laser’, ’camera’, or ’ranger’ (for IR sensors)[11]. In Webots actuators are grouped into ’servo’, ’differential wheel’, etc[12]. The MRDS platform uses groupings such as ’analog sensor’, ’camera’, ’sonar’, and ’motor’[10].

Another common approach seen in RDEs, usually complementing the component based organizational structure, is the driver library design pattern. Driver libraries are collections of peripheral speciﬁc code used

CHAPTER 1: INTRODUCTION 1.1 REVIEW OF LITERATURE 4 to bring a particular device into line with an RDE’s component model. The largest robotic driver library, with over three thousand entries, is attached to the ROS[8] project. Another notable archive is the Orca[13] library. While Orca is signiﬁcantly smaller than ROS, it is attached to Orocos[14] RDE, which posses real- time support (a rarity among PC based tools.) usually generated by combining large numbers of community written code, which can create problems in obtaining consistent quality and device feature coverage.

Obviously RDEs are not the only tools available for robot control on PC platforms. Industrial packages such as The Mathworks’ Matlab and National Instrument’s (NI) LabVIEW have features which can make them useful for such tasks. Simulation packages such as Matlab have excellent facilities for modeling controllers and physical systems using a signal flow design pattern. The signal flow pattern is similar to the state representation being sought, but without the critical ability to easily interface with hardware. There are facilities within Matlab for exporting controllers either directly to hardware (via Real Time Workshop) or to an intermediate C code stage. Unfortunately, in the former case only a few hardware platforms are supported and the technology is relatively expensive. In the latter case, while the logic for the controller is exported to code the user must provide all hardware interface code for the target platform. NI’s LabVIEW and DAQ line of devices have the opposite problem. They provide excellent hardware interfaces to most forms of analog and digital signaling though they provide software interfaces which are difficult to use for any type of complex task. In addition to any other flaws, both of these systems are computationally intensive (requiring substantial processing hardware) and can be prohibitively expensive.

So far, only high level control of robots as a system-of-systems approach has been discussed. Equally important is the domain of real time kernels which run on micro controller platforms and aid in the creation of individual sensor or actuator packages. Dozens of real time kernels exist, among them are FreeRTOS, eCOS, and BeRTOS. The majority of the kernels including FreeRTOS[15] provide only scheduling algorithms and cannot interface with the micro controller’s peripherals (ADCs, PWM generators, etc) excepting circumstances when the user adds additional, usually custom, code. Another group of kernels, which includes both eCOS[16] and BeRTOS, provides some minimal accessor methods for the peripheral hardware. Even in these cases, the accessor methods provide only instantaneous access to a peripheral and have no power to establish relationships between peripherals or other parts of the controller software.

CHAPTER 1: INTRODUCTION 1.1 REVIEW OF LITERATURE 5

The category of software that comes closest to the state representation envisioned is found in a somewhat unexpected place: sensor networks research. Sensor networks research has developed tools such as LiteOS and TinyOS. LiteOS provides a Unix-like interface through which a user can query and modify the sensors

(states) of the system[17]. TinyOS possesses something very similar to a state based representation - inputs and outputs can be tied to each other either directly or through an intermediate software processing layer[18].

In the TinyOS representation, facilitated by a custom event-based programming language called nesC[19], a triggering of sample acquisition can automatically be used to propagate updates to the system’s data structures further downstream.

While it is difﬁcult to prove a negative, the examination of the tools above coupled with a wide practical experience suggests there is no software package that supports a state based approach to robotic controllers.

The remainder of this thesis deals with the design, development, and testing of tools that provide state based representations in software for control systems.

1.2 Proposed Solution

Two programs have been designed which attempt to create a state based controls representation at distinct but equally important computational levels. The ﬁrst operates on PC-based hardware, at the system-of-systems level where a robot is viewed as a collection of discrete sensor and actuator units that each possess some type of internal processor. Within the software, the user reads from ’state’ nodes that represent physical quantities of interest on the robot. These could be joint angles, accelerometer values, etc. Set points can be written to corresponding states which then attempt to manipulate actuators in a way that achieves the condition speci-

ﬁed. The goal of the representation is to provide an easy way to implement dynamic controllers on unstable robotic systems. To accomplish this, the software extends existing an RDE tool called Orocos. Orocos provides a component framework, threading facilities, and thread safe data structures for inter-component communication. By building on top of this existing tool, repetition of labor is avoided while allowing the desired representation to be achieved as quickly as possible.

The second program creates a similar representation for micro controller hardware. Each of the micro controller’s peripherals (ADCs, PWM generators, IO pins, etc) are connected with a state node. State nodes can be directly connected to map in input terminal to an output, or can be interrelated through user deﬁned

CHAPTER 1: INTRODUCTION 1.2 PROPOSED SOLUTION 6 equations to create basic forms of control law. This level of operation is much better suited to high frequency fast response controllers that are close to the physical hardware they are controlling. As in the ﬁrst, the program is built on top of existing tools to maximize productivity. The application is designed on top of the

FreeRTOS real-time kernel to provide threading and context switching.

Between these two programs, an approach is realized which allows a user to port controls equations easily between machines with very different underlying hardware provided that the form remains roughly analogous. This, in turn, allows for the introduction of scaling between miniature and full sized systems.

Once scaling is possible, the V&V stage can be introduced into robotic development as it becomes possible to move between design, scale model, and full size prototype with relative ease.

1.2.1 Conductor

The ﬁrst program, called Conductor, arose as part of an attempt at the Drexel Autonomous Systems Labora- tory (DASL) to build a multi-platform controller system for humanoid robotics as part of the NSF’s Partner- ships for International Research and Education (PIRE): Humanoid’s project[20]. The goal of the effort was to develop a controller program which could run on simulated, adult-size, and miniature humanoid robots with minimal changes at the controller level. Humanoids are a particularly difﬁcult challenge in that they are inherently unstable within their regular operating mode and are relatively intricate machines. The Hubo robot, which serves as the main operating platform for the PIRE grant has 41 degrees of freedom. Porting such a complex controller, if coupled to the particular hardware in any way, would prove to be an incredibly daunting task. Instead, DASL opted to build Conductor, which allows creation of a state oriented view of the robot. Conductor provides a framework for creating abstractions written over existing peripheral APIs to create the state view of the robot.

Chapter 2 begins with a high level overview of the framework’s component structure and the techniques used to represent raw sensor data as state information. Each of the components in the framework are then described in detail. The names of the components are: State, Controller, Device, Protocol, and Hardware. Once each of the components have been detailed Chapter 3 goes on to show a characterization of the framework and an example case in which the framework is applied and used to control simulated and physical humanoid robots.

CHAPTER 1: INTRODUCTION 1.2 PROPOSED SOLUTION 7

Design Goals

1. State Representation of System Parameters - The designer of the control system should never have

to deal directly with a vendor API to interact with devices. Vendor APIs are hidden behind abstractions

which present critical system information in terms of states.

2. Time Domain Control - The user should be able to implement time domain controllers using the

system.

3. State Space Control - The user should be able to implement state space and frequency domain con-

trollers using the system.

4. Simple Add-On Interfaces - Addition additional modules (sensors/actuators) should be thought of as

a fundamental operation of the system. The framework should make this operation both convenient

and seamless.

5. High Degree of Freedom Control - The system should be able to scale easily to deal with as many

degrees of freedom are needed by the physical design.

1.2.2 Score

The second program, called Score, is designed to ﬁll the gaps left by Conductor. As Conductor is only capable of dealing with pre-existing robotic peripherals, it is somewhat at a loss for implementing simple feedback for an individual motors or for interfacing with bare analog sensors. Since off the shelf components are not always available to satisfy a robot’s design speciﬁcation, it is is often necessary to develop custom embedded systems to drive them. This development usually occurs at a rapid pace, and can range from motor controllers with particular characteristics to custom analog sensors. Carrying out this operation in traditional fashion can lead to some very complicated micro controller code bases often involving complex interrupt systems. The program developed to deal with this lower level problem, called Score, approaches the robotics implementation issue from the bottom up by providing the same type of state focused representation seen in Conductor. The Score system, however, focuses on doing this for micro controller peripherals on an individual chip as opposed to with an entire system.

CHAPTER 1: INTRODUCTION 1.2 PROPOSED SOLUTION 8

Chapters 4 and 5 detail the development of a ﬁrst draft of Score, an embedded design prototyping system.

Score is an embedded controller and simple scripting language which attempts to reduce the difﬁculty and complexity of implementing simple sensors and actuators using micro controllers.

Design Goals

The eventual goal of Score is to provide a control systems prototyping package with the following features:

1. Dynamically Conﬁgurable Hardware - The user should never need to touch kernel internals or

microprocessor registers in order to obtain the desired functionality from the device. Simple auto-

conﬁguration commands should be available for all common conﬁguration patterns.

2. Signal Based Programming - The values of variables are tied to the changes in the underlying system

properties through continuous sampling and recalculation.

3. Automatically Conﬁgured Sampling Frequencies - The controller should manage conﬁguring ap-

propriate sampling frequencies, based on requests made by the user.

4. Support for Time and Frequency Domain Controllers

5. Hard Real Time - Once conﬁgured, the refreshing of system state and equations should happen in a

deterministic manner.

6. Hardware Abstraction Layer - The system should be portable between micro controller and computer

architectures.

7. Cost - Most importantly, the ﬁnal Score controller should be relatively inexpensive to produce.

CHAPTER 1: INTRODUCTION 1.2 PROPOSED SOLUTION 9

Chapter 2: Conductor: Design

2.1 Concept of Operations

Conductor provides a programming framework with a speciﬁc upper level abstraction: states. At the up- permost level of the system, a quantity of interest is presented to the user as a single state with get or set operations as appropriate. A single device, such as a motor, can be realized at this level by multiple states

(position, torque, etc). The user through a console interface, or controller programs written by the user, can interact with the states to acquire information about the system or issue commands to its various components.

External controllers can be directly compiled into the system, run through an included scripting language, or interact with Conductor through a simple protocol over UDP. In addition to the upper level interfaces, the framework provides special emphasis on integrating new devices by providing appropriate hooks and overrides.

2.2 Framework Overview

The Conductor framework provides two main sections using a ﬁve layered component based design. The

ﬁrst is the control section. Constituted by the upper two layers of the program, the control section provides

Figure 2.1: Schematic diagram of data ﬂow through the Conductor system: Data enters at either the State or Hardware level traveling downstream or upstream respectively. At each level the requests are cached and concentrated as appropriate to make efﬁcient usage of bus resources. Each component in the system is tagged with its section number in the document, for easy reference. 10

Conductor’s ability to abstract system hardware in terms of components called ’States’. The States mirror as closely as possible the concept of state variables from control theory. The second section, called the communications section, is formed by the three remaining layers. It is a communication stack designed to ease common implementation problems when using robotics hardware. These features are enabled by the threaded real time (RT) architecture and publish/subscribe model upon which Conductor is built. The particular layered design and data ﬂow through the system provides convenient ways to implement common tasks along with the extensibility to handle unusual cases.1

The components making up the control section, known as the State and Controller, attempt to provide a bridge between the mathematical design of robot controllers and their implementation within the computer.

When an engineer designs a controller mathematically, concern is only with the physical principles at work in the robot’s environment, its sensors, and its actuators. When the controller implementation is made within the computer, however, the engineer must also consider numerous aspects of the computation itself. The

Controller and State components alleviate some of these concerns. The State presents to the user a software component which embodies a single physical quantity such as a position, velocity, acceleration, current, etc, independent of any hardware interaction concerns required to obtain the data. It automatically communicates with the lower layers to ensure that its current data state reflects the actual sensor value. Additionally, it can provide quantities which are mathematically, though not computationally, easy to access such as a time history, integrals/derivatives, filtered versions of the signal, transform methods, etc. The Controller component provides a platform from which the program’s States can be accessed and from which the user can define a control algorithm. The user can author a mathematical controller within the software Controller component using notation very similar to that used in the scientific literature.

The components making up the communications section, known as Device, Protocol, and Hardware, provide methods to easily interact with robotics hardware and help to facilitate usage of the control components.

Robotics hardware (sensors and actuators) are often stand alone devices which exist separately from the control unit and must be accessed through some bus. The communication components are designed to mirror this topology. The Device provides a software stand in for the individual node on the bus; this node could be

1Through the rest of the document a number of software components are discussed which share names with common concepts in engineering. Where there is a reuse of a generic name, the software speciﬁc component is capitalized, while the common usage is left as usual. E.g. ”State” (software component) vs. ”state” (control theory variable).

CHAPTER 2: CONDUCTOR: DESIGN 2.2 FRAMEWORK OVERVIEW 11 a motor controller, accelerometer, etc. The Protocol is a representation for whatever communication format the manufacturer has speciﬁed; this could be a completely custom packet format or a simple scaling function for an analog-to-digital converter. The Hardware component provides the interface between the Conductor framework and the operating system; its primary function is as a gatekeeper task.

To provide the desired level of flexibility to the user, Conductor is designed in a reconfigurable, thread based, and real time manner. Multiple instances of the various components can be created and interconnected in a way which mirrors the configuration of the physical components. Each instance of a component in the system is driven by its own thread, with an individually assigned priority and update frequency. A master clock is used to ’trigger’ or ’tick’ each component when its period has elapsed. The triggering process wakes the appropriate component and instructs it to perform any associated algorithms. When finished, the component becomes dormant until triggered again. The thread based design along with the RT periodicity helps to remove the strict coupling that often exists between hardware sampling and the controller implementation.

Additionally, this design allows an easy mechanism for allotting appropriate operational frequencies to the various parts of the system. To implement the threading and RT features a package called Orocos (Open

RObot COntrol Software)[14] was used. Orocos and its RTT (Real Time Toolkit)[21] provide a clean interface to the kernel level scheduling mechanisms needed to implement Conductor as a real time program. In addition to providing the real time facilities for the program, Orocos also provides thread-safe data structures for inter-component communication, which are the main method of data passing within Conductor.

Conductor has taken a cue from the publish/subscribe pattern followed in ROS and Player/Stage (and the similar event-based pattern seen in systems like Tekkotsu[22] and ASEBA[23]) in implementing its internal communication. The publish/subscribe pattern functions somewhat analogously to radio transmission, in which the radio transmitter is the publisher and the individual receivers are the subscribers. The publisher makes its data available globally while each subscriber has discretion over what information it takes in. The overall approach provides for a very ﬂexible and easily reconﬁgurable system.

The data ﬂow through the system (see Figure 2.1) is simple in conception with the majority of system action being initiated by the State components. The States request an update to the system’s knowledge of their values each time they trigger. A State’s request for data proceeds along what is called the downstream

CHAPTER 2: CONDUCTOR: DESIGN 2.2 FRAMEWORK OVERVIEW 12 path; it is passed from State to Device to Protocol to Hardware. At each level, the receiving component will be dormant at the time of reception. Because it cannot operate while dormant, the receiver will cache the request until it is woken by a clock tick. At that point, multiple requests may have accumulated in the cache. Each request will be processed, some will be bundled or otherwise modiﬁed based on the topology of the components. Afterward each request will be delivered to the next level in the stack. Data ﬂow from the

Hardware to the States, called upstream communication, is initiated by an actual hardware event and is the same as downstream communication except that components are visited in the opposite order.

2.3 Control Components

2.3.1 State

The State component is conceptuallythe most important part of the framework. It represents a single ’quantity of interest’ in the system which is either sensed directly by some unit of hardware or is derived from the values of other States. This quantity could be a voltage at some node, an angular rate, a linear or rotational acceleration, etc. Multiple States can be associated with a single piece of physical hardware. For example: a motor has both an angular velocity and a current consumption - both of which are good candidates for States.

It is this data which robotics deals with on a fundamental level, and this data which is of true interest to the designer. The purpose of the State abstraction is to give the user unfettered access to this information in a convenient and meaningful way. The State component is responsible for providing the system’s most up to date picture of physical reality to the user, and for maintaining that picture to within a pre-speciﬁed time period automatically. The most important part of the State, however, is the avenue through which it presents

Figure 2.2: Schematic diagram of the State’s functionality: The State acts as a gatekeeperto a numberof computationally useful tools including the quantity’s history, ﬁltered versions, projections, and transform methods.

CHAPTER 2: CONDUCTOR: DESIGN 2.3 CONTROL COMPONENTS 13 the data to the user.

The State’s data presentation method is the basis for its name. The concept for the State is to embody what would normally be represented as a state variable during system design. Consider a State ω, associated with a motor’s angular rate, as sensed by an encoder (handled by the lower level abstractions). The State allows all functions on ω which would normally be operable on a state variable. This includes functionality such as time history, ﬁltering, estimation, transform methods, etc.

The State is charged with maintaining a time history of the sensor values so that accesses such as ω[t] where t is on the time interval [−histmax, 0] return the appropriate value. (histmax is the size of the history the user requested at initiation). The current value of the sensed quantity is given by ω[0] for calculation purposes. Values on the time interval (0, ∞) or on the time interval (−∞, −histmax) can be estimated if the user speciﬁes an appropriate estimation algorithm. Transforms into the frequency domain can be achieved by specifying an appropriately sized windowing period, and using the historical data to generate the appropriate transform. (See Figure 2.2)

Because the program is implemented within a computer the sampling is inherently discrete time. The

1 1 2 program only has access to the values at integer multiples of the sampling period f ,(ω[0], ω[− f ], ω[− f ], etc.) values between these must be extrapolated from the samples. The program can do this extrapolation seamlessly per the user’s request and speciﬁcation of an algorithm to do so (linear ﬁt, nth order polynomial,

3 etc). Thereby a request for ω[− 2f ] returns the average of the two surrounding points.

Another function of the State is as an emissary of the Controller. The State must understand whether it is read only or if it is also writable. Readable States perform all the actions described above while writable

States must also be able to communicate requests for change of condition from the Controller to the lower levels. When writable States receive such a set request from a Controller, they must package this data in the appropriate container and send it downstream for interpretation by the communications components.

The State component is intentionally left as an abstract type. The only identifying data the State carries are its name, a node ID number (which is speciﬁc to the Device it is connected to), its data type, and the information that constitutes its connections to other parts of the system (Devices and Controllers). This design allows a very easy transition between two systems that follow the same mathematical models but have

CHAPTER 2: CONDUCTOR: DESIGN 2.3 CONTROL COMPONENTS 14 very different hardware. The States and Devices remain unmodiﬁed, while only the Protocol and Hardware need to be changed. The disadvantage of this ﬂexibility is the additional effort required to identify where the information is going as it travels through the system. This is accomplished by tagging the State and Device with ID numbers that are unique to the Device and Protocol, respectively.

2.3.2 Controller

The Controller component is the hand off between the user (and higher level robotics frameworks) and Con- ductor. It provides centralized read and write access to the various States within the system, and can be extended for accessibility from other programs. Because of this, it is the most loosely deﬁned component.

Conductor speciﬁes functionality for the Controller with respect to adding connections to States, and for sending and receiving data from States. The Controller can request the value of any State or its computed components at any time. (Note that this request for data is one of the few non-event driven data access methods in Conductor.)

The user speciﬁes a control algorithm for the Controller to follow and sets an execution frequency. The control algorithm can be deﬁned either directly in C++ or using a scripting language implemented by Orocos.

In either case, the user has access to all the features the State component provides. The algorithm can be implemented as either a single equation, or as a state machine (a collection of separate equations executed one at a time, with deﬁned transition conditions between them). At the beginning of its execution interval, the

Controller gathers the value of the States required for its calculation. When it has obtained these values, the

Controller performsthe deﬁned algorithm, and generates a control signal. Once the signal has been generated, it is passed to the State it is intended to modify which in turn communicates with Conductor’s lower layers to bring about the requested change.

A Logger component is developed by slight modification of the Controller component. The Logger functions in the same manner as the Controller with respect to the sampling procedure. The difference between the two is that instead of calculating and broadcasting a control signal the values that the Logger samples are sent to a file log. The log can be a plain text file, a relational database program, or other storage medium.

CHAPTER 2: CONDUCTOR: DESIGN 2.3 CONTROL COMPONENTS 15

2.4 Communication Components

2.4.1 Device

The function of the Device component is to represent a physical node on a bus, such as a motor controller.

Each Device can be associated with one or more States (in the case of a motor controller, these would be angular velocity, current draw, etc). The Device’s purpose is to help in performing down- and up-select operations as data passes through it en route to either the Protocol or State components. On the downstream the goal is to ensure that the Protocol and Hardware components have enough identifying information available to them to construct a properly formed data packet. The Device accomplishes this goal by tagging the request with identifying credentials. On the upstream the goal for the Device is simply to determine if the received message and its corresponding data should be carried along to the Device’s associated states. This is done by simple comparison between the reconstructed message and the Device’s tagged identifying information.

The data ﬂow in the downstream direction functionsas follows (Figure 2.3): Each request that is generated by a State has a placeholder for a data structure called a ’credential’. The credentials contain the bus speciﬁc identifying information needed for packet generation further downstream. When a request reaches the Device from the State through its event handler, the request is placed on a queue. When the Device’s tick expires, it processes each of the elements in the queue. For each queued message, the Device places a reference to its credential on the message, and emits an event containing the message to its subscribed Protocols.

Which Protocols the Device is connected to determines which physical buses the message will eventually be delivered to, along with its format and character. In this way, the State’s relationship to a particular physical

Figure 2.3: Schematic diagram of the Device’s functionality: The Device’s responsibility on the downstream is to tag the message with identifying information that will be needed by the Protocol to perform its task.

CHAPTER 2: CONDUCTOR: DESIGN 2.4 COMMUNICATION COMPONENTS 16

Figure 2.4: Schematic diagram of the Protocol’s downstream functionality: The Protocol’s responsibility in the downstream direction is to buffer and condense messages in order to facilitate efﬁcient usage of bus bandwidth.

bus is deﬁned by which Device it is connected to, as opposed to particular conﬁguration information placed on it. To port a controller from one system to another becomes a simple function of rearranging the States to connect with different Devices and Protocols associated with the new physical hardware.

The data ﬂow in the upstream direction functions as follows: When the device receives a message from a

Protocol through its event handler, it places a copy of the message on a local queue. When the Device’s tick expires, the device iterates through the elements in the queue. For each element in the upstream queue, the

Device examines the credential information contained in the message that the Protocol has constructed. The credentials are compared to the ones assigned to the Device by the user. If they match, the Device accepts the message and emits it to its subscribed States.

2.4.2 Protocol

It is common for manufacturers of the various sensors and actuators used in robotics to design custom data protocols for a particular product or line of products. For the sake of this document, a data protocol is a deﬁned structure (including order, endian-ness, etc) on a grouping of digital words used to convey some information between two devices. It is the unfortunate state of robotics that manufacturers rarely collaborate in designing these protocols, and almost no standards exist among them. This lack of standards is part of the reason that implementing robotics experiments takes extraordinary amounts of time. Despite the lack of collaboration, it is not uncommon for protocols from separate manufactures to share common characteristics.

CHAPTER 2: CONDUCTOR: DESIGN 2.4 COMMUNICATION COMPONENTS 17

For a given classiﬁcation of device (motor controller, accelerometer, etc), the types of data sent across the bus are nearly identical even if the structure of transmission changes. This allows for a certain level of abstraction to be made over the classiﬁcation.

The Protocol component of Conductor is a software representation of some packet or stream based data transmission format on the bus. As such it acquires an important role in organizing data ﬂow in both the downstream and upstream directions. On the downstream path, the Protocol buffers and arranges requests in such a way as to maximize bandwidth efﬁciency of the bus. In the upstream direction, the Protocol must convert a word stream into a data structure which can be used by the other components (Device, State) to identify the intended destination of the data.

The data flow in the downstream direction functions as follows (Figure 2.4): Requests are received from the State via the Device through an event emission and handler function. These requests are placed onto a queue. When the Protocol’s tick expires, all elements within the queue are examined by a user defined optimization function. It is the goal of this function to generate a word level packet which efficiently represents the requests on the queue. This behavior is beneficial because it allows maximal usage of the bus. Many vendors will design protocols so that they are most bandwidth efficient when multiple devices are commanded at the same time (shared headerand check sum over a packet with numerous commands). Once the optimization algorithm has finished processing, it passes the result to the Hardware component via an event emission.

The data ﬂow in the upstream direction functions as follows (Figure 2.5): First, an encapsulated word is

Figure 2.5: Schematic diagram of the Protocol’s upstream functionality: In the upstream direction, the Protocol performs pattern matching on word sequences against user deﬁned templates. Here it has determined the Current data sequence cannot match P0 or P1. When it receives the next word, the sequence will match P3, which it will use to interpret the data.

CHAPTER 2: CONDUCTOR: DESIGN 2.4 COMMUNICATION COMPONENTS 18

Robot Joint State

2 RHY(value) RHR(value) RHP(value) RKP(value) RAP(value) 1.5 RAR(value)

1 Joint Position (rad) 0.5

−0.5 10 15 20 25 30 Time (s)

Figure 2.6: Example of Conductor log ﬁles: Plotted log ﬁles taken from a run of Conductor driving the simulated mini-Hubo platform seen in Figure 3.6. The collective angle set represents the walking trajectory of the robot.

received by a handler function in the Protocol via an event emitted by the Hardware. This capsule is placed in a queue data structure. When the Protocol’s device tick triggers it removes the oldest element from the queue and processes it. For processing, the word is unpacked and fed into a lexical analyzer (scanner)[24] deﬁned by the user for the speciﬁc Protocol type. 2

The scanner compares the word stream against a listing of user deﬁned templates. If the scanner has made a match with the incoming word stream, it extracts the meaningful data from the stream, and places it within a data structure. The structure is attached to an event and emitted to the Devices. It then processes the remaining words in the queue until either there are no more words or the time allotment runs out.

2.4.3 Hardware

The Hardware component is the simplest to understand, because its functionality is very limited. It serves primarily to act as a gatekeeper to the physical hardware from the core of the program. Its job is twofold: to take the message packets that it receives from the Protocol component and place them onto the physical bus, and to take the data words which are received from the physical bus and assure that they reach the subscribed

2A scanner is a pattern matching program usually used in the design of programming languages. Such programs convert strings of characters (source code) that are arranged according to regular expressions into symbolic tokens that can be used by a higher level program (parser) to create a meaningful interpretation (binary executable) of the original string. At its core, the scanner is simply a way of deﬁning state-machine based pattern recognition programs in a standard format.

CHAPTER 2: CONDUCTOR: DESIGN 2.4 COMMUNICATION COMPONENTS 19

Protocols. In most cases, the Hardware acts as a bridge between the user-space program and the kernel-space hardware driver by accessing the ﬁle system node associated with that hardware. This assures that multiple

Protocols do not mangle each other’s messages while contesting with each other for bus access.

The data ﬂow in the downstream direction functions as follows: the Protocol, having assembled a message at the word level, passes the message to the Hardware by emitting an event with a copy of the message.

This event is picked up by the Hardware’s event handler, and the Message is placed on a queue. When the

Hardware’s tick triggers it examines the queue for data, executes a user deﬁned processing function to render the Message packet into individual words, and places those words onto the bus.

The data ﬂow in the upstream direction functions as follows: The Hardware receives words from the bus in one of two fashions (dependent on the particular type of physical hardware involved). The ﬁrst possibility is that an event is generated whenever new data becomes available. The second possibility is that the new data is quietly placed on a buffer that is part of the kernel driver. In the former case, an event handler is placed on the Hardware which copies the word onto a queue managed by the Hardware object. In the latter case, the

Hardware object examines the driver’s buffer when it wakes for its tick and places the words into its queue.

In either case, when the Hardware wakes for its tick, it examines its queue, encapsulates the words using a user deﬁned handling function, and emits them as events to any subscribed Protocols.

2.5 Interfacing with Conductor

Conductor’s States can be directed in a number of ways, both internal and external to the core program, though the interfaces are slightly different depending on the access route used.

2.5.1 Internal Access

When accessing the States from within Conductor or from the attached scripting language of Orocos a system of buffered commands is used. In many cases when we are interested in refreshing the state of a robot, it is important that a number of commands are received near simultaneously. To achieve this, the user ﬁrst places the set points for the States in question into a buffer using a function call. Then, the user ﬂushes the buffer and applies all the states using a second function. Functions are also provided for reading from states.

The functions are as follows:

CHAPTER 2: CONDUCTOR: DESIGN 2.5 INTERFACING WITH CONDUCTOR 20

1. addCtrl(stateName, value) - Add a set point of value for State stateName to the buffer.

2. sendCtrl() - Empty the buffer and apply and set points in it.

3. checkCtrl(stateName) - Check the buffer for a set point for state stateName and return its value.

4. getSurface(stateName) - Poll the State for its record of the quantity’s last known value.

2.5.2 External Access

Accessing the States from programs other than Conductor is possible, though it requires a slightly different interface. When each State is created within Conductor it is associated with a UDP port for external communication. By opening this port and sending the appropriate commands, the state can be manipulated. The commands are sent plain-text to the port associated with the state, there is no buffer element associated with external commands. This method of interface can be used to access Conductor’s States from programs such as Matlab, the ROS suite, etc.

The commands are as follows:

1. SET value -(value should be encoded as an ASCII ﬂoating point number) - Sending a SET to the State

applies value as a set point.

2. SMP - Sending SMP to the State will cause it to sample its underlying hardware in order to update its

available information.

3. GET - Sending a GET to the State will cause it to respond with the most recent sampled value.

CHAPTER 2: CONDUCTOR: DESIGN 2.5 INTERFACING WITH CONDUCTOR 21

Chapter 3: Conductor: Testing/Results

3.1 Stress Testing

To gain an idea of the upper limits of the system a stress test has been conducted. The test consisted of creating abstract states which function as follows: When the program starts, the states begin to sample the value of their corresponding ’physical hardware’. The sample request is transmitted to the lower level system nodes (Device to Protocol to Hardware). Since there is no actual hardware embodiment for theses states, an artiﬁcial Hardware node responds to the sample by sending back the value of sin tS where tS is the time stamp placed on the sample request. The response is passed back upwards through the system (Protocol to Device to State) and the value is assigned to the states value. The values of the States are periodically recorded by a Logger node which is external to the States.

The stress test was conducted for three State nodes and their corresponding support nodes at sampling frequencies ranging from 50Hz to 10kHz. The results take the form of an evaluated error between the value sampled by the Logger (which is tS) and sin tL where tL is the time at which the logging operation occurred.

The expected result of this test is that there will be a small sinusoidal error during correct operation due to the difference between tS and tL. When the system becomes overloaded, we expect to see degradation of the error signal with large spikes appearing at points where the scheduler has missed deadlines resulting in irregular gaps between tS and tL.

The results of the test are encapsulated in Figure 3.1. Where we see that, as expected, the error at low frequencies is both sinusoidal and smooth. This trend persists at 50Hz, and 100Hz sampling frequencies. We see slight breakdown of the real time characteristic at 500Hz and 1kHz with decreasing magnitude of error.

Between 1kHz and 5kHz a severe breakdown of the real time characteristic of the system occurs and high levels of noise are introduced into the system.

It is important to note in this analysis that the results of such characterization tests are only valid for the system on which they are performed or an exact copy. This is because the actual execution time of a given algorithm is dependent entirely on the hardware on which it is run. Factors such as the processor speed, cache 22

Conductor State Error for Various Sampling Frequencies 0.05 50Hz 100Hz 0.04 500Hz 1000Hz 5000Hz 0.03 10000Hz

0.02

0.01

0 Error

−0.01 (sin(t) − StateValue(t))

−0.02

−0.03

−0.04

−0.05 0 5 10 15 Time (s)

Figure 3.1: Conductor stress testing results: These results show degredation of performance at operating frequencies above 1000Hz

sizes, memory latency and dozens of others determine the eventual result. For this reason the exact frequency of the failure has not been located, but could be found by iterating the test with values between 1kHz and

5kHz for this machine.

3.2 Example/Implementation

To illustrate the capabilities of Conductor, the following section details its application to our miniature and simulated miniature humanoid robots known as the mini-Hubo. The two primary benefits of Conductor are its ability to a) deal with high DOF systems and b) translate relatively seamlessly between such systems. These tasks are accomplished by providing user hooks into the Conductor framework for both direct extensions and external program interfaces. The driver author first identifies how to structure the data flow to and from a device using Conductor’s four stage framework. Then, the programmer derives the template classes provided with the framework and overrides a small number of virtual functions to support the device. After this, the quantities of interest for the device can be exposed as states and accessed or manipulated as appropriate. As a reference for the amount of code required for different devices: the extensions which support the highly featured Robotis servo motors used in the mini-Hubo require 1100 lines of C++ code. Interface code to the

CHAPTER 3: CONDUCTOR: TESTING/RESULTS 3.2 EXAMPLE/IMPLEMENTATION 23

State Device Protocol Hardware

Force Z

Force/Torque ADC Moment X UART2 Sensor Protocol

Moment Y

Right Ankle Right Ankle Roll Angle Roll Motor

Right Ankle Right Ankle Pitch Angle Pitch Motor

Right Knee Right Knee Pitch Angle Pitch Motor Robotis UART1 Protocol Right Hip Right Hip Roll Angle Pitch Motor

Right Hip Right Hip Pitch Angle Roll Motor

Right Hip Right Hip Yaw Angle Yaw Motor

Figure 3.2: Example Conductor node arrangement: An illustration of the node arrangement in the example system, shows the topology for implementing the right leg controller of mini-Hubo.

Webots Open Dynamics Engine based simulator program used for virtual mini-Hubo required 600 lines. A simple USB ADC requires about 200 lines for full integration into Conductor.

3.2.1 High Degree of Freedom Systems

To design a dynamic controller with Conductor after support code has been written for all appropriate hardware is mostly a matter of configuring the topology of the system. This configuration is done using a C-like scripting language that is part of the Orocos package upon which Conductor is built. The specification of interconnection between the nodes tells the Conductor components how to pass data from the abstract State nodes down to the appropriate physical hardware. The topology required to setup a leg of the mini-Hubo is depicted in Figure 3.2. The configuration file required to realize this topology is shown in Figure 3.3. It is worth pointing out that porting between the physical and virtual mini-Hubo requires the same topology, and only minimal changes to the actual configuration file.

Once the States and underlying support topology have been conﬁgured, the user can begin to take take advantage of Conductor’s real beneﬁts by authoring controllers. Controllers can be written as plugins in C++, as part of a separate program accessing the States through a UDP protocol, or can be written using a custom

CHAPTER 3: CONDUCTOR: TESTING/RESULTS 3.2 EXAMPLE/IMPLEMENTATION 24

1 program main { 2 //Add the Hardware guard to the system 3 Hardware(”hw”, 1, 2000, ”Robotis”, 4 ”/dev/ttyUSB0”); 5 6 //Add the Protocol interpreter 7 addProtocol(”pcol”, 2, 50, ”Robotis”); 8 9 //Connect the hw and pcol nodes 10 linkHP(”hw”, ”pcol”); 11 12 //Add the Device nodes to the system 13 Device(”dRAR”, 5, 250,”Robotis”,46, 2.61, −1); 14 ... 15 Device(”dRHY”, 5, 250,”Robotis”,48, 2.57, −1); 16 17 //Add the States to the system 18 State(”RAR 5 50”, ”Robotis”, ”Joint”, true); 19 ... 20 State(”RHY 5 50”, ”Robotis”, ”Joint”, true); 21 22 //Connect the Protocol to all devices and all 23 //Devices to the corresponding States 24 linkPD(”pcol”, ”dRAR”); linkDS(”dRAR”, ”RAR”); 25 ... 26 linkPD(”pcol”, ”dRHY”); linkDS(”dRHY”, ”RHY”); 27 28 start(); //Initialize the defined system 29 }

Figure 3.3: The code to realize the motor arrangement in Figure 3.2

1 var float Kp=10; 2 var float Ki= 1000; 3 var floatKd=1; 4 5 while(true){ 6 //Access the current values of the 7 //properties monitored by the system 8 var float theta = getSurface(”theta”, 9 ”value ”); 10 var float thetaDot = getSurface(”theta”, 11 ”diff ”); 12 var float intTheta = getSurface(”theta”, 13 ”integral ”); 14 15 //Calculate the control signal 16 var float ctrl=Kp∗ theta + Ki∗ intTheta 17 + Kd∗ thetaDot ; 18 19 //Apply the control 20 //addCtrl(arg1, arg2) applies the value in arg2 as a set po int to the state 21 //represented by arg1 22 addCtrl(”x”, ctrl); 23 sendCtrl(); 24 yield(); //Wait until the next clock tick 25 }

Figure 3.4: Example Conductor/Orocos PID controller. The example was written using the scripting language that Conductor inherits from Orocos.

CHAPTER 3: CONDUCTOR: TESTING/RESULTS 3.2 EXAMPLE/IMPLEMENTATION 25

1 % Setup Matlab variables for communication 2 % w/Conductor (Argument is UDP port #) 3 x1 = initState(2827); 4 x2 = initState(2828); 5 ... 6 xn = initState(2827+n −1); 7 8 %Model of the system 9 A= [nxn]; B= [nxp]; 10 C= [q x n]; D= 0; 11 12 %Observer gains , computed off −line 13 K= [p x n]; 14 15 while(1) 16 %Obtain the (n x 1) state vector from Conductor 17 %Each readState() command sends a UDP packet to Conductor instance and 18 %fetches the result that is sent back to Matlab. 19 x = [readState(x1); 20 readState(x2); 21 ... 22 readState(xn); ]; 23 24 %Observer prediction for x(k+1) 25 xk1=(A−B∗K)∗ x + B∗u ; 26 27 %Evaluate control law (system appropriate) 28 u = ctrLaw(A, B, C, D, xk1); 29 30 %Apply Control 31 %Each cmdState() call sends a UDP packet to Conductor which causes the 32 %value in arg2 to be applied to the State associated w/arg1 33 cmdState(x1, u1); 34 cmdState(x2, u2); 35 ... 36 cmdState(xn, un); 37 end

Figure 3.5: Example Conductor-Matlab interface code. This code illustrates how a state observer design pattern can be easily implemented in Conductor.

scripting language that Conductor inherits from Orocos. Designing controllers using these techniques are

all very straightforward. Recall, the Conductor framework handles lower level communication and mainte-

nance of present State data, so authoring the Controller requires only deﬁning the mathematical control law.

Beginning with a very simple example, Figure 3.4 shows an example PID controller in the Orocos scripting

language.

More complex behaviors are equally easy to achieve. Figure 3.5 shows the implementation of a state

observer design pattern. The pattern is implemented in Matlab and is seamlessly back-ended to Conductor to

perform the hardware interfacing. Since Conductor already represents data in terms of states, interfacing with

controls tools such as Matlab is relatively easy. In addition to classical controls implementation, Conductor

CHAPTER 3: CONDUCTOR: TESTING/RESULTS 3.2 EXAMPLE/IMPLEMENTATION 26

Figure 3.6: Walking trajectory time progression with virtual mini-Hubo. The trajectory is shown in Figure 2.6

can also be used to implement hybrid controllers. The Conductor framework has been used to develop full walking controllers for the mini-Hubo. A time-lapse of a walking pattern is displayed in Figure 3.6 and the corresponding log ﬁle is plotted in Figure 2.6. A more impressive controller has been realized in a stair climbing application also developed for the mini-Hubo. A time lapse of this behavior is shown in Figure 3.7.

3.2.2 Portability

The goal of the conductor system on a whole has been to make the portability of controllers for high degree of freedom systems simple by choosing meaningful abstractions. To demonstrate this functionality, a basic controller was ported across three humanoid robot systems: the Hubo, mini-Hubo, and virtual-Hubo. This controller was designed to manipulate the robot’s upper body in order to knock a box off of a pedestal.

The controller was run the two physical robots and their corresponding simulations with great success. A comparison frame showing the different systems accomplishing the same task is given in Figure 3.8.

3.3 Summary

This section has detailed the design and usage of the Conductor robotics programming framework. The layers of the framework have each been described in detail. By extending these components it is possible to create a software representation of a robotic system in terms of its fundamental state variables. Example

CHAPTER 3: CONDUCTOR: TESTING/RESULTS 3.3 SUMMARY 27

Figure 3.7: The mini-Hubo climbing stairs while driven by Conductor

CHAPTER 3: CONDUCTOR: TESTING/RESULTS 3.3 SUMMARY 28

Figure 3.8: The three test systems for Conductor performing the box task. No modiﬁcation of the controller code is made between the systems.

controllers and test system results have been shown to illustrate the beneﬁts of such a representation. These examples show clearly the ease with which controllers can be implemented when a system is expressed in an intuitive manner. While a designer must expend some initial extra effort to realize this representation, it quickly provides a return on the investment when implementing control algorithms.

CHAPTER 3: CONDUCTOR: TESTING/RESULTS 3.3 SUMMARY 29

Chapter 4: Score: Design

4.1 Concept of Operations

Score acts as an operating system for a low resource (8k-256K RAM, 65K ROM) micro controller. The user interacts with the system through a simple serial terminal over either RS-485 or RS-232. The terminal interface presents the user with a menu that allows them to access the system peripherals. Each peripheral can be can be configured according to its particular parameters e.g. a PWM module would be configurable by frequency and duty cycle. The interface also allows the user to define dynamically updated equations.

Each equation is stored in the system symbolically and is updated according to a user deﬁned frequency. The equations can include basic arithmetic functions, various scaling functions, and basic numerical operations

(integrals, derivatives). Once deﬁned in the system, the equations can be used as inputs and outputs to the various controller peripherals.

4.2 Systems Architecture

4.2.1 Overview

The system is broken into a number of modules that handle each of its main functionalities. These can be divided into three rough groups: hardware auto-configuration, symbol/variable computations, and user interface. (See Figure 4.1) The hardware auto-configuration portions are responsible for performing all underlying register manipulations and function calls to configure and activate the different system peripherals as requested by the user or by the symbolic computations. This includes altering ADC sampling frequencies,

PWM duty cycles, UART baud rates, etc. The symbolic expression manager is the main mechanism for realizing the signal based programming discussed in the introduction. It maintains a symbol table with all expressions entered by the user that require periodic updates. It also works the the underlyingreal time kernel

(FreeRTOS) to choose the appropriate frequency for these updates. The user interface module provides the structured menu system presented to the user for interacting with the hardware auto-conﬁguration and symbolic expression modules. It handles command interpretation, subsequent calls to the other modules, and any 30 required error handling. It is also the access point for the help system.

All actions are initiated by the user though the Command Interpreter. If the action initiated is a direct request for a certain action on the hardware, the interpreter calls the appropriate functions from the PWM,

ADC, or UART modules. These in turn call a sub-module for appropriately conﬁguring the underlying directionality on the hardware pins of the chip (GPIO module), and perform any peripheral speciﬁc actions.

If the action initiated involves a symbolic expression, the appropriateentry is added to the symbol table which keeps a record of all expressions and symbol ties created by the user.

4.2.2 Hardware Auto-Conﬁguration System Timing - FreeRTOS

The majority of control systems are greatly dependent on Real Time (RT) functionality. A system which is

RT is capable of meeting speciﬁc requirements for execution times for each of the algorithms making up the system. It is a guarantee that a function which must run at a particular periodicity will execute at the required time instant and ﬁnish its execution before the next trigger time. (See Appendix B for additional information

Figure 4.1: Score system architecture

CHAPTER 4: SCORE: DESIGN 4.2 SYSTEMS ARCHITECTURE 31 on RT Systems.)

To meet the RT requirements for Score an existing kernel has been chosen. Using an existing system provides the beneﬁt of a full RT OS including a scheduler, common data structures for RT exchanges (mu- texes, semaphores, queues, etc), and a priority system. It also comes with the level of testing and community support that cannot be readily garnered with a new design.

The particular OS chosen was FreeRTOS, an open source project which supports 23 separate micro controller architectures along with x86 (Intel CPU) hardware. The project is well supported and well regarded as stable and reliable. In addition to the open source versions there are also commercially license-able and safety certiﬁed versions of the software. Another beneﬁt of FreeRTOS is its size - the entire kernel can be compiled in 4-9KB depending on the feature set chosen.

In the FreeRTOS system, every periodic action is associated with a thread that is assigned a frequency, an algorithm, and a priority. The frequency determines the time between updates. At each update, the assigned algorithm is executed. If conﬂicts occur (two threads attempt to run their respective algorithms at the same time) the priority numbers are used to resolve the dispute or initiate time sharing as appropriate.

Hardware which is used by more than one thread (electrical buses, communications hardware, terminal access) must be guarded by a gatekeeper thread, which assigns usage rights to the hardware. This is done to prevent two threads from juggling a resource and thereby making any input/output operations unintelligible.

Data members which need to be updated by any of the periodic functions must be made ’thread safe’.

This means they must be encapsulated within specially designed data structures. Thread safe data structures are designed in such a way that read and write operations to and from them can be temporarily interrupted without compromising the integrity of the data. They can also be safely accessed by multiple threads at the same time.

Hardware Abstraction within FreeRTOS Testing and debugging algorithms on a micro controller system is incredibly difficult. The lack of user interfaces makes extracting even the simplest data for debugging a program very difficult. Additionally, micro controller programs are primarily stored in flash memory, which has agonizingly slow write times. Because of this, each iteration of testing, which requires the transfer of the new program binary to the target, can take on the order of minutes. Therefore, it is convenient if the

CHAPTER 4: SCORE: DESIGN 4.2 SYSTEMS ARCHITECTURE 32 algorithmic components of the software can be tested directly on a PC before being loaded on the micro controller. Each iteration of testing can be done much more quickly, while access to data storage and retrieval facilities is much easier.

One of the beneﬁts of using FreeRTOS as the underlying RT Kernel is its great portability. One of its available ports is for an x86 target (industrial PC) running a Posix operating system. The Posix version cannot be used for production, as it relaxes some of the timing requirements of the system, but guarantees that order of execution is preserved. By compiling the Score program for FreeRTOS’s x86 target, it is possible to test the algorithms behind Score almost entirely on a computer terminal, without needing to involve the time consuming micro controller hardware.

Obviously, not all features of the system can be tested this way. The utility of the approach ends where the speciﬁcs of the micro controller begin. It is obviously not possible to use PWM generators, ADC samplers, or similar functionality on the x86 port. To test as much as is possible, the lowest level hardware must be slightly abstracted. This is not difﬁcult given that the original design planned for the requirement. Separate

Figure 4.2: Testing Flowchart for Score. The development of the x86 port allows for quick testing and veriﬁcation of algorithms on PC hardware, followed by a direct switch to a micro controller version.

CHAPTER 4: SCORE: DESIGN 4.2 SYSTEMS ARCHITECTURE 33 initialization functions must be written for each peripheral on each target. For the case of the x86 target, which does not possess most of the relevant hardware peripherals, the initialization functions simply print status messages to allow the user to know they have been called.

ADC Subsystem

Figure 4.3: Data structure for the ADC auto-conﬁguration code

On a given micro controller the Analog to Digital Converter (ADC) is the peripheral with the largest number of configuration options available. The majority of ADCs are designed against a sample-and-hold design pattern where multiple ADC channels will sample their pins simultaneously and then hold the voltage while a single successive approximation circuit tests each in turn. The user will usually have the option of configuring not only which pins of the ADC are active, but also the ordering that the approximations are carried out in. All of these operations can usually be performed in multiple groupings so that different config- urations can be established and rapidly switched between during the device’s operation. Additionally, ADCs can usually be configured to sample single-ended (based on an absolute ground reference) or differentially

(potential difference between two pins of the ADC). In order to handle such a complicated chain of events in a general fashion, Score introduces some limitations on the functionality of the ADC in preference of ease of conﬁguration.

The basic unit for Score’ ADC auto-conﬁguration is a sequence data structure, which describes an order-

CHAPTER 4: SCORE: DESIGN 4.2 SYSTEMS ARCHITECTURE 34 ing in which ADC pins should be sampled. This ordering is represented by a list of step data structures. Each step structure denotes a particular ADC channel to be sampled and its associated conﬁguration options (single or differential, etc). These data structures are created in response to user requests to establish a sampling pattern. When they are passed to their interpretation functions they are used to set the appropriate registers in hardware to establish the requested sampling pattern.

PWM Subsystem

The PWM module is relatively easy to configure. The only configuration information required is a desired frequency, duty cycle, and whether the selected pin (or pair) is single ended or complementary. From this information a direct call can be made to configuration functions which have been designed to use a series of look up tables to acquire the correct set of registers and apply them to the hardware to initiate the requested

PWM mode.

UART Conﬁguration Module

The Universal Asynchronous Receiver/Transmitter (UART) is the serial terminal driver in the system and the main mode of communication between Score and the user. The UART is conﬁgured to a set of default parameters on startup (115200baud, 8 Data Bits, 1 Stop Bit, No Parity) and can be reconﬁgured by the user to suit the application requirements.

4.2.3 Symbolic Expression Manager

One of the fundamental operations of Score is to allow the development of arbitrary controller patterns.

As explained in the Design Objectives section, Score attempts to simplify this task by using signal based variables. Rather than fixed assignments, variables continuously update in order to maintain the relation with which they were originally defined. To achieve this functionality Score maintains a symbol/expression table that stores each symbol and the equation used to define it. It also presents a series of functions to the user to allow interaction and modification of the table. The table and its supporting functions are used to ensure that the current value of each variable is maintained according to the requested frequencies and that any dependent hardware functionality (such as PWM updates) is refreshed.

The current state of the system is summarized in the class hierarchy diagram for the system. This diagram

CHAPTER 4: SCORE: DESIGN 4.2 SYSTEMS ARCHITECTURE 35 shows all types of symbolic expressions within Score and their inheritance hierarchy.

Symbol Table

The symbol table is the heart of the symbolic expression manager. This table stores the original expression, the name it has be assigned to and, if applicable, the frequency at which it must be updated. Some symbols, e.g. scaling functions, are only evaluated when they are needed for some dependent calculation or when the user requests their value explicitly. Other symbols, such as those tied to PWM outputs, have associated update frequencies which determine their recalculation and application periods. Whenever a new symbol with a periodic component is added to the table, FreeRTOS is invoked to create a new thread at the appropriate frequency.

Bare Expressions

Bare expressions are one form of expression which may be placed by the user into the symbol table. Their deﬁning characteristic is that they are purely a computational construct and will not have a direct relationship with the hardware. Bare expressions include a number of sub-categories, including:

• General Symbolic - This category of expressions are arbitrary arithmetic operations or constants de-

ﬁned in terms of numbers or other system variables. They are expressed in Score as postﬁx operations.

Example (The following are equivalent):

Postﬁx (Score): ctrl = Kp target * Ki int target * + Kd dtarget dt*+

dtarget Inﬁx (Standard): ctrl = (KP target) + (KI R target dt + (KD dt )

• Scale - This expression deﬁnes a symbol which will express the value of another speciﬁed symbol

scaled to a different range. That is, it linearly scales a variable on interval [a,b] to interval [c,d] and

hard limits at [c,d] if the result should happen to exceed it. The intended usage of this expression is as

a shortcut to map some arbitrary quantity to the ranges needed for the common outputs.

• Saturation - The saturation expression is simply a way of re-expressing a quantity already in the

system but with a deﬁned upper and lower bound.

• Integral - This expression creates a symbol which will contain the integral of another system symbol.

(x1+x2) The integral is calculated using trapezoidal approximation. Inext = Icurrent + (t2 − t1) 2

CHAPTER 4: SCORE: DESIGN 4.2 SYSTEMS ARCHITECTURE 36

• Windup Integrator - This expression creates a symbol containing the special purpose Windup Inte-

grator described in Section 5.2.3

• Derivative - This expression creates a symbol which will contain the derivative of another system

symbol. The derivative is calculated using a two point approximation. dx x2−x1 dt = t2−t1

Tied Expressions

Tied Expressions are those which are coupled in some way to the input or output hardware which they represent. There are presently two types of Tied Expressions:

• ADC - ADC expressions are created automatically when the user initializes an ADC module. A vari-

able is placed in the symbol table which matches the name of the initialized hardware. This variable

represents the raw ADC value read from the hardware register (range 0-1023).

Example: If the user initializes ADC 0 the variables ’adc0’ will be created representing the raw and

voltage value.

• PWM - A PWM expression controlsthe duty cycle of a PWM module. When created the user provides

an update frequency and the name of a variable which the PWM module should follow. At each update

interval for the given frequency, the value of the given variable is read and the PWM module’s duty

cycle is set to its value.

• File Input - (x86 Port Only) Allows a ﬁle on a disk to be used as an input source for the program.

• File Output - (x86 Port Only) Allows a ﬁle on a disk to be used as on output from the program.

Name Table Structure

The system architecture is most easily summarized by examining an example of a run time name table. The name table is the data structure used to map each of the variables in the system to the textual name by which the user identiﬁes it. Figure 4.4 shows an example name table. Each entry in the box at the left represents a named system variable, and the leading arrows indicate a data structure that the name refers to. Some data elements, such as those pointed to by KP , KI , and KD, are represented by constants, which are standard data types that do not change over the course of the program’s execution. Entries such as prop indicate a scaled

CHAPTER 4: SCORE: DESIGN 4.2 SYSTEMS ARCHITECTURE 37

Figure 4.4: A graphical depiction of an example system name table. Section 4.2.3 provides a more detailed description of its meaning

relationship between the value of prop and the last sampling done by the thread assigned to keeping the

ADC data current. Shaded entries indicates threads that are tied to physical hardware, while entries encircled by arrows indicated a periodic update. Entries like Iprop and Dprop represent an integral and derivative of the target node (prop in this case), with the value of the integral/derivative being updated each time the timer is triggered for its respective thread. The last noteworthy element is the PID node, which is tied to an expression tree. Expression trees represent arithmetical relationships between the different variables of the system. Their values are not periodically updated, but instead evaluated when another system resource requests the value of the expression as a whole.

4.2.4 System Input/Output

*Note: The functions described in this section are available only in the x86 port.

Script File Support

The features described in this section have been added to allow the reading of script ﬁles into the program.

These scripts consist of the same commands which can be entered at the system prompt. They are stored in a plain-text ﬁle, one per line. By executing a single command at the prompt, the script can be read and run.

CHAPTER 4: SCORE: DESIGN 4.2 SYSTEMS ARCHITECTURE 38

This command is ”$> script (ﬁlename)”

An example script is included in Figure 4.5

Data Logging Support

One critical feature needed for system analysis and testing is data logging. Logs of both applied control

signals (outputs) and the observed inputs are desired. A simple command has been added to initiate this

data recording. Entering at the prompt: ”$> log (varname) (frequency) (ﬁlename)” will examine the variable

1 (varname) every frequency s and store the observed value into the ﬁle (ﬁlename). This logging is done in

plain text, with a single entry to a line. E.g. if the variable x were logged at 100Hz for 2 seconds, a 200

line ﬁle would be produced, with a single data point per line. Such ﬁles can be reloaded into Score using the

playback feature, or easily loaded into mathematics packages for graphing, analysis, etc.

Data Playback Support

The foil to data logging, data playback, is also supported. Data playback allows data taken from previous

program instances, or test data which was artiﬁcially created, to be used as an input to the system. Input

ﬁles must be in plain text format, with one entry per line. When Score is instructed to load data from a log

file, it caches the entire contents of the file into memory. Once the file is cached, it creates a system variable

of the given name, and presents the ﬁrst loaded number as that variable’s value. Each time an update is

triggered, it proceeds to change to presented value to the next one in the ﬁle. When the last entry is reached,

the system loops back to the beginning of the data sequence. Therefore, input ﬁles are implicitly periodic. By

constructing the input playback in this fashion, the loaded inputs appear as a named variable - no differently

from any other system quantity. In this way, it becomes very easy to abstract the input source between testing

and actual system implementation.

1 var 2 defint Ione one 100 3 defdiff Done one 100 4 log Lone Ione 100 int.log 5 log Ltwo Done 100 dif.log 6 infile one 100 100SIN.HZ

Figure 4.5: Example score script ﬁle

CHAPTER 4: SCORE: DESIGN 4.2 SYSTEMS ARCHITECTURE 39

Chapter 5: Score: Testing/Results

5.1 Hardware Auto Conﬁguration Testing

The ﬁrst round of testing focused on the hardwareauto-conﬁgurationmodules as stand alone entities to ensure that these basic building blocks of system functioned properly before building further complexity above them.

5.1.1 ADC Auto-Conﬁguration

Figure 5.1 shows a Score terminal session showing the ADC being initialized to read the voltages from a simple ladder resistive network. When the ADC is initialized, the expression manager creates two variables that reﬂect the value of the ADC. ADC names in capital letters are the raw data from the ADC module, the lower case variable names have been scaled to the [0-3]V (for single ended) or [-1.5, 1.5]V (for differential) range that the raw numbers represent. The sample shows multiple reads of the same variables producing different results. This is a byproduct of the signal based variables. The quantities are dynamically updated when the ADC is refreshed.

5.1.2 PWM Auto-Conﬁguration

The PWM generator was tested to ensure that arbitrary frequencies and duty cycles could be generated accurately from the Score user interface. Figure 5.2 contains two sample Score sessions and corresponding scope traces demonstrating the functionality of the PWM auto-configuration module. The session in Figures 5.3a and 5.3b focus on single ended PWM generation while Figures 5.3c and 5.3d shows the device operating in differential mode. Please note that in the first session, the frequencies are kept the same for each pair of signals because the pair shares an underlying timer generator and cannot be uncoupled due to hardware limitations. In each of the graphics below the upper (yellow) channel of the trace is the PWM channel 2 on the chip, which is the first channel of complementary pair B. The lower trace (green) is PWM channel 3 and the second channel of complementary pair B. The shared frequency and the duty cycle of each channel is printed on the trace and is seen to correspond exactly with the user request. 40

1 Welcome to SEEPS (v0.1) 2 (Systems Engineering Embedded Prototyping System) 3 (C) Bob Sherbert 2010 4 Type ’help’ for a command listing 5 6 $> help adc 7 Configure the ADC Module 8 Format of the command is: 9 adc channel frequency 10 11 channel(0 −3,A,B): The channel to sample. Codes 0−3 indicate the numbered 12 ADCs and A, B are the differential ADCs. 13 frequency(1 −100000): Desired sampling frequency 14 $> adc 0 100 (set adc 0 to 100Hz) 15 16 $> adc 1 100 (set adc 1 to 100Hz) 17 18 $> adc 2 200 (set adc 2 to 200Hz) 19 20 $> adc 3 400 (set adc 3 to 400Hz) 21 22 $> var (switch to variables menu) 23 24 Available commands 25 (Type ’help cmdname’ for more information on a command) 26 −−−−−−−−−−−−−−−−−− 27 main : Return to the main menu 28 printv : Print value of a variable 29 printvars : Print the variable table 30 help : Display list of commands 31 32 $> help printvars 33 Print the symbolic variable table along with its current v alues . 34 $> printvars 35 ADC0 − 330.000000 36 ADC1 − 914.000000 37 ADC2 − 617.000000 38 ADC3 − 331.000000 39 adc0 − 0.964809 40 adc1 − 2.680352 41 adc2 − 1.835777 42 adc3 − 0.982405 43 44 $> printvars 45 ADC0 − 331.000000 46 ADC1 − 916.000000 47 ADC2 − 616.000000 48 ADC3 − 336.000000 49 adc0 − 0.970674 50 adc1 − 2.686217 51 adc2 − 1.835777 52 adc3 − 0.976540 53 54 $> printvars 55 ADC0 − 333.000000 56 ADC1 − 909.000000 57 ADC2 − 624.000000 58 ADC3 − 329.000000 59 adc0 − 0.976540 60 adc1 − 2.665689 61 adc2 − 1.815249 62 adc3 − 0.979472

Figure 5.1: Initializing an ADC with Score

CHAPTER 5: SCORE: TESTING/RESULTS 5.1 HARDWARE AUTO CONFIGURATION TESTING 41

1 Welcome to SEEPS (v0.1) 2 (C) Bob Sherbert 2010 3 Type ’help’ for a command listing 1 Welcome to SEEPS (v0.1) 4 2 (C) Bob Sherbert 2010 5 $> pwm 2 10000 25 3 Type ’help’ for a command listing 6 4 7 $> pwm 3 10000 60 5 $> pwm B 250 25 8 (Image A) 6 (Image A) 9 7 10 $> pwm 2 100000 33 8 $> pwm B 12390 88 11 9 (Image B) > 12 $ pwm 3 100000 66 (b) Session 2: Complementary Outputs 13 (Image B) (a) Session 1: Single Ended Outputs

Figure 5.2: PWM generator testing: conﬁg ﬁles

CHAPTER 5: SCORE: TESTING/RESULTS 5.1 HARDWARE AUTO CONFIGURATION TESTING 42

(a) Session 1 A - Single Ended 10kHz, 60% Duty (b) Session 1 B - Single Ended 100kHz, 66% Duty Cycle Cycle

Figure 5.3: PWM generator testing: osilliscope traces

CHAPTER 5: SCORE: TESTING/RESULTS 5.1 HARDWARE AUTO CONFIGURATION TESTING 43

5.2 Time Domain Controller Testing

After testing each of the individual hardware auto-conﬁguration modules, they were tested in coordination

with each other and with the Expression Manager. Tests were conducted for proportional, integral, and

derivative controls independently using simpliﬁed systems such as potentiometers, resistive ladders, and di-

rect outputs from function generators. These simpliﬁed test cases established the functionality of the building

blocks for a PID controller. Following the individual tests, the system was tested as a proportional feedback

controller in a simple test circuit.

Two roundsof testing were performed. The ﬁrst was a set of Algorithmic Tests, which used the x86 Posix

port of Score and sanitized input/output in order to verify that the algorithms were producing the expected

results. This can be thought of as the theoretically ideal testing conditions. The second set of tests occurred

on the actual hardware and were used to verify that the theoretic results were being accurately produced on

the micro controller hardware.

5.2.1 Proportional Response Test Algorithmic Test

1 The ﬁrst test conductedwas to re-sample a test input ﬁle. A sine wave with frequency of 2π Hz was generated

and stored as a decimated signal with 100 points per second in a log ﬁle. Score was instructed to load this

ﬁle and to playback its contents as a system variable. A sampler was then assigned to record the value

of the corresponding system variable to a ﬁle at 100Hz. By showing that this operation can be completed

successfully, we demonstrate that the sampling functionalityneeded internally to generate the the proportional

arm of PID is operational. The Score code that constitutes the test program is included in Figure 5.4, and the

results of the sampling are plotted in Figure 5.5.

1 var 2 log Lone one 100 prop 3 infile one 100 100SIN.HZ

Figure 5.4: Score code for re-sampling

CHAPTER 5: SCORE: TESTING/RESULTS 5.2 TIME DOMAIN CONTROLLER TESTING 44

−7 x 10 Error − SEEPS Signal Sampling, Sin(t) Sampled @ 100Hz 0

−0.5

−1

−1.5

−2 Error (V) −2.5

−3

−3.5

−4 0 1 2 3 4 5 6 7 8 9 10 Time (s)

Figure 5.5: Proportional test error: Difference between the original signal and the recorded re-sampling. Zero indicates no difference between two signals. The spikes are regions where the playback and re- sampling were not entirely in sync, and are not a cause for concern.

Hardware Test

To test if the proportional controller was functioning correctly a ladder network of three 50kΩ resistors was constructed and a 5V potential was placed across the network. One of the three resistors was chosen as a potentiometer. Channel A of the ADC was configured to sample across this potentiometer at 1000Hz, and a PWM channel was configured to output a duty cycle scaled to use its entire range. (See the configuration session in Figure 5.6.) Figure 5.7 shows the measured duty cycle as a function of the voltage placed across the ADC. The result is exactly that which is desired in a proportional response.

CHAPTER 5: SCORE: TESTING/RESULTS 5.2 TIME DOMAIN CONTROLLER TESTING 45

1 Welcome to SEEPS (v0.1) 2 (Systems Engineering Embedded Prototyping System) 3 (C) Bob Sherbert 2010 4 Type ’help’ for a command listing 5 6 $> adc A 1000 7 8 9 $> v ar 10 11 $> defscale Padca adca −1.5 1.5 0 100 12 13 $> defpwm pw2 Padca 1000 2 1000 14 15 16 $> printvars 17 18 ADCA − 530.000000 19 Padca − 52.199413 20 adca − 0.057185 21 pw2 − 51.906158 22 23 $> printvars 24 25 ADCA − 591.000000 26 Padca − 57.771259 27 adca − 0.230205 28 pw2 − 57.673508

Figure 5.6: Session to conﬁgure proportional test

CHAPTER 5: SCORE: TESTING/RESULTS 5.2 TIME DOMAIN CONTROLLER TESTING 46

SEEPS Proportional Response Testing 100 y = 32.9*x + 50.38 r = 0.79639 90

40 Duty Cycle of Response [%]

10 Response Duty Cycle

0 −1.5 −1 −0.5 0 0.5 1 1.5 SEEPS Measured ADCA [V]

Figure 5.7: Testing the Proportional Response - Conﬁgured duty cycles plotted against the results of corresponding ADC measurements.

5.2.2 Integral Response Test Algorithmic Test

The second test conducted was a veriﬁcation that the integral functioned properly. To perform this test, the

1 same 2π Hz sine wave used for the proportional test was loaded as a system variable, to which an integrator was assigned. Since the input signal was sin t, its integral should be − cos t + C. Figure 5.8 shows the graph of the difference between these two signals. The error level is acceptably small (on the order of 10−6) and it is possible to see the characteristic drift of a numerical integrator. The Score code used to generate this data is included in Figure 5.15.

Hardware Test

To test the integral handling capabilities of Score, an integrator variable was created and tied to the first ADC sampling channel. (The configuration session is included in Figure 5.9.) A zero centered 50Hz 0.5V P-P square wave was fed into the ADC which was configured to sample at a rate of 1000Hz. The integrator of the

CHAPTER 5: SCORE: TESTING/RESULTS 5.2 TIME DOMAIN CONTROLLER TESTING 47

−6 x 10 Error − SEEPS Integral vs Theoretical, Sin(t) Sampled @ 100Hz 20

10 Error (V) 5

0 Error = −cos(t) − int( sin(t) ) [as per SEEPS evaluation]

−5 0 1 2 3 4 5 6 7 8 9 10 Time (s)

Figure 5.8: Error between the Score integration of sint and the ideal − cos t

ADC was scaled and used to drive one of the PWM generators. Both the input square wave and the output

PWM signal were measured and have been included as Figures 5.10a and 5.10b. To show the integrator is operating correctly, the trace was frozen and the width of the response pulse measured in both the positive and negative region of the input signal. If the integrator is functioning properly, the duty cycle of the response should be longer in the positive region of the input than it is in the negative. This is due to the positive accumulation in the integrator and the fact that the PWM output is directly tied to the value of the integral.

The traces included here show that this is the observed trend.

5.2.3 Windup Integrator Algorithmic Test

The system also contains an integrator with an anti-windup feature. A common problem for systems with integrator controllers is the saturation of an actuator (a valve can only be open so far), which leads to a runaway integration term. While the system is saturated, integrators involved in the control law will accumulate to very large values. When the system is released from this state, and can once again control the device, its

ﬁrst response will be to drive the system to the opposite extreme and stay there until the accumulation in the integrator has been dissipated, at which point it will begin to oscillate down towards its set point.

CHAPTER 5: SCORE: TESTING/RESULTS 5.2 TIME DOMAIN CONTROLLER TESTING 48

An anti-windup attempts to address this problem by taking the saturation of the actuator into account. A

simple topology for addressing this problem is shown in Figure 5.11. The applied control signal is recorded

before it is applied to the actuator ’preSat’ and differenced from its value after application ’postSat’. This

difference, when fed back and combined with the input signal ’target’, is used to correct the integrator, and

prevent runaway accumulation.

The effect of the anti-windup is shown in Figure 5.12 where the simple PI controller shown in Figure 5.11

1 Welcome to SEEPS (v0.1) 2 (Systems Engineering Embedded Prototyping System) 3 (C) Bob Sherbert 2010 4 Type ’help’ for a command listing 5 6 $> adc A 1000 7 8 9 $> v ar 10 11 $> defint Iadca adca 1000 12 13 $> defscale SIadca Iadca −1.8 −1.2 0 100 14 15 $> defpwm pw2 SIadca 100 2 1000 16 17 $> defscale SIIadca Iadca −.5 .5 0 100 18 19 $> defpwm pw2 SIIadca 100 2 1000 20 21 $> printvars 22 23 ADCA − 429.000000 24 Iadca − −0.398424 25 SIIadca − 10.158504 26 SIadca − 100.000000 27 adca − −0.239003 28 pw2 − 10.159091 29 30 $> printvars 31 32 ADCA − 611.000000 33 Iadca − −0.492125 34 SIIadca − 0.795748 35 SIadca − 100.000000 36 adca − 0.291789 37 pw2 − 0.795748 38 39 $> printvars 40 41 ADCA − 440.000000 42 Iadca − 0.127512 43 SIIadca − 62.808651 44 SIadca − 100.000000 45 adca − −0.209677 46 pw2 − 62.816864

Figure 5.9: Session to conﬁgure integral test

CHAPTER 5: SCORE: TESTING/RESULTS 5.2 TIME DOMAIN CONTROLLER TESTING 49

(a) Integral Response, Negative Region, 30% Duty (b) Integral Response, Positive Region, 31% Duty Cycle Observed Cycle Observed

Figure 5.10: Hardware testing of the integral feature

is fed a nonsense signal (sine wave). The code used to realize this system in Score is shown in Figure 5.13.

When the anti-windup is disabled (switch ’s1’ open), the integrator saturates on the positive end and never fully comes back down because of accumulation in the integrator. When the anti-windup is enabled (switch

’s1’ closed), the anti-windup resists the tendency of the integrator to accumulate beyond saturation, and when the signal returns to the lower range, the system is able to respond immediately.

Figure 5.11: Topology of a simple anti-windup technique attached to a PI controller

5.2.4 Derivative Response Test Algorithmic Test

The third test was a conﬁrmation of the functionality of the derivative component. This test used the same

pt 1 100 s decimated 2π Hz sine input as the previous two tests. A Score derivative node was created to sample this function and compute its derivative. The result was logged and compared with the ideal cos t. The graph of the error between these two signals is included as Figure 5.14. Note that the error is signiﬁcantly

CHAPTER 5: SCORE: TESTING/RESULTS 5.2 TIME DOMAIN CONTROLLER TESTING 50

Testing the Windup Integrator 2

1.5

0.5

0 Signal (V) −0.5

−1 Input Output w/out Windup Output w/Windup −1.5

−2 0 1 2 3 4 5 6 7 8 9 10 Time (s)

Figure 5.12: Testing the anti-windup shown in Figure 5.11. When s1 is open the anti-windup is disabled and the integrator accumulates beyond saturation, slowing response when the control signal returns to the valid range. When s1 is closed the anti-windup is enabled and resists this accumulation prompting an immediate response when the control signal returns to the valid range.

higher than that of the integrator (10−3 as opposed to 10−6), and that the signal is much more noisy. All this, however, is expected for a numerical derivative. The Score code used to generate this test is included in

Figure 5.15.

1 var 1 var 2 defexp kp 1.0 2 defexp kp 1.0 3 defexp ki 1.0 3 defexp ki 1.0 4 infile sine 100 SINT100.HZ 4 infile sine 100 SINT100.HZ 5 defint Isine sine 100 5 defwind Isine sine 100 1.0 pi satsin 6 defexp pi kp sine ∗ ki Isine ∗ + 6 defexp pi kp sine ∗ ki Isine ∗ + 7 defsat satsin pi −1.0 1.0 7 defsat satsin pi −1.0 1.0 8 log lsin pi 100 PRESAT.LOG 8 log lsin pi 100 PRESAT2.LOG 9 log l2sin satsin 100 POSTSAT.LOG 9 log l2sin satsin 100 POSTSAT2.LOG (a) Without Anti-Windup (b) With Anti-Windup

Figure 5.13: Code for testing the anti-windup

CHAPTER 5: SCORE: TESTING/RESULTS 5.2 TIME DOMAIN CONTROLLER TESTING 51

−3 x 10 Error − SEEPS Derivative vs Theoretical, Sin(t) Sampled @ 100Hz

0 Error (V) −2

−4 Error = cos(t) − d/dt( sin(t) ) [as per SEEPS evaluation]

−6

0 1 2 3 4 5 6 7 8 9 10 Time (s)

Figure 5.14: Error between the Score derivative of sint and the ideal − cos t

Hardware Test

To test the derivative response, a 0.5V ramp input was placed across ADC channel A. One of the PWM

channels was configured to be a proportional output to the configured derivative variable. (The configuration

session is included below) Figure 5.17 shows a sample input and output trace. Figure 5.18 gives a summary

chart showing the measured derivative (expressed as the output duty cycle of the PWM channel) against

the actual derivative of the input slope waveform. Considering that a simple two-point calculation of the

derivative is being used, the results are surprisingly close to the ideal response. This indicates that the time

calculations Score makes in order to perform the derivative are accurate to a high degree.

1 var 2 defint Ione one 100 3 defdiff Done one 100 4 log Lone Ione 100 int.log 5 log Ltwo Done 100 dif.log 6 infile one 100 100SIN.HZ

Figure 5.15: Score code for integration and differentiation tests

CHAPTER 5: SCORE: TESTING/RESULTS 5.2 TIME DOMAIN CONTROLLER TESTING 52

1 Welcome to SEEPS (v0.1) 2 (Systems Engineering Embedded Prototyping System) 3 (C) Bob Sherbert 2010 4 Type ’help’ for a command listing 5 6 $> adc 1 5000 7 8 9 $> v ar 10 11 $> defdiff Dadc1 adc1 500 12 13 14 $> help defpwm 15 16 defpwm (varname) (basename) (refreshFreq) (channel) (f requency ) 17 $> defpwm pw2 Dadc1 100 2 1000 18 19 20 $> printvars 21 22 ADC1 − 72.000000 23 Dadc1 − 22.660624 24 adc1 − 0.114370 25 pw2 − 18.661690

Figure 5.16: Session to conﬁgure derivative test

Figure 5.17: Derivative response sample I/O, input is 120Hz 0.5V ramp

CHAPTER 5: SCORE: TESTING/RESULTS 5.2 TIME DOMAIN CONTROLLER TESTING 53

SEEPS Derivative Response Testing 100 Observed Ideal 90

40 Output Duty Cycle [%]

0 0 10 20 30 40 50 60 70 80 90 100 Input Signal (ADCA) Derivative [V/t]

Figure 5.18: Measured derivative (via PWM duty cycle) vs true ramp waveform derivative

CHAPTER 5: SCORE: TESTING/RESULTS 5.2 TIME DOMAIN CONTROLLER TESTING 54

5.2.5 Integrated System Test Algorithmic PID Test

Having successfully tested each of the components individually, a ﬁnal full system test was conducted to verify that a PID controller could be realized using Score. To perform this test, a model was created in

Simulink representing a standard DC Motor in a servo loop conﬁguration (see Figure 5.19, motor properties listed in Appendix A). This simulated motor was loaded with a constanttorque of 2lb and commandedto seek a particular radial position. When using a simple feedback loop, the motor cannot track the desired position.

To overcome this, a PID controller is added to the feedback loop, to increase the order of the controller and eliminate the steady state error.

Once inserted into the system, the PID controller was tuned using the Ziegler-Nichols tuning method.

The ultimated gain KU was found to be 35, and the oscillation period at this point PU was found to be 0.03s.

The PID gains were found using the Zigler-Nichols equations:

K . K K 2KP K KP PU . (5.1) P =0 60 U = 21 I = PU = 1400 D = 8 =0 0252 and were then further tuned to obtain the ﬁnal values of:

KP = 21 KI = 350 KD =0.0788 (5.2)

Figure 5.19: Simulink model of DC motor in servo conﬁguration

CHAPTER 5: SCORE: TESTING/RESULTS 5.2 TIME DOMAIN CONTROLLER TESTING 55

1 var 2 defdiff Dsgnl sgnl 1000 3 defint Isgnl sgnl 1000 4 defexp Iterm Isgnl Ki ∗ 5 defexp Dterm Dsgnl Kd ∗ 6 defexp Pterm sgnl Kp ∗ 7 defexp PID Pterm Iterm + Dterm + 8 defexp Kp 21 9 defexp Ki 350 10 defexp Kd 0.0788 11 infile sgnl 1000 mpidin.log 12 log LPID PID 500 spidout

Figure 5.20: Score code for PID system test

which were used in the controller. Figure 5.21 shows the position error of the motor while tracking a sinusoid

after the PID controller is inserted, conﬁrming that the controller has improved the performance of the servo

system.

Having established the appropriate gains for a functional PID controller in this system, the Score imple-

mentation may now be tested. Unfortunately, there is presently no way to tie the Score simulation directly into

a simulation of the system physics. In order to ensure that the controller is working properly, a comparative

analysis must be made to a known functional controller.

A known controller is available (the Matlab version). The input to this controller, which is the output

of the system differenced with the set-point, taken from Matlab (’pidin’ Figure 5.19) already embodies the

system physics. The output from this controller (’pidout’ Figure 5.19) interacts with the model of the plant,

and produces the input for the next time step. So long as the Score output of the controller can be correctly

replicated, we know that the response of the plant will be the same, and the same input for the next time step

will be produced. These assumptions, of course, fall apart if the output is not the same because the simulation

of the plant will respond with a different output, invalidating the input stream we have assumed to be correct.

In short, Score goal is to replicate the output of the Matlab controller, given the same input. First, a run

of the system was made in Simulink. The node ’pidin’ in Figure 5.19 was saved to a log ﬁle. This input to

the PID controller was then loaded into Score (Figure 5.20, line 11). The PID controller is then constructed

around this input, and the output logged to a ﬁle. This log is then compared with a log of the node ’pidout’

from Figure 5.19. Provided that the Score PID controller functions the same as Simulink implementation, we

CHAPTER 5: SCORE: TESTING/RESULTS 5.2 TIME DOMAIN CONTROLLER TESTING 56

PID Servo Controller Position Error 0.02

0.015

0.01

0.005

Error (rad) 0

−0.005

−0.01

−0.015 0 0.5 1 1.5 Time (s)

Figure 5.21: Position error of the servo motor after addition of the PID controller

can be conﬁdent that the algorithm is working properly.

A side-by-side comparison of the Matlab and Score signals is shows in Figure 5.22, and a calculation of the difference between them is given in Figure 5.23. From these two graphs, we can see that the error of the

Score controller (with respect to the Simulink version) is relatively low (10−2) and that the signals have the same overall shape and character. This gives us conﬁdence in concluding that Score’ PID controller could be used in place of the Matlab controller in a production system.

Hardware PID Test - Circuit Based

The full system was tested on the circuit in Figure 5.24 to verify that all the components would work when interconnected. This example also demonstrates a key feature of Score - the brevity of expression in which a controller can be expressed. The proportional controller used here is deﬁned by the user in only ﬁve lines of code. A full proportional-integral-derivative controller could be implemented in less than ten lines.

Circuit The circuit used for the test consists of a simple op-amp ﬁlter (Figure 5.24, far right) which has a control signal at its positive terminal and an arbitrary disturbance at the negative terminal. The remainder of the circuit is support to convert the complementary PWM output from Score (at the inputs to the left hand

CHAPTER 5: SCORE: TESTING/RESULTS 5.2 TIME DOMAIN CONTROLLER TESTING 57

Comparison of MATLAB and SEEPS PID Controllers − Run @ 500Hz

SEEPS PID Controller 0.4 MATLAB PID Controller

0.2

−0.2 Control Signal (V)

−0.4

−0.6

0 0.1 0.2 0.3 0.4 0.5 0.6 Time (s)

Figure 5.22: Comparison of the Simulink and Score PID controllers

Output Difference of MATLAB and SEEPS PID Controllers

0.03

0.02

0.01

−0.01

Controller Difference (V) −0.02

−0.03

−0.04

0 0.1 0.2 0.3 0.4 0.5 0.6 Time (s)

Figure 5.23: Difference between Simulink and Score PID controllers

CHAPTER 5: SCORE: TESTING/RESULTS 5.2 TIME DOMAIN CONTROLLER TESTING 58 side op-amps) into a usable control signal. This is done by feeding the PWM signal through the pair of op- amps, which are simple low pass ﬁlters with corner frequencies at 10kHz, to smooth the chip’s PWM output.

The center op-amp acts as an adder of the two signals, allowing the control signal to achieve a range from

[-3, 3]V depending on the duty cycle of the complementary signal. For a cycle of 0-50% the lower channel is dominant and the output of the adder is negative. For a cycle of 51-100% the upper channel dominates and the signal is positive. Score was inserted to the circuit by attaching ADC channel A in differential mode across the lower half of the far right hand side voltage divider. PWM port B was attached at the two inputs on the left hand side op-amps.

Figure 5.24: System test circuit

CHAPTER 5: SCORE: TESTING/RESULTS 5.2 TIME DOMAIN CONTROLLER TESTING 59

Score Conﬁguration Score was conﬁgured to act as a negative feedback proportional controller targeting

a -1V output. The session establishing this conﬁguration is provided in Figure 5.25. The key point to observe

is that the controller in implemented in only ﬁve user commands.

1 Welcome to SEEPS (v0.1) 2 (Systems Engineering Embedded Prototyping System) 3 (C) Bob Sherbert 2010 4 Type ’help’ for a command listing 5 6 $> adc A 1000 7 8 $> v ar 9 10 $> defexp target adca 1 − 11 12 − 13 adca 14 1.000000 15 16 $> printvars 17 18 ADCA − 624.000000 19 adca − 0.065982 20 target − −1.221408 21 22 $> printvars 23 24 ADCA − 622.000000 25 adca − 0.324047 26 target − −0.954545 27 28 $> defscale Starget target −1.5 1.5 0 100 29 30 $> printvars 31 32 ADCA − 409.000000 33 Starget − 6.744868 34 adca − −0.297654 35 target − −1.253666 36 37 $> defpwm pw2 Starget 1000 B 10000

Figure 5.25: Code conﬁguring Score as a negative feedback controler

Results A number of different disturbance signals (Vdisturbance in Figure 5.24) were applied to the ﬁlter

to see if the controller could stabilize them. In Figure 5.26a and Figure 5.26b the uncompensated and com-

pensated signals when a 2V 500Hz sine wave was used as a disturbance are shown. Figure 5.26b shows

that Score was able to damp out large portions of the disturbance signal. The one unsatisfactory result is the

approximately 0.5V steady state error in the compensated signal. This is a discrepancy from the 1V target

that was set for the system and has not yet been accounted for. The same trend is shown in Figures 5.26c

CHAPTER 5: SCORE: TESTING/RESULTS 5.2 TIME DOMAIN CONTROLLER TESTING 60 and 5.26d which show an uncompensated and compensated 1V square wave at 500Hz. The same trends are observed in terms of damping of the disturbance and the steady state offset.

(a) 500Hz 2V Sine Wave Input and Uncompensated (b) 500Hz 2V Sine Wave Input and Compensated Output Signal Ampliﬁed Signal Channel 1: VDisturbance=2VP-P 510Hz Sine , Channel 1: VDisturbance=2VP-P 510Hz Sine , Channel 2: Vout 1.3VP-P 510Hz Sine Channel 2: Vout 0.2VP-P 510Hz Sine

(c) 500Hz 1V Sine Wave Input and Uncompensated (d) 500Hz 1V Sine Wave Input and Compensated Ampliﬁed Signal Ampliﬁed Signal Channel 1: VDisturbance=1VP-P 500Hz Square , Channel 1: VDisturbance=1VP-P 500Hz Square , Channel 2: Vout 1VP-P 500Hz Square Channel 2: Vout 0.14VP-P 500Hz Square

Figure 5.26: Negative feedback controller testing

CHAPTER 5: SCORE: TESTING/RESULTS 5.2 TIME DOMAIN CONTROLLER TESTING 61

5.3 Summary

This section detailed the design and testing of the Score system. Score allows a user to easily conﬁgure and interrelate the various peripheral subsystems of a micro controller using a state based representation. By interrelation of the states it is possible to simply and easily craft a controller for an embedded system. The platform can be used both for developing simple controllers and for building interfaces to analog sensors.

Testing of all the major components has been detailed for both algorithmic and actual hardware implementations.

CHAPTER 5: SCORE: TESTING/RESULTS 5.3 SUMMARY 62

Chapter 6: Conclusions

This thesis makes the assertion that current software tools are inadequate for addressing the implementation requirements of high degree of freedom systems. In support of this, numerous failures of the discipline are noted, fragmentation and duplication of effort amongst existing tools are described, and the inability to effectively prototype complex systems is indicated. As a solution to the problem this thesis posits a shift in the abstractions used within robotics development environments. The shift is from a current representation in terms of a ’component driven’ approach to a ’state based’ representation.

To explore the effectiveness of the state based representation for robotics software, two systems have been implemented. The ﬁrst, titled Conductor, is a framework which acts to extend existing high level RDEs and allow a controller designer to interact with subcomponents (sensors and actuators) of the system in terms of simple state variables. The second, called Score, develops the same type of representation for micro controller platforms by allowing the chip’s peripherals (ADCs, PWM generators, etc) to be accessed directly through variables within the system. Using these systems

Below are design results for each system in terms of the objectives set out in Sections 1.2.1 and 1.2.2.

Example systems were detailed in Chapters 3 and 5 respectively.

6.1 Results from Conductor

The main goals for the Score system are restated here, along with their current completion status.

1. State Representation of System Parameters - The framework provides a representation in which any

peripheral can be easily integrated into the system and represented as an appropriate state. Once the

state representation is designed, interaction with other states, controllers, and hardware is easy.

2. Time Domain Control - Time domain control based on system states can be implemented simply

using the built in scripting language.

3. State Space Control - While frequency domain control has not been directly implemented within the

system, a UDP interface has been provided which allows the package to interact with others that are 63

more suited to carrying out such calculations (Matlab).

4. Simple Add-On Interfaces - The framework provides a number of tools, utilities, and approaches for

integrating new peripherals easily.

5. High Degree of Freedom Control - The software, which is highly threaded, scales easily and linearly

as the complexity of the system being controlled increases in complexity.

6.2 Results from Score

The main goals for the Score system are restated here, along with their current completion status.

1. Dynamically Conﬁgurable Hardware - Finished for the Luminary Micro Stellaris chip set Cortex-

M3 processor family. Peripherals such as the ADC, PWM generator, GPIO, Tachometer and UART

can be conﬁgured in one-two function calls each.

2. Signal Based Programming - Finished: The conceptlaid out for the system has been achieved entirely.

3. Automatically Conﬁgured Sampling Frequencies - Finished: The functionality for frequency conﬁg-

uration and the appropriate processor interrupt system have been delegated to the core of the FreeRTOS

kernel which runs the system.

4. Support for Time and Frequency Domain Controllers - Support for time domain controllers is

provided through the signal based programming approach used within the Score system. Support for

frequency domain controllers has sadly been unachievable with the Stellaris hardware. The system will

need to be ported to chip set with vector operation support to provide this.

5. Hard Real Time - Once the system has been initially been conﬁgured the core of the system updates

happen in a hard RT fashion.

6. Hardware Abstraction Layer - The hardware conﬁguration algorithms have been designed in such

a way as to allow easy porting to another architecture should it be required. The conﬁguration for

each peripheral is separated into a series of function calls which are independent. The success of the

abstraction can be seen in the existence of the x86 port of the system.

CHAPTER 6: CONCLUSIONS 6.2 RESULTS FROM SCORE 64

7. Cost - Estimates indicate that a basic controller running the Score software can be produced for $20

in bulk quantities.

6.3 Contributions

The Conductor and Score programs open a number of avenues towards a future of increasingly complex robotics research. With many of the barriers to scaling and portability reduced or removed, it will be possible for researchers to address problems of much greater complexity than in the past. Projects like the 3-tier humanoids framework described in the introduction, which incorporates elements of scaling, simulation, and actual system testing can begin to become the norm in robotics. It will no longer be the case that robotics is conﬁned to a testing loop that includes only a full scale prototype - an expense in terms of fragility and cost.

Instead, progress can be made towards examining the forms of robots themselves. The community can begin to ask questions such as ”How do similar equations or controllers play out at different scales?”

When reimplementing equations on different systems becomes easy, the replication of experiments, something of a rarity in the robotics world, can become more common. This in turn increases the quality and speed of the scientiﬁc discourse.

6.4 Future Possibilities

The tools provided in Conductor and Score open new techniques previously unavailable to researchers. Par- ticularly, the ability to easily perform experiments relying on the concepts of scaling or simulation become much easier. Applications of high complexity or expense are particularly suited to this approach. Among possible avenues opened by these new tools are research into various forms of walking robots (bipedal, quadrupedal, etc), airborne robots with manipulators, and other highly articulated robots. It is in these high degree of freedom systems where the tools described here are especially useful.

CHAPTER 6: CONCLUSIONS 6.3 CONTRIBUTIONS 65

Bibliography

[1] Ill-Woo Park, Jung-Yup Kim, Jungho Lee, and Jun-Ho Oh. Mechanical design of humanoid robot platform KHR-3 (KAIST humanoid robot 3: HUBO). In Humanoid Robots, 2005 5th IEEE-RAS Inter- national Conference on, pages 321–326, December 2005. [2] Marc Raibert, Kevin Blankespoor, Gabriel Nelson, Rob Playter, and the BigDog Team. BigDog, the Rough-Terrain quadruped robot. In Proceedings of the 17th World Congress The International Federa- tion of Automatic Control, July 2008. [3] B. H. Wilcox. ATHLETE: A cargo and habitat transporter for the moon. In Aerospace conference, 2009 IEEE, pages 1–7, 2009. [4] James Kramer and Matthias Scheutz. Development environments for autonomous mobile robots: A survey. Autonomous Robots, 22:101–132, 2007. [5] N. Mohamed, J. Al-Jaroodi, and I. Jawhar. Middleware for robotics: A survey. In Robotics, Automation and Mechatronics, 2008 IEEE Conference on, pages 736–742, 2008. [6] Azamat Shakhimardanov, Jan Paulus, Nico Hochgeschwender, Michael Reckhaus, and Gerhard K. Kraetzschma. Best practice in robotics: Best practice assessment of software technologies for robotics. Technical report, Bonn-Rhein-Sieg University, 2010. [7] Toby H. J. Collett and Bruce A. Macdonald. Player 2.0: Toward a practical robot programming framework. In in Proc. of the Australasian Conference on Robotics and Automation (ACRA, 2005. [8] Morgan Quigley, Ken Conley, Brian Gerkey, Josh Faust, Tully Foote, Jeremy Leibs, Rob Wheeler, and Andrew Y. Ng. ROS: an open-source robot operating system. In ICRA Workshop on Open Source Software, 2009. [9] Rosen Diankov and James Kuffner. OpenRAVE: A planning architecture for autonomous robotics. Technical Report CMU-RI-TR-08-34, Robotics Institute, Pittsburgh, PA, July 2008. [10] J. Jackson. Microsoft robotics studio: A technical introduction. Robotics Automation Magazine, IEEE, 14(4):82–87, December 2007. [11] Brian Gerkey, Kasper Støy, and Richard T. Vaughan. Player robot server. Technical report, November 2000. [12] Webots Reference Manual, 6.3.1 edition, October 2010. [13] Alex Brooks, Tobias Kaupp, Alexei Makarenko, Stefan Williams, and Anders Oreb¨ack. Orca: A component model and repository. In Davide Brugali, editor, Software Engineering for Experimental Robotics, volume 30 of Springer Tracts in Advanced Robotics, pages 231–251. Springer Berlin / Heidelberg, 2007. [14] Peter Soetens. A Software Framework for Real-Time and Distributed Robot and Machine Control. PhD thesis, Department of Mechanical Engineering, Katholieke Universiteit Leuven, Belgium, May 2006. [15] Richard Barry. Using the FreeRTOS Real Time Kernel. 1.3.0 edition, 2010. [16] eCos Reference Manual, 3.0 edition, 2008. [17] Qing Cao, Tarek Abdelzaher, John Stankovic, and Tian He. The LiteOS operating system: Towards Unix-Like abstractions for wireless sensor networks. In Proceedings of the 7th international conference on Information processing in sensor networks, IPSN ’08, pages 233–244, Washington, DC, USA, 2008. IEEE Computer Society. 66

[18] P. Levis, S. Madden, J. Polastre, R. Szewczyk, K. Whitehouse, A. Woo, D. Gay, J. Hill, M. Welsh, E. Brewer, and D. Culler. TinyOS: An operating system for sensor networks. In Werner Weber, Jan M. Rabaey, and Emile Aarts, editors, Ambient Intelligence, chapter 7, pages 115–148. Springer-Verlag, Berlin/Heidelberg, 2005. [19] David Gay, Matt Welsh, Philip Levis, Eric Brewer, Robert Von Behren, and David Culler. The nesC language: A holistic approach to networked embedded systems. In In Proceedings of Programming Language Design and Implementation (PLDI, pages 1–11, 2003. [20] Robert Ellenberg, Robert Sherbert, Paul Y. Oh, Alex Alspach, Roy J. Gross, and JunHo Oh. A common interface for humanoid simulation and hardware. pages 587–592, December 2010. [21] Herman Bruyninckx,Peter Soetens, and Bob Koninckx. The real-time motion control core of the Orocos project. In IEEE International Conference on Robotics and Automation, pages 2766–2771, 2003. [22] David S. Touretzky and Ethan J. Tira-Thompson. Tekkotsu: A framework for AIBO cognitive robotics. In The Twentieth National Conference on Artificial Intelligence (AAAI-05). Association for the Ad- vancement of Artificial Intelligence, July 2005. [23] Stéphane Magnenat, Philippe Rétornaz, Michael Bonani, Valentin Longchamp, and Francesco Mon- dada. ASEBA: A modular architecture for Event-Based control of complex robots. IEEE/ASME Trans- actions on Mechatronics, 16(2):321–329, April 2011. [24] M. E. Lesk and E. Schmidt. Lex a lexical analyzer generator. Comp. Sci. Tech. Rep., 39, October 1975.

BIBLIOGRAPHY 67

Appendix A: DC Motor Model Parameters

2 −4 oz · in J =0.0038oz in s B =9.55 ∗ 10 rad/s

−3 Ra =1.64Ω La =3.39 ∗ 10 H (A.1)

oz · in V Kt = 10.02 A Kv =0.0708rad/s 68

Appendix B: Primer on RT Systems & FreeRTOS

B.1 Function/Purpose

On a typical multitasking computer system little concern is given to priority scheduling. Generally speaking, hardware interrupts (disk access, networking cards, etc), and operating system functions have the highest priority, and other tasks are all lumped together. A simple scheduler in the operating system kernel decides how to divvy up processor time between the hundredsof tasks that vie for usage rights. It’s an inexact process that often fails to allot resources the same way the user might, given the choice. (If you have ever had your mouse cursor hang, or your typing outpace your word processor’s ability to keep you, you are a victim of this lackadaisical scheduling.)

There is nothing wrong with this type of design, it has worked perfectly well for a long time. It is the consequence of design decisions made early on in the history of multitasking computers which have been carried over to today. For most users, the occasionally inoperative mouse is a non-issue. The problem also goes relatively unaddressed because it is easy to outgrow with increases in hardware functionality. A faster processor will, from the user perspective, solve the problem just as well as redesigning the operating system internals. And since chip manufacturers were going to keep increasing those speeds anyway...

Fixing this ’problem’ is a complicated task that involves special programming at the kernel level of the computer, but is occasionally needed. For example: what if the cursor-hang problem suddenly developed in the computer running the tail ﬂaps of a commercial air-liner?1 For such tasks a special kind of operating system called a Real-Time (RT) OS is used. The difference between an RT system and a non-RT one lies in the scheduler. The RT system uses a scheduler in which every task has a deﬁned relative priority. Any number of tasks can be ’active’ at one time. When a task is active, it is essentially requesting processor resources.

The highest priority function which is active at any time is the one which receives the CPU time to complete its execution. If multiple tasks have the same priority, the scheduler will alternate between them until one has

ﬁnished. 1A: A lot of people have a very bad day. 69

B.2 Concepts/Terminology

• Kernel - A kernel is the core of an operating system. It acts as the bridge between software and

hardware within the system. It is responsible for such critical tasks as hardware management, memory

allocation and deallocation, and process management. Every computer system has a kernel. A user

will never deal directly with the kernel and programmers do so only to implement things like device

drivers and RT systems.

• Bare-Metal - A program built to run without an operating system is said to run ’Bare-Metal’. Tech-

nically speaking, a bare metal program is a kernel unto itself, though such a program will not usually

implement features such as memory or process management because it is not sharing system resources

with other programs.

• Thread/Process - Parallel sequences of execution within a computer system are known as threads/pro-

cesses. They contain information about the sequence of commands to execute, as well as memory

allocated for their particular task. When they exist at the program level they are known as a processes,

when they exist within an individual program, they are called a threads. For real time design, it is very

important to keep in mind that a thread can be interrupted during any instruction which is not an atomic

operation.

• Atomic Operation - An operation which can be completed in one instruction cycle of a processor. I.e.

An operation embodied in a single assembly-code instruction (with the exception of shortcut codes that

embody multiple instructions.)

• Scheduler - Computers are inherently serial (with the exception of multicore systems, which will not

be discussed here), and can only be executing one sequence of instructions at a time. When multiple

sequences need to be run at once, a method must exist for switching between the separate lines of

execution without loss of information. The scheduler is the algorithm within the kernel which makes

decisions about which thread should have control of the processor at any given time. (Remember,

because the scheduler is itself run within a thread, it is not always running.)

APPENDIX B: PRIMER ON RT SYSTEMS & FREERTOS B.2CONCEPTS/TERMINOLOGY 70

• Heaps - A heap is an allocated portion of memory that a thread has total control over. It is assigned to

a thread by the kernel, and is reserved for use only by that thread until it is released by the thread.

• Thread Safe Operation - A operationis said to be ’threadsafe’ if it can be interrupted during any point

of its execution and not leave data in an unusable state. This means that any complex data structures

the thread accesses must be guarded during usage, the

• Queue - Generally speaking, a ﬁrst-in ﬁrst-out (FIFO) data, structure. In the case of FreeRTOS, the

Queue is the basic thread-safe data structure provided by the OS.

• Blocked/Blocking - A task is said to be blocked when it is waiting for another task to release some

resource that it requires to ﬁnish execution.

B.3 Types of RT Systems

B.3.1 RT Systems on Micro controllers

Real time systems on micro controllers generally implement a minimalist kernel which provides memory management and scheduler services. They do not usually provide peripheral hardware support, because access to peripheral hardware on micro controllers varies greatly between manufacturers. Real time kernels for micro controllers generally aim to support numerous hardware architectures, which and range anywhere from ﬁve to the high twenties. There are dozens of such RT systems on the market including both free and commercial solutions.

B.3.2 RT Systems on x86 Computers

Real time systems on desktop computers follow one of three routes. They can exist as a micro-kernels, as modiﬁcations to an existing kernel, or as a bare-metal implementation.

Design patterns following the micro-kernel approach create a miniature kernel with basic functionality that runs a regular operating system as one of its tasks. That is, the regular operating system still exists on the computer, but the micro-kernel has control over it, and can suspend it in order to give other prioritized tasks control of the system. Using this approach, access to system hardware (DAQ card, CAN card, etc) requires special drivers to be implemented. Additionally communication between the prioritized tasks and tasks within

APPENDIX B: PRIMER ON RT SYSTEMS & FREERTOS B.3TYPES OF RT SYSTEMS 71 the slave operating system requires special effort. Projects which take this approach include RTAI (Real Time

Application Interface), and Xenomai for Linux, and RTX (Real Time Extensions) for Windows.

Designs that modify an existing kernel are a second approach to the problem. They involve directly changing the kernel source of existing operating systems to make that system real time. Modern operating system kernels, however, consist of millions of lines of code. Being certain that the system is truly real time under such conditions is difﬁcult at best. This approach is used widely, as it is the easiest to implement from the user’s perspective. Project that use this design pattern include the CONFIG PREEMPT RT patch for the

Linux, Wind River Linux, Windows Embedded and Windows CE.

The ﬁnal design approach is the bare metal approach, in which an entirely new kernel is written. This is an uncommon approach on desktop computers because the x86 architecture is incredibly complex. One notable exception to this is Wind River VxWorks, one of the most widely deployed RT systems.

B.4 FreeRTOS

FreeRTOS (Real-Time Operating System), is not an operating system in the traditional sense. It follows the standard design pattern for a real time system for micro controllers. It does not provide user interface elements, or functions for interacting with systems peripherals. FreeRTOS provides only a scheduler, memory manager, basic data structure implementations, and functions for conﬁguration/debugging applications written using it.

FreeRTOS operates as a collection of independent user created tasks which share resources according to the scheduler’s decision making. When a task becomes active, the scheduler gives it control of the processor.

The task will run its associated function until completion, or until another task becomes active. If two tasks are active at the same time, the scheduler will decide which executes based on the given priority values. If the priorities are the same, the scheduler will assign alternating time chunks to the tasks until one completes.

B.4.1 Tasks

To implement a task in FreeRTOS the user must provide three things. First, and most importantly, the user must provide a handle to the function which the task will oversee. It is this function which is executed by the task each time the task is woken by the scheduler. Second, the user must provide a memory heap for the task

APPENDIX B: PRIMER ON RT SYSTEMS & FREERTOS B.4FREERTOS 72 to use, this heap must be large enough to satisfy the given function’s usage requirements. Additionally, the user must provide a priority value for the task. The priority is used in determining which task controls the processor if a conﬂict occurs.

B.4.2 Inter-Task Communication

It is a common requirement for multiple tasks to communicate with each other by passing data. In order to do this safely within the RT framework, thread safe data structures must be implemented. FreeRTOS provides such a data structure through its Queue implementation. When a queue is created, it is allocated memory from the system’s heap (as opposed to an individual task’s heap). Each task wishing to communicate is given a reference to this queue. When a task wishes to write to the queue it ’locks’ the queue by marking a ﬂag within the queue’s memory (an atomic operation). It then adds the data it wishes to pass to the end of the queue (non-atomic). If another task attempts to write to the queue during this time, it will see that the queue has been locked and wait until the lock is released before performing its operation. When the task is done writing to the queue it removes the lock ﬂag. The process to read from the queue is identical with respect to the locking procedure. The difference is that the write operation is replaced by a copy operation followed by a delete operation (to remove the element).

APPENDIX B: PRIMER ON RT SYSTEMS & FREERTOS B.4FREERTOS