Design Construction and Control of An in

Design, Construction
and Control of an
Industrial Scale Biped Robot
Volume 1
Joe Cronin
Supervisors
Associate Professor RA Willgoss
Associate Professor R Ford
Revision 2.0
December 2005
A thesis submitted in fulfilment of the requirements
for the degree of doctor of philosophy.
ABSTRACT
A 500Kg, self-contained biped robot, named Roboshift, has been conceived and tested
to investigate issues associated with the control of industrial scale biped robots. This
project represents the first credible attempt to build a heavy weight autonomous biped
robot. The recent expansion in humanoid robot development has highlighted advances
made in anthropomorphic biped technology. Current research into speech recognition,
vision systems, laser topography, artificial intelligence and electroactive polymers will
ultimately achieve an Android capable of human like actions and thought processes.
Justification for this most demanding and expensive research is based on
philanthropic models that suggest these robots will attend to the bedridden, or replace
humans in dangerous areas. However, the cost of a biped robot when compared to that of
a wheeled or tracked vehicle restricts commercialisation for these applications. As well,
the size and working capacity of current humanoid robots is not compatible with the
heavy lifting requirements found in such environments.
It is proposed that only biped robots of an industrial scale, possessing a capacity much
greater than that of a human, will be of commercial value in the future. Typical
applications may include the handling of materials in confined or uneven terrain, where
a forklift or other commercially available materials handling equipment would be
unsuitable. For example, field handling in military, mining or geological environments.
Minimal research has been conducted into the realisation of such a device, which
presents challenges in terms of the magnitude of dynamic forces produced and of the
systems required to control the robot in real-time.
Review of relevant literature reveals that little research has been completed in this field.
Therefore, operational characteristics for an industrial scale biped robot are defined. The
design then details the structure and integration of mechanical, hydraulic, and electrical
systems. Roboshift is powered by an internal combustion engine and is the first biped
robot with a capacity for extended operation. Modelling was conducted to determine
joint trajectories, power requirements, hydraulic flow parameters and dynamic
characteristics. The robot is controlled by a distributed, hierarchical system comprising
sixteen microprocessors, a control computer acting as the midbrain and a
communications computer acting as the central nervous system. Sensors measure attitude
Abstract
and heading (vestibular system) as well as ground reaction forces and joint angles
(propreoception). The control strategy is based on feed forward trajectories generated by
inverse kinematic analysis. Corrections to trajectories are made in real time by higher
level routines running on the main control computer. Joint position is achieved by local
feedback control. Software for the robot was written in the C language.
Experimental results are presented detailing the performance of the robot in
comparison to theoretical analysis. After construction and testing of actuators and
sensors, calibration software was tested successfully. Once calibrated, the robot was
lowered to the ground where the active balance software was able to control the robot in
the frontal and sagittal planes. Frontal sway software was tested with mixed success as
natural oscillation of the structure, which was not detectable by the control system, led
to erroneous force data. Detailed dynamic modelling was then completed to determine
the causes of oscillation in the robot. The modelling led to the formulation of a control
strategy where non-collocated sensors are used to measure link strain as a feedback to a
modified proportional controller.
The project has demonstrated that an industrial scale biped incorporating an internal
combustion engine and hydraulic power system is feasible.. Analysis presented
proposes that as the height of a biped robot increases, the expected elastic deformation
of the structure increases as the cube of the height, making control extremely
challenging. A strategy for the control of heavy-weight robots is suggested It is also
proposed that technology incorporated in current humanoid robots can not be scaled to
control industrial bipeds.
TABLE OF CONTENTS
Abstract
Acknowledgements
Contents
List of Figures, Tables and Flowcharts

List of Special Names and Abbreviations
VI
XII
1.0 Introduction
1.1 The Quest for Biped Motion
1-1
1.2 Embodiments of Bipedal Motion
1-4
1.3 About this Project
1-7
1.4 Structure of this Document
1-8
2.0 Walking Robots

2.1 Walking robots
2-2
2.1.1 Wheels versus Legs
2-4
2.1.2 Legged Robots
2-6
2.2 Biped Robots
2 - 10
2.3 Control of Biped Robots
2 - 22
2.4 Design Criteria of an Industrial Biped
2 - 33
2.5 Conclusion
2 - 36
3.0 Mechanical Design of Roboshift

3.1 Design Philosophy
3-1
3.2 Mechanical Configuration
3-4
3.2.1 Basic Structure
3-4
3.2.2 Configuration of Joints
3-6
3.2.3 Range of Movement
3-8
3.2.4 Mechanical Design for Aspects of Control System

3.2.4.1
Transducers
3 - 10
3 - 11
I
Table of Contents
3.2.4.2
Actuators
3 - 12
3.2.4.3
Power and Electrical Systems
3 - 13
3.3 Detail Mechanical Design
3 - 14
3.3.1 Feet
3 - 14
3.3.2 Knees, Lower and Upper Legs
3 - 21
3.3.3 Hip
3 - 23
3.3.4 Body
3 - 25
3.3.5 Construction
3 - 26
4.0 Power and Electrical Design

4.1 Hydraulic Design
4-2
4.1.1 Flow Calculation
4-2
4.1.2 Hydraulic Circuit Design
4-4
4.1.3 Conclusions on Hydraulic Design

4.2 Electrical Design
4 - 10
4 - 10
4.2.1 Electrical Power Requirements
4 - 10
4.2.2 Circuit Protection
4 - 11
4.2.3 Power Distribution
4 - 12
4.2.4 Wiring
4 - 14
4.2.5 Conclusions on Electrical Power
4 - 15
4.3 The Engine
4 - 15
4.3.1 Engine Power
4 - 16
4.3.2 Engine Selection
4 - 18
5.0 Control System Philosophy

5.1 Robot vs Cybermachine
5-1
5.2 Control System Overview
5-3
5.3 Control Strategy
5-5
5.4 Walking Control
5-8
II
Table of Contents
5.5 Software Structure
5-9
6.0 Control System Hardware

6.1 Main Control Computer (PC)
6-1
6.2 Communications PC
6-3
6.3 Local Joint Controllers
6-3
6.4 Sensors
6-6
6.4.1 Joint Movement (Opto Encoders & Limit Switches)
6-6
6.4.2 Attitude (Gyro & Flux Gate Compass)
6-9
6.4.3 Force (Strain Gauges and Amplifiers)
6 - 12
6.4.4 Foot Contact Switches
6 - 15
6.5 Conclusions on Control Hardware
6 - 15
7.0 Control System Modelling

7.1 Kinematic Simulation
7-1
7.2 Dynamic Simulation
7-4
7.3 Formulation of Joint Trajectories

Conclusion
7 - 12
7 - 13
8.0 Control System Software

8.1 Main Control Program
8-1
8.1.1 Data Acquisition Routine
8-4
8.1.2 Maintenance Routines
8-6
8.1.3 Calibration Routine
8-7
8.1.4 Locomotion Routine
8 - 10
8.1.4.1
Frontal Balance
8 - 19
8.1.4.2
Frontal Sway
8 - 21
8.2 Communications Program
8 - 24
8.2.1 Receiving Data from The Microprocessors
8 - 26
8.2.2 Sending Data
8 - 28
8.3 Local Joint Control Software
8 - 30
III
Table of Contents
8.3.1 Main Control Loop
8 - 30
8.3.2 Interrupt Service Routine
8 - 32
8.3.3 Calibration Routine
8 - 35
8.3.4 Motion Control Routine
8 - 38
8.4 Summary of Control Software Implementation
8 - 40
8.5 Software Development and Testing
8 - 40
8.6 Starting the Robot
8 - 46
9.0 Operational Trials on Roboshift

9.1 Transducer Calibration and Performance
9-1
9.1.1 Force Transducers
9-1
9.1.2 Artificial Horizon and Compass
9-6
9.1.3 Shaft Encoders
9-7
9.1.4 Electronic Noise
9-8
9.2 Power Pack
9 - 10
9.3 Joint Control
9 - 12
9.4 Initialisation Program
9 - 13
9.5 Stationary Balancing Program
9 - 15
9.6 Locomotion
9 - 18
9.5.1 Frontal Sway
9 - 19
10.0 Discussion
10.1 Mechanical Design
10 - 1
10.1.1 Compliance
10 - 1
10.1.2 Joints
10 - 2
10.1.3 Hydraulic Actuation
10 - 3
10.2.1 Power
10 - 4
10.2.2 Electronic Noise
10 - 5
10.3 Control Hardware

10.3.1
Sensors
10 - 5
10 - 6
IV
Table of Contents
10.4 Control Software
10 - 8
10.4.1
Local Joint Control
10 - 8
10.4.2
Communication Software
10 - 9
10.4.3
Calibration
10 - 9
10.4.4
Balance
10 - 10
10.4.5
Sway
10 - 10
10.4.6
Project
10 - 11
11 0 Additional Modeling
11.1 Modeling Strategy
11 - 2
11.2 Matlab Model
11 - 3
11.3 Effects of Link Stiffness
11 - 5
11.3.1 Frontal Sway Driven by Feed Forward

11 - 6
Sinusoidal Local Joint Trajectories using a range of Link Stiffness
11.3.2 Frontal Sway Driven by Feed Forward Sinusoidal
Local Joint Trajectories using a Calculated Member Stiffness
11 - 10
11.4 System Response to Step Input
11 - 11
11.5 System Response to Square Wave Force Input
11 - 15
11.6 Force Feedback Control
11 - 17
11.7 Strain Feedback Control
11 - 21
11.8 Summary of Simulation
11 - 29
12.0 Conclusion
12.1 Achievement of design criteria
12 - 1
12.2 Novel research
12 - 3
12.3 Future work
12 - 4
12.0 References & Bibliography
R-1
LIST OF FIGURES
Figure
Name
Page
1.1
Robby the Robot
1-1
1.2
Kawadas HRP2, Hondas Asimo and

Toyotas trumpet playing humanoid
1-2
1.3
Roboshift
1-7
2.1
Japanese toy Android
2-6
2.2
Ryggs mechanical horse
2-7
2.3
The Adaptive Suspension Vehicle
2 - 10
2.4
Range of biped robot weights
2 - 11
2.5
Classes of legged robots
2 - 13
2.6
The GEC Hardiman exoskeleton
2 - 17
2.7
Waseda Universitys WH 11
2 - 19
2.8
Hondas humanoid robot Asimo
2 - 20
2.9
Scatter plot of mass vs height of documented biped robots
2 - 21
2.10
All terrain forklift
2 - 36
3.1
Device configuration decision matrix
3-2
3.2
Schematic of degrees of freedom
3-5
3.3
Hip, knee, ankle and foot angles (a) interpolated

from Braume (b) from the modified Hartrum model
3-9
3.4
Mounting of shaft encoder
3 - 11
3.5
Hip extention shaft encoders
3 - 12
3.6
Knee extension cyclinder
3 - 13
3.7
Various concepts for foot design
3 - 15
3.8
Range of movement of foot
3 - 16
3.9(a)
Rearview of a Roboshifts foot
3 - 17
3.9(b)
Sideview of a Roboshifts foot
3 - 17
3.10
Range of movement of foot
3 -18
3.11
Initial design of ankle frame
3 - 19
VI
3.12
Ankle assembly
3 - 20
3.13
Ankle/lower leg swing arm connection
3 - 21
3.14
Swing arm arrangement
3 - 22
3.15
Knee and thigh assembly
3 - 23
3.16
Hip rotation mechanism
3 - 24
3.17
Layout of hips
3 - 24
3.18
Initial design of Robotshift showing operator cabin
3 - 26
3.19
Mechanical structure of Roboshift
3 - 27
4.1
Joint angles over single gait cycle
4-2
4.2
Total flow requirements for single gait cycle
4-3
4.3
Total hydraulic flow
4-4
4.4
Hydraulic valve manifolds
4-5
4.5
Hydraulic circuit drawing
4-7
4.6
Hydraulic power pack
4-8
4.7
VT1001 hydraulic valve control amplifier enclosure
4-9
4.8
Main distribution enclosure
4 - 12
4.9
Electrical distribution circuit
4 - 13
4.10
Main distribution panel
4 - 14
4.11
Briggs & Statton 20Hp Vanguad
4 - 20
5.1
Boundary control of joint trajectories
5-6
5.2
Inputs to control system
5-7
5.3
Feed forward joint trajectory generation
5-8
5.4
Data network
5 - 11
5.5
Flow of data
5 - 12
6.1
Schematic of control system
6-2
6.2
The F1 board and the expanded I/O board
6-4
6.3
Microprocessor enclosure
6-5
6.4
Arrangements of shaft encoders
6-8
6.5
Fluxgate compass mounting
6 - 10
VII
6.6
Artificial horizon
6 - 10
6.7
Air pump controller
6 - 11
6.8
Strain gauge locations
6 - 12
6.9
Strain gauge amplifier enclosure
6 - 13
6.10
Swing arm strain gauge configuration
6 - 13
6.11
Ankle strain gauge configuration
6 - 13
6.12
Signal conditioning modules
6 - 14
6.13
Foot contact switches
6 - 15
7.1
Output of Hartrum model for lower limbs
7-2
7.2
Modified foot model
7-3
7.3
Modified kinematic model for lower limbs and hip
7-5
7.4
Full kinetic model of human locomotion
7-5
7.5
Upper body segments for two gait cycle iteration
7-6
7.6
Angular accelerations of upper body segments
7-8
7.7
Forces and torques produced at the hip by

motion of upper body segments
7-9
7.8
Basic geometry of single mass attached to hips
7-9
7.9
Displacement of single mass suspended at hip.
7 - 11
7.10
Motion of counterbalance suspended from hips.
7 - 11
7.11
Schematic of lower limbs in the sagittal plane
7 - 12
7.12
Hip, knee and ankle angles
7 - 13
8.1
Control system software
8-1
8.2
Main control program display
8-6
8.3
Motion control routine display
8 - 18
8.4
Weight transfer via hip abduction
8 - 19
8.5
Frontal trim limits
8 - 21
8.6
Leg geometry
8 - 22
8.7
Communications program display
8 - 26
8.8
Proportional valve characteristics
8 - 35
VIII
8.9
Small wheelled test robot
8 - 43
8.10
Output data from wheeled robot
8 - 44
8.11
Error and control signals from PID control
8 - 45
8.12
Error and control signals of fuzzy control
8 - 45
9.1
Strain gauge calibration rig
9-2
9.2
Calibration of left foot strain gauge (front loading)
9-3
9.3
Calibration of left foot strain gauge (heel loading)
9-4
9.4
Strain gauge data as robot is lowered to the floor
9-4
9.5
Testing of strain gauges
9-5
9.6
Strain gauge outputs under full frontal sway
9-6
9.7
Calibration of artificial horizon
9-7
9.8
Spikes on shaft encoder output
9-9
9.9
Spikes on shaft encoder supply
9-9
9.10
High frequency interference
9-9
9.11
High frequency noise
9.- 9
9.12
Hydraulic oil temperature and 12V current draw during engine test
9 - 10
9.13
Step response of hip abduction cyclinders
9 - 12
9.14
Step response using determined zero control value
9 - 13
9.15 (a -h) Calibration of foot cylinders and amplifiers
9 - 14
9.16
Robot hanging stance position
9 - 16
9.17
Robot in active balance trim limits set between 0.45 and 0.55
9 - 17
9.18
Robot in active balance with trim limits set beween 0.48 and 0.52
9 - 17
9.19
Robot balances with trim limits

between 0.48 and 0.52 and reduced gain
9 - 18
9.20
Hip abduction and trim output during continuous frountal sway
9 - 19
9.21
Robot in frontal sway
9 - 20
9.22
Data from robot in frontal sway
9 - 21
9.23a
Original foot configuration
9 - 21
9.23b
Widened foot configuration
9 - 21
9.24
Failure of foot rotation cylinder connection
9 - 22
IX
9.25
Robot in frontal balance with modified feet
9 - 22
9.26
Three link biped model
9 - 23
9.27
Simulation of robot coming to rest when

released from central position with 500nm/rad ankle torque
9 - 25
Simulation of robot coming to rest when released

from central position with 1800nm/rad ankle torque
9 - 26
9.29
Simulation of robot reacting to control torque
9 - 26
10.1
Biped in single support phase
10 - 11
10.2
Expected leg deflection
10 - 12
11.1
Modelled links
11 - 2
11.2
FEA of robot link
11 - 3
11.3
Machine model of biped
11 - 4
11.4
Joint driver subsystem
11 - 4
11.5
Simechanics model
11 - 5
11.6
Hip position vs joint stiffness
11 - 6
11.7
Flexible robot at limit of frontal sway
11 - 7
11.8
Hip position plots
11 - 7
11.9
Reaction of robot to high local joint gain
11 - 8
11.10
Transients in control torque at high control gains
11 - 9
11.11
Sway simulation using actual robot joint stiffness
11 - 10
11.12
Actual data from robot trials
11 - 11
11.13
Simulation global control block
11 - 12
11.14
Step response simulation with low gain
11 - 13
11.15
Step response simulation with high gain
11 - 14
11.16
Response of simulation to square pulse force input
11 - 14
11.17
11 - 15
11.18
11 - 16
11.19
Response of simulated control system with various values
11 - 16
11.20
Foot resction forces produced by hip torque
11 - 17
9.28
11.21a-d Ground reaction from hip torque
11 - 18
X
11.22
Relationship between hip torque and foot ground reaction
11 - 19
11.23
Modified simulink model with torque compensation
11 - 19
11.24
Plot of torque, hip displacement and ground reaction
11 - 20
11.25
Modified contoller with strain feedback
11 - 23
11.26
System with and without strain feedback
11 - 24
11.27
Effect of strain feedback gain
11 - 24
11.28
Effect of strain feedback gain
11 - 25
11.29
Effect of ground reaction feedback gain
11 - 25
11.30
Effect of integral of ground reaction feedback gain
11 - 27
11.31
Effect of combinations of feedback to step perturbation
11 - 27
11.32
System response to step input
11 - 28
11.33
System response to ramp input
11 - 28
LIST OF TABLES
1.1
Design criteria for an industrial biped
1-5
1.2
Roboshift specifications
1-6
2.1
Survey of biped robot research
3.1
Roboshift design criteria
3-1
3.2
Human lower limb joint movement
3-6
3.3
Simulation of human lower limb joints
3-7
3.4
Human lower limb joint movement
3 - 10
4.1
Inventory of on board power requirements
4 - 10
4.2
Engine comparison
4 - 19
5.1
Control tasks
5 - 10
6.1
Schematic of control system
6-2
8.1
Calibration module keyboard command map
8-9
8.2
Joint controller task / function map
8 - 30
8.3
Testing of hardware/software functionality
8 - 42
9.1
Frontal sway paremeters
9 - 24
2 - 14
XI
LIST OF FLOWCHARTS
8.1
Main control program
8-3
8.2
Data acquisition routine
8-5
8.3
Calibration module
8-8
8.4a
Motion control routine
8 - 11
8.4b
8 - 13
8.4c
8 - 15
8.4d
8 - 17
8.5
Main communications program
8 - 25
8.6
Receive and write routine
8 - 27
8.7
Structure for readdata() routine
8 - 29
8.8
Local joint controller main control loop
8 - 31
8.9
Local joint controller interrupt service routine
8 - 34
8.10a
Local joint controller calibration software
8 - 36
8.10b
Local joint controller calibration software
8 - 37
8.11
Postion control routine
8 - 39
8.12
Path for joint movement
8 - 41
8.13
Boot and calibration sequence
8 - 47
XII
LIST OF SPECIAL NAMES & ABBREVIATIONS

A/D
Analogue to Digital.
Anthropomorphic Based on the human form i.e. Legs, arms, etc.

Android
An anthropomorphic Droid.
Agent
See Droid.
Automata
See Automaton.
Automaton
Machine designed to automatically follow a

precise sequence of actions.
Cybernetic
The science of communication and control theory that is

concerned especially with the comparative study of automatic
control systems (Encyclopedia Britannica).
COG
Centre of gravity
D/A
Digital to Analogue.
Droid
An autonomous robot.
Exoskeleton
The exterior protective or supporting structure or shell of many

animals (especially invertebrates). In terms of robotics, the word
defines a structure attached externally to humans who can be
considered inverterbrates in robotic applications.
F1
F1 Microcontroller.
FIFO
First In First Out. A term used to describe a data buffer where

the data is transmitted in the order it was received.
Frontal Plane
The plane defined by X and Z coordinates, where X is in the

direction from left to positive right of the robot and Z is in the
positive up direction.
HC11
Motorola 68HC11 microprocessor.

XII
Intelligent Agent
A term that may have several meaning across disciplines. I.e.; In

telecommunications an intelligent agent may describe a phone
system or network router. In software engineering, it may describe
a subroutine. In this document and, generally, in robotics, the term
describes an autonomous robot.
I/O
Input / Output.
IP67
International standard for sealing of enclosures. 67 indicates dust

and splash proof.
Longitudinal
Direction along the foot from heel to toe.
LJC
Local Joint Controller - the Motorola M68 HC11 F1 controller

points responsible for the local control of joint motion.
Lateral
Direction across the foot.
Mechatronics
A discipline of engineering which combines aspects of

mechanical,electrical, electronic, and software engineering.
MCC
Main Control Computer.
MCS
Main Control Software.
PC
Personal Computer.
PDA
Personal Digital Assistant.
Real-Time
Control
Processing data sufficiently rapidly to be able to control

the source of the data. (Word Reference.com).
Resolved
Motion Rate
The frequency at which the control system is able to compute one

complete control iteration.
RMR
Resolved Motion Rate.
ROM
Read Only Memory.
Unimate
First industrial robot developed by Unimate in 1961.

XIII
Sagittal Plane
The plane defined by Y and Z coordinates, where Y is in the

direction of positive forward motion of the robot and Z is in the
positive up direction.
SCADA
Security, Control and Data Acquisition.
Stance
Lateral distance between feet.
Telechirs
Remotely controlled manipulators the action of which is

controlled by a human manipulating a smaller version of the
mechanism.
Trim
The percentage of total weight of the robot on the left leg.
ZMP
Zero Moment Point.
Terms for Degrees of Freedom of Robot:

RiHiAb
Right Hip Abduction
LeHiAb
Left Hip Abduction
RiHiRo
Right Hip Rotation
LeHiRo
Left Hip Rotation
RiHiEx
Right Hip Extension
LeHiEx
Left Hip Extension
RiKnEx
Right Knee Extension
LeKnEx
Left Knee Extension
RiFoEx
Right Foot Extension
LeFoEx
Left Foot Extension
RiFoRo
LeFoRo
Left Foot Rotation
RiToEx
Right Toe Extension
LeToEx
Left Toe Extension
RHA
Right Hip Abduction
XIV
LHA
Left Hip Abduction
RHR
Right Hip Rotation
LHR
Left Hip Rotation
RHE
Right Hip Extension
LHE
Left Hip Extension
RKE
Right Knee Extension
LKE
Left Knee Extension
RFE
LFE
Left Foot Extension
RFE
LFR
Left Foot Rotation
RTE
Right Toe Extension
LTE
Left Toe Extension
XV
INTRODUCTION
Imagination is more important than knowledge.

Albert Einstein
1.1 THE QUEST FOR BIPED MOTION

From the 1950s man has dreamed of the day when robots will stand side by side with
us, in our image. We now live in an era where the previously well defined dimensions of
imagination and reality are beginning to blur at the boundaries. For many years, society
has accepted the persona of automata and robots. The January 2001 edition of People
Magazine included Robby the Robot (Figure 1.1) as one of the twenty five most
intriguing people of the century. While the hardened roboticist may dismiss the science
fiction factor as fantasy, it cannot be ignored as a driving force in robotics research.
Albert Einstein recognised the power of imagination as a driving force in research.
Japans fascination with androids and the personification of electronic devices has driven the development of products as diverse as miniature PDAs that need to be fed and
cared for on a regular basis, to robotic maids that are able to vacuum a room.
We Japanese love new, advanced things, says
Minoru Asada, an Osaka University
scientist developing soccer-playing robots. Its
more than just owning them. They are our
friends, and we want to integrate them into
(Time, 2000)
society.
More recently, the development of Sonys
SDR-3x , Hondas Asimo and Toyotas
trumpet playing humanoid (Figure 1.2) demonstrated beyond question, that the Japanese have
piloted the development of the humanoid robot
or android [(Sony, 2000), (Honda, 2003a),
(Wolfe, 2004), (AIST,2003)]. Sony market the
Figure 1.1 Robby the Robot from the 1950s
movie Forbidden Planet
SDR-3X as an entertainment robot as its small
1-1
Chapter 1 - Introduction
size, lack of dexterity and intelligence make it incapable of performing useful

service tasks. Realistically, it represents a continuation into the next generation of the
extremely popular post war clockwork or battery powered tin toy robot. However, the
development of the device has focused engineers, marketers, industrial designers,
software developers and psychologists onto the tasks that will one day deliver a
realistically priced and capable android. Effectively, they have taken the first steps of the
long march that will end with the fulfilment of the science fiction dream. Asimo is of a
larger scale and is marketed as a service robot. The increase in size carries an additional
level of complexity in terms of engineering, control and actuators. The robots significant
processing power and human-like qualities either satisfy the developers craving for
creation or the markets demand for anthropomorphic devices.
While mechanical walking machines have been proposed, patented and built from the
beginning of this century, it is only since the availability of low-cost microcomputers that
electronically controlled devices have become viable. The vast majority of walking
robots that have been built are modelled on the human form. The geometry presented by
an anthropomorphic device and the inherent instability of bipedal locomotion increase
both the complexity and cost of the device in terms of construction and control hardware.
The construction of robotic biped walking devices is expensive, labour intensive and
demanding in terms of programming time. Researchers involved in this field have
tended to justify their endeavours in philanthropic rather than economic terms. Such
justification is embodied in two propositions that include the study of biped walking
machines so that:
Figure 1.2 Hondas Asimo, Kawadas HRP2 and Toyotas trumpet playing humanoid
1-2
Biped devices may replace humans performing hazardous or degrading work

(Golliday and Hemami, 1977)
The study of bipedal control will result in a better understanding of the human
gait and lead to devices that will assist with the mobility of people who have
lost the use of their legs.[(Todd, 1985), (Kato et al, 1987), (Hemami et al,
1973), (Yamashita, 1993)]
While these propositions may be worthy, the cost of a biped robot compared to that of
a wheeled or tracked device inhibits commercialisation of biped robots in the first
proposition.
Another justification has been based on the development of robotic-type orthotic
devices to aid people with paraplegia. Even if such devices were to be realised, they
would be prohibitively expensive to manufacture and maintain, placing them out of reach
of all but the most wealthy patient. Further, the requirement for an onboard power
supply would render the device bulky, cumbersome, and with the current efficiency of
batteries, it would have a very short period of operation. Current research in
biomechanics suggests that functional electrical stimulation of nerves and muscles will
be significantly more viable in the restoration of locomotion. (Popovic et al, 1999)
Robots such as Hondas anthropomorphic droid have attempted to closely imitate the
human form. Takanishi et al. (2005) from Waseda University, where the development of
autonomous biped robots began in 1973, suggest the reason for humanoid appearance is
that it is a requirement if humans are to work side by side with androids;
By constructing anthropomorphic/humanoid robots that function and behave
like a human, we are attempting to develop a design method of a humanoid robot
having human friendliness to coexist with humans naturally and symbiotically.
These robots are research platforms crammed with a range of technologies such as
voice and image recognition, as well as gait and balance control systems. Ultimately, this
research
may
lead
to
device
which
would
replicate
some
human
characteristics. Sony (2003) justify their biped robot as a proving ground for the demonstration of new technology.;
next generation technology is functional device technology that correspond
to the five senses.
1-3
Given a plentiful supply of humans however, the usefulness of such a device would be
limited to applications where there is significant hazard and likely risk of injury.
Applications may include working in hazardous areas such as bomb disposal,
surveillance and the nuclear industry. More conventional arguments would suggest that
legged vehicles would traverse irregular terrain inaccessible to conventional wheeled or
tracked vehicles [(Raibert, 1986) (Kato et al, 1987)].
The support base of a biped is an order of magnitude smaller than that of any other
vehicle. Bipeds also possess the ability to turn in their own space, lift heavy objects by
adjustment of posture rather than by increasing their support base and traverse
discontinuous surfaces. Here lies the true usefulness of a biped device; its ability to
achieve what is beyond the capabilities of contemporary materials handling vehicles and
certainly beyond the capabilities of a human.
1.2 EMBODIMENTS OF BIPEDAL MOTION

Biped robot research could be classified as pure research as it does not necessarily
satisfy a practical demand. For example, it does not aim to cure a disease, though it
claims to investigate a solution to paraplegia. It does not offer to make more efficient an
industrial process, but suggests it may make some processes less hazardous to humans.
In the case of humanoid biped robots there is no current demand for a device that is less
intelligent, less dexterous and less enduring than an able-bodied human. In the instance
of an industrial scale biped robot, there is no demand for a device that possesses no
capability beyond that of a forklift truck. However, as the industrial environment is
currently designed around existing materials handling technology, any device that
significantly improved the capability of conventional materials handling plant has the
potential to alter that environment. In particular, it is proposed that the
development of a biped materials handling platform will not only offer materials
handling in confined, uneven terrain where a forklift or other lifting device would be
unsuitable, rather it would allow the development of industrial processes that were
previously impracticable. Possible situations would be field handling in a military
environment, on board a ship or industrial applications in the field such as geological or
mining applications. This project endeavours to demonstrate that biped robotic materials
handling is viable by the construction and operation of an industrial scale device.
1-4
Capable of lifting 500kg
1st Criterion
Able to traverse 500mm discontinuities
2nd Criterion
Robust both physically and electronically
3rd Criterion
Completely self contained
4th Criterion
Able to work for long periods
5th Criterion
Easily maintained
6th Criterion
Table 1.1 Design criteria for an industrial biped
The design of such a device would rely on a set of performance parameters based on
the range of tasks it would be expected to perform. Given that no such robot is in
existence, these tasks have yet to be defined. However, based on current
complex or hazardous materials handling conditions, a set of parameters has been
formulated for the first time. For a biped to be industrially viable, it is proposed that it
must meet the criteria in Table 1.1.
The following document outlines the design, construction and control of the device that
has been built to satisfy these criteria. The result of the integration of the mechanical,
electrical, electronic, software and control engineering undertaken in this project is
shown in Figure 1.3. Named Roboshift, the biped robot stands 2.4 metres tall, weighs
500 kg and is completely self-contained. As such, it is the largest autonomous biped robot
to be built. It is the only biped robot which has achieved an industrially viable scale.
Table 1.2 outlines the as-built specifications of Roboshift.
The author has presented two research papers on the project. The first was delivered at
the 1999 International Symposium on Computational Intelligence in robotics in
Monterey, California (Cronin et al, 1999), and the second at the 2004 Australian
Automation and Robotics Association in Brisbane, Australia (Cronin et al 2004).
The major contributions made to the field of robotics by this project include;
A foundation set of design criteria for an industrial scale biped robot have
been determined.
A comprehensive, self contained, full scale prototype of an industrial biped robot
has been designed and constructed.
Roboshift is the first industrial scale robot to be fully self contained, carrying onboard all power, actuation and processing systems required for continuous and
extended operation.
1-5
INDUSTRIAL BIPED ROBOT SPECIFICATIONS

Height
2.4 Metres
Width
1.3 Metres
Length
1.2 Metres
Weight
494 Kg
Power
20 Hp LPG Engine (air cooled)
Actuation
Hydraulic (12 Cylinders, 2 Motors)

3 x 3500psi Gear pumps.
30l litre reservoir.
14 Rexroth WRE proportional valves.
14 x VT10001 PWM Valve Amplifiers
Cooling
3.5 Hp 12V Fan forced oil radiator
DOF
14
Electrical Power
Bosch 12V 60amp Alternator

2 x 600 W 12VDC to 240VAC Inverters (PC Power)
3 x 12 Volt Batteries (Instrumentation)
Processors
Pentium III 100MHz (Global Control)

Pentium III 100MHz (Communications)
14 x Motorola 68HC11 (local Joint Control)
1 x Motorola 68HC11 (Artificial Horizon)
1 x Motorola 68HC11 (Artificial Horizon Compressor)
Sensors
Air driven absolute artificial horizon (Pitch & Roll)

Flux gate compass (yaw)
2 x Strain gauge bridges each leg
14 x Quadrature encoders (one per joint)
Table 1.2 Roboshift specifications
Roboshift is the first industrial scale biped to demonstrate active balance in the
frontal and sagittal planes, and to achieve frontal sway.
The experiments conducted on the biped robot represent the first credible
research into the challenges presented by an industrial scale biped robot in terms
of its design, construction, power and control systems.
The project is the first research to identify compliance as a major issue in the
operation and control of an industrial scale biped. It is also the first research to
dynamically model a large scale biped robot and to provide solutions to the
control of a compliant biped.
The project has established the requirements of the control system of an industrial
biped.
1-6
Figure 1.3 Roboshift
1.3 ABOUT THE PROJECT

The project to build an industrial biped robot commenced after the author attended the
opening of the movie Aliens in which a teleoperated robotic loader was used not only
to transfer cargo, but to defeat the alien life form. Impressed with the concept of the
loader, research indicated the concept was based on General Electrics Hardiman (Weiss,
2001). Finding no reliable published data on the device, or any similar industrial scale
biped, the author attempted to determine why no such research was being attempted.
1-7
Originally intending to complete a masters degree on the project of a concept design for
such a system, the degree was upgraded to a PhD when it became evident that industry
assistance would be available to complete a working prototype. Apart from the generous
assistance of his supervisors, the fabrication and welding of the aluminium sections of
the robot, the design of the F1 Controller I/O boards and assistance with the
communications interrupt routines, the entire project has been completed by the author.
This has included:
Conceptual design of the robot
Full mechanical design of the robot
Mechanical assembly of the robot including the fabrication of all non
aluminium components
Design of the electrical system
Installation of the electrical system
Design of the control system
Construction and installation of the control system and transducers
Design and coding of the control software
1.4 STRUCTURE OF THIS DOCUMENT.

Chapter 2 reviews biped robot research literature. In particular, it explores the
following :
Development of walking robots
Development of biped robots
Development of walking robot control systems
The review details the major classes of control systems that have been developed as
well as establishing a base for the research in this project. It shows that the majority of
biped research has revolved around devices of a scale unsuitable for commercial
development. Finally, the discussion concludes with the development of a design
specification for an industrial scale biped.
Chapter 3 details the conceptual and mechanical design of the robot including structure
and actuators. To enable such a large and complex project to be completed as a PhD, it was
necessary to fast track the design process. The use of existing expertise, in combination
with the availability of resources, led to a refined solution space using hydraulics as the
motive force. Initially, mathematical and kinematic models are used to determine joint
trajectories so that the degree of freedom and range of movement is able to be determined
1-8
for each joint. Further analysis is used to examine the geometry of the actuator/joint
combinations. The chapter concludes with photographs of the completed structure which
show the body suspended under the hips. This configuration, previously only seen in the
science fiction realm, was realised for the first time in this project.
The hydraulic and electrical systems that provide power to the robot are detailed in
Chapter 4. Based on limb trajectory models, the flow requirements are calculated for
each of the hydraulically operated joints. To avoid the potential for hydraulic crosstalk
as encountered with General Electrics Hardiman, the robot described in this project uses
separate hydraulic systems to operate each of the legs and the hips. Schematics of the
hydraulic and electrical systems are included. The electrical system provides high current
power for the hydraulic valve amplifiers, the inverters that power the two on board
computers and low current power for other on board systems such as processors and
transducers.
An overview of the control system hardware, software and modelling is given in
Chapter 5. The chapter begins with a discussion of robotics and cybernetics. It suggests
that the difference between industrial robots and advanced mobile robots is the ability to
deal with unexpected information. It discusses the results of the review of previous
walking robot control systems and builds on that knowledge base. The control system is
hierarchical and distributed using a separate computer to facilitate communications
between the sixteen microprocessors on board. By breaking the control task into local
and global control, the system mimics that of the human with reactive control occurring
at the joints and cerebral processing occurring in the main control computer.
Chapter 6 details the development of the control system electronics including
processors, transducers and communications modules. The connection of the major
components is detailed in a schematic of the control system hardware.
Chapter 7 explains the kinematic and dynamic models that were used to design the
robot mechanically and to design the software that controls it. Graphical output is
provided from the AutoCAD Advanced Development System software that was written
to display the results of dynamic modelling.
1-9
Chapter 8 outlines the structure of the software which controls the various behaviours
of the robot. Initially, flowcharts are used to illustrate the hierarchy of the software and
the distribution of processing tasks. The software can be broken into three main sections:
Main control software running on the main control computer
Communications software running on the communications computer
Local joint control software running on the Motorola M68HC11s
While other robots have adopted the use of distributed processing, this system is the
first to use a dedicated processor to distribute information. This configuration offers the
advantage that the communications processing demands on the local joint processors and
the main communications PC are reduced by making them invisible to each other. While
the joint control and communications software routines are standard for all robot
functions, the control software is segmented into three main functions. The first is
calibration which homes the robot, calibrating position encoders and proportional valve
control. Secondly, the static balancing software uses the robots vestibular and strain
gauge transducers to maintain the centre of gravity of the robot within the reaction
polygon of the feet. Finally, the locomotion software initiates frontal sway and controls
the forward motion of the robot. This is the dominant software active when the robot has
been calibrated and is in an operational mode.
The testing of Roboshift is described in Chapter 9. Each of the stages of development
was tested to ensure that classes of systems were performing to specification as they were
integrated. Initially the local joint control was confirmed by the use of a small wheeled
robot that was fitted with the hardware system developed for joint control. A PC was then
networked with the joint control microprocessors to confirm and optimise the serial data
transfer routines. Once the reliability and the operation of the joint control software had
been proved, the software was loaded to the robot where communications were
confirmed for the entire system.
With data transfer confirmed, calibration and then motion control was tested for each
joint, for two joints simultaneously, and then for the entire system. The testing showed
that the system was robust and able to communicate at eight cycles per second. Once the
robot was able to be calibrated and moved into a passive balancing condition, the
balancing software was then successfully tested.
1 - 10
Chapter 10 examines the outcome of the testing of the robot and reviews the project
in terms of the design and the performance of the mechanical, electrical and control
systems. Modifications are suggested for the next iteration of the robot including a
review of the design of joints and limbs to reduce compliance and vibration in the
structure. The choice of transducers is discussed with a recommendation that each degree
of freedom is sensed by two independent means.
The performance of the control system hardware and software was surprisingly stable.
Both the F1 Controller boards and the two PCs survived a range of mishaps including
severe shock and voltage fluctuations. The control software achieved all system
specifications proving extraordinarily robust.
Based on the performance of the structure, an initial analysis is conducted into the
degree of flex that could be expected in links of an industrial biped.
Chapter 11 continues with the analysis of flex in the structure. A finite element model
is created and analysed for a typical link to estimate the stiffness of the structure. A
Matlab Simmechanics model is then constructed and analysed to determin the dynamic
response of the structure. The model demonstrates the problems created by collocation
of sensors and actuators in the control of a flexible structure. the performance of a control strategy using non-collocated sensors is then modelled.
Chapter 12 discusses the major accomplishments and failures of the project.
The projects contributions to the research area are outlined including:
Establishment of design criteria for an industrial biped
Roboshift is the largest biped robot to demonstrate active balance in the
frontal and sagittal planes as well as limited sway in the frontal plane.
Roboshift is the first biped to incorporate a self contained power
system capable of operating the device for extended periods and the
first to incorporate an internal combustion engine.
Roboshift is the first biped robot to be built on an industrial scale so
that challenges in terms of structure and control of an industrial biped
can be identified. At 2.4 metres in height and 500kg in weight,
Roboshift is the heaviest autonomous biped robot yet built.
Establishment of the requirements for the control system of industrial
scale biped robots.
1 - 11
Identification of compliance as a major issue in the structural design

and in the design of the control systems of industrial scale biped
robots
The modelling and analysis of the compliant structure and teh presentation of strategies to deal with compliance by teh use of non collocated sensors.
Finally, areas for future work are detailed including;
The continuation of frontal balance and locomotion trials.
The formulation of a dynamic frontal sway model which incorporates
compliance in the structure of the robot.
Comparison between theoretical output of the upgraded dynamic
model and experimental data acquired from trials of the robot.
Continuation of locomotion trials.
1 - 12
WALKING ROBOTS
Methinks that the moment my legs begin to move, my thoughts

begin to flow.
Henry David Thoreau
This chapter presents the results of the search for literature relevant to the project. A
mobile robot can be characterised as the integration of a range of technologies and
research combined to construct and to control an autonomous vehicle. Biped robotics
research has become a discipline within mobile robotics. However, it cannot be studied
in isolation from the engineering and bioengineering disciplines it draws from so
heavily. A biped robot is a mechanism, the movements of which are controlled by
software processed by microprocessor based electronic hardware. The design of the
mechanism is dependent on the definition of movement; the design of the processing
platform is dependent on the structure of the controlling software. The definition of
movement and mechanism design are dealt with by the mechanical engineering
disciplines of design, kinematics and dynamics, and the science discipline of biomechanics. Software and hardware design are disciplines of electronic engineering. In recent
years, mechanical engineering has seen the introduction of the discipline of Mechatronics
that includes all of the areas described above. This is a strong indication that, in the field
of robotics, mechanism design and control design are strongly inter-related.
In the case of a wheeled robot, the mechanical design component of the project is
usually less complex than that of a legged robot. For this reason, the majority of research
presented on wheeled robots revolves around the issues of sensing and navigation. In the
case of multi legged robots, the research tends to focus on gait patterns and the actuation
of the legs. In biped robots, where the major control task is not to fall over, the research
focuses on the design of the leg system, the stability and dynamics of motion and the
architecture of the control system. For a biped robot to walk with a dynamic gait it
requires a control system capable of processing sensory data, solving dynamic motion
equations and controlling actuators in real time.
The focus of this literature search is broken into three parts.;
1. The development of walking robots. Establishing a broad history of legged
2-1
Chapter 2 - Walking Robots
robot research allows the identification of technologies that may be relevant to

this project in terms of leg actuation, sensors and control system and software
architecture. Here, the focus is on the configuration of previous walking robots
without in-depth analysis of gait models or control systems.
2. The development of biped robots.- It could be argued that, given the high degree
of failure to achieve dynamic walking, the value of previous biped research to this
project is questionable. However, what is commendable is the contribution to
science made by researchers who have tirelessly adapted technology to the task as
it has become available. As processing power has decreased in cost and smaller,
more powerful microprocessor have become available, these have been absorbed
into biped robot control systems. This has also been the case with new
developments in control theory. With the increase in processing power and
distributed processing, more complex control systems and more sophisticated gait
models have been able to be represented in software. Like few other areas of
endeavour, the field of biped robotics has insatiably adapted emerging
technology from fields as diverse as control theory, avionics, image processing,
polymer research and biomechanics.
3. The development of walking robot control systems.- Finally, the literature
search establishes the current state of the art so that the work in this project may
benefit from, and contribute to the current level of knowledge in the field. The last
endeavour is more difficult to achieve as the literature search shows that very
little research has been conducted into the development of an industrial scale
biped. In fact, Honda, the leading researchers in biped robotics, has reduced the
size of their latest biped prototype from that of the previous iteration. This section
concentrates on the hardware and software used on previous biped robots.
Literature outside of these areas will be included, where appropriate, to further explain
the development of previous research and the research conducted in this project.
2.1 WALKING ROBOTS

The human body represents the ultimate example of a fully integrated mechanism and
control system. Through its five senses, it is able to gain an enormous amount of
information, process it on several layers of consciousness and then actuate the motion
and control the force of muscles, both centrally and peripherally. But in some situations,
where tasks are narrowly defined and repetitive, robots have been able to replace the
2-2
human by carrying out these tasks more quickly and accurately.

As is widely reported and accepted, the term robot was first used by the Bohemian
playwright Karl Capek, in his play Rossums Universal Robots (Capek, 1920). In its cast,
the play included creatures called Robotnics , a term derived from robota which is
the Russian word used to described repetitive, labour intensive work. Shahinpoor
(Shahinpoor, 1987), defines a robot as;
..a re-programmable multifunctional manipulator designed to move materials, parts,
tools, or specialised devices, through variable programmed motions for the perform ance of a variety of tasks.
This definition describes the industrial robot, which is an extension of the first
automated machines introduced to the textile industry during the Industrial Revolution.
While the means and complexity of programming have changed, todays industrial robot
is simply an intelligently controlled machine, a machine designed to carry out repetitive
and laborious tasks as highlighted in Kapeks play.
In parallel with the development of the industrial robot, an area of robotics has existed
which has focused around the droid or artificial life form. In 1962, the Britannica World
Language Dictionary defined a robot as ;
...a manufactured, mechanical person that performs all hard work.
This definition is based on Isaac Asimovs droids rather than on the industrial robot
under development at that time. It was only after the first Unimate was installed in Japan
in 1969, that the term robot was used to describe re-programmable machines. The
droid has become the mobile intelligent agent the development of which has been
driven by several stimuli. The first stimulus for research was the romance of science
fiction and an inexplicable desire of robot engineers to imitate human and animal
behaviour. Examples of such robots are tracked, wheeled or even winged platforms
which are fitted with a variety of transducers and at least one microprocessor.
Continually scanning their environment, the robots react to inputs with pre-programmed
behaviours. These types of mobile robots are most commonly found in mechanical
engineering and computer science schools of universities around the world.
The second stimulus for mobile robot research is a requirement for mobile platforms
2-3
used to convey sensors and surveillance equipment into areas inaccessible or hazardous
to humans. Bomb disposal, pipeline inspection and nuclear plant monitoring are
common tasks performed by mobile robots. While tele-operated by humans, these robots
generally possess control systems capable of accepting both operator and local sensory
inputs, which are processed before actuation is enabled. This is especially the case where
the possibility exists for interruption of communication with the command console.
When this occurs, the robot uses its default behaviour to either continue its mission or
attempt to re-establish communications.
The recent expeditions of the Mars Rover are an example where a significant delay
exists between command transmission from Earth and feedback from the Mars Rover.
As an earth-bound robot operator would not be able to see an obstacle before the Mars
Rover came into contact with it, the robot was programmed to navigate its way around
the obstacle.
Legged robots are mobile robots or droids which use legs, rather than tracks or wheels,
for their mobility. Biped robots are a subset of legged robots that attempt to imitate
human locomotion. The Holy Grail of mobile robot engineering is a droid which walks,
talks and thinks like a human being. Sias and Zheng (Sias and Zheng 1987) suggests that;
...the ultimate mobile robot is a device that can emulate the agility and autonomous
behaviour of the human being...
As seen in the following sections, considerable resources have been invested in
anthropomorphic robots and into the development of artificial people and animals.
2.1.1 WHEELS VERSUS LEGS
Classically, research into legged walking machines and robots has been justified by two
main arguments. The first suggests that legged vehicles could work on terrain not
accessible to tracked or wheeled vehicles. Specifically, legged vehicles can step over
uneven or unstable terrain, placing feet only on firm ground. Raibert (1986) highlights
the fact that animals can reach a greater area on foot than is accessible to wheeled or
tracked vehicles, and proposes that legged vehicles will go places that only animals can
now reach. Kato et al (1987) recognised that while wheeled and tracked vehicles operate
on a continuous surface, legged vehicles can operate on a discontinuous surface. What
Kato fails to highlight is that the continuity of a plane is effectively a function of the
2-4
diameter of the wheels or the length of a track. Increasing the diameter of a wheel
proportionally increases the size of discontinuity that the vehicle is able to traverse.
The real advantage of legged vehicles is that the size of discontinuity they are able to
traverse is significantly greater than for a wheeled or tracked vehicle of the same size. By
continuously changing shape and centre of gravity, legged vehicles would be far more
manoeuvreable than tracked vehicles. Certainly, biped robots would be more adapted to
the human environment (labelled the anthroposphere by some researchers). In particular,
steps would be more easily traversed by legged vehicles than wheeled or tracked
vehicles. In the case of domestic service robots, legged robots would require little or no
modification to the existing structure of a house to be able to move freely within it.
A second argument for legged vehicles has been that the development of legged
vehicles will assist our understanding of animal and human locomotion. Todd (1984),
Raibert et al (1987), Hemani et al (1973), Yamashita and Yamada (1993) all suggest that
the development of biped robots will assist with research into orthotic devices. Research
by Yamaguchi and Zajac (1996) suggests that the possibility of using controlled
functional stimulation of nerves and muscles is more likely to ultimately assist those who
have lost the use of lower limbs. Interestingly, their research suggests that crutches or
other walking aids would be more than acceptable to those who are wheelchair bound.
Therefore, as balance could be achieved using muscle groups of the upper body, open
loop control of lower limbs would be possible. Certainly, it seems likely that biped robot
control systems could be adapted to control such stimulation.
A third justification for development of legged mobile robots has been their use as a
proving ground for artificial intelligence research. As previously highlighted, those
involved in robotic research, especially mobile research, have concentrated on the
behavioural aspects of the control system. Cybernetic control strategies such as
subsumption architecture, fuzzy logic, neural networks and other expert systems attempt
to imitate the behaviour of biological control systems. It is natural then, that these
systems are demonstrated on platforms which themselves imitate biological forms.
Similarly, In a symbiotic relationship, the very complexity of legged locomotion systems
has required new methods of control to drive leg actuators.
2-5
Figure 2-1. Japanese toy Android
Regardless of the reason or justification, considerable effort has gone into legged
vehicle and legged robot research.
2.1.2 LEGGED ROBOTS
Clockwork tin-plate toys were the first examples of walking machines. Generally
bipeds, these toys were spring powered and used cranks to actuate single or double link
legs. Produced in Japan and Germany between the First and Second World Wars, the
devices represent the initial attempt to replicate human motion. Japan continued to
mass-produce battery powered toy robots after the Second World War (see Figure 2-1).
These toys were based on science fiction characters of the time and contained quite
complex arrangements of cams and/or cranks. The relevance of these devices to walking
robot research is the evidence they provide of Japans fascination with the android or
anthropomorphic droid. As discussed in the introduction, this fascination currently drives
the most advanced robotic research in Japan if not the world.
Like the mechanisms in these toy robots, early walking machines depended on
complex linkages to move the legs. An example of such a machine was A. Ryggs pedalpowered mechanical horse, patented in 1893 (see Figure 2-2).
2-6
Figure 2-2. Ryggs

Mechanical Horse
The further problem encountered by researchers was the method of providing

propulsion to the legs. It was only after the advent of the internal combustion engine that
legged vehicles became feasible. Like aircraft, they required a compact, relatively
lightweight power source compared to steam engines. Without the availability of flexible
hydraulic power transmission systems, the legs of early walking vehicles relied on a
direct drive train from the power source. The inability to continuously modify the gait of
the device left these vehicles with an inability to adapt to varying terrains. As Raibert
(1986) highlights, it became apparent that for a walking vehicle to be feasible, adaptable
control over individual legs would be required.
The most promising initial research into walking machines was overwhelmingly
driven by the requirements of transport and materials handling. Unlike other areas of
mobile robotic research, the development of legged vehicles has seen the involvement of
large government and private organisations. The first serious attempts at legged vehicle
design were initiated by the military, both in England and the USA.
Todd (1984) attributes the first walking machine with independent leg control to A. C.
Hutchinson and F. S. Smith in 1940. Hutchinson and Smith built a model of a proposed
four-legged 1000-ton armoured walking vehicle with individual hydraulic control of the
2-7
legs. The model was driven by an operator whose hand and foot movements were
transferred by cable to the model. While the model was able to climb over a pile of books,
the army was not persuaded to fund further development.
The early 1960s was a volatile time for robotics as teams working in many parts of the
world developed new ideas and prototypes. During this period, the USAs Defence
Advanced Research Projects Agency (DARPA), through the US Army Tank-Automotive
Centres, funded the Land Locomotion Laboratory, a cooperative venture with the
University of Michigan.
As recounted by Todd (1984), in 1962, the laboratory was approached by H. Aurand of
General Electric who proposed a bipedal walking machine using force feedback
control by a human operator. While models and designs were built and refined, as is often
the case with engineering companies, the marketing department, ignoring technical
requirements, decided that customer appeal would be better satisfied with a quadruped
device.
Raibert (1986) suggests the resulting quadruped walking truck, designed by Ralph
Mosher was the first successful walking vehicle. Using human control, in a similar
approach to Smith and Hutchinson, the vehicle was developed by General Electric in
1965. Hydraulically driven and weighing 1400 kg, the truck had legs which were
controlled by pedals which, in turn, were operated by the drivers hands and feet. This
was part of an ongoing experiment in force feedback, and the driver was able to feel
the vehicles legs touch the ground. With considerable practice, ultimately the driver was
able to manoeuvre the vehicle easily over and around obstacles. This walking truck was,
effectively, a mobile robot with a human control system. Although this vehicle
successfully demonstrated the principle of independent leg movement, operating it was
exceedingly demanding on the operator. Had the laboratory proceeded with a biped, its
control movements would obviously have to have been more natural for the operator.
Legged robotics was put back many years by the decision to develop a quadruped
vehicle.
While various researchers such as Shigley (1957), Liston (1964), Morrison (1968) and
Vukobratovik (1973) continued with walking vehicle designs, the problem of controlling
the movement of legs prevented further success. As was the case for industrial
2-8
automation, it was necessary for the human to be replaced with a device that was more
reliable and precise. This became possible with the advent of the mini computer. While
not as portable or powerful as todays desk-top personal computers, units such as Digital
Electronic Corporations PDP1170 became common objects in mechanical engineering
and computer science schools around the world. Access to this equipment provided
researchers with the processing power required to solve inverse kinematic equations in
real-time.
Robert McGhee (1977), also at the Land Locomotion Laboratory, saw the potential for
electronic control of the limbs of walking machines. In 1966 he built a quadruped device
based on simple digital control of the legs. Labelled the Phony Pony , the quadruped
weighed 50kg, used electric motors to drive two degrees of freedom per leg, and used
very wide feet for lateral stability.
Encouraged by his experiment with simple digital control, McGhee built a hexapod
vehicle in 1977. Each leg possessed three degrees of freedom, each degree of which was
operated by an electric motor and reduction gear set. The vehicle was connected to a
digital PDP11 computer via an umbilical cord carrying sensory information and control
signals. The computer was used to solve the inverse kinematic equations and generate
outputs to triac controllers that powered the motors. Without doubt, this was the first
successful walking robot.
DARPA continued its development of legged vehicles, funding the development of the
Adaptive Suspension Vehicle (ASV) (Johnston, 1985). Another hexapod, this vehicle
was built by Kenneth Waldron in 1985 and represents the most realistic attempt at
development of a commercial all-terrain walking vehicle to date (see Figure 2-3). This
vehicle weighed 2.7 tonnes and was capable of climbing over a two-metre high object. It
was originally manoeuvred by an on-board operator who was able to place the vehicles
feet individually, or in an automated mode that cycled legs as groups. Later, the vehicle
was operated autonomously, demonstrating a variety of gaits that had been developed for
particular terrains. At all times, the control system kept the centre of gravity of the
vehicle inside the instantaneous polygon of feet in contact with the ground. Essentially
the vehicle was in a stable, supportive mode at all times.
Although it was promising as a transport vehicle for the field, the length and size of the
2-9
Figure 2-3. The Adaptive Suspension Vehicle.
ASV excluded it from working in confined spaces. Other hexapods and quadrupeds have
been developed on a much smaller scale, however the ASV appears to have eclipsed
research into truck-scale walking vehicles. Despite the promise shown by the ASV, it
would appear that institutions capable of funding such research are also those that are
most resistant to change. It would take a brave general, indeed, to stand before his peers,
commanding a battalion of infantry supported by walking vehicles.
One other group of non-biped legged robots, while not practical as transport vehicles,
is worthy of mention. These are the insect-like creatures developed by Brooks at MIT,
who introduced the concept of layered control systems for mobile robots (Brooks 1986).
He showed that by breaking down the tasks of a robot into multiple goals of layered
priority, complex control systems could be decomposed into low level and high-level
behaviour. He demonstrated, using small multi-legged mobile robots, that a simple
low-level algorithm could control individual joint movement, while navigation could be
performed at higher levels of control. Further, by rewarding those joint movements
(behaviours) that resulted in moving the robot forward, the robot was able to establish a
learned gait.
2.2 BIPED ROBOTS

Justification for biped walking machines and biped robotic research has been argued in
a similar approach to that for machines with more than two legs. In the case of bipeds,
2 - 10
the contention that research will assist with the understanding of human locomotion is
more persuasive.
Early biped devices can be separated into two main groups. The first includes walking
aids or prostheses designed to assist humans with mobility, while the second group
consists of stand-alone walking machines designed to walk independently of humans. As
highlighted by the problem definition outlined in earlier sections, it is the second group
that is applicable to this project. Prosthesis-type devices will not form a major part of this
thesis unless aspects of individual devices are specifically relevant.
As described in the introduction to this text, biped robotic research has flourished since
the early 1980s. Almost all of this work has been conducted in institutions attached to or
affiliated with universities. In rare cases, large automotive or electronically based
institutions such as Honda and General Electric have undertaken biped research. A list of
biped robotic vehicles is shown in Table 2-1. Figure 2.4 shows biped robots by mass.
Mass of Biped Robots

Robot Weight (Kg)
600.0
500.0
400.0
300.0
200.0
100.0
H6
Isam
u
d R
obot
Arne
#1
& Ar
nea
BIP
2000
Wab
ot
-1
Azim
o
Rob
oshi
ft
Bipe
Luc
C B y
iped
HITB
WRIII
John
nie
Bart
Shad
-UH
ow
Walk
er
Kais
t
3D
Bipe
d
WL-1
2RV
1
UWC
Bipe
d R
obot
Rob
#6
o E
rectu
Bipe
s
d R
obot
#7
CUR
Spri
BI
ng
Turk
ey
Ninjy
a
S
AICO
Rob
ot
Bipe
de 1
KDW
Bipe
d R
obot
Bipe
#2
d R
obot
#3
Mon
roe
Guru
0.0
Figure 2.4 Range of biped robot weights
In general,these bipeds can be divided into three main areas of research;

Laboratory biped Robots
Often characterized by proportionally large feet to provide an extended support
polygon (similar to that of the Japanese toy bipeds previously discussed), laboratory
bipeds provide an apparatus for gait analysis and experimentation. Due to their small
2 - 11
size and mass and associated reduction in inertial and dynamic forces, these bipeds are
unlikely to be damaged in the event of a fall. As well, the availability of low cost, high
power to weight ratio actuators (developed for use in the remote control model
market) allow for the rapid prototyping of structures.
While the majority of these structures have been anthropomorphic, several have
been based on the avian model. It has been argued by some researchers [( Hugel et al.,
2003), MIT, 2005) that the legged system of the bird (the only other bipedal animal)
is more stable than that of humans. Unlike the human hind leg, the wide elongated
four fingered foot of the bird results in a well supported, redundantly jointed leg
comprised of three segments. While the possibility of an avian leg system was
considered for this project, human one was chosen. Accordingly, the literature search
focuses on anthropomorphic bipeds.
Androids
Androids are immediately identified by their totally anthropomorphic form. These
robots are often referred to as Humanoids by their constructors, a title which not
only describes their appearance but which is used to suggest a measure of human like
intelligence. Humanoids can also be easily identified by their characteristic, highly
polished plastic, carbon fibre or fiberglass shrouding. This is also an indication of the
focus of the projects; these robots are meant to look good. They are predominantly
used to display the level of technical competence of the companies that own them. As
well the finish of these robots demonstrates the resources available to develop them.
In the case of Honda and Toyota, many years of experience in the design,
manufacture and finishing of electromechanical machinery have gone into the design
of these robots. Kawada industries not only developed HR2, but developed the servo
systems that actuate the robot based on their experience of developing similar systems
to control their large scale unmanned helicopters.
While examiners may yearn for a plethora of peer reviewed citations from
respectable journals, the state of the art in biped robots is being advanced by large
organisations at huge cost. These companies that continually out perform their
competitors are unlikely to spill their intellectual property portfolios via conference
papers.
2 - 12
Industrial Bipeds.
Currently, there are no industrial bipeds in existence with this project being the first
to attempt to develop an industrial scale autonomous biped robot. Therefore it is
difficult to determine the characteristics of this class of biped robot. Previous work on
manually operated industrial scale exoskeletons and the work completed in this
project would suggest that the devices will be predominantly manufactured from steel,
will be powered by internal combustion engines and will be hydraulically actuated.
The requirement for safety and reliability and the complexity of the control task will
result in the characteristics of the control systems being similar to those found in small
commercial airliners.
Figure 2.5 shows the relationship between these families of bipeds robots, and indictates
the emphasis of this project which is an industrial biped.
Figure 2.5 Classes of legged robots

2 - 13
UCLA Commotion lab
Nagoya University
Hori Laboratories
MIT Leg Lab
MIT Leg Lab
Uni of Michigan
Robtica, Mexico
MIT Leg Lab
Harvard Robotics Lab
Osaka University
Koube University
Singapore Polytechnic
Keio University
Biped robot #3
Biped Robot #5
Biped-Bike
Geekbot
M2
Passive Walker
Piernuda
Planar Biped
Smooth Walker
Idaten-II
Biped Robot #6
Robo Erectus
Biped Robot #7
# Estimated from photograph. * No data available
Institution
Machine Name
4
8
0.6#
0.67
0.5
0.02
0.8
0.9#
*
0.8#
*
Mass (kg)
0.6
Height
22
12
13
DOF
Impedence Control
central
Neuro Oscillator
Hierarchial -reactive
Smooth Walking - Constrained
Constrained motion planar
Dynamic - non powered
Hierarchial - state control
Virtual Inverted Pendulum
GA & Recurrent Neural Networks
Control Structure
1997
1994
1981
1996
www.yamazaki.
mech.keio.ac.jp
www.robo-erectus.org
ziong.cs.kobe-u.ac.jp
www.dyna.ccm.eng.
osaka-u.ac.jp
hrl.harvard.edu
www.ai.mit.edu/
projects/leglab/
www.132.248.59.55/
piernuda/piernuda.html
www.ai.mit.edu/
projects/leglab/
www.ai.mit.edu/
projects/leglab/
www.hp73.nagaokaut.ac.jp
www.muster.cs.ucla.edu
Site
Table 2.1 Survey of biped robot research
1985-1990
1996-2000
1996
1994-1995
1997
1997
1995
Year
Miyazaki University
Mexico DF
LSHT GRAVIR
Changsha Institute
Kobe University
Osaka University
Tohoku University
Uni of Queensland
VUB Uni Brussells
Uni of Wales, Cardiff
Harbin Institute of Tech. 1.0
Munich Technical Uni
Ninjya
SAICO
Robot Bipede 1
KDW
Biped Robot #2
Biped Robot #3
Monroe
Guroo
Lucy
UWCC Biped
HITBWR-III
Johnnie
MIT Leg Lab
Spring Turkey
1.8
1.0
1.5
1.2
1.2
1.1
0.85
0.8
0.8
1.1
0.7
0.89
Ohio State
CURBI
Height
Institution
Machine Name
40
40
30
30
30
22
22
18
16.3
15
12
12
10
9.16
17
12
23
12
12
Mass (kg) DOF
1987-1995
1991-1994
1997
1980-1996
1994-1996
1995
Year
Central
Gait Control
Hierarchical
Neural / Heirarchical
Hierarchical
Hierarchial - Low order
www.amm.mw.tumuenchen.
de/forschung/zweibeiner/
johnnie_e.html
www.lucy.vub.ac.be
www.csee.uq.edu.au
www.mechatronics.mech.
tohoku.ac.jp/research/biped/
www.dyna.ccm.
eng.osaka-u.ac.jp
www.ziong.cs.kobe-u.ac.jp
www.ai.mit.edu/
projects/leglab/
www.ee.eng.ohio-state.edu
Site
1988-1995
1995
1990
2003
2003
1986
Inverted Pendulum Neural Network 1989
Sequential control
Constrained planar biped
Jacobian Motion control
Control Structure
Waseda University
Tokyo University
Kawada Industries
University of Kentucky
New Era, Russia
BIP team
Waseda University
Honda
UNSW
WL-12RV1
H6
Isamu
Biped Robot #1
Arne & Arnea
BIP 2000
Wabot - 1
Asimo
Roboshift
MIT Leg Lab
2.4
500
210
130
1.5#
1.2
105
61
60
55
1.8
1.23
0.7
1.5
55
50
1.4#
1.37
50
48
40
1.4#
Korea Advanced Institute 1.2
Kaist
3D Biped
Shadow Robot Company 1.4
Shadow Walker
14
15
12
32
24
8#
6#
21
12
40
1.2#
Hannover University
Bart-UH
Mass (kg) DOF
Height
Institution
Machine Name
Hybrid Classic/Hormonal
Kinematic - Static
Reduced order dynamic Jacobian
Heirarchical
Hierarchical
Central
Central
Control Structure
www.mech.unsw.edu.au/mech/
Mechlab/mechatronics.htm
www.honda.com.jp
www.humanoid.waseda.ac.jp
www.inria.fr/rrrt/rt-0243.html
www.robotarena.com
www.crms.engr.uky.edu
www.kawada.co.jp/ams/isamu/
www.jsk.t.u-tokyo.ac.jp/
research/h6/h6.html
www.humanoid.waseda.ac.jp
www.ai.mit.edu/
projects/leglab/
www.Ohzlab.kaist.ac.kr/khr_
robot/khr_humanoid.html
www.shadow.org.uk/
projects/biped.shtml
Site
1994-2004
1997
1988
1999
1996
1987
2003
2003
1996
1989-1995
2003
1987
2000
Year
The concept of a powered exoskeleton, to either assist a human to walk, or increase the
materials handling abilities of a worker, was first investigated by Cornell Aeronautical
Laboratories (CAL) in the mid 1950s. Using the natural movements of the human to
activate and control actuators, the device was essentially to be a motion and force
amplifier. CAL built an unpowered prototype, but shelved the project and concentrated
on medical robotics.
During the early 1960s, General Electric was developing remotely controlled
manipulators or Telechirs, the research was driven by the requirement to handle
hazardous materials such as radioactive elements required for the growing nuclear power
industry. Using force feedback to feel the force on the work-piece, an operator used his
arms to control the movement of the manipulator.
Employing experience gained in this research, the corporation also began to investigate
powered exoskeletons. A similar device to CALs man amplifier was designed which
used the natural movements of the operator to actuate servo-drives powering the
exoskeletons joints. This device, the GEC Hardiman (see Figure 2-6) was also shelved
after the prototype phase (Wiess, 2001).
While neither of these biped exoskeletal
devices were successfully built or tested, they
highlight two points that are most relevant to this
project. The first is that two large corporations,
both involved in materials handling and robotics
perceived a requirement for an operated bipedal
materials handling platform. This suggests that
while a market for such a device existed, the
technology was not available at the time to
realise a working prototype. The defence
industry focus of both of these companies
suggests that the market for a materials handling
biped robot would be military organisations.
Figure 2-6. The

GEC Hardiman
Exoskeleton
The
second
point
highlighted
by
the
exoskeleton research, is that it was the control of

2 -17
the exoskeletons that proved to be the hurdle for development of a fully functional
prototype. Todd (1984) discounts the relevance of exoskeletons to biped robotics, as they
were not autonomously controlled robots. This seems specious however, as a successful
exoskeleton would not be able to rely purely on the human operator for safe operation.
The control system of such a device would use human input only as movement
commands. It would interpret the commands and control the actuators in such a manner
that balance was maintained. Essentially, where a conflict existed between what the
operators movements requested and what could be safely actuated, the control system
would have to override the human input. The device would function as an autonomous
robot, taking only high-level movement or task commands from the operator. In reality,
the controls for a biped materials handling platform would be quite similar to those of a
standard forklift truck. One joystick could control the direction and heading of the
device, and a second could control the material handling equipment. All functions of gait,
balance and joint movement would be autonomously controlled by the supervisory
system.
The early 1970s saw the leading edge of biped robotics development shift from
America to Japan. In particular, a group at the Waseda University, led by Ichiro Kato
(1987), focused heavily on biped robots from the early 1970s. Kato built a number of
bipeds including WAP-1, WAP-2 and WAP-3 which were able to walk on uneven
surfaces and to turn. In joint research with Hitachi, Kato and Waseda University
developed the first full-scale android-like biped in 1973. The device named Wabot-1, not
only exhibited anthropomorphic legs but also upper limbs and hands. In addition, the
robot was fitted with visual sensors and voice-synthesised communications. The robot
remained statically stable by keeping its centre of gravity over one of its large feet at all
times. Wabot-1 was hydraulically driven; but the power source was not carried on board.
Later bipeds built by Kato concentrated on simulation of the biped gait without the
distraction of other anthropomorphic features such as arms and hands. Kato and Hitachi
constructed Waseda Hitachi Leg-11 (WHL-11) in 1985 (Kato et al 1987). This biped
robot, again hydraulically actuated, walked for 40km at The International Science and
Technology Exhibition Tsukaba EXPO 85, where it was demonstrated at the Japanese
Government Pavilion.
2 -18
WHL-11 (see Figure 2-7) displayed a quasi-dynamic gait which Kato describes as a
gait where dynamic walking only takes place in the leg changeover period (Kato 1987).
The robot was statically stable during the gait cycle except for the stage in the cycle when
it transferred weight from one foot to the other. The gait cycle was originally generated
by simulation in two parts; the static, single support phase and the dynamic leg
changeover phase. This simulation was then used to control the robot in walking trials
where the model was modified using the results of repeated tests. Finally, the modified
joint trajectory data was loaded into ROM on board the robot. Because the robot did not
possess enough memory to load all of the joint data, it was only loaded in point form.
During the gait cycle, the control system interpolated between these points to calculate
the required joint positions. WHL-11 was the result of twenty years of research at Waseda
that is evidence not only of the investment into biped robotics, but also of the
complexity of the task.
In 1986, the Japan based Honda Motor Company entered the race to manufacture
androids and built eleven biped robots over the next two decades. In similar fashion to the
Waseda bipeds, Honda concentrated its early research on the problem of bipedal
stability. The first seven prototypes between 1986 and 1993 consisted of a mass supported
Figure 2.7
Waseda
Universitys
WH 11
2 -19
by two legs. The fact that a large institution that was the size of Honda took thirteen years
to develop a stable biped platform is a further indication of the complexity of the task.
In 1993, Honda released a humanoid robot named P1 (Honda, 2003b) which was the
next iteration of their android. The biped included upper arms, hands and a large box
which may be taken for a head. P2 (Honda, 2003b) followed in 1996, P3 (Honda, 2003b)
in 1997 and the culmination of their endeavours, Asimo (see Figure 2.8) was released in
2000. Asimo is a remarkable technological achievement. The robot is able to walk
dynamically at 1.6 km/h, is able to recognise faces and to communicate with people.
Honda market the android as a service robot which one day will be able to act as a carer
for invalids in the home. Asimo stands approximately 1.4 metres tall which Honda says
is the perfect height for helping a person who is bed ridden or is confined to a wheelchair.
It is interesting to note that Honda has decreased the size and mass of its latest androids
from P1 (1.92m & 175kg) to Asimo (1.2m & 130kg). The author, based on the research
conducted during this project suspects that the effect of internal forces and associated
compliance produced by the larger bipeds presented problems for the development of the
robots control system. Accordingly, more rapid and spectacular progress would have
been available with the development of a smaller robot. It is speculated by the author
Figure 2.8
Hondas
humanoid robot
Asimo
2 -20
Figure 2.9 Scatter plot of mass vs height of documented biped robots
(technical data is unavailable from Honda), that Honda is currently developing a much
larger biped of industrial scale. However, as no other industrial scale devices exist, it
would be difficult to predict the size of such a device. Figure 2.9 shows a scatter plot of
the mass of reported biped robots versus their height. It can be seen that the data loosely
fits a curve which is the cube of the robots height divided by 3. The significance of this
equation is not fully understood. However, it would predict that an industrial biped of the
scale exhibited in this project might weigh between 450 and 550 kilograms. It is
interesting to note that when the average height and weight of an adult male human are
plotted on the same graph, it lies well away from the equation described above, i.e.
capability versus weight is better in humans.
As discussed in the introduction, while a service robot would be of use to invalid
patients, the cost of the device, and the wide availability of un-skilled labour, excludes
the commercial viability of Asimo for such roles. Only an android with greater
capability and dexterity than that of a human will be accepted by the market place. For
example, in a typical display of the cooperation between large Japanese corporations,
Kawasaki has developed a commercial application for the Honda Asimo android. In
2002, Kawasaki fitted an Asimo with protective clothing and then developed
teleoperation software so that the android could be used to remotely operate construction
2 -21
equipment (AIST, 2002). The research allowed the robot to mimic the movements of a
human operator effectively turning the robot into a telechir. In situations such as
earthquakes or industrial disasters, there is a requirement for heavy equipment to be used
in areas hazardous to humans. For the last decade, with the development of spread
spectrum wireless data communications, some construction equipment has been
operated remotely via the uses of wireless remote control systems. However, the
availability of such equipment at the site of a disaster is unlikely. Kawasakis research
would enable the Asimo variant named the HRP-1S to be deployed to the disaster area
where it would operate standard construction equipment. Therefore, the development of
the Asimo variant has added value to construction equipment around the world. By
investing in the Kawasaki and Honda technology, once it is available commercially, the
operators and owners of these assets would increase the capability and potential market
for their equipment. This would constitute the first commercial application of an android,
and the state of the art in android development2.
The basic physical form of the android has been achieved by Honda. In terms of
mechanical design, new technologies such as artificial muscles and electroactive
polymers (NDEAA, 2003) will be introduced to android structures as they become
available. However, it is in the area of control and processing systems where the major
advances will continue in this decade.
2.3 CONTROL OF BIPED ROBOTS

A person can walk with a ninety-pound pack on their back, their gait instantly
adjusting to the sudden change in balance. People regularly amble along at a decent pace
with a stiff knee, or maybe an ankle brace, or perhaps a child hanging onto each leg.
Women, regularly walk with dangerously high heels and appear to have no problem
adjusting their gait to accommodate the reduction of ankle flexion. Any circus will
exhibit the skill of a stilt walker whose shank is extended by several hundred percent, but
still manages to walk and to juggle as well. The key to locomotion is control. Try walking after you have been sitting on your legs for half an hour. The lack of blood flow
decreases proprioception, limiting feedback to the local control system. Alcohol, drugs,
head injuries and strokes all affect the ability of the brain to process information. The
2
Honda has used Asimo extensively for publicity purposes. Pictures of the Android shaking hands with world
leaders has promoted the Honda brand in the same way that the development of Formula 1 racing cars has done so
in recent years. Therefore, advertising may have been the first commercial use of an android.
2 -22
geometry of the biped is far less critical than the control system that coordinates its
movement.
The diversity found in the mechanical configuration of biped robots is not reflected in
the structure of control systems that have been designed to automate their movement. A
range of methodologies has been used for global control. However, the most popular
model has been to generate a feed forward trajectory and then to sense deviation from the
trajectory as the global control input. The advantage of this method is that the complex
dynamic equations governing bipedal motion do not have to be solved in real time. In fact,
this research has indicated that the more complex the robot is, the more unlikely it is that
the control system will attempt to incorporate and solve the gait dynamics. Almost
universally, hierarchical control systems have been used to manage the range of control
tasks required to generate biped locomotion. In general, at the highest layer, a main control processor monitors the global status of the robot and generates joint trajectories. At
the lowest layer, joint position sensors provide feedback to local processors that control
the angle of the joint. Between layers, some form of communication is enabled to pass
data from the high-level global control system to the local joint control. While each
control system may deviate in some way from the basic structure, the attempt to separate
the control tasks is similar.
The technology to build a biped has been available for decades. Hydraulics, Servo
motors, sensors and transducers have been readily available since the beginning of the
Second World War. Early attempts to build walking robots relied on the human to
coordinate the motion of the joints. It is interesting that while the human can readily adapt
to changes in geometry, these attempts were unsuccessful, as the brain appeared to be
unable to adapt to controlling teleoperated exoskeletons. Development of full size
walking machines constitutes a systems integration process. It is the role of the walking
robot engineer to bring together all of the available technology required to design, build
and control the device. Given that a wide range of motion control hardware is available
off the shelf, typically it is the structure and the software development that represent the
majority of engineering time consumed during the project. The definitive task of a biped
robot control system is to maintain balance while standing and while in locomotion.
Dynamic locomotion involves acceleration of the centre of gravity of the robot by manipulation of the resultant reaction vector at the foot/feet in contact with the ground. For this
2 -23
reason the tasks of balance and movement cannot be separated. Walking robot control
requires the solution of complex differential and inverse kinematic equations involved
with the control of joint motion. Invariably, these equations become non linear and cannot
be solved in real time. The almost universal approach of the engineer has been to reduce
the amount of data to be processed and to process that data as quickly as possible.
Generally, tasks are dealt with at several levels, to reduce the processing load by
distributing the control functions. One would speculate that this was particularly the case
with early walking machine development, where processing power was limited.
However, the first robotic1 walking machines used a single mainframe computer to
control all aspects of locomotion. GEs walking quadruped truck was controlled by a
human operator tele-operating the vehicles legs by manipulating levers and pedals.
While not a mainframe as such, the operator acted as total control system, taking visual
and sensory data, then controlling the hydraulic valves to coordinate the machines
motion. Ideally, if GE had been able to replace the human operator with a digital control
system capable of comparable levels of control, the vehicle may have been a great
success. Perhaps, today, digital control is achieving similar levels of control to that which
can be offered by a human operator.
Waldrons Adaptive suspension vehicle, a hexapod with multiple statically balanced
gaits (Johnston, 1985) used a single mainframe computer to replace the human operator,
becoming the first successful digitally controlled walking vehicle. Kato et al (1974)
developed a hydraulically controlled biped in 1980. The Wabot-1 is universally accepted
as the first biped to walk with a quasi-dynamic gait. During the gait cycle of the robot,
there were periods between stance phases where the robot effectively fell dynamically
from one leg to the next. While Wabot-1 used a single computer to control the motion of
the robot, a software scheduler was used to switch processes within the control system.
Effectively, the control software was multi-tasked, breaking elements of control into
discrete programs that were processed in a sequential manner. Though the processing of
data was not distributed, the strategy was certainly an attempt at a segmented control
system. The software scheduler recorded requests from input/output devices, referring
The definition of a robot is source for continual argument. It is outside the scope of this project to fundamentally
define it. Some would argue that a numerically controlled machine, running punched tape or operating from cams
would constitute a robot. Others would suggest that an industrial robot is nothing more than a numerically controlled
machine. Karl Kopec who first used the term would suggest that it represents zombie like, repetitive behaviour, certainly not an intelligently controlled machine. For the purposes of this project, the word robot will be taken to mean a
microprocessor-controlled machine.
1
2 -24
to a lookup table to determine in what preference the requests were processed. Given that
WABOT1 was fitted with optical sensors as well as voice recognition and speech
synthesis systems, the amount of data to be processed was considerable for the
processing power available. The use of the scheduler constituted a hierarchical level of
control where the scheduler became the supervisory processor, determining the priority of
data to be processed. During the early 1980s smaller and more powerful microprocessor
became more widely available. At the same time an explosion of computer science
research led to the development and widespread industrial use of layered control systems.
The developers of biped robots, essentially systems integrators, incorporated these
technologies in their research.
Wagner et al (1988) used transputers and the OCCAM parallel processing language to
distribute the control tasks to separate processors that shared memory via a data bus. At
the joint level, foot sensors and shaft encoders provided feedback to those transputers
dedicated to local control.
Monroe (Kumagai, 2000), a 1.2m, 22kg biped built by the Mechatronics Department of
the Tohoku University also uses a hierarchical control system consisting of two 486 and
one 386 computers. Global feed forward trajectories based on empirical human data, and
orientation data are processed by one 486 processor, while local feedback joint control is
processed by the other 486 and 386 processors. Communication is via shared memory.
What makes this hierarchical control system unique is that the same family of processor
has been used for all levels of the hierarchy. Most commonly, hierarchical systems use
processors capable of being programmed in a high level language for global control
where more complex mathematical and data processing functions are required in the
programming language. At a lower level, simpler processors only call for basic
mathematical functions to perform PID type control.
In 1986, Zheng et al (1986) developed a hierarchical control system to automate the
motion of a biped robot. By using four joint processors to facilitate local feedback
control of joints, a central computer was released to coordinate global control. Of
interest was the projects strategy to use digital I/O to communicate across layers of the
control system, rather than serial communications or shared memory. Zheng used eight
channels of bi-directional eight bit parallel I/O to transfer position commands between
the central computer and the joint processors. A further four bit channel was used to
2 -25
control the data transfer. A similar system was investigated for the transfer of data in this
project, however, once constructed, the system can not be expanded without the addition
of further I/O ports. In the case of the F1 controller boards which were selected for local
joint control, no further I/O ports could be fitted.
Zheng suggests that if the period taken to complete a single iteration of the main control system loop is denoted as T, then 1/T gives the frequency or resolved motion rate
(RMR) for the control system. He further suggests that the RMR for the control system
of a biped should be ten times the natural frequency of the robot. Biped robots are
frequently modelled as inverted pendulums [(Kitamura et al 1990), Hemami (1977)
Hemami et al (1973)] which require restoring torque to remain stable. However, as will
be discussed in Chapter 9 of this document, the natural frequency of the inverted
pendulum system will then be a function of the gain of the control system controlling the
restoring torque.
Research that involves simulation of biped robot dynamics often leads to graphical
output of joint torques during the gait cycle. It is interesting to note that all previous biped
robots have used shaft encoders to provide feedback from the joints. It is suggested that
if torque control as opposed to position or velocity control had been used in biped
robotics, the control would either have been open loop, or via the use of servo amplifiers.
Such amplifiers would not only sense the current in the motor, but must also have been
able to sense the change in friction characteristics as the load on the joint varied during
the gait cycle.
An exception to the above is the BIP 2000 biped robot developed by INRIA (Azevedo
2000). The fifteen degree of freedom robot stands 1.8 metres tall and weighs 105kg.
Brushless DC motors are used to move the bipeds joints via reduction drives or
screwcrank systems. The BIP team use an external Unix workstation running a generic
robot control system to dynamically model the robots motion before downloading joint
trajectories or robot tasks to an on board VME processor. During motion, the robot
senses which foot is in contact with the ground and then controls the torque of the joint
actuators based on the error from the predetermined trajectory, the position of the
gravitational vector (sensed from foot force sensors) and predetermined friction
coefficients based on previous data. The group have achieved a high level of success,
with the robot able to balance and to walk while constrained in the frontal plane. The use
2 -26
of dynamically accurate joint trajectories represents a major factor in the efficiency of

biped control systems.
The majority of biped robot control systems attempt to reduce control processing by the
use of pre-generated joint trajectories. Often, as would be expected, these are modifications of the trajectories of the human leg joints during normal gait. The control system
then monitors the performance of the robot against these trajectories, applying restoring
forces when actual values depart from the bounds of expected values. In the case of
Baltes, the only feedback to the control system is the angular velocity of the torso in the
frontal and sagittal plane. These values are monitored to ensure they are within predetermined limits. Additional extension or flexion (over pre-generated trajectory values) of
joints is then used to stablise the gait.
A number of methods have been used to generate joint trajectories. Shimoyama et al
(1985) used simulated trajectories that were stored in the main control processor. These
were based on human locomotion data and then sent to the local joint processors
sequentially. Using dynamic models, the control system calculated modifications to the
feed forward actuator torques to maintain balance and to force the actual trajectories to
converge with simulated data. In the case of the biped robot BIPER-4 the error was
allowed to accumulate during the initial swing phase, but was corrected by the
positioning of the swing foot as it landed. Wagner et al (1988) also used a stored set of
joint trajectories that had been generated through the use of optimised (reduced order)
dynamic motion equations. Rather than use separate processors to achieve hierarchical
control, Wagner incorporated a multitasking operating system to parallel process the
global and local motion control tasks. To minimise error from the predetermined
trajectory, a knowledge-based system adapted control parameters based on historical
values. Of course, the control system relied on a sufficiently accurate simulation to
ensure that the biped stayed upright for long enough to generate valid historical data.
Several researchers [(Batlle et al., 1999), (Baltes et al., 2004)] have attempted to reduce
the complexity of the control task by reducing not only the number of inputs and outputs
in the system (by reducing the number of degrees of freedom) but by reducing the number of feedback variables. As highlighted by One novel approach to the complexities of
the multiple degrees of freedom of the biped leg system was to articulate the foot with
passive compliant actuators. Batlle also eliminated the requirement for hip abduction by
2 -27
providing the body with a counterweight that was able to be moved in the frontal plane
in order to generate frontal sway for the swing phase. This configuration results in only
two degrees of freedom for each leg; knee extension and hip extension. While unable to
produce a dynamic gait, their robot was able to walk with a ten second period.
Wollherr et al. (2003) recognize that the constraints of a foot or feet in contact with the
ground and controlled torso motion result in unique solutions to hip and knee kinematics. Recursive multi-body algorithms are then use to evaluate reduced body dynamics
without actually extracting them. This reduced order processing can be conducted in realtime allowing the control to model pre-calculated trajectories in selected directions to
reduce stability deviation.
In a similar method, Vanderborght et al. (2004) use pre-generated trajectories to stabilize the pneumatically actuated biped Lucy. Initially the natural motion of the torso is
determined. Polynomial knee and hip trajectories are then generated kinematically from
this motion. The control system then uses ankle actuators to superimpose restorative
torque over the predetermined hip and knee torque to ensure the torso motion adheres to
the pre-generated path. Vermeulen et al. (2204) explains that, as with other bipeds, the
control systems, batteries and motive force are contained within the torso of the biped
Lucy, making it the heaviest link of the robot. By allowing the torso to maintain a natural motion energy requirements are minimised. This highlights the strong link between
the structure of the robot and the design of the control system. As highlighted by Vaughan
et al. (2004), it is also desirable to reduce the moment of inertia of limbs by controlling
the maximum weight of the links. In particular, they suggest, as in the case of the human,
the foot should restricted to weigh less than the shank, the shank should weigh less than
the thigh, etc. By reducing the inertia of the extended limb, power requirements to drive
the limb are also minimised. Carl et al. (2005) suggest the most critical criteria in the
design of a biped is to maintain the centre of mass of the robot at the pelvis. They argue
that a lower COG would require larger oscillations of the trunk to maintain balance in the
sagittal plane.
Isik and Meystel (1988) suggested that a hierarchical structure based on resolution
relevance for a wheeled mobile robot would reduce reaction time by concentrating
processing on areas most relevant to the navigation of the robot. General areas were
scanned in low resolution and then subsections of interest were rescanned in a higher
2 -28
resolution. This strategy allowed image processing to be concentrated on those areas

most relevant to the navigation of the robot. Given that the human lower limb system
contains fourteen principle degrees of freedom (DOF), a similar strategy would
concentrate processor time on the control of those degrees that are most directly linked
to the control of the stability of the robot at any given time. For example, during single
leg support, the motion of the joints in the support leg exert considerably more control
over the centre of gravity of the robot than those of the swing leg. By using either a set
trajectory for the joint positions of the swing leg, or a linear approximation as suggested
by Lee and Mansour (1984), central processing would then be concentrated on the ankle
knee and hip joints of the support leg. Effectively, this would reduce a 14 degree of
freedom system to a six DOF system.
The team developing the Shadow Biped (Shadow 2003) have used some interesting
strategies to control their robot. They use a series of hand generated rules to control the
valves that inflate and deflate the pneumatic actuators of the robot. No trajectories or
dynamic algorithms are used to control the positions of the joints. A series of fuzzy sets
determines the outputs to the valves based on a series of inputs. The membership
functions of the sets are then hand modified based on observation of the robots reaction
to external forces. The Shadow team have been able to balance the robot using this
strategy. They have also been able to control the robot to take two steps prior to falling.
The suggestion is that by continually modifying the fuzzy controller sets, the robot will
achieve continuous biped locomotion.
The drawback of such a control strategy is the reliance of the control system on
practical data as opposed to a dynamic model of the robot. While the hand generated
reactions may provide finer control than a linearised dynamic model, the control
envelope is confined to the range of inputs that have been experienced during trials. In
the event that the robot is subjected to an external force larger than previously
experienced, the reaction to the force may be unpredictable.
The Shadow Group acknowledge this drawback and suggest that the use of a neural
network may be used to control the robot. A neural network offers the opportunity to
expose the robots control system to a greater range of inputs and to develop a much
greater range of output behaviours than would be possible by hand. However, the
disadvantage of neural networks is that the network must be given a series of inputs and
2 -29
outputs to learn to generate control behaviours. As highlighted by the Shadow Group,

these relationships must either be derived from accurate dynamic models (in which case
there is no requirement for non linear control), or developed from continuous
experimentation. In the latter case, as a random element is required, and as initial trials
would be with an uneducated network, the result would be continuous falling or
unpredictable behaviour which would be guaranteed to destroy the robot, in the case of
Roboshift. Those who have developed neural networks which control bipeds in a
simulation do not realise that each unsuccessful trial that took two minutes on a
computer screen, may have resulted in days of repairs in the laboratory.
An area of humanoid robotics where the technology has advanced in recent years is that
of soccer playing robots participating in the Human Soccer Robot League. The small size
of the robots enables the use of off the shelf technology such as servo drives from the
remote control model market. The development is heavily focused toward the vision
processing and decision making abilities of the robots as they attempt to kick small balls
past each other. Again, the feet of these robots are disproportionally large providing an
extremely stable base. Some robots such as Clyon (Senior & Tosunoglu, 2005), with
no facility for hip abduction, are still able to statically shift the centre of gravity of the
robot completely over one foot while it rotates around the ankle in swing phase. With the
relative simplicity and low cost of the structure and actuators, the developers are able to
concentrate on software development which has produced novel and efficient biped robot
control strategies.
Zhou et al. (2003) reduces the demands of three dimensional dynamics by decoupling
the frontal and sagittal planes. Using a neural fuzzy system two FRL agents search frontal
and sagittal state spaces to speed up learning. The advantages of a fuzzy system lend
themselves to the non-linear multi input and output control of a biped robot. The disadvantage of a fuzzy system lies in the unpredictability of the response to inputs not previously encountered. While not critical in a small robot, this may cause disastrous results
in an industrial scale biped weighing several hundred kilograms.
The Institute of Applied Mechanics at the Munich Technical University (TUM) have
developed a 16 degree of freedom biped named Johnnie (TUM, 2003). The humanoid
robot is self contained except for power supply which is provided by an umbilical cable.
The biped uses a hierarchical control system with three levels of control. The highest
2 -30
layer computes the joint trajectories which are based on an optimised gait pattern
developed from dynamic modelling. The second layer monitors basic dynamic
characteristics of the system superimposing dynamic adjustments to the feed forward
joint trajectories. The lowest layer control the individual joint motors using PID control
and a friction compensation algorithm. The robot has achieved dynamic locomotion
while walking on a conveyor belt. The control system used in this project raises two
interesting issues.
The first question that arises in the TUM project is that, given that the joint trajectories
were developed from dynamic modelling, why is it then necessary to use a second layer
of control to maintain dynamic stability? The most likely answer is that the dynamic
model used to generate joint trajectories was not sufficiently accurate to account for
compliance in the system or non-linear characteristics of the drive system. However, by
processing the majority of dynamic analysis externally, the control system is able to
focus on a reduced order model during locomotion. Essentially, this strategy increases the
computational efficiency of the control system while maintaining a pseudo dynamic
model.
Given the wide availability of powerful, low-cost microprocessor that are easily able to
be networked, there is no limit to the number of models that could be used to control a
biped in real time. For example, one processor might contain a reduced order dynamic
model, another may contain a neural network based on historical data, yet another may
contain an inverted pendulum model etc. The final control output may be some form of
weighted average of the outputs of all models in the control system. Taken to the extreme,
a final processor may use fuzzy sets to compute the outputs based on the membership of
the outputs of the previous models. If nothing else, this proposition strongly suggests the
use of an expandable control structure.
The second question raised by the TUM project is the lack of an onboard power
supply. Given the success of the project, it would appear to be a simple task, and a minor
modification to the dynamic model, to mount the power supply on board the robot. Based
on the experience gained in this project, it is suggested that the additional weight of
batteries would increase the non-linear dynamic characteristics of the robot beyond the
capacity of the control system.
2 -31
Another group to make use of extensive dynamic modelling is the Mobile Robots
Group at the University of Queensland, Australia. They use the DynaMechs package to
model their 1.2m tall, 38kg, and 23DOF humanoid named GuRoo (Roberts et al, 2003).
They have achieved an extremely high level of correlation between simulated and
measured data. The robots hierarchical control system consists of a Compaq Ipaq as the
main processor for global control and five Texas Instruments DSP boards each
controlling three local joint motors. The robots control software is modelled and
optimised using the DynaMechs package prior to downloading to the robot. The group
have achieved balance using local control but are yet to achieve global motion control.
Some researchers such as KAIST (Kim and Oh 2002) have embraced the concept of the
zero moment point (ZMP). This is a point on the ground, about which the sum of all
moments produced by forces in the robot (including gravity) equates to zero. In static
balance this point must be kept between the toe and heel in the sagittal plane and between
the feet in the frontal plane. The application of ZMP control in bipeds can be achieved at
two levels of hierarchical control. At the lower level, fine motor adjustments of ankle and
foot actuators move the point of actuation of ground reaction forces providing an offset
torque to balance minor unbalanced forces within the biped structure. At a higher level,
movement of larger masses at a greater distance from the ground, such as the hips,
provide larger torques to offset accelerations produced by external forces. Again, ZMP
control can be used to control the error of the system from predefined open loop or
loosely coupled trajectories.
During the late 1980s and early 1990s, development of biped robots proliferated. Key
institutions in Japan such as The Tokyo, Nagoya, Osaka, and Kobe Universities, as well
as American groups such as the Harvard Robotics Lab, the MIT Leg Lab and Ohio
University all began to develop biped projects which will be discussed in the following
sections. This acceleration in the development of the android over the last 20 years has
been astounding. In recent years humanoid robots have appeared all over the globe.
Amazingly, though it took Honda over 20 years and eleven prototypes to develop their
humanoid, groups with no previous history of android development are announcing
humanoids with similar capabilities to Asimo. For example, New Era, a St Petersburg
company in Russia (New Era 2003), have developed two 1.23m, 61kg humanoids that
can supposedly recognise and follow people while avoiding obstacles. Further pushing
back the frontiers of android research, the company claims that one robot ARNE is male,
2 -32
the other ARNEA, is female. The author of this document was relieved to learn that;
..there are no obvious physical differences
between male and female, apart from their colour..
The Kibertron Humanoid Robot Project (Kibertron 2004) also appears to be
progressing well. Their 1.67m, 90kg, 92 DOF humanoid is still in the design stages,
however research has progressed to the stage where the group has determined that by
using self-education;
..extremely complicated and intelligent units, which do not
need very large computing power are very effective and efficient.
The control strategy of this group is to build an android with more degrees of freedom
than any biped to date, and then to use simpler processors to control it. It appears we have
come a long way in the last 20 years from Kato whose successful philosophy was to keep
the mechanics simple and throw every bit of the latest processing capacity at the motion
problem!
2.4 DESIGN CRITERIA OF AN INDUSTRIAL BIPED

Having investigated the history and the state of the art in biped robotics, it is clear that
there is little or no research being conducted specifically in the field of industrial scale
bipeds. Accordingly, no benchmark performance or operational specification was
available for the design or capabilities of such a device. While this categorised the
research presented in this project as unique, it also present a requirement for the
formulation of a set of design criteria for the device. The following sections outline the
characteristics that have been developed for the design of the biped robot presented in
this project.
Naturally, much biped research can be applied to all bipeds regardless of scale.
However, research presented later in this document will show that the scale of an
industrial biped presents unique challenges in the design and control of such a device.
During dynamic walk, or during testing of an automated device possessing fourteen
hydraulic actuators, large dynamic forces are generated. These forces are unique to biped
robots and are not experienced by industrial robots or wheeled or tracked mobile robots.
Further, transducers such as shaft encoders and orientation sensors used in mobile or
industrial robotic applications have not been designed for shock loadings that may be
experienced in biped applications. When unexpected behaviour occurs, these forces may
2 -33
cause damage to the structure. This is evidenced in the literature. For example, M2 is a
3D biped robot developed by the MIT Leg Lab (2001) In their description of the device,
MIT state that one of the design goals of the robot is the ability to be
readily used to perform experiments without breaking .
The Shadow robot company report that initial experiments with their biped, the
Shadow biped (Shadow 2003), suffered from repeated mechanical failure with an MTBF
of less than two minutes. In the case of an industrial scale biped, where most failures
require welding to repair, such breakages considerably frustrate the testing process.
Choong et al. (2003) suggest that a basic set of specifications for a biped robot would
include a requirement that it be robust and reliable and that it should be easily
maintained. Certainly these are common requirements of all industrial plant.
Therefore the first two design requirements of this project shall be;
that the industrial biped robot is physically robust
(Design criterion #1)
wherever possible, off the shelf components should

be incorporated so that repairs can be expedited.
A final major item that was excluded from the analysis of leg deflection was the
additional weight of a payload. Any industrial or domestic biped robot must be able to
lift substantial objects to be useful. Australia Occupational Health and Safety legislation
prescribes that 25kg is the maximum unaided lift to be attempted by employees. One
industry where the incidence of manual handling or overexertion injuries is high is the
health industry. The US Department of Labor and the Bureau of Labor Statistics has
reported that the frequency of lost time injuries due to overexertion is at the rate 474 per
10,000 workers. The South Australian Workcover Office reports that the average cost of
an overexertion injury is in the order of $8,000. This industry, according to Hondas
publicity, is the target market for Asimo.
These statistics offer two insights. Firstly, the cost of injuries per 10,000 workers is
approximately $3,000,000. Even if the cost of an Asimo was reduced to $300,000 by
mass production, and it was assumed that one Asimo would replace up to 3 workers, the
implementation of the robots would only reduce injuries by 3%. As a service robot in the
health care industry, the device is commercially unviable. Secondly, if it is assumed that
2 -34
the regulated 25kg lift is considered as safe weight, workers must regularly be required
to lift a substantially greater mass to cause such a high frequency of injuries. It is
suggested that a safe working load of at least 40kg would be required of any robot
working in the industry.
In the case of an industrial biped, the effect of payload would be considerably greater.
Ideally, an industrial biped would sustain a payload of 1000kg. However, based on the
analysis of Figure 2.10, such a load would increase the leg deflection of the to the order
of 100mm. This value of error would prove difficult for the control system to manage. In
the event that the control system was able to function with this magnitude of position
error, an error of 100mm at an end effector would render the device useless for military
applications such as the loading of bombs to aircraft.
To determine a realistic capacity for the prototype designed and constructed in this
project, a payload of one 500lb bomb was selected. Therefore the third design criterion is;
A working capacity of 250kg.
The use of the device for military materials handling applications suggests that the
biped would be used in the field, in environments unsuitable for conventual materials
handling equipment. As previously discussed, while completing the review of literature
for this project, no references were found in respect to the performance parameters of an
industrial scale materials handling biped robot. The author then attempted to approach
major manufacturers of materials handling equipment to canvass opinions on the subject.
Unfortunately, the sheer size of these organisations rendered them incapable of
providing access to the senior engineers responsible for research and development in
these areas. The major piece of equipment used by industry and the military to handle
material in off road situations is the all-terrain forklift. Surprisingly, these vehicles
(which the author has operated extensively during the course of employment) can only
manage quite small discontinuities given the size and bulk of the vehicles. For example,
the forklift shown in Figure 2.10 has a capacity of over 2000kg yet it only has a ground
clearance of 304mm. This height is only slightly higher than a standard stair riser. Not
only should an industrial biped be able to climb similar stairs to humans (provided the
stairs are able to bear the weight), but it should also be able to climb or step over larger
2 -35
Figure 2.10 All terrain forklift
discontinuities. Therefore, another design criterion is set as;

Capable of traversing a 500mm discontinuity.
(Design Criterion #3)
The final design criteria, which would be expected of any industrial vehicle, are;
Able to work for long periods.
Completely self contained.
2.5 CONCLUSION
Based on the literature review conducted for this project, no industrial scale materials
handling robot has been previously designed or built. Therefore, the research conducted
in this project constitutes the first serious attempt to build and to control such a device.
While smaller humanoid biped robots have been successfully demonstrated, the
potential working load of the devices make them unsuitable for the applications for
which they are proposed. It is also suggested that even if the cost of the devices was
reduced to $300,000, they would not be commercially viable. As quoted by Lytle (BBC
2003), a gentleman visiting a recent robot exhibition in Japan was impressed by Hondas
ASIMO:
I dont know how useful a robot like ASIMO is, but I wouldnt mind having one at home
for the kids.
2 -36
MECHANICAL DESIGN OF ROBOSHIFT
A good scientist is a person with original ideas. A good engineer

is a person who makes a design that works with as few original
ideas as possible.
- Freeman Dyson
It is remarkable that when a mechanical device is modelled on a biological form, or

when a machine attempts to imitate the actions of an animal, the geometry takes on a
biological appearance. The following sections detail the evolution of the mechanical
design of Roboshift concluding with photographs of the completed machine.
3.1 DESIGN PHILOSOPHY

Further to the conclusions drawn from Chapter 2, this project endeavours to
demonstrate that an industrial scale biped robotic materials handling device is viable. The
performance specification for the device has been defined in Chapter 2 and is repeated in
Table 3.1.
(1st Criterion)
Easily maintained
(2nd Criterion)
(3rd Criterion)
(4th Criterion)
Able to traverse 500mm discontinuities
(5th Criterion)
(6th Criterion)
Table 3.1 Roboshift design criteria
Given the scale and complex nature of the design, construction and testing of a device
of this scale, it was necessary to Fast-Track the project. Given the available resources,
the knowledge base of those involved, and the timescale available, the following
decisions were made very early in the project:
The device would be an anthropomorphic Biped
The Software would be C based
Communications would be RS232
Power source would be an Internal Combustion Engine
3-1
Chapter 3 - Mechanical design of Roboshift
Motive force would be Hydraulics

Power would be 12 Volt
Control Hardware would be PC and Motorola HC11 Based
Figure 3.1 shows a semantic net of the initial decision making process.
Figure 3.1 Device configuration decision matrix
The drawback of fast-tracking the initial design was the possibility that an error of
judgement made early in the project would lead to more serious problems later on.
However, the amount of research required for full confidence in each aspect of the initial
decision and design process would result in the project never being realised. As will be
discussed in Chapter 12, with the benefit of hindsight, there are aspects of the robots
design that could be improved. However, the majority of these issues would only have
3-2
come to light after testing of the robot and it is unlikely that any amount of analysis
would have prevented the difficulties that were encountered. Any complex robotic
project relying on limited funding continues to work with the original prototype where,
if funds were available, two or three iterations would be built prior to the final prototype.
For this reason, significant resources are expended attempting to maximise the return on
existing designs.
In terms of the mechanical configuration, the overriding design philosophy was to
imitate the human lower limb structure as closely as possible. Where it was not possible
to follow the human structure exactly, then analysis and modelling would be used to
confirm that divergence from the model did not affect the integrity of the philosophy. For
the robot to be able to walk, it was believed that if the robot:
Possessed all of the degrees of freedom of the human lower legs and hips
Was constructed with joints that exhibited a similar range of movement to that of
the human
Could be controlled with sufficient speed and accuracy
The philosophy was based on the facts that:
1. Humans are able to walk with knee injuries, foot injuries, hip and ankle
injuries, a child hanging onto one leg or both legs, their own weight on their
shoulders or any number of pathological or structural modifications.
2. The wide range and variants of the basic biped structure displayed in the
designs of other biped researchers indicate that while the design of leg
systems may be appropriate for the project, their exact configuration is not
critical.
3. The range of gaits used by the human including skipping, running, jogging and
even hopping, suggest that the human lower limb system has not been
optimised for any particular gait. Rather, the control system is so completely
adaptable that it adjusts joint trajectories to optimise any of the modes of
locomotion based on the existing leg system.
Therefore, given the above philosophy, it is proposed that it is the ability of the control
system to manipulate the joints of the legs that is more critical to locomotion than the
actual structure of the leg system.
3-3
3.2 MECHANICAL CONFIGURATION

The following sections outline the design processes that led to the final configuration
of Roboshift. As shown in Figure 3.1, the decision to build a biped robot immediately
determined the basic geometry of the device. The use of hydraulic actuation and an
internal combustion engine as the power source also determined many characteristics of
the mechanical design. While not having as substantial an impact, the choice of
transducers, mounting of sensors, requirement for 12 VDC power and the mounting of
hydraulic valves also imposed constraints on the mechanical design.
3.2.1 BASIC STRUCTURE
Since 1965, attempts have been made to realise a bipedal walking materials handling
robot. Previous attempts have been based on force multiplying exoskeletons such as the
General Electric Hardiman, previously discussed in Chapter 2. This was a teleoperated
device with the operator strapped into the exoskeleton using his/her limbs to control the
robots motion. Given the available technology, this project was adventurous and may
have proceeded had it been commenced even ten years later when more compact micro
processing became available. Information on the Hardiman is limited; however reports
suggest that the device was uncontrollable. Current research into exoskeletons is being
conducted at the University of California, Berkeley, where a motorised exoskeleton is
being developed (New Scientist, 2004). The research has attracted DARPA funding
which reinforces the suggestion that the most appropriate use and commercial market for
such devices will be the defence industry.
Regardless of the size or configuration of an exoskeleton, any device must be
significantly larger and heavier than the human to which it is strapped. The centre of
gravity of the device, as well as the centres of percussion of the joints must shift from
those of the operator. As well, to avoid accidental operation of actuators, a positive force
would be required from the operator to initiate control. When these characteristics are
combined, the operator would become both mentally and physically fatigued in a short
period as he or she fought to positively control their own limbs in unnatural trajectories,
while applying continuous force. As discussed in Chapter 2, this also appeared to be the
case with the Moshers walking truck. For this reason, any kind of imitative or limb
tracking teleoperation was rejected. Therefore, the decision was made to use a
fly-by-wire control system where the commands of the operator would be interpreted by
the control system to move the robot in a given direction, or to lift an object. The control
3-4
Figure 3.2 Schematic

of degrees of Freedom
system would then translate these commands into joint movements. Whether the
operator was on board the device, or was operating the robot remotely made little
difference to the control system.
To increase the mobility and stability of the robot without increasing its size, the
decision was made to attach the hips to the shoulder of the body as shown in Figure 3.2.
Thus, for the same size, its stride length was increased twofold. As well, the moment of
inertia of the robots upper body about the foot would be increased along with the
period of oscillation and the time available for the control system to react. A mathematical model was developed to confirm the feasibility of such a configuration (see chapter 4).
To satisfy the first design criterion (see Table 3.1) the ability to transport 250kg, the
biped had to be able to balance as a human does. Balancing of the robot requires fine
motor adjustments to the feet allowing stable motion on non-uniform surfaces. This led
to the development of a fully anthropomorphic robotic foot and ankle. No biped device
to date has incorporated a foot that consists of a separate toe, ball and heel. To provide
3-5
the required force locally to each joint, in as compact a manner as possible, it was
decided that all joints would be hydraulically driven. The advantage hydraulic power
presents over electric servo-motors is a higher power to weight ratio of the actuator
which can be mounted separately from the power source.
For the robot to be physically robust and easily serviceable, it was decided to construct
the major members from Aluminium, using roller bearings at each joint and to use
off-the-shelf hydraulic actuators and valves. The effects of these overall design paradigms and the evolution of the design will be discussed in the following sections.
3.2.2 CONFIGURATION OF JOINTS
The overall design philosophy for the robot was the premise that if the device possessed
similar degrees of freedom to those of the human, it should be able to walk. The human
joints and type of joint that required replication are shown in Table 3.2:
Joint
Limbs joined
Type of joint
Metatarso phalangeal joint
Toe and forefoot
Ball and socket
Tarso-metatarsal
Midfoot and forefoot
Hinge
Ankle and subtalar
Midfoot and leg
Perpendicular Hinges
Knee and patello-femoral
Leg and thigh
Double Hinge
Hip
Thigh and Pelvis
Ball and Socket
Table 3.2 Human lower limb joint movement
By use of tendons, ligaments, muscles and bones, humans and animals possess fine
control of highly articulated joints. Significant research has been conducted into the
imitation of these biological systems and the control of them by the use of
electromyographic signals. Groups such as the Department of Mechanical Systems
Engineering at the Tokyo University of Agriculture and Technology, the Department of
Computer science, Institute for Control and Robotics, University of Karlsruhe, Germany
and the Institut Automatisierungstechnik at the University of Bremen have developed
artificial hands for installation on service robots. These manipulators are designed to
increase the service capacity of robots by allowing a greater degree of dexterity and the
ability to operate in the human environment where artefacts and machinery have been
designed to be used and operated by the human hand. Such end effectors are promising
for this purpose, however they are of little significance to this project as an industrial
3-6
scale robot will be used in an environment where items have been designed for
manipulation by equipment such as forklifts and cranes. For example heavy objects are
fitted with lifting lugs or strapped to pallets rather than being fitted with handles.
Electroactive Polymer Actuators (EPAs) or artificial muscles show enormous potential
for the imitation of the muscular function both in the construction of artificial
anthropomorphic systems (Androids etc) and for the replacement or implantation of
human limbs or organs such as the heart. In terms of advanced android research, the
future may see the development of an anthropomorphic robot using this technology.
When combined with the technology developed to imitate the human hand, it may be
possible to closely reproduce human joints using artificial materials. Realistically, using
such expertise to construct and control the joints of an industrial biped is beyond the
scope and resources of this project. Rather, the challenge of the design of the joints of the
robot was to reproduce the joint movement using available industrial hardware.
Based on the available joint movement data and models [(Fischer & Braune, 1987),
(Hartrum, 1973)] it was determined that fourteen degrees of freedom were critical to the
replication of the movement of the human lower limbs. In the human, joints such as the
hip joint exhibit up to three degrees of freedom. To simplify design, fabrication and
control, these joints were deconstructed to a combination of single degree of freedom
systems. In the case of the hip joint, the motion has been broken into two hinge and one
rotational degree of freedom. The schematic of the degrees of freedom required by the
robot can be seen in Figure 3.2, found earlier in this chapter, where the seven distinct
movements available to each leg are illustrated. The human joints and the types of joints
that were used to simulate those of the human lower limb are listed together with
included limbs in Table 3.3.
Limbs joined
Movement
Type of joint
Toe and forefoot
Plantar flexion/extension
Hinge
Midfoot and lower leg
Ankle flexion/extension
Hinge
Subtalor abduction/aduction Axial rotation
Leg and thigh
Leg extension/flexion
Hinge
Thigh and pelvis
Thigh extension/flexion
Thigh abduction/adduction
Thigh rotation
Hinge
Hinge
Axial rotation
Table 3.3 Simulation of human lower limb joints

3-7
3.2.3 RANGE OF MOVEMENT

Maintaining the design philosophy that the robot should exhibit the same range of
movement of the human lower limb, data of joint trajectory angles was plotted to
determine the maximum joint trajectories. The primary joint movements responsible for
bipedal locomotion in the sagittal plane are:
Hip extension
Knee extension
Foot flexion.
Toe flexion, hip abduction and ankle abduction serve to stabilise the trajectory during
the single support or swing phase. Additionally, hip abduction and ankle abduction allow
the transfer of weight in the frontal plane during the initiation of locomotion and during
double support phase.
To determine the range of movement required for the hip extension, knee extension and
ankle flexion, a range of data from the examination of human movement was required.
Braune and Fischer (1987) used time lapse photography to highlight joint trajectories
through a range of standard, loaded and pathological locomotion cycles. As this data was
acquired from individuals, even the standard locomotion data could be assumed to
display individual characteristics of the subjects gait. In 1973, Hartrum (1973) used
Fourier Transforms to generate a parametric model from Braune and Fischers data.
While this model smoothed the data it was found to be discontinuous at the boundaries
of the phases of locomotion where the minor terms of the Fourier series had been
ignored. While not sufficient for use in the generation of joint trajectories for control of
the robot, it was decided to use data generated from the use of the model for mechanical
design purposes. However, as Hartrums model did not include the metatarsals, the model
was modified for this project to include a simplified foot joint and toes (see analysis in
Chapter 4). As no data was available for the trajectory of the toes during swing phase, it
was assumed that they would return to the pre heel-strike position in a linear motion from
lift off. This phase of the toe motion was not modelled and appears discontinuous
in the plotted ranges. Joint trajectory curves plotted from Braunes data and from the
modified Hartrum model are shown in Figure 3.3 (a) and (b).
3-8
Hip, Knee & Ankle Angle

Iterpolated from Braune
1.4
2
1.8
1.2
1.6
Angle (Radians)
1.4
1.2
0.8
1
0.6
0.8
0.6
0.4
0.4
0.2
0.2
0
0
Phase (Radians)
Knee Extension
Ankle Flexion
Hip Extension
(a)
Hip, Knee, Ankle & Foot Angles

Parametric Model
2.5
6
5
Angle (Radians)
1.5
3
1
2
0.5
Phase (Radians)
Knee Extension
Foot Flexion
Hip Extension
Toe Extension
(b)
Figure 3.3 Hip, knee, ankle and foot angles (a) interpolated from Braume (b) from the
modified Hartrum model
3-9
Angle
Minimum
Maximum
Determined by;
Hip Extension
0.994857
1.780196
Modelling
Hip Abduction
-03.492
0.5238
Estimation
Hip rotation
-1.0
1.0
Estimation
Knee Extension
0.0698
1.256119
Modelling
Ankle Flexion
1.287423
1.922746
Modelling
Ankle Abduction
-1.0
1.0
Estimation
Toe Flexion
1.153757
2.531241
Modelling
Table 3.4 Human lower limb joint movement
Table 3.4 records the joint angles from the analysis of human locomotion. As
indicated, the primary driving angle ranges were modelled whereas the minor angle
ranges were estimated based on Braune and Fischers data. These minor angle ranges
were later confirmed during kinematic and dynamic modelling (see Chapter 4).
The angles above were viewed as the minimum required for locomotion. It could be
expected that an industrial scale biped would require a range of gaits for lifting large
objects, moving on unlevel ground or negotiating discontinuities. Therefore it should be
expected that the required range of joint motion would be greater than that which could
be seen in normal, level locomotion. The initial aim of the project was to produce a biped
capable of locomotion on level terrain. However, where the joint could accommodate a
greater range of movement this was incorporated into the design. Ideally, joint motion
would be designed to reduce the risk of damage from interference between limbs. For the
robot to exhibit the same degrees of freedom found in the human lower limbs, this
method of inbuilt safety was not possible. The robot relies on software to determine the
range of movement permitted during active motion.
3.2.4 MECHANICAL DESIGN FOR ASPECTS OF CONTROL SYSTEM
In terms of mechanical design, the requirements of the control system for the design of
the structure are:
The location and adaptation of transducers
The location and connection of actuators
The location and mounting of power and electrical systems
3 - 10
Figure 3.4 Mounting of shaft encoder
3.2.4.1 TRANSDUCERS
Having decided to use digital shaft encoders to provide the level of accuracy and
repeatability required for joint control, the selection of encoder was considered.
Industrial digital shaft encoders include their own bearings, seals and shafts and are
typically housed in containers sealed to IP67 or greater. Most importantly they
incorporate an input shaft that is then connected to the machine shaft via a flexible
coupling which allows for any misalignment in the connection. Unfortunately such
encoders are expensive, prohibiting their use in this project. To achieve a high level of
accuracy for a realistic price, Hewlett Packard HEDS 5540 encoders were used. In most
cases, these encoders are located on extensions of the joint hinge shaft. As shown in
Figure 3.4, a machined stud is threaded into the end of the joint hinge shaft which
protrudes through the nut which locates the shaft. The shaft encoder then measures the
rotation of one joint member relative to the other.
As the encoders are not connected via flexible couplings, the alignment of the shaft
encoder on the shaft is critical. For this reason, bearings were used in all joints to reduce
any slack in the location of the shafts. In the case of the hip extension shafts which
support the full load of the robot, excessive flex was found at the joint. This led to
misalignment of the shaft encoder and a resulting loss of data. To overcome this
situation, the shaft encoders were mounted remotely, and then connected using timing
3 - 11
Figure 3.5 Hip extension shaft encoders
belts and pulleys (see Figure 3.5). An additional advantage of this configuration was that
the rotation angle of the hip extension shafts was multiplied by the use of different sized
timing pulleys which allowed a greater resolution of the hip extension angle. Generally,
the shaft encoders are mounted on the outside of the robot to minimise
damage from limb to limb contact. Where this has not been possible, they have been
mounted in recessed areas.
3.2.4.2 ACTUATORS
In keeping with the philosophy that only readily available, off the shelf components
are used in the project wherever possible, the decision was made to use Vickers
Hydro-Line hydraulic cylinders and Vickers B1-Series hydraulic gear motors to actuate
the robot. The use of hydraulic cylinders requires greater space for actuation in the
vicinity of the joint than would be the case with servomotors. As the cylinders are linear
actuators, only the tangential component of force relative to the centre of rotation of the
joint is available to produce torque. In some cases the angle between the member and the
cylinder is small, thus the radial component i.e. the sine of the angle between the axis of
the limb and the axis of the cylinder, must be relatively high to provide the required
torque in the joint. This is typically the case in joints such as the knee as shown in
3 - 12
Figure 3.6 Knee

extension
cylinder
Figure 3.6. The joint must therefore be designed to withstand the forces produced by the
actuator in addition to the forces produced by the dynamics of the robot. This design must
include stronger sections around actuator connection points. As the axis of the cylinder
changes as the joint moves, the motion of the cylinders relative to the joint must be
accounted for when designing the joints of the robot. The details of these design issues
will be illustrated during the discussion of the design of each joint.
3.2.4.3 POWER AND ELECTRICAL SYSTEMS.
Generally, the design process commenced with the modelling of the required
movement of the robot. From these models the limb and actuator design was developed.
Power and electrical devices were then incorporated into the structure so that the centre
of gravity of the robot was generally maintained coincident with the hip extension axis.
Devices were placed in positions where they were accessible when required for
maintenance and would not interfere with the movement of the joints.
Typically, power, control and electrical components are contained within modules that
are mounted to the robots frame. The internal combustion engine, alternator, hydraulic
pumps and batteries are housed within the engine module which is manufactured of
tubular steel. While the overall dimensions of this module were derived from the design
process, the placement and mounting of components within the module were determined
3 - 13
by investigation. The installation of electrical and control wiring and hydraulic and
pneumatic piping was also determined by investigation of the completed structure.
3.3 DETAILED MECHANICAL DESIGN

The following sections detail the evolution of the design of the individual components
that make up the robot. The design was driven by a combination of:
1. The prime design paradigm; that if the robot had the same basic form of the
human lower limbs, and same degrees of freedom, it would be able to walk
2. The five design criteria as listed in Table 3.1
3. The requirements of the control system, actuators and power and electrical
systems
3.3.1 FEET
Effectively end effectors; the feet of the robot are the only components that are in
contact with the outside world during locomotion. Ground reaction and gravity are the
only external forces available to maintain the equilibrium of the robot. For these reasons
it is critical that the feet are able to move in such a manner as to develop the required
forces and torques to maintain the robots stability. In the human large muscle groups
such as the gastrocnemii (calf muscles - plantar flexors), tibialus anterior, extensis
hallucis longis, flexor digitorum longus and perenius tertius (anterior plantar extensors)
and flexor hallucis longus control the motion of the foot and toes via tendons such as the
tendo Achilles and the tendon of the flexor hallucis longus in the case of the toes. Located
in the lower leg, remote from the foot and toes, these powerful muscles transfer their
force via the tendons which run over grooves in bones. This configuration, common in
animals, allows the joint to be compact and allows for a greater range of movement than
would be possible if it were encumbered by large muscles located at the joint.
As discussed above, significant research is being conducted into the design of
anthropomorphic systems that use artificial muscles and tendons to reduce the size of the
structure required for the control of dextrous effectors. While various types of linkages
were considered for the control of the foot, the precision required to eliminate any slack
in the control of the joint, or stretch in control cables was deemed to be beyond the
capability of the project. Given the forces involved, and the size of control actuators
required, it was decided to control each degree of freedom by direct actuation. The
subsequent challenge in the mechanical design of the robot was to determine the struc3 - 14
tural arrangement for the mounting of actuators. This process was common to all joints.
As the foot/ankle configuration was the most complex, concentrating several degrees of
actuation in a single region, the design issues will be discussed in the following
paragraphs. Other joints found in the robot were designed with similar analysis,
however the design of these joints will not be detailed to the same extent.
Figure 3.7 shows various concepts that were investigated to incorporate the
mechanical linkages required to move the toe and foot and the abduction of the foot. The
design of the foot was conducted as an iterative process with the overriding design
philosophy being to model the human lower limb. Various combinations of actuators and
foot components were analysed to determine their ability to replicate the freedom of
movement of the ankle and foot. One of the challenges presented in the design of the joint
was to incorporate the ability to control the toes independently of the foot. Ideally the toe
angle would remain constant regardless of the angle of the foot. To achieve this, the
control mechanism for the toe would either have to be completely contained within the
foot and toe, or the control software would have to maintain the angle of the toe relative
Figure 3.7 Various concepts for foot design

3 - 15
Figure 3.8 Range of

movement of foot
to that of the foot. Similarly, the control of the foot relative to that of the ankle would
require the foot control to be completely incorporated into the ankle, or for software to
maintain the angle of the foot relative to that of the ankle. Figure 3.8 shows the basic foot
structure and its degrees of freedom. A range of toe configurations were investigated
including three, two and single toed feet. The ability to move toes independently allows
for stabilisation of the foot in the frontal plane. It was decided that this control would be
achieved through foot abduction or roll and that a single toe would be used to stabilise
the foot in the sagittal plane. The analysis showed that it would be extremely difficult to
mount any actuator between two joints on the foot. This led to the decision to incorporate the foot and ankle into a combined structure that attached to the lower leg. The foot
itself was designed as a three component device with a heel, ball and toe. To simplify the
3 - 16
Figure 3.9 (a) Rear view of Roboshifts foot
Figure 3.9 (b) Side view of Roboshifts foot
3 - 17
ankle, the rear of the heel has been radiused to allow for ankle abduction without the
requirement for a separate joint. Photographs of Roboshifts feet can be seen in the photographs of Figures 3.9 (a) and (b).
Having made the decision to actuate the joints directly, the total length and stroke of
the hydraulic cylinders was determined from algebraic analysis. Figure 3.10 shows the
range of movement required by the foot. For small angles about the vertical, it can be
seen that:
X MIN = X L Sin()
(1)
X MAX = X + L Sin( )
(2)
Given the length of a hydraulic cylinder in the closed position is equal to the fixed
length of end components plus the stroke length;
X MIN = L Fixedcomponents + S
The length of the extended cylinder is equal to that of the fixed cylinder plus the stroke
length:
X MAX = X MIN + S
Then:
X MAX X MIN = S = X + L Sin( ) X + L Sin()
(2) - (1)
Stroke = L[Sin( ) + Sin()]
Figure 3.10 Range of movement of foot

3 - 18
Figure 3.11 Initial

design of ankle frame
Having determined the stroke required of the hydraulic cylinders, the length of the
cylinder and the position of the mounting point of the top of the cylinder can be
determined. As the foot cylinders would be counteracting the full ground reactions of the
robot, the mounting structure was required to be extremely strong. However, as the
structure would be attached to the end of the lower leg its mass would have a significant
effect on the mass moment of inertia of the leg. For this reason various configurations of
space frames were considered to minimise the weight. In the frontal plane the feet are
positioned under the body as is the case in the human. However, the thighs of the robot
were to be positioned on either side of the body. Such an arrangement required a
transition of the lower leg towards the centre of the robot. As the entire robots weight
would be taken by the lower leg, a significant moment would be produced due to the
horizontal translation of the vertical load. Again, the space frame configuration lends
itself to the application of these loads. Initial solutions (see Figure 3.11 ) included a
rotation of the ankle in the vertical plane. While not a degree of freedom found in the
human lower limbs, it was thought that this movement would allow more efficient turns.
This initial design was based around a welded box section frame with mounts for
bearings and the hydraulic cylinder clevis pins. As the foot would be capable of both
3 - 19
extension and abduction, the cylinders would rotate in two planes. Universal joints,
incorporating roller bearings, were included in the design of the cylinder attachment
points to allow this motion and to minimise any slack in the joint. Using solid modelling
utilities in AutoCAD, the model was moved to the extremes of motion of the cylinders
to determine if there was any interference in the joints and that the mechanics of the joint
allowed the degree of freedom required of the foot segments. This analysis showed that
the two mounts for the foot rotation and the foot extension cylinders would have to be
split as the cylinders subtended different angles in the sagittal plane when the foot was
both extended and abducted. Additionally, further consideration determined that turning
could be facilitated by hip rotation, eliminating the requirement for foot rotation about a
vertical axis.
A number of other design issues, such as the method of attachment to the lower leg led
to a redesign of the frame. The final configuration of the foot/ankle assembly is found in
Figure 3.12. There is no question that the design could have been further refined. Perhaps
Figure 3.12 Ankle

assembly
3 - 20
Figure 3.13 Ankle/

lower leg swing arm
connection
the major drawback of attempting a large and complex project without a substantial
budget is the limited number of iterations available during the design process. While
technology such as solid modelling is available, it is only through the construction of
prototypes that the design process can be completely optimised
3.3.2 KNEES, LOWER AND UPPER LEGS.
Having completed the design of the ankle, the next section to be created was the lower
leg and transverse attachment to the ankle. Again, various configurations were considered before a parallel swing arm arrangement was selected. This design offered the additional advantage of allowing for accurate indication of the load on the feet by measurement of the bending strain in one of the swing arms. It was anticipated that significant
shock loads could be generated at the ankle structure contacting the ground at heel strike.
To minimise the risk of damage, automotive, pneumatic type shock absorbers were incorporated into the joint with the ability to control spring force via adjustment of air pressure. It was essential that the swing arms would be sufficiently rigid to transfer the
moment of the lower leg in the sagittal plane. Each of the ends of the swing arms were
fitted into substantial channel sections using ball bearings and stainless steel shafts to
ensure that there was no slack in the joint and that the motion would be absolutely parallel. Figure 3.13 shows the parallel swing-arm arrangement which can also be seen in
the photograph of Figure 3.14.
3 - 21
Figure 3.14 Swing arm arrangement
The knee of the robot was designed as a straight hinge joint directly driven by a
hydraulic cylinder. The joint is formed by a hinge pin running through two roller
bearings contained within a housing located at the lower end of the thigh and the top end
of the lower leg. Two grub screws located within the cheeks at the top of the lower leg
hold the hinge pin laterally and also ensure that it rotates with the lower leg. The knee
angle shaft encoder is mounted on the thigh and measures the rotation of the hinge pin.
The hydraulic cylinder driving the knee is located within the thigh which was fabricated
as a box section structure.
The human hip consists of a ball joint where the femur is able to rotate freely in the
socket about three axes, one of which is about the axis of the femur itself. By
translating the point of rotation along the axis of the femur, it was possible to limit the
hip itself to two degrees of freedom. The thigh (Figure 3.15) is connected to the hip by a
40mm stainless steel shaft which runs down into the thigh through two thrust bearings
3 - 22
Figure 3.15 Knee and thigh assembly
which support the weight of the robot. These thrust bearings are mounted in opposite
directions so that the leg is also supported during swing phase. A hydraulic motor is
mounted to the back of the thigh which rotates the thigh around the shaft. This is
achieved via a small sprocket on the motor which drives a larger sprocket on the hip
rotation shaft (Figure 3.16). The shaft itself is fixed into the base of the hip section and
does not rotate. The hydraulic motors are slot mounted allowing for tension adjustment
of the chain. Unlike the hydraulic cylinders used to control other degrees of freedom, the
motor will allow slip with the controlling valve in the closed position.
3.3.3 HIP
Again, a number of configurations were considered for the hip. The final arrangement
is capable of both hip abduction and hip extension with both axes driven by hydraulic
cylinders. The hip extension shaft is located by two Plummer block bearings mounted on
the suspension plate of the robot. A torque arm is integral to one end of the shaft to which
the hip extension actuating cylinder is connected. Extension and contraction of the
3 - 23
Figure 3.16 Hip rotation mechanism
cylinder rotates the shaft. Mounted on the other end of the cylinder is a boss fitted with
a shaft about which the hip is free to abduct, controlled by the hip abduction cylinder
mounted above the hip extension shaft. Figure 3.17 shows the layout of the joint. Two
grub screws are fitted to the boss which hold the shaft stationary while the hip rotates
about it. A shaft encoder mounted on the front cheek plate of the hip measures the
movement of the hip relative to the boss, thus giving the hip abduction angle.
Figure 3.17 Layout of hips

3 - 24
3.3.4 BODY
While General Electrics Hardiman exoskeleton can be considered the first attempt at
a biped materials handling platform, it was not strictly a robot as it relied on the human
operators own control abilities to manoeuvre it. As discussed previously, the device was
controlled by the movement of the pilots limbs directly operating hydraulic valves.
Ultimately, the control of the device proved too difficult to coordinate and the project was
scrapped. Perhaps, if the project were to be recommenced today, it would be possible to
insert a level of digital control between the operator and the hydraulic valves. Effectively
the control would become fly by wire with the operator indicating their intentions to
the control system which would then calculate joint trajectories to achieve the required
motion while maintaining stability. The control system would prioritise the control tasks,
with balance and safety systems having the highest priority, and with the operators
requests being the lowest priority. Such a system would no longer require the operator to
coordinate the movement of the Robots legs, rather the operator would simply indicate
via a joystick that the robot should move in a given direction. Once this level of
automation had been achieved, there would no longer be a requirement for the operator
to be on board the device. Given a communications link with sufficient bandwidth for
stereoscopic vision and control data, it would be possible for the operator to be located
anywhere in the world. Perhaps, ultimately, the device would become completely
autonomous.
Effectively, the ultimate conclusion from such discussions is that
industrial scale exoskeletons would require such a level of control to convert the
operators intentions to movement of the device, that there would be no requirement for
the operator to be on board.
Initial designs of Roboshift displayed accommodation for an operator (see Figure 3.18).
However, after analysing power and control system requirements, it was determined that
there was no longer space available for an on board operator. Of course there may be
some applications where it would be advantageous to have the operator on board,
however there would be no requirement for the device to be built as an exoskeleton. It is
anticipated that once biped materials handling robots become commercially viable,
specialisation of components will significantly reduce the space requirements of the
onboard systems allowing for accommodation of an operator.
3 - 25
Figure 3.18 Initial design of Roboshift showing operator cabin
3.3.5 CONSTRUCTION
As Roboshift is a prototype of an industrial scale device it is to be expected that some
components or systems would require repair or replacement during or after initial trials.
Such failure could arise from an incorrect design decision during the initial fast tracked
design process, or from electrical or mechanical failure. To minimise unavailability, the
primary strategy for the construction of the robot was to ensure that disassembly and the
replacement of parts would be as efficient as possible.
For the major box sections such as lower and upper thighs, hips and ankle space frames
emphasis was placed on dimensional and spatial accuracy. To achieve a high degree of
precision between left and right hand members, drawings were prepared from the solid
models of components used to design the robot. These were then downloaded to a
numerically controlled water cutting machine which then cut the frame panels. The
panels were then fabricated into frames using TIG welding to stitch the panels together. Once the stitched fabrications had been checked for accuracy, they were them MIG
3 - 26
welded after being heated to ensure penetration. Where additional strength was required
around bearing mounts or where hinge pins were located, machined components were
welded to the frames. Power and control system hardware making up the body of the
robot have been arranged in modular form under the hips in such a manner as to
maintain the centre of gravity of the body under the hips. These modules are discussed
in the following chapter.
Once all of the aluminium body parts had been fabricated, they were assembled
together with the hydraulic actuators to form the basic structure shown in Figure 3.19.
The photographs clearly show the hydraulic actuators and the bearings that coincide with
the degrees of freedom of the joints. The instrumentation console can be seen hanging
below the hips in the initial configuration where hydraulic power and main control
processing were externally located. Once initial testing was completed, the hydraulic
power pack was fitted under the hips with the instrumentation console mounted above
and aft of it.
Figure 3.19 Mechanical structure of Roboshift

3 - 27
POWER AND ELECTRICAL DESIGN
I am an old man now, and when I die and go to Heaven there are
two matters on which I hope for enlightenment. One is quantum
electrodynamics and the other is the turbulent motion of fluids.
And about the former I am rather more optimistic.
- Sir Horace Lamb
The world is full of mobile systems relying on self contained power units to provide
motive force and electrical power. The car, for example contains an internal combustion
engine which provides drive and also powers the alternator to provide electrical power
and a hydraulic pump to provide hydraulic flow to drive the power steering. Cranes,
garbage trucks, excavators and forklifts all use internal combustion engines to provide
electrical and hydraulic power. In very few cases do any of these systems rely on
real-time control for the actuation of their systems. In the case of a forklift truck, the
sizing of the hydraulic pump is dependent on the weight to be lifted and the speed at
which it is to be raised. The desired speed is a function of the required productivity of the
vehicle, the size of the engine and the viable cost of materials of the forklift. Whether the
pallet takes four seconds to reach the desired height or four and a half seconds to reach
the desired height is not critical to the operation of the device. The manufacturer makes
a financial decision based on market research to determine the acceptable speed of lift in
order to minimise the cost of the hydraulic pump and engine of the device.
In the case of a biped robot, it is critical that the motion of the joints is able to be
controlled in real-time if the robot is to actively balance. Achievement of this constraint
is dependent on a control system capable of processing the required information in the
required time, and an actuation system capable of moving the joints with the required
velocity. As discussed in the previous chapter, a hydraulic system was chosen as the
motive force in this project for the following reasons:
The skill set of those involved in the project
The capability to deliver required force
The available resources
The fact that the Wabot (Kato, 1987)) was hydraulically driven and was able to
walk with a quasi static gait
4-1
Chapter 4 - Power and Electrical Design
This chapter details the design of the hydraulic and electrical power systems to achieve
the motion required for locomotion. In section 4.1.1, the required hydraulic flows are
calculated based on the expected motion of the robot and the size of actuators. Pump
configuration is then discussed in section 4.1.2 as is the problem of hydraulic crossflow.
A schematic of the hydraulic system is provided showing the three separate pressure
systems used to drive each leg and the hips. The section concludes with a photograph of
the power pack as built. Electrical systems are described in section 4.2 culminating in a
schematic of the system and the sizing of the alternator. This information then allows the
internal combustion engine to be sized and the final configuration of the power pack is
realised.
4.1 HYDRAULIC DESIGN

To determine the required capacity of the hydraulic power pack it was necessary to
establish the peak flows required by the hydraulic components during the gait cycle. As
previously discussed, kinematic models were used to determine the range of movement
required by the joints. These models were then used to calculate the peak joint velocities
and the corresponding hydraulic component flows.
4.1.1 FLOW CALCULATION
The chart shown in Figure 4.1 displays the joint trajectory angles for a single human gait
cycle. Once again it was assumed, during the design phase of the project, that if the
geometry of the robot was similar to that of the human lower limb system, and could be
controlled, the robot would be capable of biped locomotion. It was therefore assumed that
the gait cycle would be somewhat similar to that of the human. For this reason the human
Joint Angles
3.5
Angle (Radians)
3
2.5
2
1.5
1
0.5
0
0
0.2
0.4
0.6
0.8
1.2
1.4
1.6
1.8
2.2
Time (Sec)
Left Hip Ex
Right Hip Ex
Left Knee Ex
Right Knee Ex
Left Foot Ex
Right Foot Ex
Left Toe Ex
Right Toe Ex
Figure 4.1 Joint angles over single gait cycle

4-2
gait cycle was used as the basis for the calculation of hydraulic flow required for
locomotion. A target velocity of 1.4 metres per second or 5km/h was used to calculate
hydraulic flow. At this velocity the gait cycle would be completed in approximately 2.5
seconds.
The following analysis was conducted to determine the hydraulic flow required for
each joint:
The cylinder length was calculated as a function of joint angle
The cylinder length was numerically differentiated from one iteration to the next
to determine the cylinder velocity
If the velocity was positive, it was multiplied by the cylinder end area to
determine the flow. If the velocity was negative, it was multiplied by the rod
end area (the cylinder area less the area of the rod) to determine the flow
The results of the analysis shown in Figure 4.2 indicate that the maximum flow occurs
in the toe cylinders. On first inspection it may appear unexpected that movement of the
toe would require a high capacity of oil. However, with no toe movement relative to the
foot, the toe cylinder must move proportionally with the foot to maintain the toe angle.
Given that the toe is located further from the ankle than the foot, the velocity required to
maintain the toe angle is higher than that required to move the foot itself. When this
demand is combined with the demand required for actual toe movement relative to the
foot, the flow spike can be seen as shown in Figure 4.2 after 1.2 seconds.
Hydraulic Oil Flow
2.5
Litres/Second
1.5
0.5
0
0
0.2
0.4
0.6
0.8
1.2
1.4
1.6
1.8
2.2
2.4
Time (Sec)
Left Hip Ex
Right Hip Ex
Left Knee Ex
Right Knee Ex
Left Foot Ex
Right Foot Ex
Left Toe Ex
Right Toe Ex
Figure 4.2 Total flow requirements for single gait cycle

4-3
Accumulators are used in hydraulic circuits to temporarily store hydraulic fluid under
pressure so that it can be released during periods of high demand. By acting much as
capacitors in electrical circuits, the accumulators reduce the maximum flow requirements
of the hydraulic pump by storing oil during periods of lower demand. To determine the
appropriate size of the accumulator, the individual flow requirements were summed to
produce the total flow requirement seen in Figure 4.3. The spike can be seen again in
Figure 4.3 after 1.2 seconds, however it has been amplified as the foot cylinder flow has
now been added to that of the toe. This spike occurs immediately after heel strike as the
foot and toe quickly extend to the ground. The flow analysis was achieved by
manipulating the flow and accumulator variables on a spreadsheet. Values for pump flow
rate and the size of the accumulator were determined. The blue line in Figure 4.3 shows
the oil stored in the accumulator during the gait cycle given a pump capacity of 2.6
litres/second and an accumulator capacity of 1 litre. As discussed, this analysis was based
on the motion of the human lower limbs during normal gait. By varying the gait, it may
have been possible to reduce the spikes seen in the foot and toe cylinders. However,
without substantial analysis such a deviation from the strategy to maintain human like
characteristics was not considered. Having established the flow required, the method of
distribution of oil was then considered.
4.1.2 HYDRAULIC CIRCUIT DESIGN
In typical industrial hydraulic applications the motion of actuators is well defined.
Systems are designed so that ample flow is available for each axis of motion in order that
the operation of one actuator does not affect the supply of oil to the other. This effect,
known as crosstalk, can make the control of hydraulic actuators difficult and
Total Hydraulic Oil Flow
Flow (Litres/Second)
6
5
4
3
2
1
0
0
0.2
0.4
0.6
0.8
1.2
1.4
1.6
1.8
2.2
2.4
Time (Sec)
Total Hydraulic Oil Flow
Total Flow with Accumulators
Figure 4.3 Total hydraulic flow

4-4
unpredictable. As an example of this in robot design, unqualified accounts such as

reported by Wiess (2001) would indicate that General Electrics Hardiman suffered from
severe hydraulic crosstalk.
The robot, as heavy as a car, would have enabled a person to lift a refrigerator as
though it were a bag of potatoes. However, the machines inventors could only get one
arm of the device to work. And attempts to operate both legs at once would lead to
violent and uncontrollable motion, according to an old report on the project.
Hydraulic crosstalk can be explained in terms of domestic water pressure. It is not
uncommon for the water pressure in the shower of a house to be affected by a washing
machine or someone using the kitchen sink. However, as the street supply is at a higher
pressure and higher flow, turning a shower on in one house does not affect the shower
pressure in the house next door. Similarly, hydraulic cross flow can be dealt with by
ensuring that the pressure on the supply side of the controlling valves is always
substantially greater than that required by the actuators. A second method to deal with
cross flow is to provide independent supplies for actuators. In the extreme, this method
would involve a separate hydraulic pump for each actuator. The drawback to this
solution is the complexity of the pump arrangement and the amount of space required for
Figure 4.4 hydraulic valve manifolds

4-5
the pump units. In the case of Roboshift, the decision was made to use three separate
hydraulic pumps. One pump is used to drive the actuators on each of the lower legs with
the third pump used to drive the hip actuators. In this way, the large flow requirements
of the feet and toe cylinders on one leg do not affect the flow or pressure on the
cylinders of the other leg. As seen in Figure 4.2, the flows required by the hip cylinders
are not large, therefore given enough pressure and flow, the operation of the hip
actuators on one side of the robot should not affect those on the other side. To facilitate
supplying the appropriate groups of actuators, the hydraulic valves were mounted on a
number of manifolds as shown in Figure 4.4.
The analysis used to determine total flow was used once more to determine the size of
pump and accumulator required for each leg and for the hips. Pumps used for the legs
were 50 litre/minute and the pump for the hips was sized at 30litres/minute. All of the
pumps were selected as gear pumps which required the use of a pressure relief valve to
maintain the systems pressure. An advantage of this configuration is that the system
pressure can be changed easily by adjustment of the relief setting. The circuit design
included an accumulator in each pressure branch as well as an inline filter to protect the
valves. Valves chosen were Rexroth WRE10-6 proportional valves. A common return
was used for the three branches of the hydraulic system. A schematic of the hydraulic
circuit can be seen in Figure 4.5 on the following page. The flow of hydraulic oil through
the system and particularly the flow of oil over pressure relief valves leads to a build up
of heat in the oil due to friction losses. In the majority of hydraulic applications, the oil
reservoir is sized so that heat is dissipated via natural radiation through the tank walls and
via convection from the tank walls. As well, the majority of hydraulic systems are
designed to be run on a non continuous basis. In the case of this robot, not only must the
system run continuously, but space and weight constraints reduce the size of hydraulic
oil reservoir able to be carried on board. To address the build up of heat in the hydraulic
oil, a fan forced radiator was placed in a branch of the common return line. By the
setting of a needle valve, the proportion of oil returning to the tank via the cooler can be
adjusted.
Once the decision was made to dissipate heat via a forced fan radiator, the main design
criteria for the hydraulic tank became:
Size to be able to fit between the robots legs
4-6
Figure 4.6 Hydraulic power pack

4-8
Sufficient length of flow and baffling to ensure separation of entrained air

Sufficient volume to provide positive pressure in the suction area
Positioned so that suction pipe length was kept to a minimum
The installed 35 litre stainless steel tank can be seen in Figure 4.6.
Proportional directional hydraulic control valves require amplifiers to convert an
analogue control signal to a coil control current. Rexroth/Bosch VT1001 valve amplifiers
were used to control the 4WRE6 valves installed on the robot. As the current is controlled
by pulse width modulation, the amplifier wires and valve coils can generate significant
electro magnetic fields. To reduce any interference effects, the amplifiers are mounted in
an enclosure that is placed away from transducers and other electronic equipment that may
be sensitive to electronic noise. As well, the valve banks have been mounted as far as possible from the body of the robot, under the amplifier box so that the lengths of valve control wires could be kept to a minimum. Figure 4.7 shows the control valve enclosure.
The hydraulic hosing of the robot was completed in situ as it would be exceedingly
difficult to model the hose work in three dimensions. Given the complexity of the circuit,
Figure 4.7 VT1001 hydraulic valve control amplifier enclosure

4-9
the hoses are tightly packed and proved extremely difficult to connect and tighten.
4.1.3 CONCLUSIONS ON HYDRAULIC DESIGN
The research conducted during the design and construction of the robot has led to the
identification of a number of design challenges that will be in common with any other
industrial biped robot that will be built in the future. Certainly, any large organisation
equipped with sufficient resources will be able to design and produce custom hardware
to solve a number of design issues such as hosing. However, flow requirements,
dissipation of heat, hydraulic crosstalk, location and sizing of reservoirs and the
production of power to drive the hydraulic system represent design issues for an
industrial scale biped which have been addressed for the first time by this project.
4.2 ELECTRICAL DESIGN

Design criteria four and five required that the robot be self contained and capable of
continuous operation. As the project is based on an industrial scale biped, it should be
expected that the robot would be able to work continuously for a four hour period. Brief
analysis suggested that a load of twenty to thirty Amps of 12V and 24V power would be
required to maintain on board systems. Given that an internal combustion engine was
available, the use of an alternator was deemed to be more efficient than batteries to
supply power. This section details the various on board systems requiring electrical
power and outlines the circuitry that provides it.
4.2.1 ELECTRICAL POWER REQUIREMENTS
To determine the amount of on board power required, an inventory of electrical
equipment was produced as found in Table 4.1.
In the case of the hydraulic valve amplifiers the average flow of all valves over a gait
System
Power
Valve amplifiers
3A @ 24VDC
Oil cooling fan
14A @ 12VDC
Artificial horizon compressor
8A @ 12VDC
Inverter 1
12A @ 12VDC
Inverter 2
12A @ 12VDC
Microprocessor enclosure
4A @ 12VDC, 1A @ 24VDC, 1A @ -24VDC
Audible alarm system
5A @ 12VDC
Table 4.1 Inventory of on board power requirements

4 - 10
cycle was determined to be 2.6 litres/minute. This equates to an average flow of 0.19
litres/valve/second. Based on the data sheet for the 4WRE6 valves, this flow rate equates
to a coil current of 0.15 Amps. Conservatively, the hydraulic amplifier demand was
deemed to be of the order of 3 amps at 24 Volts DC. Ideally, a separate alternator would
be provided for both 12V DC and 24V DC power. However, due to space constraints, only
a 12V alternator was fitted to the robot. For this reason, the alternator provides 3 Amps @
12VDC to the amplifiers with a battery also supplying 3 Amps @12VDC in series.
Table 4.1 details several electrical units that require -24V DC power. In general this
voltage is required for the instrumentation amplifiers found in the strain gauge amplifier
enclosures and for the analogue output generated by the I/O boards in the microprocessor
enclosure. Two 80 Amp hour batteries are used in series to provide this power.
In addition the starter motor of the engine would require about 35 Amps at 12VDC
which is another major current draw. However, the demand is brief and would not
contribute to the overall power requirements during normal operation. As the starter
motor will create a significant voltage drop during starting, the electrical circuit has been
designed so that all onboard systems can be supplied by the battery charger during engine
starting. The switch panel is designed so that the alternator circuit can be connected prior
to the battery charger being disconnected.
4.2.2 CIRCUIT PROTECTION
One of the interesting questions facing the designer of mission critical electronics is
that of circuit protection. This is particularly the case in the aviation industry where a loss
of power to flight critical components will cause a total loss of control. In these
applications the use of circuit breakers is augmented by redundant systems, so that if one
circuit blows, the alternate system provides the functionality to keep the plane flying.
Like a modern fighter, a biped robot relies on automatic control systems to stay up right.
The potential for a failure to cause catastrophic damage to the vehicle, to property or to
the pilot or other persons is extremely high. In the case of this project all electrical and
electronic systems are protected by fuses. At the time of writing, all testing of the
vehicle has been conducted while the robot has been loosely tethered from an overhead
suspension point. During testing the safety slings are lowered to provide the robot
unrestrained movement. However, in the event of a failure, the device can only lean to a
limited extend before the tension in the slings prevents further falling. In the critical
4 - 11
Figure 4.8 Main distribution enclosure
event of a circuit fuse blowing, the robot will lose control and become unstable.
As can be seen in the robots electrical circuit found in Figure 4.7, all of the 12V DC
batteries are protected by 50 Amp fuses. These batteries are Sonnersheim gel cell type
batteries capable of providing up to 270 amps in a short circuit situation. The fuses are
located immediately at the positive terminal of the batteries to minimise the possibility
of fire or explosion in the event of a battery circuit shorting. The main distribution box
also includes fuses on the incoming -24V, 0V, 12V and 24V DC circuits as well as 10
Amp fuses on the supply to heavy current devices. Finally, all of the major electrical
components and enclosures include fuses in their incoming circuits. The primary purpose
of the fuses is to protect the electronics on the robot which have consumed considerable
resources during their manufacture and installation. As the device and the majority of the
electrical componentry are of a prototype nature, the probability of circuit failure is
significantly higher than that which could be expected of mass produced devices and
components certified by quality assurance procedures. Any commercial device would
require the use of redundant components and circuitry to ensure that if one fuse were to
blow, the continuity of mission critical functionality would be assured.
4.2.3 POWER DISTRIBUTION
As previously discussed, power from the batteries is fed to the main distribution
4 - 12
enclosure (see Figure 4.7). As shown in the

circuit diagram of Figure 4.8, the four main
switches of the enclosure allow the
batteries to be disconnected, switched to
the battery charger or switched to the main
circuit to provide power for the robot. The
battery charger connection can also be
switched from charging mode to a boost
mode for the primary 12V battery. In the
boost configuration, the battery charger is
Figure 4.10 main distribution panel
connected in parallel with the primary
battery and is used when testing the robot for long periods when the alternator is not providing power. An additional four switches provide power to the two inverters, the air
pump and the audible alarm system. Finally, power to the three hydraulic dump valves is
provided by the operation of three switches. When energised the dump valves close,
forcing the hydraulic fluid past the three relief valves sustaining system pressure. The
purpose of the dump valves is to decrease the starting torque of the engine.
Power to the rest of the system is controlled by the remote pendant (see Figure 4.9).
This enclosure houses a number of switches which activate relays in the main
distribution box. Primarily the pendant is a safety device that allows all power to be cut
in the event of malfunction. Particularly valuable during initial testing, the box allowed
power to be cut to the hydraulic valve amplifiers when potentially damaging motion was
observed. Cutting power to the amplifiers immediately centred the valve spools, cutting
all flow to the hydraulic actuators. The momentary switch on the pendant allowed the
operator to energise the hydraulics for brief periods so that direction and velocity of
actuation could be confirmed.
4.2.4 WIRING
All power wiring has been completed in silicon rubber high temperature cabling. This
material was used as it presents a high resistance to hydraulic oil as well as resistance to
heat generated by the engine exhaust components and hydraulic lines. Neutrix Type and
Contact Type connectors have been used for cable terminations as they provide positive
locking to prevent intermittent connections caused by vibration from the LPG engine. All
4 - 14
battery cables consist of 18mm2 cable, however the immediate connection to the battery
is of 6mm2 cable, so that in the event of a complete short circuit the smaller cable
becomes a fusible link.
4.2.5 CONCLUSIONS ON ELECTRICAL POWER
From Table 4.1, it can be seen that, even though the robot is hydraulically actuated, up
to 60 Amps of 12VDC power is required to maintain auxiliary and control systems. This
represents up to 0.75 kW of engine output. A forklift truck may require three or four
Amps of 12VDC power during normal operation. At such time as a large manufacture
commences production of biped materials handling platforms, economies of scale may
result in substantial rationalisation of electrical power requirements. However, the work
conducted in this project has identified the efficient use of electric power as another
challenge associated with the development of industrial scale biped robots. Further, the
large disparity in the electrical power requirements of a biped robot compared to that of
a wheeled vehicle demonstrates the increase in the level of complexity between wheeled
and legged vehicles so that the supply to the microprocessors will not be interrupted.
4.3 THE ENGINE

Traditional materials handling equipment relies on low speed, reliable, heavy engines
as power sources. As the vehicles are wheeled and are designed to be heavy in order to
counterbalance the load, there is no requirement to lighten the engine or to increase its
power to weight ratio. A forklift with a similar capacity and size to that of the industrial
biped proposed in this project, the Toyota 40-3FG7, is powered by a 4 cylinder gasoline
engine with a displacement of 1486cc delivering 31Hp @ 2400rpm and 98Nm @
1800rpm1. Clearly, the motor is designed to produce high torque at low motor speed.
Just as the first powered flight only became possible after the advent of higher speed
lighter engines, industrial bipeds will also rely on lighter, more powerful engines than
those found in traditional materials handling equipment. The selection of engine for the
robot was based on the following criteria:
Readily available with a ready supply of parts
Proven industrially robust
Extensive knowledge base of performance characteristics
Lightweight
1 www.hino.com.tw/toyota/B2_3_1a_2.asp
4 - 15
Able to provide the required power and torque

An extensive knowledge-base of the performance characteristics was required as the
engine aboard an industrial biped would be subjected to a range of conditions that may
be beyond the design envelope of some engines. As the space requirements of the biped
are at a minimum, the engine would be difficult to access and overhaul. For this reason,
the engine would need to be robust and relatively maintenance free. The engine would
be subject to forces and accelerations that no other industrial engine has ever seen before;
that of biped locomotion. While not considered excessive, the motion may interfere with
such processes as carburation and fuel flow. It would be expected that an engine used in
a wide range of machinery would stand the highest probability of withstanding such
forces with no degradation in performance. The first stage of engine selection was to
choose engines that provided the required torque and power to drive the hydraulic and
electrical systems. The second was to consider these environmental effects.
4.3.1 ENGINE POWER
To determine the required power to drive the hydraulic system, the system operational
pressure and the system flow rate are required. The system operational pressure for each
pump is calculated from the maximum pressure required by any one cylinder supplied
with oil from that pump. To determine which actuator in each pump system required the
maximum pressure, forces and pressures within all hydraulic actuators must be
calculated. As the exact gait of the robot had not been determined at this stage of the
design, such an analysis would be based on a level of estimation that would make
detailed analysis superfluous. Alternatively, as dynamic calculations had been used to
verify the hip slung body configuration of the robot, it was decided to analyse the
kinematic model used for the mechanical design of the robot to determine the stages of
the gait where maximum acceleration occurred. Investigation of the kinematic model
showed that toward the end of swing phase, the foot and toe cylinders of one leg
accelerate the entire robot in the vertical direction. At this point in the gate cycle, the
entire weight of the robot is taken by the two foot cylinders and toe cylinder of one leg.
The triangle of contact of these cylinders fits inside a 100mm circle drawn on the ground.
It was unrealistic to attempt to determine the weight distribution within this triangle
without knowing the exact gait of the robot and within the limits of error introduced by
compliance, expansion of hydraulic hoses and other unknowns. To continue the analysis,
it was assumed that the load would be shared by the two foot cylinders combining to take
half the load and the toe cylinder taking the other half of the load. This assumption leads
4 - 16
to the toe cylinder supporting half of the total load of the robot and accelerating that load
600
FCyl =
(1 + 0.15) g = 3.4kN
2
by 0.15g. The cylinder force would then be, with an estimated total weight of 600kg:
FCyl
PCyl =
A
Cyl
3400
=
= 3Mpa = 30bar 500 psi

1134

Given an internal cylinder diameter of 38mm, the pressure was calculated as:
Essentially the system pressure would be in the order of 3Mpa. However, this figure
does not take into account pressure drop and power loss through friction in the hydraulic
system. It is at this point that the black art of hydraulic system design comes into play2.
Using an efficiency of 0.6, and a duty cycle of 0.7 (assuming that the swing leg of the
robot would not require the full system pressure), the power requirement for the system
Q Average p System C Duty
PRe quired =
System

was calculated as:

= 9.1kW = 13Hp.
The electrical load of 0.75kW, calculated in the previous section is added at this stage
of the analysis. Under most safety legislation, there is a recommendation3 that internal
combustion engines are fuelled by liquid petroleum gas (LPG) when working in confined
industrial environments. A power loss of 15% to 20% can be expected from the use of
LPG which would increase the required standard engine power to approximately 12kW
or 15Hp. To determine the required torque of the engine, the pump displacement per
revolution is required. Given that gear pumps will be used with an optimal
2 Given sufficient resources, time and expertise, it may be possible that the exact dynamics of the robot and the of
the hydraulic system could be determined. However, given the time available to complete this project, such an analysis was not possible, nor would such an analysis be completely accurate. Having access to a number of highly respected hydraulic experts (acknowledged at the front of this document), after inordinate arguments over countless cups of
coffee, it was decided that a factor of 0.6 be used to take into account pump efficiency, loss and friction in cylinders.
While some may argue that there is no place in a scientific document, such as a PhD, for such engineering, the system to be modelled was small compared to other systems that have been modelled using proprietary hydraulic software from companies such as Rexroth. Copies of the software were made available to the writer, however the results
were inconclusive as often the input was interpreted from tables that did not provide data for the flow levels and pressures that were expected to be used in the robot. Again, given access to respected experts in their field, the writer argues
that where definitive technical analysis is not available, the combined experience of those available represented valid
data from which to make sound and reasonable judgement..
3 www.ohsrep.org.au/hazards/confinedspaces.html
4 - 17
Q
2.6
=
= 0.0624l
3000

Displ . (p out p in ) C Duty

=
60

hydromechanical efficiency of 0.85 at 3000rpm, this gives a displacement of:
Displ . =
This equates to a torque of:

= 0.0624 3000 0.7 /
= 41Nm
Effectively the engine specifications are 12kW and 41Nm @ 3000rpm.
4.3.2 ENGINE SELECTION
Fortunately, these specifications lead to the upper end of the industrial range of air
cooled stationary engines from a number of manufacturers. Table 4.2 shows the three
main contenders from Kawasaki, Honda and Briggs & Stratton. Comparison suggests
that this size of engine is widely used for industrial applications as the specifications are
almost identical.
These engines are used in equipment as varied as pumps, generators, concrete cutters,
ride-on mowers etc. Such wide usage suggests that the forces generated from biped
locomotion would not interfere with the operation of the engines. The flange mount
dimensions on these three engines are identical. The base mount dimensions on the
Briggs and Stratton and the Honda are identical. Essentially these engines are designed
to be interchangeable in most applications. On face value, it can be seen that the Honda
possesses slightly more torque due to the increased stroke, but loses power and torque at
higher RPM. However, the Briggs and Stratton motor maintains power at higher rpm
with a torque curve that displays a steeper positive slope at the operating point. As the
engine speed drops, torque would increase under load.
Due to the allowances made during the analysis, all of these engines were expected to
be oversized for the task. However, the engine is a major piece of hardware; difficult to
replace, even more difficult to replace with a larger one. The Briggs and Stratton motor
is an extremely reliable engine that has been available for approximately 10 years in the
4 - 18
Manuf. Briggs & Stratton
Honda
Kawasaki
Model
VT-20HP
GX-620
FD-611V
Cyl.
90 Degree V-Twin
90 Degree V-Twin
90 Degree V-Twin
Valve
OHV
OHV
OHV
Displ.
570cc
614cc
585cc
Bore
72mm
77mm
74mm
Stroke
70mm
66mm
68mm
Weight
42kg
42kg
44.7kg
16.0
16.1
15.5
41
42
41
Curves
KW
@3000
Torque
@3000
Table 4.2 Engine comparison.
same basic configuration.

An extensive knowledge base exists on the performance, durability and strength of this
engine as it is used for kart and small car racing4. If required, parts, instructions and fuels
4 www.briggsracing.com/racing_engines/vanguard.html
4 - 19
Figure 4.10 Briggs & Stratton 20Hp Vanguard
are readily available to increase the power of the engine from 20 to 60 horsepower.
Should the standard engine prove to be undersized, the ability to increase its size without changing the actual engine is available. With all other parameters essentially similar,
this factor lends the engine (Figure 4.10) to the solution of the power requirement for the
project.
It is interesting to note that the engine selected is in the same order of power as that for
a similar sized forklift. However, it is lighter, has two less cylinders and will run at almost
twice the speed. This work has established that biped materials handling platforms will
sound far more like motorbikes or aeroplanes than the slow thud of existing forklifts.
More importantly, they will require a much higher level of maintenance.
4 - 20
CONTROL SYSTEM PHILOSOPHY
Any sufficiently advanced technology is indistinguishable

from magic.
Arthur C. Clarke (1917 - ) Technology and the Future
5.1 ROBOT VERSUS CYBERMACHINE

The control routines of an industrial robot are well defined, as are its environment, the
ranges of its input variables and expected reactions to them. Essentially, an industrial
robots operation can be thoroughly mathematically modelled.
The science fiction writers of the 1950s and 1960s have portrayed robots as highly
intelligent machines possessing human-like reasoning and behaviour. Isaac Asimov is
acknowledged as the founder of the science fiction robot. While every existing robot of
his time was bolted to a factory floor, he portrayed robots as intelligent, mobile devices,
capable of rational thought and decision-making. His vision bore no relevance to
industrial robots as they toiled, exhibiting anything but human-like qualities. However,
he understood that a machine could be programmed to achieve tasks that, though menial,
had previously required human perception and coordination. Projecting these abilities to
the extreme, Asimov anticipated a future where humanoid robots served mankind as
intelligent devices that were seen as fellow beings.
The contrast between a robot and an automatically-controlled machine lies in the
ability of a robot to cope with and react to the occurrence of situations which have been
recognised as likely, but which are not specifically anticipated at any particular moment
of operation. On the other hand a biped robot is extremely difficult to accurately model
mathematically. As well, the degree of non-linearity in the dynamics and inverse
kinematics, combined with non-linear response from actuators, make real-time solution
of the control equations prohibitive in terms of programming and computation time. As
can be seen from the results of the literature search, the more complex the device, in
terms of degrees of freedom, the more reactive and hierarchical the control structure has
tended to be, aiming to react to groups of inputs rather than to solve the entire dynamics
5-1
Chapter 5 - Control System Philosophy
of the device. Effectively, the programmer imbues the device with a behavioural
characteristic rather than a precise routine to drive its operation. While ultimately
nothing more than cleverly nested sub-routines, this type of control system is described
as artificial intelligence and gives the device a human characteristic.
The term cybernetics refers to the study of those methods of communication and
control that are common to living organisms and machines. This term more closely
describes the area of systems research that attempts to replicate human thought, senses
and form. The term machine can be used to describe complex arrangements of parts be
they mechanical or biological. The term is often interchangeable with the term
organism when used to describe a complex operating system. ie, the machine of
government. Again, this term more accurately reflects the devices found in the field of
research robotics. If the terms cybernetics and machine are combined the word
cybermachine is formed. This new term describes accurately any complex device fitted
with an automatic control system designed to take advantage of or to simulate
human-like behaviours. Behaviours that simulate intelligence in order to automatically
control a machine that can react to a range of expected or unexpected inputs.
In the case of legged walking devices, an automatically controlled machine could be
programmed to walk on different terrains and sense that the terrain is different, but it
would have difficulty sensing and reacting to an un-anticipated terrain. A cybermachine,
however, will use the same set of routines to traverse any terrain. It will continually
analyse its environment to produce a rational, computed real-time response. The
developing expertise in knowledge based, fuzzy and expert systems has enabled the
automatic control of processes that were previously manageable only by the expertise of
a human operator (Zadeh, 1998). The major successful applications of this control
theory are those where classical models have failed due to the highly non-linear nature
of the processes. Accordingly, the expert systems may contain little or no dynamic or
classic model of the system being controlled.
Given the complex nature of the dynamics and non-linear characteristics of an
industrial size biped, it is unlikely that classical control theory could be used
successfully to automate the device. The mechanical, electrical and electronic systems
described in the previous chapters define a complex machine. For the machine to
successfully perform stable bipedal locomotion, it will require high and low level
5-2
behaviours that replicate those of the human. This combination of anthropomorphic

structure and control system is classed as a cybermachine.
5.2 CONTROL SYSTEM OVERVIEW

As identified by Gurfinkel (1981);
The problem of control seems to be the main problem of the walking robot...
Shimoyama et al (1985) demonstrated an actively balancing biped in 1980. They
proposed that if one is building a biped;
The mechanism should be as simple as possible. Sophisticated control algorithms
should be applied, if necessary
Further, they suggest that a dynamic walk should be realised and that the latest
technology should be used in the control system. Again, the foremost challenge of the
realisation of a walking robot is the design and implementation of the control system.
As recognised by Hemami (1977), the biped gait is notionally periodic and
symmetrical, but unstable. The gait consists of falling from one leg (accelerating in
forward direction while decreasing potential energy) onto the other leg (decelerating in
the forward direction while increasing potential energy). If disturbed from either
trajectory, the system requires a force input to stabilise. Argument exists to suggest that
biped walking is an instinctive trait pre-programmed or burnt into the human midbrain.
The argument suggests that learning to walk is the process of refining the constants of
the control algorithms governing the motion. Strong evidence to support such a theory is
the fact that humans are born with a lower limb system, the geometry of which is finely
tuned to upright bipedal locomotion.
The argument further suggests that the
evolutionary advantage of bipedal geometry is only available if the software is there to

drive it. It is certainly beyond the scope of this project to conduct an indepth analysis of
this argument or alternative theories. However, the open loop with closed loop correction
model is certainly supported by such a theory.
Force inputs to stabilise motion are dealt with by the human on two levels. First, as
highlighted by Sias (1987), the nervous centres of the midbrain develop the feed forward
data for gait motion. Sensory organs such as the vestibular system provide feedback error
data for the midbrain to initiate coarse posture corrections. Second, at a lower level,
fusimotor and Golgi tendons provide propreoception and fine motor controls for precise
5-3
positional control of individual joints. Functions such as walking are initiated by the
midbrain, but fine movements, such as rotations of the foot to equalise reaction force
across it, are controlled locally by twitch muscles controlling the joint. Propreoception
acts as a local control system, superimposing joint movements required for local joint
stabilisation with global movement commands from the midbrain. In the case of
dynamic or ballistic bipedal walking, twitch muscles predominantly control the ankle of
the stance leg (in contact with the ground).
These features of the human control system have been recognised by designers of biped
robots. Furusho (1986), Shih (1992) and Gurfinkel (1981) have all used gaits which react
to disturbances or feedback errors generated when actual trajectories fail to match
computed ones. Also recognised by biped robot researchers is the complexity of
equations governing biped motion. Furusho (1986) found that his 5 link biped model
resulted in non-linear differential equations of order 10.
Provided with sufficient processing power, ultra-reliable sensors and extremely fast
actuation, it may be possible to use classical control methods to coordinate a biped robot
in dynamic gait. Most researchers, however, have attempted to reduce the order of the
equations by a number of methods. Vukobratovic (1970) suggests that designing the
body so that limbs can be described as point masses eliminates or minimises secondary
inertia terms. Using feed-forward control with local, distributed negative feedback
stabilisation has also proved popular and effective [(Furusho, 1986), (Gurfinkel, 1981),
(Shih, 1992)]. The configuration of the control system employed by the biped robot in
this project addresses the complexity of the control problem by separating the control
tasks and distributing the processing of tasks.
Research into the mechanics of gait conducted by McMahon (1984), using force plates
to measure ground reaction forces, suggests that bipedal locomotion in the sagittal plane
consists primarily of a three link (fixed length, rigid leg and pelvis) compass gait. The
addition of knee flexion and ankle plantar flexion simply smooth the transition between
the arcs of the compass gait. Ankle and hip abduction provide fine control over the
ballistic path of the centre of gravity in the frontal plane. Modelling the swing leg as a
jointed pendulum hanging from a fixed point predicted a longer period than was observed
experimentally. McMahon suggests that while little or no muscular activity has been
observed in the swing leg, there must be close coupling between the stance leg and the
5-4
swing leg. Using McMahons model, one strategy to control bipedal walking is to use
global open loop, predetermined trajectory control of hip abduction and hip extension,
while using local closed loop control of knee extension, ankle plantar extension and
abduction to minimise error between the predicted and measured position of the centre
of gravity of the robot.
5.3 CONTROL STRATEGY
To effect walking control of Roboshift, a similar global open loop, local closed loop
control strategy has been developed. At the highest layer, open loop, feed-forward,
periodic gait trajectories are computed by a main control computer (MCC), which also
monitors the error of measured global parameters from predicted values. At the lowest
layer, local joint controllers (LJC) use closed loop control to position joints.
The control system has been designed to minimise demand on the control computer by
distributing joint position control and handling communications separately. During
normal walk the control computer generates joint trajectories from coarse kinematic and
dynamic calculations. These equate to the high level, midbrain generated locomotion
patterns in humans, governing the movement of functionally related leg muscles (hips,
legs, and ankles).
Incremental demand positions are transmitted to the joint F1 micro-controllers every
100ms. The controllers then drive the hydraulic valves to achieve demand positions in
the following 100ms. Given that control trajectories are calculated using reduced-order
dynamic equations, errors between actual motion and ideal motion are to be expected.
Provided errors do not cause incursion into areas of un-recoverable instability (Figure
5.1), it is entirely possible that controlled joint positions may never equate to command
positions. The control computer also acts on data from the artificial horizon, maintaining
gross balance in the frontal and sagittal planes by adjustment to hip and knee joint
trajectories. Like the joint controllers, the main control software is reactive to errors in
measured and expected data. The software is based on a hierarchical structure where the
higher level software functions generate gait patterns and monitor the resulting motion.
At a lower level, continuous monitoring of hormone variables (pitch, roll, force
distribution at feet) result in reflex behaviour when boundary values are approached.
Nashner (1980) found that muscular reactions in humans who were perturbed while
walking were rapid, large in magnitude and movement specific i.e. reflex actions. By
5-5
Figure 5.1 Boundary control of joint trajectories
monitoring hormone variables, the control computer and the joint micro-controllers are
able to react to serious instability in a coordinated behaviour without the requirement for
communication. The over-riding behaviour of the biped is the maintenance of stability
within recoverable boundaries. This is the case whether the robot is standing or walking.
Given the complexity of the dynamic equations involved, a low order approach to the
solution of inverse kinematics and complex differential equations is required. Chapter 7
details the strategy taken to develop the reduced order kinematic models while Chapter
8 describes the software developed to process them. Fundamentally, the control system
computes a basic gait, and then modifies that gait in real time to react to the robots
environment or to inputs from the control system. The four classes of inputs to the
control system are defined in Figure 5.2.
The robot reacts to these inputs, attempting to maintain motion by generating output
signals that control fourteen hydraulic cylinders and motors. While it can be considered
that kinematic and dynamic models are not inputs as such, they effectively dictate the
frame of reference used to process the data from other inputs. Certainly, one could
change the dynamic and kinematic models of the control system without altering any
other software. The robot would exhibit altered behaviour as a result, effectively
reacting to the input. The procedure in which the software reacts to the inputs is detailed
in Chapter 8.
5-6
Figure 5.2 Inputs to control System
The control system hardware of the robot is broken into four sections:
the main control computer
the communications computer
local joint processors
sensory inputs and the hydraulic control valves
A schematic diagram and explanation of the control system hardware is included in
Chapter 6. The main control computer (control PC) represents the midbrain. It computes
the feed-forward control data and monitors the inputs from the artificial horizon and
compass (vestibular system). These transducers provide immediate response mission
critical data that provide the midbrain with global information on the balance of the
robot. The control PC also receives force data and positional data from local joint
processors. It communicates with the rest of the body through the communications
computer i.e. the spinal cord, which contains a 16 port RS-232 expansion board. 14
Motorola M68-HC11 microprocessor and interface boards control local joint motion i.e.
propreoception and fine motor control, via analogue outputs and digital encoder position
inputs. The microprocessors communicate with each other and with the control
computer via the communications computer. In this configuration, both the control PC
and the local joint processors are able to expend the vast majority of processing time on
the control task, rather than the communications task.
5-7
5.4 WALKING CONTROL

While some aspects of the kinematics of the human gait may be relevant to biped
robotics, the same cannot be said of human gait dynamics. Industrial biped robots would
be designed to carry masses approaching their own structure mass. In the human case,
such loads produce gaits characterised by stability optimisation rather than energy
minimisation. Also, as previously discussed, on board electrical drive is not feasible for
industrial scale bipeds. Once the decision is made to use petrol or gas powered
high-speed engines as a power source, energy minimisation is no longer a primary issue
though fuel consumption and emission quality of commercial models would be critical.
The basic strategy of the control software is the forward planning of path trajectories. The
sagittal
sagittal
sagittal
sagittal
sagittal
Figure 5.3 Feed forward joint trajectory generation

5-8
mathematical models developed in Chapter 7 determine the basic joint trajectories

required for any given motion. Trajectories are calculated using inverse kinematic
models; errors between actual motion and ideal motion are to be expected.
Joint trajectories are generated parametrically from the input of desired walking
velocity. Figure 5.3 shows the generation of the feed forward joint trajectories. Initially,
a walking velocity is input, either by the joystick on the control pendant, or via software.
Based on this velocity and the geometry of the robots limbs, a stride length and gait
period is computed. Using the kinematic model described in Chapter 7, joint trajectories
for the stance leg in the sagittal plane are calculated for hip extension, knee extension and
ankle plantar flexion. In the case of the swing leg, a simple linear approximation as
described by Lee and Mansour (1984) is used to generate joint trajectories. In the frontal
plane, the amplitude and period of sway is computed from close coupling of the period
of locomotion in the frontal plane. These open loop trajectories are computed in real time
by the main control computer.
Given the abundance of power available, there is no requirement for the energy saving,
locked-knee movement of the leg in support phase. This allows minimisation of the
potential/kinetic energy conversion during the gait cycle resulting in a smoother
load-carrying gait. It also allows the use of the support-phase knee joint to quickly
change the position of the centre of gravity of the robot, enhancing balance in the
sagittal plane.
5.5 SOFTWARE STRUCTURE

Chapter 8 describes the design of the control system hardware. Naturally, the design of
the control system, and the structure of the software, could not be completed in isolation.
Prior to the design of the control hardware, the basic strategy of the software had been
formulated in terms of the processing tasks and of the information flow between them.
The overall design of the control system was based on criteria to process information as
efficiently as possible. The control software is distributed, layered and hierarchical.
The overriding strategy of the software is to:
Process the minimum amount of data required for locomotion as
rapidly as possible
Distribute the processing of routine data to reduce the demand on the
supervisory system
5-9
Prioritise the data to be processed by the supervisory system to that which

is critical to maintain stability
To achieve this strategy all motion control is processed by the 14 M68HC11
microcontrollers responsible for joint movement. Communications between these
microcontrollers and the supervisory computer are handled by a dedicated
communications computer. Strain gauge and attitude data is acquired and conditioned by
a separate HC11 microcontroller. Table 5.1 lists the control tasks as well as the systems
that process them.
The information transfer between the software modules is shown in the schematic in
Figure 5.4. As can be seen from the schematic, three major groups of data are
transmitted throughout the control system:
Task
Software
System
Vestibular system
Main control program
Control PC
Data acquisition from artificial

horizon and compass.
Data Acquisition Subroutine
Proprioception
HC11 Data program
Data Acquisition from Strain

Gauges
Data Acquisition and

Conditioning Subroutine
Proprioception
HC11 Joint
control software
Data acquisition from

joint shaft encoders
Data acquisition HC11
Joint control HC11s
Data acquisition subroutine
Proprioception
HC11 Joint
Local control of joint movement control Software
Joint control HC11s
Motion control subroutine
Cerebral system
Communications program
Communications computer
Cerebral system
Supervisory program
Supervisory computer
Supervisory control
Balance control
Movement control
Movement subroutine
Cerebral system
Supervisory program
Balance control
Balance monitoring routine
Communications between
supervisory computer and
distributed systems
Supervisory computer
Table 5.1 Control tasks

5 - 10
Actual joint position and velocity data is acquired locally by the joint
microprocessor software and transmitted to the main control computer by the
communications computer. Demand joint position and velocity data is
transmitted from the control computer to the joint control software via the
communications computer
Hormone variables are generated by the Balance Status Subroutine of the Main
Control Program and are then passed to the Communication Program for
distribution to the Joint Control Software
Data Aq uisition
Subroutine
Vestibular Data
Balanc e Monitoring
Software
Joint Trajectory
Subroutine
Ves tibular & G round
Reaction Data
Error Status
Status Monitoring
S oftware
Main Control Computer

Air Pump
Software
Commu nications
P rogram
Communications Computer
Joint Control & Instrumentation HC11s
Left Toe
P rogram
Left Foot
Rotation
Software
Left Foot
Extens ion
Software
Right F oot
Extens ion
Software
Right Foot
Rotation
Software
Right Toe
Software
Left
Hip
Rotation
S oftware
Left
Hip
Extension
Software
Left
Hip
Abduction
Software
Right
Hip
Abduction
Software
Right
Hip
Extension
Software
Right
Hip
Rotation
Software
Actual Position
Velocity
Demand Position / Velocity
LV DT Output
Artificial
Horizon
Software
Analogue
Input
Software
Left Knee
Software
Right Knee
Software
Strain Guage Amplifier Output

Hormone Variables
Figure 5.4 Data network

5 - 11
Data from the artificial horizon compass and strain gauges is acquired and
conditioned by local data acquisition software before being transmitted to the
Main Control Software via the Communications Program
Figure 5.5 gives an overview of the control system data network. While the
communications software will be described in detail in later sections, it is desirable for
the reader to understand the basic mechanism of the network. The process steps are as
follows, using the numbers as annotated in Figure 5.5:
1. Once the control software has computed the next iteration of output data and
commands, this information is written to a file on the Control Computers RAM
Figure 5.5
Flow of data
5 - 12
drive. A RAM drive or virtual drive is used to minimise file access time. The file
contains the data to be distributed to the local control microprocessors.
2. Via an Ethernet network. The Control Computers RAM drive is mapped to the
Communications Computer which constantly scans the drive for a new file. When
found, the Communication Computer reads the file into memory and deletes it.
3. The information is then separated into the individual data streams destined for the
local control microprocessors. The streams are sent via RS422 to the RS232
expansion board.
4. The expansion board transmits the individual data streams to the M68HC11s via
their RS232 ports.
5. After each M68HC11 microcontroller has received the data file it sends a response
containing the latest position and sensor data back to the RS232
expansion port.
6. The expansion port buffers the data prior to transmission to the Communications
Computer via the RS422 link.
7. The communications software combines the data into a single file which is
written directly the Control Computers RAM drive.
8. Finally, after 8 separate operations, the Control Computer reads the latest data file
from its RAM drive less than 100ms after the initial output file was written.
One of the great advantages of this structure is that the rest of the control system is
effectively invisible to the Control Computer; it simply writes and reads data files from
its RAM drive. By using dummy files, the control system can be run on an individual
computer in total isolation from the robot. While the communications software will be
described in more detail in the following sections, it will be assumed that the reader has
gained an understanding of the transfer of data from the Control Computer to the Local
Joint and Instrumentation microcontrollers. When, in the text, reference is made to the
Communications Computer sending a command to the Local Joint Controller, it should
be understood that the process detailed above has taken place.
Chapter 8 outlines the software programs and their role in the control of the robot. An
overview of the main control program is given, followed by an in depth discussion of its
main subroutines. The communications software and the local joint control
microprocessor programs are then discussed. Finally, the development of the software
and testing of functionality prior to the writing of the main code is reviewed.
5 - 13
CONTROL SYSTEM HARDWARE
The robot is going to lose. Not by much. But when the final score
is tallied, flesh and blood is going to beat the damn monster.
Adam Smith
The following sections describe the hardware as discussed in the previous chapter and
as illustrated in schematic in Figure 6.1. Each of the major systems that make up the
control system will be discussed as well as local and global transducers. The emphasis
in the development of the hardware was to distribute the processing tasks as widely as
possible to minimise the quantity of central processing and the time response of the
control system. Wherever possible, a dedicated off the shelf component was used to
process data locally. For example, the Motorola microprocessor boards use an add-on
board to decode and store the position of the optical shaft encoders, rather than using
interrupt routines to keep track of the pulses. A further example is the communications
PC which uses a specialised add on communications card to handle serial data
networking. In both cases, the software simply writes and reads from an address, all
handshaking is handled externally rather than tying up processor time.
6.1 MAIN CONTROL COMPUTER (PC)

The main control computer (MCC) consists of an IBM clone, Pentium II PC running at
133MHz. The computer is running MS DOS 6.22 with the Windows 3.1 networking
extension. Testing, as will be discussed in later chapters, showed that after trials of
numerous types of PC, the actual configuration of the PC produced little effect on the
operation of the control system. The computer is fitted with standard display card, IO
card and 100mbs Ethernet card. It is mounted on the side of the electronics rack, which
is supported by shock resistant mounts to limit high frequency vibration from the engine.
Of particular interest is that the MCC is running MS DOS 6.22. Initial control software
was run within Windows. Early testing showed that regardless of settings or any other
attempt to control the operating system, it was impossible to build a real-time control
system running within Windows. Even when the software was run from within a DOS
shell, the operating system would suspend operation of the software at an irregular
6-1
Chapter 6 - Control System Hardware
interval for an irregular period. Various experts were contacted to solve this problem,
however even Microsoft could not explain what the computer was processing at these
times. Attempting to solve this problem lead to the first change of hardware when the
author attempted to fix the anomaly with a 3.5kg sledge hammer.
The final configuration of the PC includes a separate DOS6.22 partition. As
previously stated, the DOS 6.22 software has been upgraded to include the networking
component of Windows 3.11. This allows the control PC to have other drives mapped to
it via an Ethernet network, while still operating at real-time speed.
6.2 COMMUNICATIONS PC
Again the communication PC is a clone Pentium II running at 133MHz. However, the
Communication PC is fitted with an additional I/O card. The Advantech PCL-16 is a 16
channel RS232 expansion card that allows FIFO buffering of data. This system allows
the communication software to write to a virtual port via a library of C subroutines
supplied with the module. All communication is then handled within the module
allowing the PC to focus on other processing tasks. Like the Control PC, the
Communications PC is fitted with an Ethernet card to allow communications with the
Control PC and any monitoring PC connected to the network.
6.3 LOCAL JOINT CONTROLLERS

Joint processors consist of F1 microcontrollers fitted with Motorola M68HC11
microprocessors. The boards are an off the shelf item widely used internationally for
robotics applications. They were developed by Peter Dunster of the University of
Wollongong in Australia. (Dunster, 2000). Dunster is an electrical engineer with
significant Programmable Logic Controller (PLC) experience. The F1 Board was developed as the brains of a modular system of control cards that were stackable, connected
with a single ribbon cable or bus. Dunster has developed servo control cards, display
cards, I/O cards and other motion control cards for the system. Combined with the
Imagecraft compiler, the board is a powerful controller, programmable in ANSI C,
equipped with 8 channel analogue input, 8 channel digital I/O and RS232 port and a bus
connector.
Based on the writers experience and a desire to measure joint position as accurately as
possible, the decision was made to use quadrature encoders on the joint axes. It would be
6-3
Figure 6.2 The F1 board and the expanded I/O board.
possible to monitor the shaft encoders using the digital I/O ports of the F1 Controller and
appropriate interrupt routines. However, the amount of processor time that would have
been devoted to the servicing of these interrupt routines would have been prohibitive.
The range of add on boards that were available for the F1 controller did not include a
product able to read a quadrature shaft encoder. Following lengthy investigations, the
decision was made to develop an add on board for the F1 controller capable of
monitoring and storing the position of a quadrature shaft encoder. Further, the card would
be able to output an analogue control voltage to the hydraulic valve amplifiers and to
provide expanded digital I/O for the F1 board. The final specifications for the add on I/O
board were as follows:
16 bit Quadrature counter
16 channels of digital I/O for limit switches and encoder index pulse
64 kilobyte address map
8 bit +/- 10V analogue output
Figure 6.2 shows a photograph of the F1 board and the expanded I/O board
To ensure accuracy of the shaft position, the high byte of the quadrature counter must
be read first. At the time of reading this byte, inhibit logic on the Hewlett Packard
HCTL-2016 quadrature encoder prevents new data from being captured by the position
data latch until after the low byte has been read. By reading the address of the I/O card
(set by jumpers on the board) using the M68HC11 LDD (Base) command, both bytes of
the encoder will be loaded into the D accumulator with a single instruction. This allows
for fast data acquisition of the shaft position.
6-4
Figure 6.3 Microprocessor enclosure
Similarly, the analogue output of the add on board is controlled by a 574A latch where
the eight bit number to be converted to an analogue voltage is held. When a number with
a least significant bit (LSB) of 1 is written to the base address (BASE) + 1, the output of
the latch is enabled allowing the conversion. When a number with a LSB of 0 is written
to (BASE) +1, the output of the latch is disabled and the number $00 is held at the input
to the converter. This feature allows the output of the D/A converter to be 0 Volts upon
reset, an additional safety feature in the rare event that the system is reset while hydraulic
pressure, and valve amplifier power is available. In this situation, no movement will
result as the control signal will be held to zero until enabled by a write to (BASE) +1.
The control which has been achieved with this combination of the F1 board and the I/0
cards has been most surprising. Due to the high speed of the M68HC11 control loop and
the resolution of the digital shaft encoders, hydraulic cylinders have been controlled with
the accuracy of servo valves for a fraction of the cost in terms of valve hardware.
As can be seen from the photograph of Figure 6.3, the F1 controller boards and the I/O
6-5
boards have been mounted on aluminium plates. There are two sets of controllers and I/O
cards mounted back-to-back, one on each side of the plate. Seven plates, supporting the
fourteen controllers for each degree of freedom of the robot, are mounted inside the slave
processor enclosure. The enclosure provides data and power distribution to the boards as
well as providing shock and vibration isolation as each of the plates is fixed using
resilient mounts. The F1 microcontroller boards as a 7805 regulator to provide 5V DC
power. As the input voltage to the regulator may be in the range of 11 to 13.5V DC,
significant heat is generated by these regulators. They have been fitted with large heat
sinks, and, due to the confinement of the enclosure, two fans have been installed to
increase air circulation between the plates.
Attached to the top of the enclosure can be seen the RS232 expansion module. This
module allows buffering of the RS232 data from the M68HC11s storing the data until it
is transferred to the communications PC via an RS422 interface. All connections to the
interface board have been secured with screws coated in Locktite to ensure the integrity
of the connection. Mounted on the front panel of the enclosure are power switches and
reset buttons for each of the M68HC11 controllers as well as an ammeter to monitor the
current being drawn for the controllers. The normal current level is marked so that any
increase in current can be detected. A neon display is incorporated into the front panel of
the enclosure so that the downloading status of the M68HC11s can be monitored. It is
written to through port 6 of the RS232 expansion module. The rear panel of the
enclosure provides Mil-Spec quality connector sockets for the interfacing of the joint
shaft encoders which will be discussed in the following section.
6.4 SENSORS
Three groups of sensors are outlined in the following section. The sensors provide data
on the robots global orientation, joint configuration and on forces applied externally. The
schematic of the control system (Figure 5.1) provides details of the connections between
the hardware components and of the types of data that flow between them.
6.4.1 JOINT MOVEMENT (OPTO ENCODERS & LIMIT SWITCHES)
All of the robots degrees of freedom are fitted with Hewlett Packard HEDS -9540 shaft
encoders. Combined with the Hewlett Packard HCTL-2016 quadrature counter, these
encoders provide a resolution of 1024 counts per revolution or 0.35 degrees per count. In
practice, it has been shown that this resolution is higher than that required by the control
6-6
system. Frequently when a single joint moves a significant distance, many other joints
record a movement of at least one quadrature count as the robot reacts to the acceleration
forces of the joint. Were the control system to react to such minor perturbations, the robot
would be in continuous motion as each joint hunted in the vicinity of its demand
position. In practice, it has been seen that an envelope of +/- 0.7 of a degree is sufficient
to avoid this situation.
One of the drawbacks of the use of optical shaft encoders is the absence of position
memory on power up. The control system possesses no information on the position of the
robots joints until such time as the robot has been homed. A simple modification to the
joint position sensors would have seen the inclusion of a potentiometer in parallel with
the shaft encoder. In this way, the control system would acquire basic position data with
no movement of the joint. Such a configuration would have dramatically reduced the
time taken for the robot to complete calibration routines.
Initially, it was intended to use the index pulse of the shaft encoders to zero the
quadrature counters. In practice it proved extremely difficult to ensure that the encoder
wheel was mounted to the shaft in the correct rotational orientation that the encoder pulse
occurred at the minimum extent of joint travel. As the shaft encoders are mounted to the
robot frame with two positioning screws, the position of the index pulse can only be
modified by rotating the shaft to which it is fixed. Minute rotations of the shaft once the
joint had been assembled proved to be unrepeatable. To overcome the problem, limit
switches were fitted to each joint to sense the end of travel. The limit switches include
fixing slots so that their position can be adjusted easily and accurately. Figure 6.4 details
of the fixing arrangements of the shaft encoders. The arrangement of the hip extension
shaft encoders has been discussed previously in Chapter 3.
These shaft encoders are of the type that fits over the spigot on the end of a shaft.
Another example of the knowledge that has been acquired during this research is the
tendency for the large forces found in the joints of the robot, and the forces that will be
found in the joints of any industrial biped, to produce some bending of the joint shafts.
In some cases this bending has led to radial translation of the encoder disk within the
encoder housing. Where an encoder disk has not been perfectly centred within the
housing, the bending has led to misalignment of the encoder disk slots and a
corresponding loss of data. Hewlett Packard also offers a similar version of the shaft
6-7
(a) Foot Rotation
(b) Toe Extension
( c) Foot Extension
(d) Knee Joint
(e) Hip Rotation
(f ) Hip Abduction
Figure 6.4 Arrangements of shaft encoders.
encoder where an integral shaft is fitted. This version of the encoder would be fitted using
a small flexible coupling so that any misalignment in the joint shaft is not transferred
directly to the encoder disc.
6-8
6.4.2 ATTITUDE (GYRO & FLUX GATE COMPASS)

Submarines and aircraft, including space vehicles, are the only controlled vessels that
are designed to travel in three dimensions. All other vehicles including boats, trains, cars
and trucks travel in essentially two-dimensions unless highly dynamic motion results in
the contact with the ground or water being broken. When in contact with the ground or
water, cars and boats rely on the forces of gravity to ensure stability. For this reason, no
measure of pitch or roll is required as feedback to the operator as these two degrees of
freedom are controlled by the stability applied by gravity and the points of reaction to the
ground or water. In the case of a plane or submarine, it is essential to maintain the
control of pitch and roll of the vehicle if navigation in a desired direction, at a desired
depth or height is to be achieved. In the case of biped locomotion, as the number and
relative position of the points in contact with the ground are constantly changing, the
vehicle is not able to passively maintain its orientation in pitch and roll. The human uses
the vestibular system to measure rotational and translational movements of the head in
three dimensions. Something similar is required for the biped.
For this project, it was decided that only rotational movements would be detected, as
translational movements could be calculated from the configuration of the joint members
and the measured global orientation of the upper body where the vestibular system was
to be located. As the writer held previous experience with the interfacing of directional
transducers from work within the defence industry, it was decided to use a standard
aircraft gyroscope to measure pitch and roll and a flux-gate compass to sense heading1.
The fluxgate compass uses an array of coils to detect orientation with the earths
magnetic field. These compasses were initially developed for sonobuoys dropped from
aircraft to detect submarines. Most aircraft and missile systems used gyro compasses to
sense heading which were significantly larger and required prohibitive amounts of power
for use in the underwater array of the sonobuoy.
The technology was quickly
commercialised and is now used in most commercial electronic marine compass systems.
The compass installed in the robot is an Autohem 600 series compass with an National
Marine Electronics Association (NMEA) output. The NMEA system is similar to an
1 At the time of the design of the robot, no low cost integrated gyroscope hardware was available for integration
with the robot. Currently, three axis gyro boards are available for as little as US$1500
(www.androidworld.com/prod55.htm). It is interesting to note that with the increase in android research, the availability of associated sensory hardware has similarly increased. This fact alone would indicate that it is entirely foreseeable
that biped service robots will become commonplace in the future.
6-9
RS232 network, and is designed so

that instruments from a range of
manufacturers can be networked
aboard
vessels
easily
and
efficiently. Once the compass has

been supplied with 12V DC, it
continually outputs a heading in
ASCII format at the rate of 3Hz. As
can be seen in the photograph of
Figure 6.5, the compass is mounted
on
an
aluminium
frame
approximately 600mm from any

ferrous
Figure 6.5 Fluxgate compass mounting.
or
electromagnetic
component of the robot.
The artificial horizon used for the project is one taken from a PC3 Orion aircraft
(see Figure 6.6). This gyro was chosen as it was fitted with LVDT outputs for
interfacing with the aircrafts automatic pilot. A standard LVDT excitation and sensing
circuit was built to convert the output of the gyro to two analogue 0V - 5VDC outputs in
pitch and roll for direct input to an M68HC11 analogue input. The artificial horizon
enclosure contains a dedicated Motorola M68EVBU controller board which converts the
analogue ouput to an RS232 data stream similar to that of the fluxgate compass.
Figure 6.6 Artificial horizon

6 - 10
Application of 12V power results in the data stream being transmitted at 5 times per
second. As the EVBU comes standard with the Motorola BUFFALO monitor on
EPROM, the command g0170 is sent to the EVBU by the communications software to
initiate the onboard vestibular data acquisition software which has been stored in
EEPROM. One advantage of the aircraft gyro over the solid state gyro is that the unit
senses actual position, rather than integrating a series of accelerations. Small and low
velocity movement can readily be detected.
The disadvantage of the aircraft type gyro is that it is designed to use the vacuum from
the aircrafts engine manifold to provide a pressure differential to drive the turbine that
spins the gyro. This is the case with most aircraft gyros as a total electrical failure will
not cause the essential instrument to fail. For use in this project an air pump was fitted to
the robot to provide a positive air supply to drive the gyro. The pump is run by a 12V
motor located beneath the engine frame of the robot. Initial testing of the gyro showed
that the 12VDC motor speed fluctuated with changes in the supply voltage caused by
changes in battery voltage, and alterations to the load on the battery as other 12VDC
systems became active. In turn these fluctuations caused variation in the supply air
pressure resulting in acceleration and deceleration of the air turbine within the gyro. The
resulting changes in rotational speed of the gyro lead to unreliable position indication. To
maintain a precise air supply, an F1 microcontroller was used to measure the gyro
supply air pressure and to control the speed of the motor to ensure that the air pressure
remained constant. Figure 6.7 shows the motor control enclosure. The calibration and
testing of the vestibular system is covered in later chapters.
Figure 6.7 Air pump controller

6 - 11
6.4.3 FORCE (STRAIN GAUGES AND AMPLIFIERS)

As discussed in previous chapters, force feedback is essential for local control of the
foot joints. As well, feedback provides data as to the magnitude and direction of the
ground reaction vector. Ideally the soles of the feet would be covered by a series of
transducers measuring the ground reaction force at each point along the foot.
Unfortunately, such technology was not available for use in this project. As an
alternative a series of strain gauges was fitted to the frame of the robots ankle frame so
that a measure of the total reaction force at the feet could be sensed as well as the
longitudinal distribution of the force along the foot. Figure 6.8 shows the location of the
strain gauges within the ankle frame.
The first set of gauges is mounted on the parallel swing arm connection from the lower
shank to the ankle frame. In this position, the strain gauges measure the entire weight on
the foot by sensing the bending in the swing arm. The second set of gauges is located at
the heel of the robot. These gauges use the Poisson relationship to sense compression in
the heel and expansion in the orthogonal direction. Four strain gauge amplifier boards
were built to convert the outputs to a 0-5VDC signal for input to the M68HC11s. As the
amplifiers could not be located next to the strain gauges, double shielded cables were
used to carry the signals to the amplifiers which were mounted at the base of the fluxgate
compass frame (see Figure 6.9). Initially the shielding was connected to the ground
Figure 6.8 Strain gauge locations

6 - 12
Figure 6.9 Strain gauge amplifier enclosure
terminal of the amplifier enclosure. However, initial testing proved that connection of the
shielding to the 0 Volt output of the amplifier reduced noise on the outputs. The signal
was further improved by connecting all limbs of the robot together with ground straps
and then connection of the ground of the strain gauge amplifier to the robot frame.
Subsequently, the ground terminals of the power supply and all instrumentation have
been connected to the robots frame. Due to the large forces carried by the swing arm
only two strain gauges are required for an accurate reading of the load on each leg. The
gauges are configured as a half bridge as shown in Figure 6.10. A full strain gauge bridge
has been installed in the ankle frame. Two gauges are positioned at the top of the frame
Figure 6.10 Swing arm strain gauge configuration
Figure 6.11 Ankle strain gauge configuration

6 - 13
Figure 6.12 Signal conditioning modules
to detect the bending compression strain as the foot and toe cylinders push down. Two
are located on the diagonal bracing member to detect the axial tension stress induced as
the cylinder mounts up against the top of the ankle frame. These gauges were arranged
as shown in Figure 6.11. The strain gauges were supplied by RS Components and are of
a type suited to aluminium. All connections from the strain gauges to the amplifier box
have been soldered to ensure that no added resistance is introduced to the circuit.
During initial testing, the output signal from the strain gauges was seen to contain
significant noise due to the high gain of the amplifiers (1000 times). As well, depending
on the motion of the robot, the output regularly exceeded the 0 - 5V DC range of the
HC11 analogue inputs. To modify the gain of the strain gauge amplifiers, it was
necessary to remove the boards from the enclosure and then to replace resistors on the
printed circuit board. Given that the full range of the output signal would remain
unknown until dynamic walk had been achieved, the decision was made to condition the
signal and to control the ultimate gain of each strain gauge channel separately from the
strain gauge enclosure. Four Brodersen PXU-20 Signal converter modules were housed
in a separate enclosure on the opposite side of the compass frame to that of the strain
gauge amplifier enclosure as shown in Figure 6.12. The converters provide low pass
filtering as well as signal gain and offset adjustment. The conditioned signal was then fed
to the analogue inputs of the HC11s in the same way as it had been directly from the
strain gauge amplifiers. Calibration of the strain gauges is detailed in Chapter 9.
6 - 14
Figure 6.13 Foot contact switches
6.4.4 FOOT CONTACT SWITCHES

To detect that the feet are in contact with the ground, limit switches (Figure 6.13) were
fitted to each side of the main pad of the robots feet. Contact with the ground has proved
difficult to detect. The control system is able to calculate the orientation of the robots
feet relative to the ground by using the vestibular system to determine the orientation of
the hip to the ground, and then by using the joint positions to determine the orientation
of the feet to the hips. However, to provide high speed local joint control
(proprioception), some method of detecting foot contact was required. Selections of limit
switches in a range of configurations were trialled. Most of the limit switches were
destroyed either when foot roll caused them to take the weight of the robot or they have
been broken off after contact with the other foot. The final arrangement, as seen below
has proven to be reasonably reliable.
6.5 CONCLUSIONS ON CONTROL HARDWARE

This completes the discussion of the control hardware. This project has shown that the
type of instrumentation required for the control of a biped robot more closely resembles
that of an aircraft than that of any other vehicle. The key issues to be considered in the
design the hardware of an industrial biped control system are:
The acquisition of reactions from the external environment - ground reaction
forces, etc.
The determination of global orientation - vestibular system
The measurement of joint angles - both with digital shaft encoders and by nonvolatile methods such as potentiometers
6 - 15
The transmission of data within the control system - network, packet protocol etc.
The protection of critical circuits - fault mode, redundancy, fault reaction and
recovery
The ability to automatically monitor and calibrate on board systems - homing etc.
In the instance that a fork lift truck malfunctions, the machine stops where it is. Were
the control system of a industrial biped robot to malfunction, the potential for loss of life
and property is large, particularly in military applications where such robots would be of
most benefit. This is another significant issue for the designers of the control systems of
industrial biped robots identified by this project.
6 - 16
CONTROL SYSTEM MODELLING
I have had my results for a long time: but I do not yet know how
I am to arrive at them.
Karl Friedrich Gauss
The most essential elements of robotic control are realistic models of the robots
motion. In this case, a model of bipedal motion were required. Several models were used
and developed during the project and these will be covered in the following sections.
Initially, a kinematic model was used for simulation during the design phase of the
project. The model generated joint coordinates at any stage of the biped gait cycle. A
dynamic model was then used to calculate joint velocities and accelerations as well as
movements of centres of mass. The focus of the design phase was to determine the
feasibility of a biped of the scale required to be viable for industrial use. Later, during the
development of software, a model was developed that was more compatible with the
generation of feed forward trajectories for the control system. These models are
discussed in the following sections.
7.1 KINEMATIC SIMULATION

Braune and Fischer (1908) are credited with the first comprehensive study of human
locomotion. Their results were first published in 1895. Their work provides a detailed
description of motion with tables of joint coordinates obtained by time-lapse
photography. The value of Braune and Fischers work is that it allows us to easily
compare the results of simulation with those of actual motion. For the kinematic
simulation of Biped locomotion, an analytical model of locomotion was required. As
well, to confirm the output of the model, a known set of joint trajectories was used to
confirm the output of the simulation. The model chosen for simulation was that of T. C.
Hartrum (1973). A FORTRAN program, written in 1973, this model was based on limb
angular displacement data reported by M. P. Murray et al (1964). Hartrum fitted
sinusoidal curves to Murrays data using these curves to calculate joint coordinates
through the gait cycle. These coordinates can be easily transferred to a CAD system
where the motion can be displayed graphically. This was achieved using a C program that
retrieved the data from file and displayed it through AutoCADs Advanced Development
7-1
Chapter 7 - Control System Modelling
System. A listing of the interface is contained in Appendix 2. The graphical output of the
simulation for the lower limbs is shown in Figure 7.1.
Figure 7.1 Output of Hartrum Model for Lower Limbs
Several drawbacks existed with this model. Firstly, it was written in FORTRAN that
was incompatible with existing software. Secondly, the model represents the foot as
having no heel, and the ankle articulated where the heel should be. In this form, the
model was unsuitable for dynamic simulation, as it could sustain no weight transfer along
the foot. The model, a set of parametric equations, was modified to represent the
articulated foot of our biped. The modification to the geometry is shown in Figure 7.2.
Taking the equations for the position of the foot joints from Hartrum (1973),
they were modified to include the articulated foot required for our simulation. The
modified equations are as follows:
In stance phase, the equations for the ankle coordinates now become (in the leg plane):
XW
AN = FCos ( HI + KN + AN + )
W
W
YAN
= YFO
=0
ZW
AN = FSin( HI + KN + AN + )
where:
HI = Hip Angle
KN = Knee Angle
AN = Ankle Angle
7-2
Figure 7.2. Modified foot model.
Equations for the heel coordinates are:

W
X HE
= FCos ( HI + KN + AN )
W
W
YHE
= YFO
=0
W
Z HE
= FSin( HI + KN + AN )
Simarly, in swing phase, the equations for the ankle coordinates are:
W
XW
AN = X KN + LL Cos ( HI + KN )
W
W
Y AN
= YKN
=0
W
ZW
AN = Z KN + LL Sin ( HI + KN )
Equations for the heel coordinates are:

W
X HE
= FCos ( HI + KN + AN )
W
W
YHE
= YFO
=0
W
Z HE
= FSin( HI + KN + AN )
Where; FO = Foot Angle (Between Foot and Toe)

HE = Heel Angle (Between Lower Leg and Heel)
These equations are transposed back into world coordinates by multiplication
with the transform matrix T:
Cos( FO ) Sin( FO )Cos ( KN )
Sin( FO )Sin( KN )

Sin( ) Cos( )Cos ( )
Cos ( )Sin( )
FO
FO
KN
FO
KN

0
Sin( KN )
Cos( KN )
To give the joint coordinate equations for the stance phase:

X AN = X FO + FCos ( HI + KN + AN + )Cos( FO )
FSin( HI + KN + AN + )Sin( FO )Sin( KN )
7-3
Y AN = YFO + FCos ( HI + KN + AN + )Sin( FO )
FSin( HI + KN + AN + )Cos ( FO )Sin( KN )
Z AN = Z FO
FSin( HI + KN + AN + )Cos ( KN )
Where; F = length of foot.

X HE = X FO + HCos ( HI + KN + AN + )Cos ( FO )
HSin( HI + KN + AN + )Sin( FO )Sin( KN )

YHE = YFO + HCos( HI + KN + AN + )Sin( FO )
HSin( HI + KN + AN + )Cos ( FO )Sin( KN )

Z HE = Z FO
HSin( HI + KN + AN + )Cos ( KN )
Where H = length of heel:

X AN = X KN + LLL Cos ( HI + KN )Cos( FO )
LLL Sin( HI + KN )Sin( FO )Sin( KN )

YAN = YKN + LLLCos ( HI + KN )Sin( FO )
LLL Sin( HI + KI )Cos( FO )Sin( KN )

Z AN = Z KN
LLL Sin( HI + KN )Cos ( KN )
Where; LLL = length of lower leg.

X HE = X FO + FCos ( HI + KN + AN )Cos ( FO )
FSin( HI + KN + AN )Sin( FO )Sin( KN )

YHE = YFO + FCos ( HI + KN + AN )Sin( FO )
FSin( HI + KN + AN )Cos ( FO )Sin( KN )
Z HE = Z FO
FSin( HI + KN + AN )Cos ( KN )
The simulation software was re-written to include these modifications. The graphical
output of the model showing the articulted foot is shown in Figure 7.3.
7.2 DYNAMIC SIMULATION

Having developed a kinematic simulation of the lower limbs and hip, the model was
expanded to determine if it was possible to modify the human geometry to that of an
industrial biped as discussed in previous chapters. In particular, could the trunk, arms and
head be replaced with a single mass hung from the hips that generated the same forces
and torques required to balance the body in the sagittal plane?
7-4
Figure 7.3 Modified kinematic model for lower limbs and hip
To analyse forces and torques generated by upper body segments, the kinematic model
was further modified to include the trunk, arms and head. The output can be seen
in Figure 7.4. The output of this model gives the joint coordinates for the upper body
segments in a series of iterations over a complete gait cycle. Figure 7.5 shows the
positions of upper body segments with the left arm removed for clarity.
By calculating the change in position of body segments, and dividing by the time
change from one iteration to the next, the velocities of the segments can be calculated.
Given the initial velocities of the segments, accelerations, forces and torques can be
calculated. The following analysis is an example of the computation of the forces and
torques generated at the hip by one body segment (the head). Similar analysis was
conducted for each of the upper body parts.
Figure 7.4 Full kinetic model of human locomotion

7-5
Figure 7.5 Upper body

segments for two gait
cycle iteration
The forces and torques produced by the head segment are as calculated as follows:
Horizontal Forces
vCH 2 x =
X CH 2
X CH 1
T
Where; vCH 2 = Average velocity of the Centre of Gravity of the head in the X direction
X CH 2 = Final X coordinate of the Centre of Gravity of the head
x
X CH 1 = Initial X coordinate of the Centre of Gravity of the head

T
= Time increment between frames
aCH 2 x =
v CH 2
v CH 1
x
Where; aCH 2 = the acceleration of the centre of gravity of the head in the X direction
x
Therefore; the horizontal force produced by the head on the hip is:
+ Fx H = M H aCH 2 x
7-6
Vertical Forces
vCH 2Y =
YCH 2
YCH 1
T
Where; vCH 2 = Average velocity of the Centre of Gravity of the head in the Y direction
Y
YCH 2 = Final Y coordinate of the Centre of Gravity of the head

YCH 1 = Initial Y coordinate of the Centre of Gravity of the head
T
= Time increment between frames
aCH 2 y =
v CH 2
v CH 1
y
Where; aCH 2 = The acceleration of the centre of gravity of the head in the Y direction.
y
= Gravitational constant
Therefore; the vertical force produced by the head on the hip is:
+ FyH = M H aCH 2 y
g
Torques
The torque generated at the hip by the motion of the head is produced by a combination of translational and angular accelerations. The angular accelerations are calculated
as follows;
The angular forces are given by:
H =
Where; H 1
H 2
H 2 =
H 2
H 1
T
Initial angular velocity of head
Final angular velocity of head and is given by:
H 2
H1
T
7-7
Where;
H1
Initial angular position of head
H 2
Final angular position of head
H 1 = a tan 2[(Y1
YH 0 ), (YH 1
YH 0 )]
H 2 = a tan 2[(YH 2
YH 1 ), (YH 2
YH 1 )]
The angular accelerations produced from this analysis for all upper body segments, are
shown in Figure 7.6. The analysis relies on the second time derivative of the angular
positions of members. For this reason no angular accelerations are generated for the first
two frames of motion.
Upper Body Motion
Angular Accelerations of Body Segments
2.0
Angular Acelerations
(Rad/s^2)
1.5
1.0
0.5
0.0
-0.5
-1.0
-1.5
-2.0
0.08 0.24 0.40 0.56 0.73 0.89 1.05 1.21 1.37 1.53 1.69 1.85 2.02 2.18 2.34 2.50
Time (Sec)
Lower Arm (L)
Upper Arm (R)
Lower Arm (R)

Trunk
Upper Arm (L)

Head
Figure 7.6 Angular Accelerations of Upper Body Segments
Once accelerations had been calculated, torques and forces acting on the hips could be
calculated. For example, the torque produced by the head at the hip is calculated as:
H = H H
Where;
H
H
H
= Torque produced by the head

= Moment of Inertia of the head
= Angular Acceleration of the head
7-8
The torque produced at the hip by the head is:

+
TH = Fx H .Ry H + Fy H .Rx H + H H
In a similar manner forces and torques for all upper body segments were calcuated.
Figure 7.7 shows the Horizontal and Vertical forces as well as the Torque produced at the
hip by the motion of the upper body segments. It can be seen that the horizontal motion
of the upper body produces the greatest force and torque on the hip.
Hip Forces
Forces and Torque acting on the hip
400
Force (N)
Torque (Nm)
300
200
100
0
-100
-200
-300
-400
0.00
0.32
0.65
0.97
1.29
1.61
1.94
2.26
Time (Sec)
Moment
Force [X]
Force [Z]
Figure 7.7 Forces and torques produced at the hip by motion of upper body segments
These are the forces that must be generated by a

single
mass
to
synthesize
bipedal
locomotion. To determine the motion of a single

mass, a basic geometry is required as shown in
Figure 7.8. The actual mechanism of the linkage
shown is not particularly relevant to this analysis as
it is assumed that the linkage has negligible mass
compared to that of the counterweight. A two-link
mechanism was chosen for the analysis to determine if the motion could be achieved with such a
configuration. In the event that the required motion
was not achievable with a two-link mechanism,
Figure 7.8 Basic geometry of single
mass attached to hips
then other configurations would be investigated.

7-9
The motion of the mass is calculated as follows:

X Displacement
+ Fx = M a
a =
Fx
M
v 2
v1 Fx
=
t
M
x 2
x1 x1
x 0

t t Fx
=
t
M
(x2
2 x1 + x0 ) = Fx
t 2
Fx
2
x2 =
t + 2 x1
x0
M
Similarly, the Y displacement is calculated as:

Fx
2
y2 =
t + 2 y1
y 0
M
The rotational displacement is more complex to calculate as the translational motion

produces additional torques. Rearranging the above equations:
x +x
Fx = 2 2 0 M
t
y +y
Fy = 2 2 0 M
t

Given:
+
M = Fx Y + Fy X + H
then:
=
M
Fx Y
Fy X

2 =
M [1
(x 2 + x 0 ) Y
(y 2 + y 0 ) X ]
0

7 - 10
The displacements produced by the analysis are shown in Figure 7.9.

Box Motion
0.6
8.0
0.5
6.0
Displacement (m)
0.4
4.0
0.3
2.0
0.2
0.0
0.1
-2.0
0.0
Angular Displ. (Rad)
Vertical And Horizontal Displacement
-4.0
-0.1
-0.2
-6.0
0.16
0.48
0.81
1.13
1.45
1.77
2.10
2.42
Time (Sec)
Lin Disp [X]
Lin Disp [Z]
Ang Disp
Figure 7.9 Displacement of Single Mass suspended at hip.
The joint position data was introduced to the Autocad ADS program, which displayed
the motion found in Figure 7.10. It can be seen from the output that the motion of the
counterbalance is not radical compared to that of the lower limbs i.e. if the motive
system was sized to accelerate the lower limbs to velocities that could sustain biped
locomotion, it would also be able to accelerate the Counterbalance to appropriate
velocities. It was therefore held that a biped of this configuration was dynamically viable.
Figure 7.10 Motion of counterbalance suspended from hips.

7 - 11
7.3 FORMULATION OF JOINT TRAJECTORIES

For the control system to generate joint trajectories, a parametric model was required.
Figure 7.11 shows a schematic of the lower limbs in the sagital plane.
Figure 7.11 Schematic of Lower limbs in the Sagital Plane
The Hip Angle and the Ankle Angle are the driven variables of forward
locomotion. The Knee Angle Q is a function of the Ankle Angle and the Hip Angle.
Horizontal Displacement X or its time derivative, velocity, is the driving input of
locomotion. Hip height Y is either maintained as a constant or varied as a function of the
gait cycle. In the case of humans, hip height rises and falls during locomotion to conserve
energy. During swing phase, the hips fall, converting potential energy into kinetic
energy. After heel strike, when the gait moves into stance phase, the kinetic energy is
converted back to potential energy as the hips rise again.
As our robot is fitted with 25kW internal combustion energy, there is no requirement
for energy minimizing gaits1. Initially, Hip height Y is held as a constant. Given a
forward velocity, v, the solution for the above angles is shown on the following pages:
Naturally, any commercial model of the robot would be designed to minimise the effects of greenhouse gases.
This is the main reason that the prototype is carrying a large cylinder of LPG gas, rather than a a litre of petrol.
1
7 - 12
From Figure 7.11;

r=
x2 + y2
x
= a sin
r
L 2 2
L12
r 2
= a cos

2 L1 r

r 2
L12
L 2 2
= a cos

2 L1 L 2

These equations were input into a spreadsheet where the hip, knee and ankle angles
were calculated for a single gait cycle. The values for these angles are shown in
Figure 7.12.
Figure 7.12 Hip, knee and ankle angles
The models used are;

1) The torque model used to show that the upper body can be replaced with a
counterweight slung under the hips which can be accelerated to produce similar
restoring torques and accelerations.
2) An analysis of a single joint and hydraulic cylinder is only included to show the
complexity of the full mathematical modelling of the joint.
3) A dynamic model of the walking device used to obtain boundary parameters for
use in the control algorithms.
7 - 13
CONTROL SYSTEM SOFTWARE
Technology... is a queer thing. It brings you great gifts with one

hand, and it stabs you in the back with the other
C. P. Snow
This chapter continues the explanation of the control system software in extended detail
to the overview found in Chapter 6. The software is broken down into three main areas;
Main Control Software, Communications Software and Local Joint Control and Data
Acquisition software. Figure 8.1 details the main software modules and the subroutines
that will be discussed in detail in this chapter. It is assumed that the reader has examined
Chapter 6 and is familiar with the control system philosophy.
8.1 MAIN CONTROL PROGRAM

The Main Control Software (MCS) runs on the Main Control Computer (MCC) and is
responsible for the major behaviours of the robot. The software has been written in C for
MSDOS. Initially it was intended to write the software in C within the Microsoft
Windows environment, however all attempts to make the system operate in real time
failed. The software would run satisfactorily for up to a minute before the operating
system switched to some unidentifiable task for up to 3 seconds. During this time,
processing of the main control program was halted. Clearly, a loss of control of an
Control
Software
Main Control
Program
Communications
Program
M68HC11
Microprocessors
Software
Data Aquisition Subroutine
68HC11 Transmit Subroutine
Communications Subroutine
Maintenance Subroutine
MCC Transmit Subroutine
Calibration Subroutine
Calibration Subroutine
Motion Subroutine
Stance Subroutine
Locomotion Subroutine
Figure 8.1 Control system software

8-1
Chapter 8 - Control System Software
inherently unstable robot for this length of time was unacceptable. The primary
advantage of the Windows operating system was the ease of networking between the
MCC and the Communications Computer. After consultation with Microsoft, it was
decided to use MSDOS as the operating system with the addition of the Windows 3.11
networking add on. Windows 3.11 ran in an MSDOS environment, loading the
networking software prior to the Windows operating system. It was therefore possible to
load the networking software as a stand alone utility. Using this system, the MCC and the
Communications Computer are networked using 10mbs Ethernet cards and coaxial
network cable. This system has proved to be extremely robust and offers the ability to
connect a third computer to the network for the downloading of software and for the
uploading of data.
As MSDOS is a single processing system, there is no ability to break the main control
software into separate programs that can be processed simultaneously. Early in the
development process, subroutines were compiled separately being linked during
compilation of the main program. This configuration proved to make debugging difficult
in the Borland C environment. From a development perspective therefore, the main
control program has been written as a single file containing all of the referenced
subroutines. Flowchart 8.1 shows the basic flow chart of the main control program. The
source code for the main program can be found in Appendix 5.
The initialisation section of the software sets up global variables which include the
global data matrix, a union which holds the actual and demand positions and velocities
for the joint controllers. Once in the main loop, five major modules are called to initiate
behaviours in the robot. These are:
1. Data acquisition subroutine
2. Maintenance subroutine
3. Calibration subroutine
4. Stance subroutine
5. Locomotion subroutine.
Essentially, the first four behaviours are used to maintain the robots systems and to
prepare it for locomotion. The final behaviour, locomotion, is the main routine that would
be active while the robot is operational in an industrial materials handling capacity. The
structure of these loops will be covered in the following sections.
8-2
Flowchart 8.1 Main

control program
8-3
8.1.1 DATA ACQUISITION ROUTINE

Flowchart 8.2 shows the structure of the data acquisition module of the MCP. The main
function of the module is to read data from the artificial horizon and compass and to read
the joint data matrix from the communications computer. As data is transferred around
the network in seven bit ASCII format, a conversion routine (to_HC11) converts the
double integers and real numbers of the position/velocity union to a format compatible
with transmission. Once the data has been converted it is written to the RAM disk under
a temporary filename to avoid a partially written file from being accessed by the
communications software. Once the file is written the name is changed to one being
searched for by the communications software. After writing the output file, the data
acquisition routine then looks for the return file containing the latest position / velocity
data. During this time the loop counter is continually updated to measure the time taken
for the incoming data file to be received. The distribution of the delay times is included
on the on screen display so that anomalies in transmission times can be recognised. Once
recognised, the data acquisition routine reads the file and deletes it immediately. It then
uses the routine from_HC11 to combine the seven bit data stream to form the double
integers and real numbers containing the joint and analogue input data. The data is then
stored in the global joint data union available for processing by other subroutines.
Once the analogue output data has been acquired, the data acquisition routine calls a
number of subroutines to update the moving historical data matrix where the previous ten
values are held. The subroutine median then calculates the median value of the historical
data for use in the motion software. The use of median values was implemented to
eliminate transient spikes in the data stream. Initially, signal conditioning was performed
in the analogue input 68HC11, however such was the speed of the processing that
transients were not eliminated. By performing the signal conditioning from within the
MCP, a similar time constant to that of the MCP iteration could be achieved resulting in
data that was more representative of the period between MCP cycles than that of the
instant prior to the commencement of an iteration. As will be seen, in the event that
control is handed from the MCP loop to that of a motion control module, similar data
processing functions to those described above are handled from within that module.
Where maintenance routines are called, the data acquisition routine maintains control of
the data acquisition, transmission and display functions.
8-4
MCP_112.C
Data Aquisition Module
The loop counter is used to
monitor the subroutine loop
speed as well as to monitor
the number of loops with errors.
Increment
Loop Count
Read Compass
and
Artificial
Horizon
The data from the compass

and artificial horizon is read.
Check for keyboard input

during loop.
Keyboard
Input ?
No
Yes
Initiate
Logging?
No
The logging function writes position

and instrumentation data to a file
for later analysis or debugging.
Yes
Convert
Joint/Global Data
To Output Form.
Transmit
Output
Data
Update Loop Error

and Timing Counters.
Update Moving
Averages and
recalculate Medians
Update
On-Screen
Display.
This function converts the double

integers and single integers and
floating point numbers to 7 bit bytes
for transmitting to the Communications
Computer.
W rite Data file to RAM Drive
The loop counters are used to count

the number of loops that have waited
for various times forthe inpput data
matrix. This feature was originally used
to optimise timing parameters, however it
is an excellent indication of a
problem with any part of the system.
All aquired data such as artificial

horizon and compass data as well
as strain guage data are held in a
moving average matrix. The matrix and
median values are updated on each
loop.
Joint position, instrumenttion
and other data are constantly
displayed on the screen.
Flowchart 8.2 Data

acquisition routine
8-5
Figure 8.2 Main control program display
8.1.2 MAINTENANCE ROUTINES

A number of subroutines are used to maintain the hydraulic systems on the robot. For
example, the subroutine cycle, continually cycles all of the proportional hydraulic valves
in sequence. This function is useful in several situations. The first occurs when the
robots limbs are in positions where incorrect travel could cause damage. This may be the
result of a trial where the system was halted when unpredicted movement was observed,
a common occurrence during early testing. In this case, after the hydraulic pump is shut
down, the valves are cycled allowing the limbs to slowly fall under gravity as fluid from
the pressure side of the cylinder is released to tank. During normal operations, it is
possible that the valves are not fully cycled through the full range of movement. The
second role of the cycling function is to ensure that the valves are regularly moved
through the full extent of travel to minimise the risk of valves wearing irregularly.
As the six hip actuators are located above the hydraulic tank the pressure in these
cylinders becomes negative relative to the atmospheric pressure when the hydraulic
pump is shut down. If the robot is not operated for several days, air is drawn past the
cylinder seals entering the chambers of the actuator. The air becomes trapped causing a
spongy reaction in the joint and causing the control of the joint to become unpredictable.
The second maintenance function move cycles all of the joints through the full range of
movement to force the air from the cylinders, back into the hydraulic reservoir. This
situation is less likely to be a cause of concern for normal industrial hydraulics as they
8-6
do not usually require the same level of precision motion control. As it is extremely
difficult to make hydraulic actuators completely airtight, it s reasonable to expected that
a production industrial biped will require a similar routine to purge air from the hydraulic
system.
8.1.3 CALIBRATION ROUTINE
Unlike analogue shaft encoders, such as potentiometers, the digital shaft encoders are
unable to store the position of the robots joints once power is lost. Each time the robot
control system is started, the shaft encoders must be synchronised with the robots joint
angles. To achieve this, each joint must be moved to a known position where the value
of the quadrature counter can be set to the known pulse count for that position. The
calibration software to control this process is located within the joint control
microprocessors. However, uncontrolled calibration of joints simultaneously will result
in damage to the robot. For example; if the toe cylinder is pushing down while the foot
rotation and foot extension cylinders are pulling up, the toe would be snapped off the
front of the robots foot. To ensure that the calibration is conducted in a safe sequence,
the calibration routine within the MCP supervises the activation of the joint control
microprocessor calibration software.
The following functionality is included in the calibration module:
Movement of individual joints in two directions
Calibration of individual joints
Calibration of the combined foot assemblies
The movement function is used to test individual joint movement, to calibrate the
position of limit switches and to recover the robot after an interrupted trial. The
calibration functions are used to calibrate the joint shaft encoders as previously discussed.
Flowchart 8.3 shows the structure of the calibration module of the MCP. Similarly to
other major subroutines, the Calibration Module simply changes values within the
global joint union structure to control functions at a joint level. The user inputs
commands to the software by pressing the appropriate keys on the computer keyboard
which has been labelled to represent these functions. The keyboard map for the
calibration software is shown in Table 8.1. As can be seen from the flowchart, the main
loop continuously looks for keyboard input. Once a character is found, it is compared
with the keyboard map to determine what action should be taken. For example, if the H
8-7
Keyboard
Input ?
No
Yes
All
Stop ?
Yes
Stop Current
Calibration?
Yes
Move Limb
Positive Drn?
Yes
Send All
Stop Comand To
All Joint Controllers
Send Stop Comand

To Joint Controllers
In Calibration Mode
Send Move
Positive Command to
Joint Controller.
No
Move Limb
Negative Drn?
Yes
Send Move
Negative Command
to Joint Controller.
No
Send Calibrate
Command to
Joint Controller
Calibrate
Limb ?
Yes
No
Set Foot
Calibration
Flag
Calibrate
Foot ?
Yes
No
Foot Calibration
Active?
Toe Cylinder
Flag Set ?
Yes
Yes
No
No
Yes
Exit ?
No
Communicate
Commands To
Joint Controllers
Foot Rotation
Flag Set ?
Send Calibrate
Command to Toe
Joint Controller
Foot Extension
Flag Set ?
Yes
Yes
No
No
Send Calibrate
Command to Foot
Rotation
Joint Controller
Send Calibrate
Command to Foot
Extension
Joint Controller
End
MCP_112.C
Calibration Module
Flowchart 8.3
calibration module
8-8
Key
Function
Key
Function
Esc
Exit program
Space
All stop
Toggle logging
Left hip abduction positive
Right hip abduction positive
Left hip abduction negative
Right hip abduction negative
Left hip abduction calibration
Right hip abduction calibration
Stop left hip abduction calibration
Stop right hip abduction calibration
Left hip extension positive
Right hip extension positive
Left hip extension negative
Right hip extension negative
Left hip extension calibration
Right hip extension calibration
Stop left hip extension calibration
Stop right hip extension calibration
Left hip rotation positive
Right hip rotation positive
Left hip rotation negative
Right hip rotation negative
Left hip rotation calibration
Right hip rotation calibration
Stop left hip rotation calibration
Stop right hip rotation calibration
Left knee extension positive
Right knee extension positive
Left knee extension negative
Right knee extension negative
Left knee extension calibration
Right knee extension calibration
Stop left knee extension calibration
Stop right knee extension calibration
Left toe extension positive
Right toe extension positive
Left toe extension negative
Right toe extension negative
Left foot rotation positive
Right foot rotation positive
Left foot rotation negative
Right foot rotation negative
Left foot extension positive
Right foot extension positive
Left foot extension negative
Right foot extension negative
Left foot calibration
Right foot calibration
Stop left foot calibration
Stop right foot calibration
Table 8.1 Calibration module keyboard command map
key is pressed, the software will set the left hip abduction command element of the
global joint union to the appropriate value as shown in the following line of code:
If (key_in == 0x48) LHA_O.cmd = 7;
8-9
At the end of the loop, the communications subroutine is called to download the
global joint union to the joint controllers. Recognising the command, the left hip
abduction joint controller will enable an open loop control signal to the proportional
valve amplifier for the particular joint.
The calibration function operates in a similar fashion for all joints other than those of
the foot assembly where the calibration process must be coordinated to avoid damage.
Once the foot calibration command is recognised, the routine initiates calibration of the
toe, prohibiting movement of all other foot actuators. When the toe joint controller has
reported successful calibration, the MCP calibration routine sets the toe calibration flag
allowing the foot rotation controller to commence calibration. Finally, once a successful
foot rotation calibration has been reported, the MCP calibration routine sets the foot
rotation calibration flag and allows the calibration of the foot extension shaft encoder.
This foot calibration process can be seen in a photographic sequence in the chapter on
testing.
8.1.4 LOCOMOTION ROUTINE
As the sequence shown in Flowchart 8.13 (later in this chapter) illustrates, the motion
control routine is the final software to be run in the boot sequence. The Locomotion
Routine is the central module of the Main Control Program. It contains all of the
behaviours that allow the robot to balance and move. All of the behaviours act by
modifying values within the Global Joint Structure which is transmitted to the
Communications Computer for decoding and transmission to the Joint Control
Microprocessors. As several behaviours may be active at any time, the resulting Joint
Structure values may be a combination of several independent calculations.
Flowchart 8.4 outlines the software of the Motion Control Routine (MCR). More detail
of the key routines will be given in the commentary below and in the subsections found
at the end of the flow chart commentary. The item numbers of the commentary relate to
the yellow line numbers on the flowchart. The sequence for the MCR is as follows:
1. & 2. On opening the MCR sends a command to the Joint Controllers1 to place
them in PID motion control mode. The Joint Controllers will then constantly look
1 The commands are actually written to the global joint matrix, converted to 7 bit data and then written to the RAN
drive of the MCC for reading and transmission to the local joint controllers by the communications software running
on the communications computer.
8 - 10
Flowchart 8.4(a)
8 - 11
for updated demand position data and will control the joint to achieve that
position.
3. The software calculates the real time of a bios tick for computation of derived
variables.
4. At the start of each loop, the routine looks for commands issued from the
keyboard. If there is no input the routine continues with whatever behaviours that
have been enabled, makes changes to the GJM and transmits this information to
the Joint Controllers.
5. The routine allows changes to be made to the stored stance positions if the joint
adjustment flag is set. A flag is used so that stance positions are not
inadvertently modified by incorrect keyboard input. This function is useful
during trials where it may be desirable to alter the joint positions in order to
calibrate encoders, strain gauges or to ensure the robot is perfectly symmetrical
prior to motion.
6. As for the calibration routine, the keyboard map is used to initiate joint adjustment
commands once the flag is enabled.
7. In the event that the strain gauges indicate that equal weight is not being taken by
each leg, the robot can be adjusted so that the readings are similar. The keyboard
is used to gradually increment or decrement the hip abduction positions so that the
centre of gravity of the robot sits directly between the two feet. After the
adjustment, setting of reset flag resets the zero hip positions to the adjusted
positions.
8. During trials, if the data logging flag is set, joint and instrument data will be
written to a data file on at the completion of each loop of the motion routine.
9. The compass and other instrument data is read on each iteration of the main loop.
10. Once the motion routine has made adjustments to demand positions and to
command values within the Global Joint Matrix, the matrix is converted to 7-bit
data for transmission to the communications computer by calling the subroutine
to_HC11().
11. Once converted, the data file is written to the RAM drive of the Main Control
Computer by the subroutine send_output().
12. Once the outgoing data file has been sent, the motion routine then looks for the
incoming data to be written onto the control computers RAM drive. Once found
it is read into memory and deleted by the subroutine get_output().
8 - 12
Flowchart 8.4(b)
8 - 13
13. The subroutine from_HC11() converts the 7 Bit numbers and combines them into
the variables of the updated GJM. As well, it audits the checksum values of the
data stream to ensure the data is valid.
14. Historical data is required to calculate time dependent variables such as
velocity, acceleration and the slew rates of measured data. Historical data is held
in a number of vectors and is updated on each iteration of the motion
control loop.
15. Median values of a range of data are use to calculate variables based on strain
gauge and other sensory input. As previously discussed median values are used
to eliminate transients in the data stream.
16. based on the calibrations of the strain gauges (see Chapter 9), the ground
reaction force on each leg is calculated as follows; mass_left = (float) (
analog_med[1] * 1.6 ); if (mass_left < 1 ) mass_left = 1; where analog_med[1] is
the median of the data from strain gauge 1. In the event that the data is negative,
it is set to zero.
17. Front trim is the measure of the weight distribution between the feet. The value
is 1 when all weight is on the left foot and 0 when on the right. As can be seen in
the list of special names it is calculated as the weight on the left foot as a fraction
of the sum of the weight on the left foot and right foot.
18. If the sway code is enabled, the motion routine calculates the joint positions
required to build up frontal sway in the robot.
19. If the reset flag is set by input from the keyboard, the balance point or centre of
frontal sway will be set to the current position.2
20. Based on the period of frontal sway, the phase of sway is calculated given the
time elapsed from the commencement of sway.
21. Once the phase has been determined the joint offsets are calculated and added to
the initial positions to calculate demand joint positions.
22. To enable the robot to walk, the swing leg must be lifted from the ground. The
height that the leg is lifted from the ground is calculated as a function of the
frontal sway position. If foot lift is enabled, the software calculates the required
length between the hip and the bottom of the foot which, effectively, is the
modified length of the leg.
23. If leg lifting is enabled the software branched to the code that computes the
2 At the time of writing, a number of code segments are included in the robots software in order to develop and test
some of the vehicles functionality. Most of these segments are flag enables so that they are bypassed during normal
execution. Once satisfied with the operation of the code and the robot, these segments will be deleted.
8 - 14
Flowchart 8.4(c)
8 - 15
required joint trajectories.

24. Once the leg lift height has been determined, the inverse kinematic equations
found in Chapter 7 are used to calculate the joint angles to achieve the desired leg
lift. The angles are converted to demand joint positions which are input to the
GJM.
25. On each iteration of the motion control loop, the statuses of the foot limit
switches are read. The positions are then sent to the foot rotation and foot
extension joint controllers.
27. Longitudinal trim is calculated for each foot. Like the frontal trim, the
longitudinal trim is a measure of the distribution of the ground reaction force
along each foot. The trim is calculated from a combination of the data produced
by the two sets of strain gauges located on each ankle assembly.
28. If input from the keyboard, a flag is set so that the foot extension joint controller
will attempt to automatically evenly distribute the ground reaction force along the
foot.
29. The foot extension joint controller adds an offset to the demand position sent by
the MCP so that, regardless of demand position, reaction force is distributed
around a given point along the foot. If the robot senses that it is overbalancing
backwards, the controller will lift the foot so that the reaction moves toward the
heel to restore balance. In the event that the robot wants to lean forward to
commence locomotion, the balance point is also moved toward the back of the
foot.
30. The trim rate is calculated as the time derivative of frontal trim. It is used in the
PD control loop that maintains the robots frontal balance.
31. If sagittal balance is enabled, the software branches into the frontal balancing
code.
32. The frontal balancing code is covered in more detail in later sections of this
chapter. The code attempts to maintain even distribution of weight between the
feet by adjustment of the hip abduction positions. Due to compliance in the
structure of the robot, it is possible for actual adjustment of the hip abduction
values to register little or no change in frontal trim. The software uses the trim
rate (previously calculated) to detect a constant change in the trim and to use it
as the derivative term in the control algorithm.
33. If the lateral compensation flag is set, the code allows lateral compensation of
the feet.
8 - 16
Flowchart 8.4(d) Motion control routine

8 - 17
Figure 8.3 Motion control routine display
34. If lateral foot compensation is enabled, the command is sent to the foot
extension and foot rotation joint controllers.
36. Checks for data logging flag.
37. If data logging is enabled, the software writes all joint positions, both demand
and actual, all measured, data and some calculated data to a data file on the
Control Computers RAM drive. The data is used to analyse the previous test and
for debugging purposes.
38. The on-screen display of the motion control routine is updated every n counts,
where n is a number determined by the operator. It has been found that writing
data to the screen is a time consuming process. As the software loops at
approximately 8 times per second, the screen is usually updated every 8 loops of
the motion control routine. The subroutine screens() writes the demand and
actual joint positions to the screen (see Figure 8.3).
39. The motion control routine writes measured and calculated data to the screen
including frontal and longitudinal trim and vestibular data.
40. If the exit command has not been issued the routine loops for the next iteration.
41. If the exit command has been issued, the motion control routine relinquishes
control of the robot back to the Main Control Program.
8 - 18
As can be seen, the software for the robot is complex and detailed. Should the same
extent of software have been written for some military system, hundreds of pages of
documentation would be provided with the package. The flowcharts and commentary
provided give an overview of the control software. In the next sections the software
critical for the robots balance will be explained in more detail.
8.1.4.1 FRONTAL BALANCE
Frontal balance is an essential behaviour of the robot as it allows the robot to adjust its
posture in the frontal plane so that equal weight can be taken on each leg. As well, it is
the major stepping stone to the development of frontal sway which allows the robot to
initiate locomotion. As the diagram of Figure 8.4 shows, frontal balance is controlled by
abduction of the hips. The demand positions of the hip abduction actuators are stored in
the Global Joint Matrix and are declared as:
RHA_O.position - Right Hip Abduction Demand Position
LHA_O.position - Left Hip Abduction Demand Position
The value of these variables is determined by the Main Control Software and is sent to
the Left Hip Abduction controller and the Right Hip Abduction controller on each
iteration of the motion control routine. The software uses the variables LHA_CAL and
RHA_CAL as the hip abduction joint positions where the load on each foot is equal. The
Figure 8.4 Weight transfer via hip abduction

8 - 19
value of these variables is determined either by a previous successful balancing trial or

by manual incrementing of the hip abduction cylinders until the front trim is measured at
0.5. Frontal trim is calculated by the following sections of code:
mass_left = (float) ( analog_med[1] * 1.6 );
mass_right = (float) ( analog_med[0] * 1.6 );
front_trim = (mass_right / (mass_left + mass_right));
where: analog_med[1] is the conditioned output from analogue output 2

analog_med[0] is the conditioned output from analogue output 1
After the front trim has been computed the rate of change of front trim is calculated by
the difference between the current and previous value divided by the time between
samples:
ft_old = ft_new;
ft_new = front_trim;
ft_rate = 100 * (ft_new - ft_old) / (float) bios_ticks;
The gain of the frontal balance control software is determined by two sets of control
limits (Figure 8.5) which are calculated at the start of the motion control software by the
following code:
up_lat_lim_1 = LAT_POINT + LAT_ZONE;
low_lat_lim_1 = LAT_POINT - LAT_ZONE;
up_lat_lim_2 = LAT_POINT + 3 * LAT_ZONE;
low_lat_lim_2 = LAT_POINT - 3 * LAT_ZONE;
The variable LAT_ZONE determines the positions of the balance limits effectively
modifying the gain of the balance control. The actual gain is set by the value of
lat_bal_inc which is the value of the compensation increase per iteration.
Depending on the value of frontal trim in relationship to these limits, the hip abduction
positions are modified by the addition of a compensation value labelled as X in Figure
8.5. The following code controls the level of hip abduction compensation to be added to
the hip abduction positions:
if ( ( SAG_ENABLE == 4) & (ft_rate < 75 ) )
{
if ( front_trim > up_lat_lim_2 ) lat_bal_float = lat_bal_float - 2 * lat_bal_inc;
if ( front_trim < low_lat_lim_1 ) lat_bal_float = lat_bal_float +1 * lat_bal_inc;
if (front_trim < low_lat_lim_2 ) lat_bal_float = lat_bal_float + 2 * lat_bal_inc;
}
8 - 20
Figure 8.5 Frontal trim limits
Effectively a proportional controller, the hip position is adjusted dependent on the

frontal trim error. The above code is only enabled if the sagittal balance flag is set and
the rate of change of frontal trim is less than a set value. The second condition limits
overshoot by maintaining the current compensation value once the body of the robot is
in motion acting as a derivative input. Finally, the output hip abduction positions are
calculated as the addition of the calibrated position and the integer conversion of the
compensation value:
lat_bal_comp = (int) (lat_bal_float);
RHA_O.position = RHA_CAL + lat_bal_comp + ab_adj;
LHA_O.position = LHA_CAL - lat_bal_comp - ab_adj;
The variable ab_adj is computed by another behaviour which will be discussed in the
following section.
8.1.4.2 FRONTAL SWAY
In bipedal systems it is essential to transfer weight from one leg to the other in order to
lift a foot from the ground. During bipedal locomotion, this becomes a continuous
process. Frontal sway consists of a sinusoidal displacement of the centre of gravity of the
body in the frontal plane. The period and amplitude of the displacement are a function of
the forward velocity of body.
Initially, the frontal sway routine uses the computers bios clock to calculate the time
elapsed from the initiation of frontal sway, t_run.
t_now = biostime(0, 0L);
t_run = t_now - t_zero;
8 - 21
The software then computes the fraction of the gait cycle represented by the current
period and then the phase of the cycle:
f_t_run = (float) (t_run);
f_period = (float) (period);
frac = (f_t_run/f_period);
phase = frac * 6.28319;
Once the phase has been calculated, the hip abduction offset for that stage of the sway
cycle can be calculated.
d_offset = (sway_amp * sin(phase));
f_lift = (lift_amp * sin(phase));
ab_adj = (int) (d_offset);
Where:
sway_amp is the amplitude of the frontal sway,

d_offset is the real value of the hip abduction offset,
f_lift is the height of the foot lift
lift_amp is the amplitude of the foot lift cycle.
ab_adj is the value to be added to the hip abduction position.
Foot lift is a separate behaviour, again flag enabled, that lifts the feet of the robot as it
sways from side to side, effectively marching on the spot. Once the degree of foot lift has
been determined the following code calculates the hip, knee and foot joint positions from
the geometry shown in Figure 8.6. First, the absolute length of each leg is calculated
Figure 8.6
Leg geometry
8 - 22
given the current hip height and the height of foot lift calculated above:
abs_dist_right = sqrt( right_hip_height * right_hip_height + right_hip_displ
* right_hip_displ );
abs_dist_left = sqrt( left_hip_height * left_hip_height + left_hip_displ
* left_hip_displ );
Using the sign rule, the knee angle is then calculated:

theta_right = rad_deg * ( acos( ( (upper_leg * upper_leg) + (lower_leg * lower_leg) (abs_dist_right * abs_dist_right) ) / ( 2 * upper_leg * lower_leg) ));
theta_left = rad_deg * ( acos( ( (upper_leg * upper_leg) + (lower_leg * lower_leg) (abs_dist_left * abs_dist_left) ) / ( 2 * upper_leg * lower_leg) ));
Intermediary angles are then calculated:

gamma_right = rad_deg * atan( right_hip_displ / right_hip_height);
gamma_left = rad_deg * atan( left_hip_displ / left_hip_height);
epsilon_right = rad_deg * ( acos( ( (lower_leg * lower_leg) - (upper_leg * upper_leg) (abs_dist_right * abs_dist_right) ) / ( -2 * upper_leg * abs_dist_right) ));
epsilon_left = rad_deg * ( acos( ( (lower_leg * lower_leg) - (upper_leg * upper_leg) (abs_dist_left * abs_dist_left) ) / ( -2 * upper_leg * abs_dist_left) ));
With intermediary angles calculated the action joint angles are calculated:
right_hip_ext = 90.0 - gamma_right - epsilon_right;
left_hip_ext = 90.0 - gamma_left - epsilon_left;
right_foot_ext = theta_right - right_hip_ext;
left_foot_ext = theta_left - left_hip_ext;
The angles are then converted to encoder positions:

RHE_O.position = (int) ((right_hip_ext - 52.5) * 12.3);
LHE_O.position = (int) ((left_hip_ext - 55.5) * 12.3);
RKE_O.position = (int) ((beta_right - 122.5) * 5.69);
LKE_O.position = (int) ((beta_left - 126.5) * 5.69);
RFO_O.position = (int) ((right_foot_ext - 70.0) * 3.0);
LFO_O.position = (int) ((left_foot_ext - 70.0) * 3.0);
Finally the encoder positions are stored in the global joint position matrix for transfer
to the joint control microprocessors. The full listing of the main control software can be
found in Appendix 5.
8 - 23
8.2 COMMUNICATIONS PROGRAM

As discussed in previous sections, the communications program runs on the
communications computer controlling the distribution of data throughout the robots
control system. The following section details the operation of the software via the use of
flowcharts and accompanying commentary. The numbers of the commentary coincide
with the numbers on the flowcharts. The flow of the software is as follows:
1. On entry, the software initiates the local system variables and arrays as well as
resetting and preparing the 16 channel RS232 interface module for
communication. The software ensures that all of the 16 ports are configured for
the correct baud rate, handshaking and parity before testing each channel.
2. Joint control software is down loaded using the Motorola BUFFALO monitor
which then requires the instruction g2000 to start the software. The artificial
horizon software is stored in EEPROM on the M68HC11 EVBU requiring a jump
instruction to the vector b60e to run. These commands are issued by the
communication software after initialisation.
3. Only two keyboard commands are recognised by the software. The ESC key
causes the program to exit while the + key toggles file logging of data.
4. The log file is only used as a debug utility which records the time and data of each
data stream.
5. The Escape key exits the program.
6. At the start of the main loop of the program, the software checks for the latest data
file from the Control Computer.
7. If the file is available, the software calls the necessary routines to read the file and
to separate the data and commands for each of the RS232 channels. These
routines will be detailed in later sections.
8. If a data file is unavailable, the software uses the time between the current and
subsequent reads to read data from the artificial horizon and compass. If required,
it also transmits commands to the artificial horizon, or sends data to the character
display on the front of the microprocessor enclosure.
9. Once the read loop has been completed, the software updates the screen display
which constantly indicates the total number of successful packets written to and
received from each of the microprocessors.
8 - 24
Flowchart 8.5 Main

communications program
8 - 25
Figure 8.7 Communications program display
As can be seen, the main function of the communications program is to pass data
throughout the control system. Two main routines are used to achieve this The first
gathers information from the M68HC11s and then transmits it to the main control
program. The second routine receives data from the main control program and then
transmits it to the M68HC11s. These two routines are covered in the following sections.
8.2.1 RECEIVING DATA FROM THE MICROPROCESSORS
Flowchart 8.6 illustrates the structure of the routine Rece_and_Write() which receives
data from the Joint Control and Data Acquisition microprocessors before transmission to
the control computer. The routine is called by the communications program once the
input file from the main control computer has been sent to the M68HC11s. On receipt of
new data from the main control computer, the M68HC11s immediately reply with
current data. Rece_and_Write() monitors the data streams from the M68HC11s, compiles
them and transmits them to the main control computer. The following commentary
explains the process of the subroutine. The comment numbers reference the numbers on
the flowchart:
1. Out the outset, local variables and arrays are initialised. The initialisation of the
P747 expansion module drivers is not covered in this commentary. The manual for
the module can be found on the Adaptec website (www.adaptec.com) and carries
an excellent explanation of the process.
8 - 26
Flowchart 8.6 Receive and write routine
2. After initialisation, the software moves into the main loop of the program which
remains operable while data is available from the M68HC11s and while the
number of failed attempts to read data from the M68HC11s is less than a
predetermined value. The software exits this loop when a successful read has been
completed or it times out when one or more of the M68HC11s has failed to
respond.
8 - 27
3. Within the main loop, an additional loop steps through each of the RS232
channels. While the data can be received into the P747s input buffer from each
of the sixteen channels simultaneously, it must be read from the buffer one
character at a time.
4. Prior to the scan through each of the channels, the character counter is set to zero.
This counter keeps the position of the read character in the data stream.
5. The software uses driver functions to determine if a character is available in the
data stream. In the event that a character is unavailable, the software increments
the retry counter (9).
6. If a character is available, it is read into the input buffer using the current
channel and character count as indices.
7. Where the read character coincides with the return character (indicating the last
character of the input stream) the read finish bit for that particular channel is set
(8) in the read_finish byte. When all bits of the byte are set, the software exits the
read loop and then returns to the main program.
Once all of the M68HC11 data streams have been read into the input buffer, the data is
then written to the RAM drive of the MCP. The process described above completes the
transfer of data from the joint control and data acquisition microprocessors to the MCP.
8.2.2 SENDING DATA
The reverse process takes the file written by the MCP and then transmits the data it
contains to the local joint and data acquisition controllers. The routine readdata() looks
for the latest input file, written by the main control program, on the main control
computers RAM drive. Once found, readdata() reads the file and then distributes the data
and commands to the local joint controllers. The structure of the software is outlined in
Flowchart 8.7. The following commentary relates to the numbers on the flowchart.
The routine is called immediately after the main control software has written the
current joint controller data to the main control computers RAM drive.
1. Local variables including the filename of the expected input file are initialised.
2. The routine continually looks for the expected file, incrementing a loop counter
on each unsuccessful attempt to read the file.
3. If the loop count exceeds a predetermined limit, the routine exits with an error
report to the main communications program.
8 - 28
Flowchart 8.7 Structure

for readdata() routine.
4. If a new file is found it is checked to determine whether it is a valid data file.

5. If the file is valid, it is immediately renamed so that there is no collision with the
next input file to be written by the main control program. Once renamed, the file
is read into a buffer, which holds the complete data stream, and then deleted.
6. A loop is then commenced to step through each of the sixteen output channels.
8 - 29
7. For each of the channels, the software searches the data in the input buffer to
detect the header string of the data for that channel.
8. If the header is not found, the software loops to the next channel.
9. When the header is found the next seven bytes of data are copied to the output
buffer for that channel.
10. Once all of the input channels have been read, the contents of the output buffers
are sent to the P747 expansion modules for transmission to the local joint
controllers.
11. Once sent, the routine returns the number of successful channels read and sent
to the 68HC11s.
This concludes the explanation of the communications software. The full listing of the
code can be found in Appendix 6.
8.3 LOCAL JOINT CONTROL SOFTWARE

The final major software group consists of the programs running on the Motorola
MC69HC11 joint controllers. The main tasks of the software and the functions which
achieve these tasks are shown in Table 8.2.
Essentially, the software operates in one of the main loops while certain parameters
remain active. When data is received from the Communications Computer the interrupt
service routine receives the data and then makes changes to the global data structure. The
currently active routine then resumes its process acting on the new data. In the event that
parameters have changed that cause it to exit, it then returns control to the main loop.
Each of these main functions will be detailed in the following sections.
8.3.1 MAIN CONTROL LOOP
The structure of the main control loop is shown in Flowchart 8.8. As with the main
control loop of the Main Control Program which runs on the MCP, the main control loop
of the joint control software hands control to the appropriate behaviour as commanded
Control Task
Subroutine / Function
Program control
Main control loop
Communication with communications PC
Interrupt service routine
Calibration of joints
Calibration routine
Movement of joints
Motion routine
Table 8.2 Joint controller task / function map

8 - 30
by the main control program. The flow of the software is described in the following
commentary, the bullet numbers correspond to those on the flowchart:
1. Within the header file (see Appendix 7) and in the declarations at the start of the
source code, the data structure and global variables are declared and initialised.
2. As a safety measure, prior to any control commands being processed, the enable
bit of the digital to analogue output is disabled so that there is no possibility of
joint movement prior to calibration. As well, the millisecond counter and
historical position vectors are reset to zero.
Flowchart 8.8
Local joint
controller main
control loop
8 - 31
3. After initialisation, the program enters the main control loop which is only exited
either by a hard reset or by a power cycle. Control is passed to one of the
subroutines when the value of the command variable corresponds to that of one
of the joints behaviours.
4. The stop function immediately disables the digital to analogue converter output
and then sets the control signal to zero. This function is only used during testing
where a major error has been detected.
5. The calibration routine determines the minimum control values for the joint and
then homes the joint. It is discussed in detail in the next section of this chapter.
6. The stance routine controls the robots joint to a predetermined position, prior to
the robot being lowered to the floor. Once lowered the routine controls the
position of the joint as commanded by the Stance routine of the robots main
control software.
7. The motion function controls the movement of the joint as commanded by the
robots motion software.
8. The Crash function is called by the main control program when it is detected that
the robot is no longer able to balance and is expected to fall. The software
attempts to move the robots joints to positions that may minimise the resulting
damage.
It can be seen that the main control loop contains no communication routines. The only
variable operated on in the main loop is the CMD variable which is updated by the
interrupt service routine when new data is received from the communications program
running on the communications computer. It is not necessary, therefore, for the main
control loop to check for new data from within the loop. An example of the main
program for the left foot extension cylinder can be found in Appendix 7.
8.3.2 INTERRUPT SERVICE ROUTINE
The interrupt service routine is responsible for the communication between the
communications computer and the local joint control software. The use of an interrupt
routine eliminates the requirement for other routines to poll for new data while
processing other tasks. The structure of the interrupt service routine is found in Flowchart
8.9. The header file containing the interrupt routine was initially developed by Chuck
McManis for the F1 controller board. By using define statements in the main F1
controller program, sections of the header will be included in the main file when the
software is compiled. The header file was then modified during this project to include the
8 - 32
communications requirements described below. The following commentary explains the

flow of the routine:
1. The interrupt routine first checks that a valid character has been read by
inspecting the RS232 port of the M68HC11.
2. If a character is available, it is read into the input buffer at the position indicated
by the pointer inptr. It then increments the pointer for the next character to be read.
3. The character is then checked to determine whether it is the end of line character
0x0d which would indicate that the entire message has been received.
4. If the whole message has been read, the first five bytes are ANDed to compute the
checksum for the string. If the whole message has not been received, the routine
exists until the next character is received.
5. The checksum is then compared to the 6th byte which contains the original
checksum computed by the communications software running on the
communications computer. If the checksums do not match, the software exits,
indicating the error by making the input command equal to 101.
6. If the checksums match, the transmitted data is decoded from the input stream.
For example, the demand input velocity is calculated as:
VEL_IN = (inbuffer[4]&3) * 100 + (inbuffer[5] & 0x7f);
if (( inbuffer[4] & 4) != 0 ) VEL_IN = VEL_IN * -1;
Once these lines of code have been processed, the value of the demand velocity
held in the local joint control data structure is updated to the most recent value.
7. Once the input data has been received and read, the outgoing data is converted to
7 bit bytes in the reverse process of step 6. For example the current velocity is
converted to the output format by the following code:
if (VEL_OUT < 0 )
{
sign = -1;
VEL_OUT = VEL_OUT * -1;
};
VEL1 = VEL_OUT / 100;
VEL2 = VEL_OUT - VEL1 * 100;
OUTPUT[4] = ((STAT_OUT & 0xf)<<3) | ( VEL1 & 3 ) | 0x80;
OUTPUT[5] = VEL2 | 0x80;
8. Once the output buffer has been assembled, the first five bytes are ANDed to
compute the checksum which becomes the sixth byte.
9. The buffer is then written to the RS232 port for transmission to the
communications computer.
8 - 33
Flowchart 8.9. Local joint controller

interrupt service routine
8 - 34
After completing the transmission, the interrupt routine exits, returning control to the
routine that was interrupted. The source code for the routine can be found in Appendix 15.
8.3.3 CALIBRATION ROUTINE
The calibration routine serves two main functions; first, as discussed in previous
sections, the software homes the joints of the robot. The shaft encoder / quadrature
counter combinations are digital devices which can only measure and record
incremental changes to the joints position. On power up, no memory of the joints
position exists, nor is the shaft encoder able to determine the current position. The
homing function moves the joint to the minimum position and then sets the shaft encoder
to zero after which time the shaft encoder and the joint position are synchronised. The
second function of the calibration software is to calibrate the valve control signal. For
each valve, in each direction of spool movement, there exists a minimum control value
i.e. voltage converted to current by the valve amplifiers, at which the spool will move
sufficiently to allow oil flow. This value will vary depending on the temperature of the
valve, the temperature of the hydraulic oil and how recently the valve has been moved.
Figure 8.8 shows the characteristics of the Rexroth proportional valves. As the control
signal is generated by a 128 bit digital to analogue converter, of which 30 bits are taken
up by the dead band of the valve, only 50 bits of control are available in each direction.
Therefore the initial value for valve movement may vary by up to four percent. This has
proved to be one of the most difficult challenges of the hydraulic valve control.
Flowchart 8.10 details the structure of the joint calibration software. The commentary
on the right of the flowchart details the processes within the calibration software.The
Figure 8.8 proportional

valve characteristics
8 - 35
HC11 Calibration Subroutine

Initialise
Set control output to zero

before disabling it. Zero the
encoder count.
Set Output to Zero

Enable Output
Set Encoder to Zero
Has the minimum up control

value been determined ?
Min_Up = 0 ?
Yes
No
While up_rate < 3
&&
Count < 20
&&
Calibrate is enabled
While the velocity is less than

3, and the position has not
changed by 20, increase the
control value.
No
Yes
Rate < 3
Increase Output
Communicate
If the joint moves more than

20 counts without achieveing
minimum velocity stop
calibration
Disable Output
Return Error
Stop Mode
Rate < 3 ?
Yes
Once the joint has achieved

minimum velocityrecord the
control value and disable
the output.
No
No
Min_Up = Output
Output = 0
Has the minimum down control

value been determined ?
Min_Dwn = 0 ?
Yes
While the velocity is less than

3, and the position has not
changed by 20, decrease the
control value.
No
While Dwn_rate < 3
&&
Count < 20
&&
Calibrate is enabled
Yes
No
Rate < 3
Check for new data from the

control PC in the middle of
every loop.
Decrease Output
Communicate
Disable Output
Return Error
Stop Mode
No
Rate < 3 ?
Once the joint has achieved

minimum velocityrecord the
control value and disable
the output.
Yes
Min_Up = Output
Output = 0
Flowchart 8.10(a)
Local joint controller
calibration software
8 - 36
Flowchart 8.10(b)
Local joint controller
calibration software
8 - 37
joint control calibration software determines the values of V_Negative_Min and

V_Positive_Min by gradually increasing the control signal to the valve in each direction
until it detects constant flow. The previous value (i.e.. The last value at which no
movement was detected) is then used as the zero control value for the spool in the given
direction. Again, due to the limited number of bits available for control, the actual degree
of valve movement for these values may vary dramatically. Due to hysteresis, friction in
the valve, the amount of oil present in the spool cylinder, etc., the value determined for
V_Positive_Min may have produced motion during initialisation, but did not produce
motion during operation. This is not seen as problematic as this value is never actually
used for valve movement.
8.3.4 MOTION CONTROL ROUTINE
The final function of the Local Joint Control Software that will be discussed in detail
is motion control. While the most critical in terms of controlling the robot, this software
is one of the more trivial in terms of complexity of code. Initially, as the joint geometry
lead to non linear dynamic equations, it was envisioned that fuzzy logic could be used to
control the joint. Experimental software, discussed later in this chapter, demonstrated that
the memory available on the joint microprocessors was not sufficient to code a fuzzy
logic control routine. The testing demonstrated that the fuzzy logic control achieved a
marginally higher level of position control than a PID controller. However, it was
decided that the overhead in terms of memory did not warrant the increase level in
control.
Flowchart 8.11 shows the structure of the position control routine. Again, it can be seen
that there is no requirement for communications from within the main loop of the
software. Data transmission to and from the main control software is processed in the
background by the interrupt service routine. Once in the main loop of the routine, the
software simply reads the current position and time, calculates the velocity and then
computes the error between the demand position and the current position. Once the error
has been established, the control output may either be calculated by the application of a
proportional constant, or by discrete intervals as shown in this code segment.
pos_e = ( posn[3] - pos_t );
if ( pos_e < -1 ) contr = min_dn - 4;
if ( pos_e > 1 ) contr = min_up + 4;
8 - 38
Flowchart 8.11
Position control
routine

The reason for the use of discrete interval control is to eliminate hunting at the target
position. The robot presents a large dynamic structure. Any movement of one joint will
result in dynamic forces being transferred to all other joints in the structure. If the gain
of the joint controllers is overly large, any action in one joint will tend to produce a
reaction in other joints. This can result in highly unpredictable unstable behaviour in the
device. The above code allows the gain to be selected based on the position error. The
values used have been determined as a result of the trials that have been conducted with
the robot. Fortunately, testing would suggest that these values work effectively to control
joints, while minimising unstable oscillations. Dynamic analysis may assist with the
determination of constants for the equations above. However, it is expected that
unknowns in the analysis such as non linear compliance or slack in the joints or
actuators, would lead to errors in the results greater than those which have been achieved
through testing and experience.
8 - 39
8.4 SUMMARY OF CONTROL

SOFTWARE IMPLEMENTATION
It can be seen that the overall control problem is complex, but has been broken down
into smaller tasks in an effort to make processing more efficient and to make the coding
of the software more manageable. The operation of the control software has been
explained in detail in the previous pages of this chapter. The overview of the control
system was given in Chapter 5. The resulting combination of hardware and software has
lead to an operational control system that has been tested successfully. The path through
the control system from Main Control Computer to joint movement has been detailed
over several chapters. Flowchart 8.12 illustrates the path in block form. It shows the
movement of data and control signals through the control systems.
8.5 SOFTWARE DEVELOPMENT AND TESTING

While testing of the robot will be discussed in subsequent chapters, testing of the
software was an integral function of the software development process. Prior to the
coding of the main software modules, it was necessary to confirm the functionality of
hardware and communications control software. Table 8.3 outlines the order of testing
and the functionality that was confirmed as the software was developed.
Not only was it essential to confirm the operation of building blocks for the main
software, it was important to build up a knowledge base of the capability of the hardware
and software libraries. While the writer was completing the mechanical design of the
robot, mainly in the afternoon and night, trials of hardware and software were
completed during the day. Early in the investigation phase it was realised that a test bed
for the software and hardware was required. To test the functionality of the F1 Board /
I/O Board combination in as close to a representative situation as possible, a small
wheeled robot was constructed.
The wheeled robot was fitted with the same M68HC11 F1 Board that is used to control
the joints of the robot. Connected to the F1 Board are two of the quadrature counter I/O
Boards used to measure joint angles on the robot. Two geared motors and reduction
transmissions are used to drive the rear wheels of the robot independently. The motors
are powered by two 12V DC motor controllers which use the analogue output of the I/O
board as the control input. A digital shaft encoder is attached to the drive shaft of each
of the back wheels providing position feedback to the microcontroller. A third motor
8 - 40
Flowchart 8.12 Flowchart showing path for joint movement.

8 - 41
Hardware/Software
Functionality
M68HC11 F1 Board
Setup of Imagecraft C cross compiler.

RS232 Interrupt function
Ability to read shaft encoder count.
Control of analogue output.
Reading of analogue Inputs.
Reading & writing of digital I/O.
I/O Board
Reading of shaft encoder count.

Speed of shaft encoder counting.
Output of analogue signal.
Mapping of I/O to HC11 address map.
PCLS-802
Ability to be linked by Borland C
RS232 Comms
Operation of drivers under MDOS 6.22 environment.
C Library
Use of library functions to download S19 code to HC11s
PCL-747 RS232
Operation of drivers under MSDOS 6.22 environment.
Expansion Board
Ability to link driver library with Borland C 5.0

Operation with Pentium II computer
Compatibility of RS232 interface with f1 Controller.
Packet Protocol
Development of packet protocol for communication

between communications PC and Microcontrollers.
Ethernet Network &
Operation of network cards under MSDOS 6.22
Network Software
Operation of Network Software under MSDOS 6.22

Ability to map RAM drive of Control PC to
communications PC.
Table 8.3 Testing of hardware/software functionality
drives a scanning sonar with potentiometer position feedback and analogue target
feedback to provide two channels of analogue input to simulate the instrumentation on
the biped robot. Limit switches mounted on the front bumper of the wheeled robot
simulate the various digital inputs found on the biped robot. As a package, the wheeled
robot found in Figure 8.9 has proven to be a valid and reliable test bed, and is
significantly more portable than a 500kg biped robot.
The first software written sent the classic phrase Hello world! from the M68HC11
back to the PC where the software was compiled. This confirmed the setup of the
8 - 42
Figure 8.9 Small wheeled test robot
Imagecraft cross compiler. A routine was then written to read the shaft encoders,
analogue inputs and digital inputs. The data was converted to text by the micro controller
and sent as an ASCII character stream back to the PC where the values were
continuously displayed on the screen. By manually rotating the wheels of the robot the
operation of the shaft encoders and the F1 board / I/O board interface was confirmed. It
was immediately learned that the shaft encoders mounted on opposite sides of the robot
read in different directions. The solution to this problem was to swap the Channel A and
Channel B pulse train outputs on one shaft encoder so that they read in the same
direction. This strategy was used on the robot where all of the encoders on the right hand
side of the robot have had their outputs crossed over. The analogue converters on the F1
board were tested by applying known voltages to the inputs. The digital inputs were
tested by operating the limit switches mounted on the bumper of the robot.
To test the analogue outputs, a control program was written to match the rotation of one
wheel to the other. By rotating the uncontrolled wheel manually, the functionality of the
software was confirmed as the controlled wheel rotated to match its position. The output
of these trials was recorded by the capture function of the Imagecraft controller. The
output was then graphed using a spreadsheet (see Figure 8.10) so that the error between
demand and actual position could be investigated. As can be seen the system was able to
monitor and control to velocities of twice what could be expected of the joints of the
8 - 43
Wheeled Robot Error Trials

10
250
200
150
100
Error
Thousands
Shaft Count
0
50
- 5
-50
-10
-100
0
50
100
150
200
250
300
X-Axis
Right Wheel
Left Wheel
Error
Figure 8.10 Output data from wheeled robot
biped robot. With a simple PID Control loop, the maximum error between the two wheels
was 180 counts equivalent to 15.5 degrees which occurred when the open loop wheel
changed direction. The software for this trial can be found in Appendix 8.
This process was then automated by having the software rotate one wheel with an open
loop output while the other wheel used close loop control to match its position. Figure
8.11 shows the error and control signal outputs from the automated trial where it can be
seen that the control signal saturates at 255 bits, an 8 bit analogue output. In the case of
the wheeled robot, as the open loop and controlled motor are identical, there is a limit to
the velocity differential that can be achieved. This factor reduces the speed at which the
error can be reduced. In the case of the biped robot, the maximum flow through the
proportional valve is the limiting factor to the speed at which position error can be
reduced.
As it was expected that the transfer function of the analogue output / valve amplifier /
proportional valve / joint combination would be non linear, it was intended to investigate
the use of fuzzy logic for the joint control software. Accordingly, a simple fuzzy logic
routine was written (see Appendix 9) to drive the controlled wheel. With only three
member sets of five grades of membership it was immediately obvious that the size of
the routine almost exhausted the memory available on the F1 controller board. What was
interesting was that the routine achieved a higher level of control than the PID controller
8 - 44
routine. Certainly, the steady state response of the system was greatly enhanced. The
output for the trial can be seen in Figure 8.12. The most significant output from the
trials was confirmation that all of the systems detailed in sections 1 & 2 of Table 8.3,
developed for control of the robots joints had been tested. Further, a greater
understanding of the hardware and software was developed.
To confirm the operation of the RS232 Expansion Port / F1 Controller interface,
software was written for the communication PC to send, receive and audit a continuous
data stream. The software used the PCL RS232 communications libraries supplied with
250
300
200
250
200
Y-Axis
100
50
150
Control Output
150
0
100
-50
-100
50
0
50
100
150
Control
200
250
300
Error
Figure 8.11 Error and control signals from PID control

10
250
200
150
100
50
Error
Position
Thousands
0
- 5
-50
-100
-10
-150
0
50
100
150
Right Wheel
Left Wheel
200
250
300
Error
Figure 8.12 Error and control signals of Fuzzy Control

8 - 45
the PCL-747 expansion module. The program PCRS_13.c (See Appendix 10)3 was
written to send a demand position to the F1 board on the wheeled robot. The position was
broken into packets and sent to the F1 board with a checksum as the last byte. The
program HCRS_13.c (See Appendix 11), running on the 68HC11, was written to receive
the packet, confirm the checksum and to return the actual position of a shaft encoder.
This exchange tested the functionality described in Section 3 and 4 of Table 8.3. Once
communications with one F1 Board was established, Software was written for the
Communications PC to communicate with multiple F1 Boards. By this stage of the
development and testing process, the functionality of the F1 Interface Board Prototype
had been confirmed allowing for the production of twenty production boards. The boards
were populated by the writer over many nights and were housed in the enclosure seen in
Chapter 5. The Communications Computer software was then upgraded to communicate
with all of the F1 board in the enclosure. The program MULTI_RS.C (see Appendix 12)
demonstrated that PCL-747 module and software allowed communications with all of the
HC11s in the same time as for a single HC11, confirming Section 5 of the functionality
detailed in Table 7.3.
The final functionality test was to install the Ethernet network on the Control PC and
the Communications PC. This was accomplished without incident. A RAM drive was set
up on the Control PC and mapped to the Communications PC. To test the speed of the
network, the MULTI_RS.C software was run on the Communications PC from the RAM
drive of the Control PC.
8.6 STARTING THE ROBOT

The second the engine of a forklift is started, the machine is ready for use. Unlike a
forklift, the operator of an industrial biped will not simply climb into the vehicle and turn
the key. The device will have to complete some kind of initialisation, including
calibration. The software on the biped robot described in this robot is held in volatile
memory. In a production version, the software would be kept in EEPROM so that
downloading on start up would not be required. Such a configuration would save
significant initialisation time. The gyroscope used would be of a solid state kind rather
3 Due to a crashed hard-drive, some of the initial communications testing software was lost. Though backed up on
3.5 disks using Norton backup, the files were unrecoverable as 2 of the 13 disks were corrupted. The software
included in Appendix 10 is functionally similar, but may not be the compileable versions. They are included to
demonstrate the development process rather than to witness the success of the trials. The fact that the robot can be
calibrated and balance proves that all of the communications software is functional.
8 - 46
than the air driven type used on this robot, again saving spin up time. With the software
controlling the robot detailed in the previous sections of this chapter, the order in which
the software is loaded is now explained.
The sequence taken to prepare the biped robot is outlined in Flowchart 8.13. The steps
of the sequence are as follows, the numbers matching those of the flowchart:
Flowchart 8.13 Boot and

calibration sequence
8 - 47
1. On power-up, the Main Control Computer is started, loading its operating system
and creating an accessible virtual drive in Random Access Memory. The
networking software is then loaded which recognises the drive and shares it on the
network. The communications drivers are then loaded so that the control
computer can download the S19 code to the air pump controller.
2. The Communications Computer is then started, loading the operating system and
the PCL communications drivers that control the PCL-747 RS232 expansion
module. When the network software loads, it maps the RAM drive of the Control
Computer to be accessed as Drive E:\.
3. The Joint Microcontrollers are powered up. All of them other than the Artificial
Horizon controller load the BUFFALO monitor.
4. The communications computer downloads the S19 code to the fourteen joint
control M68HC11s and to the analogue input M68HC11. The software then looks
for the done return from the microcontrollers and displays the number of each
successful load.
5. The MCP, is then run on the communications computer. The software starts the
downloaded programs in the joint microcontrollers and looks for a new command
file on the MCC RAM drive. The software continually displays the number
commands sent to each Joint Controller and the number of replies received.
6. The Main Control program is loaded on the Main Control Computer establishing
communications with the communications software. The software displays the
positions and current command modes of all of the Joint Controllers. The system
is now ready for input.
7. After the hydraulic pump is started, the command is input from the keyboard to
purge the air from the hydraulic system. In sequence, the hip joints move repeatedly to the full extent of travel so that any air trapped in the hip cylinders or
motors can be purged back to tank.
8. The system is now ready for calibration. Commands are entered from the
keyboard to calibrate all of the joints of the robot. Successful calibration can be
seen by the command response from the joint controller seen on the on screen
display of the Main Control Computer.
9. After successful calibration, the command is input from the keyboard to move the
robot to the stance position. The command activates the motion control software
and moves the robot to a position where it is stable upon being lowered to the
ground.
8 - 48
10. The airpump is started to spin the gyroscope to operational speed.

11. The robot is ready for motion.
This concludes the discussion of the development of the software written to control
the robot.
8 - 49
OPERATIONAL TRIALS ON ROBOSHIFT
The history of engineering is really the history of breakages, and

of learning from those breakages.
CA Claremont, Spanning Space
This chapter covers the results of trials conducted with the robot. The first part of the
chapter discusses the testing and calibration of the instrumentation system. This includes
calibration of the strain gauge sensors as well as the artificial horizon and compass.
Testing of joint shaft encoders and issues dealing with suppression of electromagnetic
interference concludes the discussion of instrumentation trials. In the second part of this
chapter the testing of the software that controls the robot is discussed, within the scope
of the main software routines which are:
Calibration
Frontal Balance
Motion
9.1 TRANSDUCER CALIBRATION

AND PERFORMANCE
The robot uses three groups of transducers to obtain real-time data on the orientation of
joints and the body as well as measuring the force distribution on each foot. The types of
transducers used are:
1. Proprioception - force feedback strain gauges
2. Vestibular - artificial horizon, flux gate compass
3. Joint position - shaft encoders, end of travel limit switches
Generally, transducer/amplifier/conversion systems were only calibrated once after
installation or when modifications had been made. However, shaft encoders were
calibrated on each power-up sequence. The following sections describe the results of the
calibration and performance of each group of transducers.
9.1.1 FORCE TRANSDUCERS
Purpose of test
The robot was equipped with four strain gauge type force transducers so that force
9-1
Chapter 9 - Operational Trials on Roboshift
Figure 9.1 Strain gauge calibration rig
feedback of ground reaction forces could be input to the control software. This data was
essential for the active balance software as well as the locomotion software. Testing of
the force transducers was designed to confirm the total force on each foot could be
measured as well as the force distribution along the foot in the sagittal plane.
Method and Results of Test
To calibrate the strain gauges, the robot was suspended then lowered onto a square
platform (see Figure 9.1). The platform was supported on four sets of bathroom scales
that had been previously calibrated by weighing measured quantities of water. Given the
size of the robot, it was not possible to apply weight to only one foot at a time so that the
total weight on the scales could be correlated to the readings on one set of strain gauges.
However, with the dimensions of the platform known and with the position of the feet
accurately measured, it was possible to determine the weight distribution between the
feet and ultimately the load on each foot. Tests were conducted where the robots hip
joints were manipulated so that more weight was taken by one foot than the other.
Similarly, the foot extension angle was varied so that the relationship of longitudinal
force distribution to strain gauge output could be determined.
The calibration was not a strict calibration of the strain gauges; rather it was a calibration of the force measurement system. The signal from each strain gauge was amplified
by a strain gauge amplifier and then input to the analogue to digital converter of a
Motorola M68HC11. The final output was a combination of the gains and errors of all of
9-2
Figure 9.2 Calibration of left foot strain gauge (front loading)
these systems. However, over a number of trials, the data was reproducible. The data was
linearised using least squares regression to generate a multiplier and offset for each strain
gauge. A sample of the data is shown in Figure 9.2, where it can be seen that each of the
strain gauges in the foot was sensitive to different types of loading. In the case of Strain
Gauge 1, it was more sensitive to heel loading than toe loading. Figure 9.3 shows that
Strain Gauge 2 was more sensitive to toe loading than heel loading.
The control software took into account the different gains of the strain gauges and
weighted output accordingly. For example when the reading on strain gauge 1 rose above
90 bits, it was assumed that the loading on the strain gauge was not sufficient to be in the
linear response zone of the transducer. In this case, the weight on the foot was calculated from strain gauge 2.
When the robot was lowered to the floor, the control software was in stance mode
configuring the geometry into a position that was known to be stable when the weight is
taken off the support ropes. This software did not actively balance the robot. It simply
held each joint in a predetermined position. Thus, while it could be expected that the
weight on each leg would be equal once the robot was self-supporting, Figure 9.4 shows
that compliance allowed the robot to slump slightly to either side.
9-3
Strain Gauge Calibration

Left foot - Heel loading
Figure 9.3 Calibration of left foot strain gauge (heel loading)

Balance Trials
Mass (Kg)
SM1508b
400
300
200
-2
100
-4
-6
0
50
100
150
200
Time
Mass LL
Mass RL
Trim
dTrim/dT
Figure 9.4 Strain gauge data as robot is lowered to the floor
The data from the strain gauges was primarily used to determine a gross estimate of the
weight on each leg and the proportion of weight on the toe and heel of the foot. Figure
9.5 shows the output of the strain gauges when the robot was in the standing position and
is alternatively pushed to the left, and then to the right. The balance control system was
disabled for the trial, consequently the robot did not react to the input. One of the main
9-4
hormones used by the control system was trim i.e. the position of the centre of mass in
the frontal plane. The variable was calculated as the percentage of the total measured
weight of the robot taken on the left leg, given as:
trim =
weight _ left _ leg

weight _ left _ leg + weight _ right _ leg
It can be seen in Figure 9.5 that, as the robot was pushed to the left, compliance in the
system allowed the right hip abduction shaft encoder to register an increase of two counts
or 0.3 of a degree. Similarly, the right hip abduction shaft encoder recorded a decrease of
3 counts. As the force was removed, the distribution of weight remained further to the
left of the point at which the trial commenced. Due to compliance in the system, without
the balance control system being active, the robot tended to lean slightly to either side.
As the robot was then pushed right, the percentage of weight on the left leg decreased as
compliance allowed the centre of gravity to move in the direction it was pushed. When
the force was removed, the centre of gravity moved back to the rest position.
Figure 9.4 shows the data produced as the robot is lowered to the ground, thus
increasing the weight on each leg. Under extreme sway, the reaction at each foot
exceeded values beyond which it was possible to calibrate the strain gauges. As can be
seen in Figure 9.6, the total weight measured on the feet i.e. the full weight of the robot,
varied as the robot reached the extremity of sway. Torques produced by acceleration of
the centre of gravity of the body may superimpose additional forces at the feet. However,
it was more likely that the signal from the strain gauges was non linear.
108
0.7
Strain Guage
Gauge Trial
Trial #3
Strain
#3
30/06/98
106
0.6
0.5
% Weight
Position (Counts)
104
102
0.4
100
0.3
98
96
0.2
10
30
50
70
Time (Sec)
% Weight on Right Leg
RiHiAb Actual
LeHiAb ACtual
Figure 9.5 Testing of strain gauges

9-5
Frontal Balance
Sagital Balance Tuning (Mass)

0801bla
500
400
Mass (Kg)
300
200
100
0
0
50
100
150
200
250
300
350
Time
Mass LL
Mass RL
Total Weight
Figure 9.6 Strain gauge outputs under full frontal sway
At the point where the signal from the strain gauges is saturating, the gauges indicated
that the majority of the weight was on one. As it was expected that the control program
would be driving outputs to saturation levels in reaction to this extreme situation and that
the artificial horizon and joint sensors would indicate the position of the robot in the
frontal plane, losing linearity and accurate force measurement at these points was not
considered to be problematic.
9.1.2 ARTIFICIAL HORIZON AND COMPASS
Purpose of test
The artificial horizon and compass form the vestibular system of the robot. This test
was designed to confirm that the pitch, roll and yaw of the body was able to be sensed to
the required accuracy.
Method and results
As in the case of the strain gauges, the calibration of the artificial horizon was a
calibration of the entire vestibular system. The calibration was conducted by rotating the
main frame of the robot in pitch and yaw, using a graduated liquid level to read deviation
from the horizontal. The output of the M68HC11 that converted the analogue signal of
the gyroscope into an eight-bit integer was graphed against the actual rotation values. The
9-6
Artificial Horizon Calibration

200
Output (Bits)
150
100
50
0
-15
-5
15
Degrees of Rotation
Pitch
Roll
Pitch Regression Fit
Roll Regression Fit
Figure 9.7 Calibration of artificial horizon.
results can be seen in Figure 9.7. It can be seen that the output of the artificial horizon
was linear in both pitch and roll. No further calibration was considered necessary.
The flux-gate compass was an off-the-shelf device made by Raytheon. It was
calibrated against a liquid-filled magnetic compass that had been previously swung and
found to provide accurate headings to one degree of azimuth.
9.1.3 SHAFT ENCODERS
Shaft encoders for each joint were not directly calibrated, but the operation of the
Quadrature counter and the M68HC11 that read each particular joint was tested. Initially,
a test rig was built incorporating a shaft supported by bearings, fitted with a shaft encoder
at one end and a protractor at the other. The shaft was manually rotated through 360
degrees in ten degree increments. In each case, the output of the M68HC11 responsible
for the joint output a position calculated to be within 0.2 of a degree of the position
shown on the protractor. Once position sensing had been confirmed, software was
written for the control of the small wheeled robot as discussed in Chapter 8. The robot
was fitted with two shaft encoders and two of the I/O boards for the F1 controller. The
results of the software testing which are discussed in Chapter 8 confirmed the operation
of the shaft encoders and quadrature counters.
9-7
During regular testing of the robot, the operations of all the shaft encoders were
automatically tested when calibrating the joints. As each joint was moved to the end of
travel limit switches and then back to the required position for stance, the position of one
foot relative to the other was determined by the result of 14 joint calibrations. As the two
feet are located at the end of 14 joints, any error in the calibration of any of the joint
encoders would result in an abnormal position of one foot relative to the other.
9.1.4 ELECTRONIC NOISE
During initial trials of the robot, erratic results were obtained during the
commissioning of the feet control systems. This section describes problems that were
encountered due to either electromagnetic interference or ground loops within the robots
structure. It is included in the text as there is no question that automatically controlled
hydraulic machinery will become more common in the future. Such machines will use
the same technology used in this project and may be subject to the same problem.
The calibration of the feet cylinders must be completed in well defined stages as the
configuration of the hydraulic cylinders would cause catastrophic damage should their
activation be uncontrolled or phased incorrectly. To minimise the risk of injury, end of
travel limit switches were fitted to the feet in order to limit the valve amplifier signals.
This signal was pulse-width modulated. As a result, when the circuit became open, the
wires connected to it became transmitter aerials. As the current control attempted to drive
the open circuit, voltages saturate. Figure 9.8 shows a screen dump from a Fluke 123
Scopemeter displaying the trace from Channel A & Channel B of the foot extension shaft
encoder during loss of control. Figure 9.8 shows clear evidence of high frequency noise.
The rightmost spike shows several peaks within the spike. What cannot be determined
from this trace is whether the spikes are present in both channels of the pulse train from
the shaft encoder.
Further testing (see Figure 9.9) shows the trace from the 5V supply to the shaft encoder.
While not of the same magnitude or frequency as that shown in Figure 9.8, the trace still
shows significant fluctuations in the supply. The second spike drops below 4V, while the
specification for supply to the shaft encoder indicates that it should remain between 4.5V
& 5.5V.
9-8
Figure 9.8 Spikes on shaft encoder output
Figure 9.10 High frequency interference
Figure 9.9 Spikes on shaft encoder supply
Figure 9.11 High frequency noise
Figure 9.10 shows Channel A voltage as an AC signal. The trace displays an

extremely high frequency interference of order 5/200ns = 10MHz. This noise disappears
when the hydraulic valve amplifiers are disabled. Figure 9.11 shows the noise produced
on Channel A when the two limit switches are contacted. There is a corresponding
increase in the position of right leg encoders as recorded by the robots control system.
It should be noted that the freestanding I/O card and processor reported a greater increase
in position when connected to the same recorder. This effect had not been seen by any of
the hydraulic experts asked to review the problem by the author. It would appear that this
project was one of the first to use proportional hydraulic valve controllers in combination
with digital shaft encoders. Ultimately, extensive shielding of the valve amplifier wires by
wrapping them in aluminium foil and grounding them to the DC earth on the robot solved
the problem.
9-9
9.2 POWER PACK

As detailed in Chapter 3, any industrial biped will require an on board power supply
capable of operating the device for extended periods. As is the case with most materials
handling plant, it is expected that bipeds of industrial scale will rely on hydraulic
actuators as a motive force. Roboshift is powered by a Briggs and Stratton LPG
powered engine fitted with three hydraulic pumps and a 12V alternator. As space was
limited within the robots frame, an electrical driven, fan-forced oil cooler was
incorporated in the hydraulic circuit to ensure the temperature of the hydraulic oil
remained within operation temperature specifications.
Purpose of test
To demonstrate that the use of hydraulics is feasible for use on an industrial biped with
the capability of extended operation, the system was run at full capacity until the temperature of the oil stabilised.
Method and Result of Test
A K-Type contact thermocouple was taped to the main hydraulic return line 50mm
prior to entry to the hydraulic tank. As this point the oil has absorbed energy without
mixing with cooler oil in the hydraulic tank. While it is not representative of the
temperature in the tank, or the temperature of oil before it flows through actuators, it does
Power Pack Testing
(smf2708)
14
60
12
50
10
8
30
6
20
4
10
Temperature (Deg. C)
Current (Amps)
40
-2
-10
0
60
120
180
240
300
360
420
Time
Main battery current
480
540
600
660
720
780
840
(Sec)
Temperature
Figure 9.12 Hydraulic oil temperature and 12V current draw during engine test
9 - 10
indicate the highest temperature of oil in the system. A clamp meter was placed around
the main supply cable from the main 12 volt battery. In this way, a measure of the
current flow from the battery was taken.
Electrical
Prior to starting the Briggs and Stratton Engine, all of the electrical systems were
energised on the robot. As can be seen from the trace in the graph of Figure 9.12,
activation of circuits including control, hydraulic amplifiers, cooling fan and inverters
led to an increase in current drawn from the battery. The engine was started around the
200 second mark where a spike can be seen in the trace indicating a sudden increase in
current. Interestingly, the starter motor for the engine is supplied by a separate, un-fused
cable. The only explanation for the spike is the high current drawn by the starter motor
must, via a ground loop, be drawing additional power through the supplies to other
circuits. This effect can be devastating for electronic circuits and will be addressed prior
to the design of the next prototype. Once the battery charging circuit is switched from the
external battery charger (not connected during test) to the alternator, the current
instantly drops to zero as power is fed back to the battery from the alternator that is also
supplying 12V power to the system. It is assumed the current value did not move further
in a negative direction due to the battery being fully charged prior to the trial.
Hydraulic
To test the hydraulic system, the pumps were run with no movement of the hydraulic
actuators. In this situation, all oil runs through the pressure relief valves back to the
hydraulic reservoir. This is the most inefficient configuration where the maximum
amount of energy is absorbed as heat by the oil. When Roboshifts actuators are
operating, up to 90% efficiency can be expected between the energy put into the oil by
pumps, and the energy extracted from the oil by actuators.
The trace shows a steady rise of the oil temperature to approximately 40 degrees
Celsius where two factors begin to slow the temperature rise. Firstly, at this temperature
the efficiency of the cooler increases, removing more heat from the oil. Secondly, as the
temperature of the oil increases, its viscosity is lowered decreasing friction losses as it
flows over the pressure relief valves. After fourteen minutes the oil temperature stabilised
at approximately 48 degrees Celsius. This is well below the maximum operating
temperature of 70 degrees Celsius.
9 - 11
9.3 JOINT CONTROL

Purpose of Test
The testing of the local joint control systems was to confirm that the joints of the robot
could be simultaneously moved to and held at demand positions.
Method and results
The joint control software was loaded into the fourteen joint control microprocessors.
Given a demand position by the control computer, each joint controller moved the joint to
the desired position with local closed loop control. One of the main concerns with the
control of the joints was that actuation of one valve could affect the control of another. As
discussed in Chapter 3, the robot was fitted with three hydraulic pumps to avoid hydraulic
pressure drop. Tests were performed to confirm that two banks of valves could be
operated simultaneously without having adverse effect on the motion. Figure 9.13 shows
an initial trial of the step response of the hip abduction cylinders. Each cylinder is driven
from a separate valve bank. The time scale is measured in iterations of the main control
software running on the main control computer at eight loops per second. The initial trial
showed a slower time response than was anticipated. After several trials, it was found that
the dead band of the proportional valves produced a dramatic effect on the cylinder motion.
As discussed in Chapter 8, the proportional valve spools must move a fixed distance before
fluid will flow through the valve. Therefore, there is a minimum value of the control
Frontal Balance
Sagital Balance
Tuning (Movement)
1507vlf
120
0.56
0.555
100
0.55
Mass
80
0.545
60
0.54
40
0.535
20
0
20
40
60
0.53
100
80
Time
LHA Demand Position
LHA Actual Position
RHA Demand Position
RHA Actual Position
Trim
Figure 9.13 Step response of hip abduction cylinders

9 - 12
Frontal Tuning
Balance
Sagital Balance
(Movement)
1507po
120
0.535
0.53
100
0.525
Mass
80
0.52
60
0.515
40
0.51
20
0.505
0
50
100
150
Time
LHA Demand Position
LHA Actual Position
RHA Demand Position
RHA Actual Position
Trim
Figure 9.14 Step response using determined zero control value
signal that corresponds to this point. The local joint control software was modified to
include a routine to determine the minimum valve control signal which produced oil flow
to the hydraulic actuator. This value was then taken to be the zero value of the control
signal in the given direction. By immediately operating at this point, rather than waiting for
the signal to build to the same value, a faster response was achieved. The calibration
routine is detailed in Chapter 8. Figure 9.14 shows the results of the same trial when the
calibrated values of V_Negative_Min and V_Positive_Min were used as the zero control
values. It can be seen that the response of the valves was up to four times faster than in the
previous trials, overcoming the initially stated problem with relative ease.
9.4 INITIALISATION PROGRAM

Purpose of test
Fourteen individual joints had to be calibrated prior to any controlled motion of the
robot. As the shaft encoders were incremental transducers, there was no memory or direct
reading of the joint position. For this reason, the shaft encoders were homed after
power-up so that their output could be calibrated to the joint position The purpose of
these tests was to confirm that the automatic initialisation software was able to move
each joint of the robot to its minimum travel and then to reset the shaft encoder count so
that the actual joint angle and the position stored by the I/O board were synchronised.
Method and results
In most cases testing of the initialisation software was a straightforward process. The
9 - 13
a) Start of calibration
b) Setting V_Pos_Min for toe
c) Setting V_Neg_Min for toe
d) Moving toe up to safe position
e) Setting V_Pos_Min for foot rotation
f ) Setting V_Pos_Min for foot extension
g) Retracting all cylinders to zero all encoders
h) Moving to calibrated position
Figure 9.15 Calibration of foot cylinders and amplifiers.

9 - 14
software successfully moved the joint to its minimum travel limit contacting a limit
switch (see Figure 9.15) at which point the quadrature counter chip memory was zeroed.
Unable to support itself during calibration, the robot was suspended while the joints were
moved through the limits of their motion.
In the case of the foot, it was necessary for the initialisation to be coordinated between
the three foot cylinders, as uncoordinated motion would result in severe structural
damage. By way of example, if both foot cylinders were pulling up while the toe
cylinder was pushing down, the toe would be snapped off the foot. Initial trials of the foot
calibration software were very stressful due to the significanat possibility that incorrect
motion would damage the robot. However, the software proved to be robust and achieved
the task without incident. The photographs in Figure 9.15 show the different stages of
initialisation.
9.5 STATIONARY BALANCING PROGRAM

Purpose of test
Balance testing was designed to confirm that the robot, once lowered to the ground,
was able to stand without falling and to maintain balance when subjected to external
forces.
Method and results
Once the initialisation software had been executed successfully, the robots joints were
moved into a stance position (see Figure 9.16), a configuration where the robot was
statically balanced as it was lowered to the ground. Joints were kept in predetermined
positions by the control system, which was operational but not actively balancing the
biped.
Due to the high mass of the robot and clearances and tolerances in the joints, as well as
compliance in the joint members, the robot would lean slightly to one side of the frontal
plane and either forward, or backward in the sagital plane. The stationary balancing
program, once activated, repositioned the robots upper body so that the centre of
gravity moved between the support bases of the feet. As discussed previously, the robot
achieved this by abducting the hips while keeping the body and feet horizontal to the
floor. Initial results showed the balancing routine to be sensitive to three parameters;
9 - 15
Figure 9.16 Robot hanging stance position
The limits set for balancing.

The distance between the feet
The gain of the control system
Figure 9.17 shows the robot in active balance. In this case, the limits for balance were
set between a trim value of 0.45 and 0.55. In this instance, the control system was able
to modify the robots shape so that the trim value fell between the set limits. While the
9 - 16
Figure 9.17 Robot in active balance with trim limits set between 0.45 and 0.55.
Figure 9.18 Robot in active balance with trim limits set between 0.48 and 0.52.
robot had successfully balanced, the centre of gravity came to rest slightly to the right of
the centre line between the two feet. When balancing limits were set between 0.48 and
0.52 (see Figure 9.18), the robot became unstable, swaying in the frontal plane as it
continuously overshot the demand operating limits. If the same trial was then attempted
with the gain of the valve control reduced, i.e. reducing the proportional constant in the
9 - 17
FrontalTuning
Balance
Sagital Balance
(Movement)
Trial# 1107blb
110
0.52
0.5
0.48
105
Position
0.46
0.44
100
0.42
0.4
95
0
50
100
0.38
200
150
Iterations
LHA Demand Position
LHA_Actual PositionP
RHA Demand PositionP
RHA Actual Position
Trim
Figure 9.19 Robot balances with limits between 0.48 and 0.52 and reduced gain
PI controller, the robot balanced successfully. However, Figure 9.19 shows the response
of the control system was slower as indicated by the more shallow slope of the joint
displacement trace.
This demonstrated the closely coupled relationship between balancing limits and the
gain of the control system. If the gain was high, the robot generated substantial
momentum before the trim fell between the set balance limits. While the proportional
constant should account for any potential overrun, the robot only had the weight of the
leg on which it is pushing to decrease angular momentum. Roboshift could generate substantial force and acceleration when pushing off from a leg, but then only had the weight
of the leg to retard the motion. While walking, the desired effect of frontal sway is
normally to lean far enough to one side that the body is almost overbalanced. The centre
of gravity then becomes placed directly over the supporting foot. This is the ideal for an
energy minimising gait, where energy dissipation through damping is minimal as we are
operating at the natural frequency of oscillation. However, this constitutes control at the
boundary of stability. With the biped, until walking control has been realised and has
become reproducible, limits for balancing will kept open so that the robot does not
overbalance.
9.6 LOCOMOTION
As discussed in Chapter 8, the locomotion software consisted of several sections. The
first phase of walking is to shift weight from one leg to the other so that a foot can be
9 - 18
Frontal Sway
Figure 9.20 Hip abduction and trim output during continuous frontal sway
lifted from the ground. As locomotion progresses this process is repeated on a cyclic
basis resulting in the robot swaying in the frontal plane. The second phase of walking was
for the robot to lean forward, shifting its centre of gravity in the sagittal plane so that the
swing foot can be moved forward while not in contact with the ground. The following
sections outline the testing of these behaviours.
9.6.1 FRONTAL SWAY
Purpose of test
The purpose of the frontal sway test is to confirm that the robot can transfer weight from
one leg to the other, in a continuous cycle, in a predictable and controllable manner.
Method and results
To prepare the robot for frontal sway testing, it was first calibrated while hanging from
the support frame and then lowered to the ground in the stance position. The frontal
balancing software was then enabled to ensure that the centre of gravity of the robot was
positioned midway between the feet. Once the robot was balanced, the frontal sway
software was enabled. Figure 9.20 shows hip abduction position and trim values for the
robot in continuous frontal sway.
As can be seen in the photograph of Figure 9.21, the robots feet were set apart so that
9 - 19
Figure 9.21 Robot in frontal sway
the robot was not being controlled at the limits of stability as it could have been for an
anthropomorphic, energy minimising gait. It can be seen that while the robot is swaying,
local joint control attempts to keep the feet relatively horizontal to the floor.
Initial attempts to generate frontal sway in the biped produced motion that was highly
dynamic. From observation of the motion, it appeared that the robot was oscillating at a
resonant frequency. Figure 9.22 shows the transfer of weight from one leg to the other as
the robot commenced frontal sway from rest. The video of the event shows that the biped
robot was swaying to the limit conditions discussed previously where the ground
reaction force on the non-supporting leg approaches zero. Correspondingly, the reaction
at the opposite foot approached the total weight of the robot. At this point, the
non-supporting foot is seen to slide toward the supporting foot as the lateral friction
forces approach zero. The foot slides as the moment created by the weight of the leg
about the hip causes elastic deformation of the leg, allowing the foot to adduct towards
9 - 20
Figure 9.22 Data from robot in frontal sway
the supporting leg. As the biped swayed back toward the centre of motion, the
non-supporting leg was now closer to the centre of gravity and subtended a larger angle
at the hip. At this point, the elastic strain energy gained as the leg deformed had not been
released. The corresponding increase in torque immediately reduced the amplitude of
sway.
From observation of these trials, it was determined that finer control would be required
at the boundary of sway. To achieve this, it was decided that the width of the foot should
be increased to provide additional ankle torque which would control the biped at the
boundary condition. The foot width was increased from 150mm to 300mm which
increased the available ankle torque to approximately 3000 Nm. Figure 9.23 shows the
original and modified configurations of the feet.
Figure 9.23a Original foot configuration
Figure 9.23b Widened foot configuration

9 - 21
Trials were then conducted in the new

configuration. Identical values for stance
width and control gain, that had
previously resulted in successful frontal
sway, were used. Contrary to expectations, the robots frontal sway behaviour
became more unpredictable and unstable
with the widened feet. However, it was
Figure 9.24 Failure of foot rotation cylinder
connection
apparent that the foot rotation cylinders

were now subjected to higher forces.
Evidence of the increased torque provided by the increase in foot width is the failure of
the foot rotation cylinder connection (see Figure 9.24).
Due to the unpredictable behaviour in frontal sway, the frontal balance software was
revisited in an attempt to determine whether a control issue may have been the cause of
the problem. After further frontal balance trials were conducted, it became evident that
there was an oscillation in the response that was not seen previously (see Figure 9.25),
when compared to the trials conducted with the original feet (see Figure 9.19). To gain
insight into this behaviour, a dynamic model was developed to examine stiffness
constants associated with the observed motion.
Figure 9.25 Robot in frontal balance with modified feet

9 - 22
Figure 9.26 Three

link biped model
Simplifications made during the derivation of the model include:
Cos ( ) = 1
Sin( ) =
for small oscillations in the vicinity of the vertical.

for small oscillations in the vicinity of the vertical.
As there can be no rotation of the body,
body = 0
The control torques are equal and in the same direction.

Ankle torques are equal and in the same direction.
The model as shown in Figure 9.26 is of a three-link biped with parallel legs. The
equation of motion for the system is found to be;

M L2
IL + B

2

where:

M gL

+ k A + k B
B = 0

2

M B = Mass of body
L = Length of legs
I L = Moment of inertia of each leg about its foot

g=
Gravitational constant.
B = k B = Control torque.
A = k A =
Ankle torque.
9 - 23
The period of oscillation of the system is calculated as:

M gL

k A + kB
B
2

=
M B L2
IL +
2
The equation shows that the dominating parameters of the biped are the length of the
legs and the magnitudes of torques at the ankle and body.
Data used for this model corresponded to the first trials of the biped which had been
conducted with a foot width of 150mm. In this configuration, with full load on one leg,
these feet could provide an ankle stiffness of approximately 2000Nm/radian (based on a
maximum ankle torque of 500Nm). Hip abduction is controlled by a hydraulic cylinder
mounted at the top of the body. For the purpose of the analysis, it will be assumed that
these joints possess a similar stiffness to that of the ankles when locked. The remaining
physical properties of the biped are listed below:
Parameter
Size
Units
Length of legs
Mass of body
300
kg
Mass of legs
80
kg
Moment of inertia of legs
33
Kgm2
Ankle stiffness
2000
Nm/rad
Body stiffness
2000
Nm/rad
Table 9.1 Frontal sway parameters
When input into the above equation, these values predict an oscillating frequency of
0.42 Hz corresponding to a period of 2.4 seconds. This corresponds to an observed
period of 2.8 seconds in the video of the trial.
As previously discussed, when the biped is first lowered to the ground, it will lean
toward one side or the other until the ankle torque becomes equal to the moment
generated by the centre of gravity about the centre of support. Figure 9.27 shows the
response of the simulated system as the model is allowed to lean from the vertical. In this
case no control torques are included. Ankle torques are set at 2000Nm/rad based on the
9 - 24
Frontal Sway Simulation
Figure 9.27 Simulation of robot coming to rest when released from central position with
500nm/rad ankle torque
original foot width and a damping coefficient has been added. The damping factor has
been estimated from observed time taken for the robot to come to rest after a step input.
The simulation shows that with ankle torque only, the biped displays a single natural
frequency of oscillation as expected for a single degree of freedom system.. When the
available ankle torque is increased to values generated by the wider feet (1800Nm), the
result shown in Figure 9.28 is seen. As would be expected with increased stiffness, the
frequency of oscillation has been increased and the system has come to come to rest
closer to the vertical.
Neither of these one degree of freedom simulations can explain the additional
frequency observed in the motion of the biped. The model was then expanded to include
control torque at the hips. The control software makes incremental changes to the
position of the hip cylinders based on the feedback error of the actual position versus the
target position. A time-history plot of the control values for the hip abduction cylinders
during frontal sway can be seen in Figure 9.28. Control torques are only active when the
position error of the hip abduction cylinders exceed a set value. Once active, their values
are proportional to error.
9 - 25
Figure 9.28 Simulation of robot coming to rest when released from central position with 1800nm/rad
ankle torque
15
1000
10
500
-500
Control Value
Degrees
Frontal
Simmulation
Frontal Sway
Sway Simulation
-5
-1000
-10
-1500
0
50
100
150
200
Seconds
Angular Position
Control
Figure 9.29 Simulation of robot reacting to control torque
The graph in Figure 9.29 shows the model used in Figure 9.27 reacting to a simulated
control torque. It can be seen that the additional stiffness of the control system modifies
the frequency of the system at the times when the control torque is acting. The greater
the control torque, the shorter the period of oscillation. The control torque is a function
of the gain of the control system and the dynamic response of the joint being controlled.
Further work will investigate the tuning of the gain of the control system for the
dynamics of each joint.
9 - 26
While inverted pendulum models have frequently been used to simulate a biped robot
in the frontal plane [(Kitamura, 1990), (Hemami, 1973)], an inverted pendulum can only
display harmonic motion if the moment generated by gravity is overcome by a restoring
torque. In the case of a biped robot in active control, it has been shown that the restoring
torque is primarily a function of the torque generated by the control system. As the
magnitude of that torque increases, the period of oscillation will decrease. The analysis
above suggests the additional torque created by the addition of the wider feet increases
the frequency response of the system decreasing the damping of the control frequency.
Thus, as the width of the feet increases the stiffness of the system, higher order responses will become apparent.
The model used to analyse the system did not explain the source of the oscillation
observed after the addition of wider feet. An alternate explanation is that this frequency is
a function of the compliance of the joint. As the hip abduction cylinders move to reduce
the error, the inertia of the system causes an amount of the movement to be taken up by
compliance. This compliance then results in an increase in torque which
accelerates the joint in proportion to the inertia of the system. Effectively, the mass of the
system acts as a low-pass filter for the control system, which runs at a frequency of 10 Hz.
To investigate this theory, a further trial was conducted to determine the contribution of
the gain of the global control system on the oscillation of the robot. The trial was
conducted in the sagittal plane so that effects of increased torque provided by the wider
feet were eliminated. For the test, Roboshift was lowered to the floor in a configuration
where the majority of weight of the robot was taken by the heels, maximising the balance
error. Sagittal balance was then enabled with the following gains set for global and local
joint control:
Gain of the sagittal balance software set to 20% of the normal value
Gain of the knee joint controllers were set to 300% of normal values
Gain of the foot extension controllers were set to normal
In this way, the global control system would react slowly to the error signal provided
by strain gauges. However, the local joint controllers would react rapidly to any error
signal transmitted by main control program.
Figure 9.30 shows the results of the trial where it can be seen that Roboshift slowly
adjusted its configuration so that weight was transferred from the heels. The video of the
9 - 27
Sagittal Balance
Figure 9.30 Sagittal balance trial
trial (included on the CD supplied), shows the left toe making contact with the ground as
weight is transferred to the front of the foot. While no major oscillations were apparent,
minor oscillations can be seen in the trace of sagittal trim. The trace recording the
positions of joints shows the knee positions increased linearly. This would indicate that
the oscillations observed in the trim value could only be caused by elastic vibration of
the structure. This can be clearly seen in the video of the trial. Due to the large mass of
the robot and the associated moment of inertia, rapid movements of the knee actuators
are initially absorbed as strain energy. The trace of sagittal trim records a series of step
responses of the structure to sudden changes in the knee positions. The response consists
of a lightly damped oscillation around the new position. This oscillation is not of the
scale seen in the frontal balancing trials. The continuous linear movement of the knee
cylinders further suggests the gain of local joint control does not contribute to the
additional frequency seen in the frontal balancing trials.
Once Roboshift had balanced, weight was applied to the front of the robot. Roboshift
adjusted to the disturbance by decreasing knee extension and by increasing hip extension
to minimise error caused as compliance in the structure allowed the nose of the robot to
pitch under the additional weight. When the weight was removed, elastic strain energy
was released causing the structure to oscillate violently. The lack of damping in the
structure increases the potential for unstable oscillation.
9 - 28
Figure 9.31 Alternate hydraulic cylinder arrangement
One drawback of hydraulic actuation revealed by these trials is the high locking force
available once the controlling valve is closed. In most cases, this force is many times the
operational force of the actuator. Unlike the case with non locking actuators such as gear
drives, the two members connected by the joint become a single, rigid structure when the
controlling valve is closed. Based on the results of the trials conducted in this project,
control would be improved if some means of damping were available; for example, a non
rigid hydraulic cylinder.
One soulution to this problem may be to drill a small hole in the piston of the hydraulic
actuator. As shown in Figure 9.31, this configuration would allow some damping at the
joint. Further, as the hydraulic valve must allow some flow to maintain the position of
the piston (matching the flow across the piston), the set point of the valve is shifted from
the non-linear region to the linear region of flow control. If this system were to be used
on Roboshift, a continuous flow of oil to the actuators would be required to maintain
position. At times when the robot was stationary, the ability to close the connection
between cylinder chambers would be an additional advantage. However, this would be
difficult to acheive at the piston. Future investigation of this arrangement, in discussion
with hydraulics manufacturers, will be focused on the development of a valve that is able
to provide controlled leakage between output ports.
9 - 29
10
DISCUSSION
The history of engineering is really the history of breakages, and

of learning from those breakages. I was taught at college the
engineer learns most on the scrapheap.
- CA Claremont Spanning Space
Every engineering project starts with a vision and a desire to achieve the vision. The
previous chapters show how this project went from the first pencil sketches through to
pictures, video and data of a 500kg, 2.4 metre biped robot in motion. The robot, designed
for materials handling, has been named Roboshift. This chapter discusses the
achievements of the project in terms of design, construction, programming and testing.
As previously highlighted in Chapter 3, one of the drawbacks of a lack of funding is the
reliance on a single prototype. Early in the testing phase of the project, it had become
apparent that various design aspects would be improved had there been available funds
to develop a second prototype. The following sections review a number of design issues
that have impacted on the results of the project.
10.1 MECHANICAL DESIGN

The mechanical design of the structure performed as expected in terms of the ability to
move the joints of the robot in the required degrees of freedom. Apart from failure of
several foot rotation cylinder connections there was no evidence of other failures or
permanent deflection of the structure at the completion of testing. The decision to
fabricate the main structure from aluminium will be reviewed as part of the design
analysis for the second prototype.
10.1.1 COMPLIANCE
During the design phase, it was recognised that the completed robot would be
extremely heavy. Roboshift is primarily constructed from aluminium to reduce the total
weight of the structure. While considered suitable for the construction of a prototype, it
was always envisioned that any robot, working in an industrial environment, being
subject to frequent minor collisions, would be fabricated from steel. The added
flexibility of aluminium over steel has led to oscillation in the structure that cannot be
10 -1
Chapter 10 Discussion
sensed or controlled by the control system. As discussed in Chapter 2, initial evidence

shows that as the height of a biped robot increases, the expected leg deflection increases
cubically. It is further suggested that other biped researchers have experienced this
phenomenon and have reduced the size of recent prototypes to decrease the effects of
compliance. Roboshift stands twice as tall as other biped robots in development around
the globe. As Roboshifts body is slung from the hips, the length of the legs is as long as
the robot is high. Effectively, Roboshifts legs are four times longer than the legs of any
previous biped robot. This configuration exacerbates the problem of compliance in the
legs significantly.
As illustrated in Chapter 3, the design of Roboshifts joints incorporated substantial
bearings and shafts. In the case of hip rotation, the shafts were manufactured from 40mm
stainless steel which was seen to flex during trials. While an initial analysis of the design
was completed to ensure the strength of the structure, no analysis was conducted to
determine the modes of oscillation of the robot. The next iteration of Roboshift will
involve a significant redesign of the structure. In particular, the joints will be designed to
reduce compliance between the limbs. An analysis will be required to determine whether
thick walled aluminium or thin walled steel box sections should be used to
fabricate the limbs and whether the increase in weight due to the steel sections will
nullify any benefit of added stiffness. Given the section size and bearing sizes required
to minimise deflection, the author expects that the next iteration of Roboshift may weigh
up to 1.5 tonnes. Naturally, a heavier structure will require larger actuators, larger
hydraulic valves, a larger hydraulic tank and a more powerful motor.
10.1.2 JOINTS
As the robot is yet to achieve dynamic locomotion, the advantage of a fully articulated
toe has not been proven. McMahon (1984) demonstrated that ankle plantar flexion was
required to smooth the transition of weight from the swing leg to the stance leg. Without
a flexible toe, a rigid foot would provide a single point of contact with the ground, unable
to provide torque in the sagittal plane to stabilise the stance leg during locomotion. The
additional level of mechanical and control complexity required to incorporate the toe has
not been justified from experimental results.
One of the challenges of industrial biped robotics brought to light by this project is the
actuation of multi degree of freedom joints. The use of three hydraulic cylinders to
10 -2
directly drive the feet results in a large mass at the extremity of the leg. Ideally, the feet
would be controlled by compact servomotors located directly at the joint as has been
demonstrated by Honda (Honda, 2003) and Toyotas (Weiss, 2004) humanoids. However,
ground reaction forces found at the feet are in the order of 1.5 times the total weight of
the robot. Hondas Asimo, and now Toyotas trumpet playing humanoid both would
experience forces at the feet of around 120 to 150 kilograms1. Acting at a moment radius
of around 200mm, the torque required, around 300Nm, is within the capacity of
lightweight, high-speed electric gear drives that use nylon gears, pressed sheet metal
housings and incorporate plane bearings. This class of servo drive may weigh five to
seven kilograms. However, an industrial biped weighing 500kg, with a foot length of
300mm, would require ankle plantar flexion torque of 2.25kNm that is well beyond the
capacity of light-weight drives designed for mobile robotics. Gear drives, with the
capacity to produce these torques are designed for industrial machinery, are encased in
steel, use steel gears and incorporate substantial bearings. This class of drive may weigh
between 100kg and 150kg. As well, the output shaft from the gearbox must be keyed into
a sufficiently substantial boss to transfer the torque to the joint.
Effectively, the mechanical design of the current state of the art humanoids cannot be
scaled to the size of hardware required for an industrial biped. To progress to an
industrial scale biped, Honda and Toyota must incorporate or develop industrial grade
hardware for the devices. Given the size and range of Toyotas industrial presence, this
would not be an insurmountable obstacle. However, it has not been achieved to date.
10.1.3 HYDRAULIC ACTUATION
Until other means to deliver the required motive force have been developed such as
electroactive polymers (NDEAA, 2003), it is expected that future industrial scale bipeds
will use hydraulics as the primary drive source. Due to budgetary constraints, this
project was unable to make use of variable displacement, pressure compensating
hydraulic pumps. These pumps automatically control their output to match the flow
required by actuators. The gear pumps used in this project provide a constant output
proportional to the engine speed. Excess flow returns to the hydraulic tank via pressure
relief valves that maintain the hydraulic circuit pressure. The heat generated by friction
as the fluid flows over the relief valves is removed from the oil by the cooling fan as
1
Based on a Benzler BG356 V 90 L-4 gear drive using a service factor of more than 200 operations per day with a
load factor of II.
10 -3
explained in Chapter 4. Testing of the hydraulic system demonstrated that the use of a
very small hydraulic reservoir, combined with a large oil cooler, was able to maintain the
temperature of the oil to within operational specifications. Further refinements of this
system are planned for the next prototype including the use of variable displacement
pumps and integrated cooler/reservoir systems.
One disadvantage in the use of hydraulic actuation, highlighted by this project, is the
rigid nature of actuators when stationary. By allowing controllable, limited flow across
the piston of a hydraulic cylinder, or between the ports of a hydraulic motor, a damping
action is possible. Additionally, the set point of the controlling valve will be shifted into
the linear region of flow control. This method of control will be investigated in future
work.
10.2.1 POWER
Due to the requirement for a range of DC voltages, four batteries were mounted on the
robot. Apart from the additional weight, a great deal of space was required to mount the
batteries. The next iteration of Roboshift will incorporate DC/DC converters to provide
the required voltages using a single 24V DC power supply. In a similar manner to
current small commuter aircraft, a separate power supply will be used to boot the control
system of the robot prior to engine starting.
The use of personal computers as the main processing platform required the supply of
240V AC. This involved the use of inverters to convert the main 12V DC power supply.
The budget for this project prohibited the use of 24V DC powered, mobile computer back
planes as used in the military. Again the space requirements to mount two PCs and two
inverters were substantial. From a safety perspective, it would be advantageous to
remove the 240V AC supply from a non-grounded vehicle, though both supplies were fed
via residual current safety devices.
In most cases 12V DC, 24V DC and -24 VDC was supplied to the electrical enclosures
that were distributed around the main frame of the robot, due to space requirements. This
arrangement led to a number of persistent ground loops. While significantly reduced by
electrically connecting all of the members of the robot via the use of ground straps, they
were not eliminated. The second prototype of Roboshift will house all electrical and
electronic devices in a single, shielded enclosure that will be isolated from the robots
10 -4
frame. Additionally, all sensors such as shaft encoders and strain gauges will be
electrically isolated from the structure of the robot.
The modifications discussed in this section will require the use of embedded hardware
such as single board computers and passive back plane enclosures as used in military and
SCADA systems. It is expected that the increase in cost of these systems would be
proportional to the inverse square of the decrease in space that will be required to house
them.
10.2.2 ELECTRONIC NOISE
Electromagnetic interference was considered during the design of electrical and
electronics systems. It was thought that routine procedures such as separation and
shielding would prove sufficient to handle the issue. Testing of Roboshift revealed a
number of areas where electronic noise had affected control systems. For example, the
use of digital shaft encoders was expected to eliminate any possibility of interference of
joint position signals. As discussed in Chapter 9, the proximity of shaft encoder cables to
hydraulic valve cabling resulted in high rates of reception of spurious electromagnetic
data by the joint control processors.
Substantial design analysis was conducted on the mechanical, control and software
systems that constitute the biped robot. However, little analysis (other than dynamic) was
undertaken of the robot as a complete entity. When compared to other complex pieces of
automatically controlled, electro-mechanical machinery, such as aircraft and military
systems, common fault modes could be expected. In the case of electromagnetic
interference, established design methodologies and off-the-shelf technology may be
available to ensure that the effects of interference are minimised. While beyond the
budget of this project, hardware such as fibre optic driver/receiver systems, RFI resistant
enclosures and grounding systems will be investigated for the second prototype.
10.3 CONTROL HARDWARE

In general, the control hardware performed as designed and proved to be most robust.
As discussed in Chapter 9, testing confirmed the functionality of the communications and
sensory systems. Frequently, the control hardware was subject to extreme conditions
including shock loading, vibration and sudden variation of supply voltage and polarity.
Considerable effort, including the use of checklists, was taken to ensure the preparation
10 -5
and operation of the robot was free from human error. However, after long hours of
testing, mistakes were made. These included the incorrect connection of batteries, over
loosening of safety ropes, allowing batteries to flatten and operation of the robot before
calibration. Other than fuses and two voltage regulators, no control system component
was replaced during the project.
The main control computer and communications computer are both mounted using
vibration isolating mounts. As detailed in Chapter 3, the Briggs and Stratton engine and
hydraulic power pack are mounted on a separate frame that is also mounted using
vibration isolating hardware, within the robots main body frame. While this
configuration was designed to minimise vibration experienced by the computers, there
was a concern that vibration would adversely affect the operation of the computers.
Testing showed the computers to be surprisingly immune to vibration and shock loading.
The second Roboshift prototype will incorporate military specification computers.
However, the testing of this robot verifies that the use of an internal combustion engine
is viable for the supply of power for an industrial biped, even with conventional PCs.
10.3.1 SENSORS
Shaft Encoders
The Hewlett Packard HEDS incremental shaft encoders provided reliable position
feedback. However, the lack of an absolute position transducer proved to be
problematic during testing of the robot. On each occasion where there was a failure of
the control system, recalibration of the robot was required to re-zero the shaft encoders.
This problem does not occur with absolute position sensors such as potentiometers. The
drawback of potentiometers is the lack of accuracy when compared to digital shaft
encoders, the susceptibility of the output signal to electronic noise, and occasional non
linearity of output. However, a combination of potentiometer and digital shaft encoder,
as used on Wabian-2 (Waseda 2003) would allow the local joint controller to quickly
move the robots joint to the shaft encoder index pulse without the requirement to move
the joint to its home position. A further refinement of the joint position sensing system is
the addition of a second digital shaft encoder. By reading two incremental shaft encoders,
the control system is instantly able to detect errors by comparing the count from the first
encoder to that of the second.
10 -6
The encoders are designed for installation in mass produced machinery where
manufacturing tolerances assure accurate positioning of the encoder. Due to variations in
the construction tolerances of the robot, alignment errors prove costly in terms of
commissioning time. All the encoders on the robot have been repositioned at some stage
during the project. Under extreme loads, deflection in joint shafts led to a loss of
position data. The lack of a flexible mount between the robot and the shaft encoder
occasionally allowed shaft deflection to cause misalignment between the encoder wheel
and the transmitter/receiver. This situation can be easily remedied by the use of an
external shaft type encoder connected to the joint shaft via a flexible coupling.
Strain Gauges
Strain gauges were used to measure ground reaction forces. As discussed in Chapter 6,
four sets of gauges were mounted to detect load on each foot and the fraction of load
taken by the front of each foot. This arrangement was designed to directly measure
ground reaction forces based on the geometry of the lower leg. Other biped robots have
used an arrangement of force transducers mounted on the bottom of the feet to measure
force at particular points, and then sum these forces to compute ground reaction forces
and the zero moment point.
Due to the geometry of the foot frame, the response of the strain gauges was often
non-linear. As testing of the robot progressed, the introduction of more complex be
haviours led to an altered set of loads to those previously experienced. Depending on the
behaviour of the robot, the structural members on which the strain gauges are mounted
are subject to stresses in more than one plane. For example, under frontal sway, the
swing arms between the lower leg and the foot frame were subject to substantial lateral
forces superimposed on the bending forces produced by the weight of the robot. In this
case the strain gauges were reading both vertical and horizontal forces. Several attempts
were made to relocate the strain gauges to maximise detection of stresses in planes most
responsive to ground reaction forces, however no satisfactory location was found. The
use of proprietary force sensors will be investigated for the next prototype.
Artificial Horizon and Compass
The artificial horizon and compass performed to design specifications. These sensors
are the only self contained, commercially available transducers used on the robot. One
drawback of the artificial horizon is the substantial auxiliary equipment required to
10 -7
supply air to the device (air pump, DC motor, pressure sensing motor controller). While
electronic drive versions of the artificial horizon are available, solid state attitude and
acceleration transducers such as the digital gyro supplied by Advanced Control and
Communications Systems.
Processing Platform
As discussed above, the processing platform for an industrial biped will require
features more commonly found in military and avionics applications. As with any
inherently unstable , mission critical system, reliability and redundancy are key requisites.
Based on the essential control system characteristics which have been highlighted in this
project, and the suggestion that initial deployment of industrial bipeds will be in military
applications, it is proposed that industrial biped control system specifications will include:
Resistance to vibration (MIL-E-5400T)
Resistance to shock (MIL-STD-810D)
Resistance to dust, humidity, hydraulic oil and moisture (MIL-E-1810E)
Resistance to EMI & RFI (MIL-E-461)
Able to operate from a wide voltage range (10V DC to 30V DC)
Ease of replacement of components
Standard interface, such as MIL-STD-1553
10.4 CONTROL SOFTWARE

The control software generally performed to specification. Communications, local joint
control, calibration and balancing software were tested successfully. Frontal sway
software was tested with mixed success.
10.4.1 LOCAL JOINT CONTROL
As reported in Chapter 9, the local joint control software performed to specification.
The software was robust and predictable. Initial testing revealed that high gains in the
PID loops within the feedback control software led to crosstalk between local joint
controllers. For example, as would be expected, both legs displayed similar natural
frequencies in the frontal plane. While the robot was suspended, with no frictional forces
on the feet and large masses at the extremities, the legs were prone to oscillate in
pendulum motion. Using fine control over hip abduction cylinders led to oscillations in
one leg that caused the opposite leg to vibrate at the same frequency. In turn, the control
system of the second leg would then attempt to reduce the error, injecting more energy
into the system.
10 -8
On several occasions these oscillations became extreme, causing large lateral

movements of the legs. In one instance, an aluminium ladder left close to the robot was
kicked several metres across the workshop. By modifying the software to use two gains
(one generally active, the second active when the error is less than a boundary value), this
behaviour was eliminated. Interestingly, attempts to initiate similar behaviour in the
sagittal plane proved unsuccessful. On investigation it was found that as oscillation
increased, errors were generated in the additional degrees of freedom in this plane (knee,
ankle and toe). As local joint control acted to minimise the errors, the movement of joints
absorbed the energy of oscillation as well as changing the natural frequency of the leg.
The modification of these gains was the only alteration made to local joint control
software.
10.4.2 COMMUNICATION SOFTWARE
The communication software performed to design specification. No modifications or
alterations were made to the communications software after it was commissioned. As the
robot did not achieve dynamic locomotion, the adequacy of a 10Hz control
resolution rate has not been established. However, it is expected that the next prototype
will incorporate a larger number of sensors to deal with compliance in real time.
Currently, the resolution rate is limited by the non-handshaking 19,200bps serial communications between the communications computer and the local joint controllers. To
increase the resolution rate of the control system a higher speed of communications is
necessary between layers of the control system. This will require the use of local joint
controllers with the facility for high-speed communications such as Ethernet or the
Controller Area Network (CAN) which has become the standard in automobiles (Bosch,
2003).
10.4.3 CALIBRATION
The calibration software performed as designed. No modifications were made to the
calibration software in either the local joint controllers or the main control program after
the completion of commissioning. The software proved to be extremely robust and did
not malfunction on any occasion.
10 -9
10.4.4 BALANCE
The balancing software was successfully implemented allowing the robot to balance
and to react to external forces. It was found that balancing is affected by:
The distance between the feet
The gain of the control software
The limits set for the operating point
Increasing the distance between the feet in the frontal plane (stance) resulted in more
robust balance behaviour. However, for the initiation of walk, weight must be transferred
from one leg to the other. As the stance increases, the magnitude and velocity of frontal
sway required to transfer weight will also increase. The motion will then become
exaggerated and more difficult to control. An optimum value of foot width, stance and
control gain must exist. These values can only be determined by the use of a realistic
model of the robot in the frontal plane that also includes compliance in the structure.
As compliance in the structure and joints allows the robot to lean slightly to one side
or the other, the control system must work continuously to reposition the robots joints if
narrow balancing limits are set. Where an industrial robot is carrying a large load, this
behaviour will be exacerbated. Again, the balancing trials have indicated that a stiffer
structure is required.
The balancing trials have highlighted the requirement for an industrial bipeds
operating system and engine to run continuously while not externally supported. The
designers of industrial robots must incorporate some mechanism so that the robot, either
through telescoping supports or other means, is able to be quickly left in an unpowered
condition. Locking the joints into a statically stable configuration while standing will not
satisfy this requirement due to the potential for overbalancing from an external force. As
comical as it may first appear, it would be a distinct advantage for an industrial biped to
possess the ability to sit.
10.4.5 SWAY
The results shown in Chapter 9 show that frontal sway was tested with mixed success.
On occasion, the robot was able to develop a constant oscillation. While the sway
appeared to be well controlled, the behaviour was far from repeatable. During most
trials the robot would commence sway and then either became unstable or the sway
became discontinuous. Initially it was thought that a greater width of foot would
10 -10
provide additional control by increasing the torque available for ankle flexion. Once
wider feet were fitted, an additional frequency was observed in the data acquired from
testing. A theoretical analysis then attempted to determine the cause of the additional
frequency concluding that it was a function of the control system gain, rather than a
dynamic of the structure. However, the analysis was unable to determine the combined
effect of control system torque and compliance in the structure. When frontal sway built
to a magnitude that caused the weight on either leg to approach zero, it was seen that the
leg then swung towards the centre of the robot. Due to the large mass of the foot frame,
compliance in the leg allowed the leg frame to distort elastically as lateral frictional
forces diminished.
From a control perspective, it is a requirement that the structure of the robot be
physically rigid.or the degree of compliance is able to be sensed as an input to the
contol system. This is necessary for two reasons. Firstly, any flex in the structure of the
biped may result in resonant oscillations which are extremely difficult to control.
Secondly, the position of the robots joints are measured either by analogue
potentiometers or digital shaft encoders. Any compliance in the limbs will result in
incorrect measurement of the position of the centre of gravity of the robot. As shown in
Figure 10.1, though the shaft encoders measuring the position of foot, knee and hip may
Figure 10.1 Biped in

single support phase.
10 -11
show the same values in each diagram, the curvature of the robots limbs results in a
position error at the foot. When using ZMP control, this position error will result in an
incorrect computation of the ZMP.
As previously discussed, it is suggested that researchers have found that heavier bipeds
proved more difficult to control. This has certainly been the experience of this project. In
an attempt to determine the effect of scale on the expected deflection between foot and
hip (as shown in Figure 10.1), a basic analysis was undertaken. Assuming the following
parameters;
1) Length of leg = 0.6 x Height of robot
2) Width of hip = 0.15 x Height of robot
3) Width of leg = 0.4 x Width of hip
4) Leg consists of box section where total wall area = 5% of section.
5) Centre of gravity is located over the support foot at extreme of frontal sway.
6) The mass of the robot was calculated as seen in Chapter 2 (Figure 2.8)
When expected leg deflection was graphed as a function of height of the robot, the
relationship illustrated in Figure 10.2 was found. It can be seen that as the height of the
robot increases, the expected deflection of the structure increases cubically. This simple
analysis does not take into account clearances in joints or backlash in actuator systems
which would also increase with the size of the biped. Nor did the analysis take into
(m)
Figure 10.2 Expected leg deflection vs height of biped

10 -12
account functional scale differences such as the increased mass resulting from the
inclusion of power generation and heavier actuators found in larger biped robots.
Naturally, heavier sections can be used to minimise deflection, however this would result
in an increase in weight which would suggest that there may be an optimum section size
for the limbs of a biped robot. The analysis does indicate that the control of deflection
effects is a major issue in the design and control of industrial biped robots. Certainly it
may explain why researchers have reduced the size of their creations from the human size
androids portrayed in 1950s science fiction which inspired them.
Clearly, flexibility in the links of the robot has presented a major challenge in this
project and is the most important area for future work. To determine the effect of flex,
and to investigate methods to control the robot, this issue will be investigated further in
Chapter 11.
10.4.6 PROJECT
In terms of the success, the majority of project goals have been achieved. There is no
question that the scale of the project was enormous for any team let alone an individual.
However, given the strong industry support available the decision was made to build a
full prototype. It was believed that more data would be gathered by a practical project
than a theoretical design study. Roboshift has been designed, built and is still being
tested. All of the mechanical, electrical, hydraulic and control hardware systems
performed as designed. A biped robot weighing half a tonne is able to balance while
being powered and controlled by on board systems. Local joint control software and
communication software operated as designed. The main control software has not been
finalised and will require additional testing and modelling. Time constraints prohibit this
work from being completed prior to the submission of this thesis.
10 -13
11
ADDITIONAL MODELLING
The major difference between a thing that might go wrong and a

thing that cannot possibly go wrong is that when a thing that
cannot possibly go wrong goes wrong, it usually turns out to be
impossible to repair.
- Douglas Adams Mostly Harmless
Traditionally, theoretical and practical robotic research has relied on rigid link
dynamic models to control and predict the performance of robots, the vast majority of
which have been bolted to the factory floor. These machines have been large, possessing
members with sufficient sectional properties to provide high levels of stiffness, making
flexibility negligible. In the case of mobile robotics, slow movement of actuators has
reduced higher order dynamic and elastodynamics. As well, such rigid analysis may also
be applicable to small scale biped robotics where flexibility in links is insignificant due
to the high stiffness to weight factors available in their smaller structures.
As robots move out of the factory and into the field where they must either be selfpropelled or mounted on other vehicles, there is now a requirement for a reduction of
weight in their structure. Reduction in weight, higher manipulator motion speeds and
greater payload demands have led to a reduction in the relative rigidity of structural
members. Additionally, the lack of a rigid connection to the ground requires controllers
to account for cross coupling and low frequency vibrations caused by the reaction of the
supporting structures. Researchers [(Ryu et al., 2000, 2004), (Xu et al., 1999), (Bridges
et al., 1994), (Dubowski et al., 1991)], have recognised the requirement for control and
modelling techniques that include flexibility in their analysis.
For biped robots to be commercially viable, they must be of a scale that offers capability
beyond that achievable by the human. The experiments conducted in this project on a full
scale industrial biped robot indicate that flexibility in the structure of the robot during dynamic motion produces uncontrollable errors in the feedback control system. In an effort to model
and predict the behaviour of an industrial scale robot, a dynamic simulation has been conducted using the MATLAB / Simulink / Simechanics suite. The aim of the simulation is to
both more accurately model the robot and to suggest methods to improve the control system.
11 -1
Chapter 11 Additional Modelling
11.1 MODELING STRATEGY

To complete the dynamic modelling without the use of expensive, complex non-linear
dynamic finite element packages such as ANSYS and DYNAStran, the flexible links of
the robot have been modelled by reducing them to a series of rigid links connected by
torsional spring connections. Figure 11.1 shows a flexible link (a) and the series of rigid
links (b) used to simulate the flexible link.
To determine the torsional stiffness of the springs to be used to model the joint, a finite
element analysis of a typical robot joint was conducted. Assuming that the link is
deformed in the linear elastic range, the sum of the deflections of each simulated joint
will be equal to the deflection of the actual joint. Figure 11.2 shows the deflection map
of the robots lower leg member from the finite element analysis.
Figure 11.1 Modeled Links.
It can be seen that under a load of 1 kN, the deflection of the joint is 3.585mm. Given
that the length of the joint is 400mm, the following equations are used to derive the torsional stiffness of the simulated joint connections;
WTotal = n *Wn
Where; n = Number of links used to simulate the joint.
Wn =
WTotal
n
Wn =
3.585
= 1.2mm
3
11 -2
Figure 11.2 FEA of robotlink.
Given the length of each link, the angle subtended by the deflection Q is calculated as;
1 .2
Q = a tan
= 0.51 deg . = 0.01rad .
133
The torque produced at the joint is constant and equal to that of the original joint, it is
calculated as;
T = F * d = 1,000 * 0.4 = 400 Nm
The stiffness of the springs is then calculated as;

Kt =
400 Nm
= 40kNm / rad
0.01rad
This is the joint stiffness that has been used throughout the MATLAB analysis.
11.2 MATLAB MODEL

The robot has been modelled in MATLAB as a series of cylindrical links for thelinks
with a single mass for the body. Figure 11.3 shows the MATLAB machine model of the
robot. The SIMechanics model of the robot can be found in figure 11.5. It can be seen
11 -3
that the only articulated joints of the robot in the frontal plane are those of the robots
hips and ankles.
Figure11.3 Machine
Model of Biped
The joint driver subsystems (Figure 11.4) are based as closely as possible on the individual joint driving systems of the robot which include a microprocessor and software.
Figure 11.4 Joint

Driver Subsystem
Items of interest in the joint driving subsystem include the saturation function which
restricts maximum and minimum joint actuation force to that available from the
hydraulic actuators of the robot and the Encoder Modification function that converts the
continuous joint position value to discrete values (i.e 2048 counts per revolution) of the
joint encoders.
11 -4
Figure 11.5
Simechanics Model
11.3 EFFECTS OF LINK STIFFNESS.

The simulation was conducted in several stages;
1. Initially the hip and ankle joints were driven sinusoidally with a range of joint
stiffness to determine the effect of link stiffness on the dynamic behaviour of the
model.
2. Once the effect of joint stiffness had been determined, the model was again run
with the stiffness as calculated in section 2.
3. Once the effect of the calculated stiffness had been determined the model was run
with hip control only. The ankle torque was set to zero. In this case, the control
input was the horizontal displacement of the body which was to be kept at zero.
11 -5
To test the model, a square pulse torque input was applied to the hip actuators for
0.5 seconds. The control system then attempted to correct any displacement
produced by the perturbation.
4. The model was run with simulated global control which attempted to equalize the
weight on each leg.
5. The model was run with non-collocated actuators and sensors with link deflection
used as the rate limiting derivative negative feedback.
11.3.1 FRONTAL SWAY DRIVEN BY FEED FORWARD SINUSOIDAL
LOCAL JOINT TRAJECTORIES USING A RANGE OF LINK
STIFFNESS.
To initiate sinusoidal frontal sway, the target position for the hip joint controllers was
driven by a sinusoidal signal representing the target trajectory. The graph found in Figure
11.6 shows the position of the left hip joint for various values of link stiffness.
Frontal Sway Simmulation Freq = 1 Rad/Sec
8
Change in Hip Position (Deg)
0
5
11
13
15
17
19
-2
-4
-6
-8
Time (Sec)
Target
15000 Nm/rad
20000 Nm/rad
25000 Nm/rad
30000 Nm/rad
35000 Nm/rad
40000 Nm/rad
Figure 11.6 Hip Position vs Joint Stiffness.
It can be seen that as stiffness increases position error decreases immediately indicating that the control of the robot is extremely sensitive to the flexibility of the links. Lower
values of joint stiffness show overshoot at the extremities of joint sway.
It can be clearly seen from figure 11.7 that, with a joint stiffness of 1500Nm/mm, the
robots links are flexing as the mass of the robot is still decelerating towards the right of
11 -6
the figure while the ankle and hip joints are driving the body mass back to the left side
of the figure. Clearly any control system that relied only on collocated joint position
indication (the overwhelming majority of theoretical and practical robot control systems)
would exhibit considerable error in the calculation of the instantaneous centre of mass of
the robot.
It has also been found through modelling that the dynamics of the control system are
Figure 11.7
Flexible robot at
limit of Frontal
Sway
extremely sensitive to the gain of the joint controllers. A series of simulation were run
with various control system gains to determine the optimum range of gain control for the
robots control system. Figure 11.8 shows the reaction of the local joint control system to
Figure 11.8 Hip

position plots
for various
values of local
joint control
system gain.
11 -7
a range of values of joint control gain in terms of its effect on the overall control
objectives.
Surprisingly, in this flexible system, higher gains led to more accurate joint trajectories
with no evidence of the joint control sub systems going into unstable oscillations or overshoot. As the stiffness of the joint actuators ( torque produced from control system gain)
exceeds that of the link stiffness, the robots links flex to accommodate any excess
change in position of the joints, effectively providing a level of damping.
From a local feedback control perspective, joint control is excellent. However, the
bending moments induced in links (by change in position of the driven joints at the ankle
and hips) produce accelerations in links that can neither be detected or controlled by a
control system which relies on collocated joint feedback alone. At the extreme of control
system gain, we see the robots legs oscillate in larger and larger displacements while the
central mass of the body remains relatively motionless. This dynamic can be seen in
Figure 11.9 Reaction of robot to high local joint control gain.
11 -8
figure 11.9. It can be seen that while local joint control has been optimized, the global
control of the robot has completely failed. Also notable is the phenomena where once a
multi link structure is made up of flexible elements, the system becomes hyperredundant, the kinematics of which is unsolvable.
One other effect of increased local joint control gain is the appearance of high
frequency transients in local joint control torque. These transients were witnessed in the
practical trial conducted on the full scale robot as discussed in Ch 10.
Figure 11.10
Transients in
control torque
at high control
gains
As can be seen in figure 11.10, the higher frequency transients increase in amplitude
with the gain of the control system. The transients are a function of the global control
software cycle time where position error is able to accumulate between transducer
samples resulting in sudden corrections to control signals.
These transient torques convert to reaction forces at the feet. As the robot measures and
uses the foot reaction forces as feedback for the control system, the forces have the
potential to destabilise the control loop if the control software attempts to react to the
unbalanced forces produced. Given that local joint controllers receive these corrections
at the same frequency as the cycle time of the global control software, there is also the
potential for negative feedback to force the control system into unstable oscillations.
In summary, it has been found that;
11 -9
1. Increased joint stiffness produces more accurate global control.

2. Decreased joint stiffness produces more accurate local joint control.
3. High gains increase local joint control, however the system becomes unstable
with a combination of high gain and low stiffness.
4. High gain leads to high frequency transients as the system reacts to errors
accumulated during the cycle time of the global control system.
11.3.2 FRONTAL SWAY DRIVEN BY FEED FORWARD SINUSOIDAL
LOCAL JOINT TRAJECTORIES USING A CALCULATED MEMBER
STIFFNESS.
Having examined the effects of joint stiffness with the SimMechanics dynamic model,
it was then run with the actual robot stiffness value of 45kN/rad as calculated in section
11.3. Figure 11.11 shows the model closely represents what was found in the data
captured during actual frontal sway trials of the biped robot (Figure 11.12). In both
graphs, the red line represents the transfer of weight from one leg to the other during
sway. Of significant interest is the Bounce that occurs in both the actual trial and
simulation at the left hand extremity of sway. At the time of writing, the reason that the
weight transfer trace is not symmetrical has not been determined. As well, it can be seen
that the actual joint position trace lags behind the target trace by approximately 20 to 30
Frontal Sway Simmulation - Stiffness = 45kNm/rad
8
300
200
100
2
0
0
5
11
13
15
17
19
-100
Change in Ankle Reaction (N)
Change in Hip Position (Deg)
-2
-200
-4
-6
-300
Time (Sec)
Left_Hip_Act
Left_Hip_Tar
Error
Figure 11.11 Sway simulation using actual robot joint stiffness.

11 -10
Figure 11.12 Actual data from robot trials
degrees. Also of significance are the shoulders that appear on the maxima of the blue
actual hip position trace.
The similarity between simulated and actual data indicates that the dynamic model is
valid. Having verified the model, the next stage of the simulation models the response of
the robot and control system to a force impulse.
11.4 SYSTEM RESPONSE TO STEP CHANGE

OF OPERATING INPUT.
To ascertain the ability of the system to operate at a set control point, the control model
was upgraded from an open loop local controller to a closed loop global system where
the horizontal position of the centre of gravity of the body mass in the sagital plane was
used as the feedback signal. The position of the centre of gravity of the body was
provided directly by a SimMechanics position sensor block. While difficult to implement
in the physical model, researchers such as Bascetta and Rocco (2004) have
demonstrated the efficiency of direct feedback of tip actuators by such methods as
visual servoing to reduce indirect measurement errors and physical estimation errors.
11 -11
Dubowsky et al. (1991) provide a method for the dynamic analysis of flexibility in
vehicle mounted manipulator systems. While not directly applicable to legged systems,
the method took into account the effects of low frequency (1.5Hz to10Hz) oscillations
caused by a vehicles suspension system. The model uses FEA to describe the distributed
mass and flexibility of the bodies that make up a total system. The analysis showed that,
for a 25kg mass at the end of a 2.25m three joint arm, deflection increases from 2.25mm
to 12mm when flexibility of the arm links is included in the simulation. Further it was
concluded that such large end effector errors would exceed the ranges of practical
end-point sensing devices.
While real-time detection of the position of the centre of gravity of an industrial scale
biped may not be practical, it was used as a first instance simplification to test the
physical validity of the Matlab model.
The test perturbation consisted of a step input to the simulated target position. Initially,
as was the case with the sinusoidal input, only the hip actuators were used to control the
motion of the robot.
Figure 11.13 Simulation

global control block.
Figure 11.13 shows the global control block which essentially changes the target X
displacement of the body from 1750mm to 2000mm at T=1.5 seconds. As with the main
control software for the robot, the global control task takes information from the
vestibular system, calculates joint trajectories to achieve global task objectives, and then
sends local joint control commands to the local joint control software.
11 -12
The chart found in figure 11.14 records the systems response to the step control input.
It can be seen that the simulated system responds extremely well to the step input. Within
2.5 seconds, the system has settled to the new operating point. Of interest is the green
trace of the reaction error; to transfer weight to the right leg requires a shift in body
position to the right which also requires an acceleration to the right. The trace shows that
the initial acceleration increases the weight on the left leg, reducing the weight on the
right leg. This response makes it extremely challenging to use ground reaction as a
control input for the robots balance and will be discussed in later sections.
Biped Simmulation - Response to Step Input
2150
10000
2100
2050
2000
0
0
6 1950
1900
-5000
position (mm)
Torque Nm | Ground Reaction N
5000
1850
1800
-10000
1750
-15000
1700
Time (Sec)
Hip Torque
Reaction Error
X_Position
Figure 11.14 Step response simulation with low gain.
After the initial success, the simulation was again run with a significantly higher local
hip control gain. Figure 11.15 demonstrates the increased gain resulted in little effect to
the performance of the control system. In both cases the local joint control displays
higher response times to that of the global system; compliance in the links allows the hip
actuators to reach demand positions before significant movement of the body mass has
occurred. This movement causes flexing of thelinks resulting in a build up of elastic
strain energy which then applies the lateral force to the central body. Regardless of the
11 -13
Biped Simmulation - Response to Step Input - High Gain

15000
2100
2050
10000
2000
5000
1950
1900
0
6
1850
-5000
1800
Figure 11.15
Step response
simulation
with high
gain.
-10000
1750
-15000
1700
Time (Sec)
Hip Torque
Reaction Error
X_Position
reaction time of the local joint control (once it is significantly greater than that of the
entire system), it is the stiffness of the robotslinks that determines the response time of
the entire system.
This is a phenomenon which has not been covered by previous biped research
literature.
System Response - 2000Nm pulse - K=45kNm/mm

6000
80
60
4000
40
20
0
0
-2000
10
-20
-40
Position (mm)
Torque (Nm) | Reaction Error (N)
2000
-4000
-60
-6000
-80
-8000
-100
-10000
-120
Time (Sec)
Pulse
Torque
Reaction Error
Figure 11.16
Response of
simulation to
Square Pulse
force input.
Position
11 -14
11.5 SYSTEM RESPONSE TO SQUARE WAVE

FORCE INPUT.
Having established the systems response to a step input, the next phase of the simulation exposed the system to a square pulse force input. The input consisted of a 2000Nm
pulse applied to the hip joints for a period of 0.5 seconds. The pulse generated acceleration and displacement of the robot from the equilibrium position. The simulated control
system then attempted to restore the robot to the initial position. Figure 11.16 shows the
reaction of the system to the pulse.
The graph shows that the system stabilises to 90% of the initial position within 3.5 seconds. Again, as the robot is driven to the left by the green square pulse seen on the graph,
the torque produces an increased ground reaction at the right leg which is contrary to
what may be anticipated at first glance. Similarly, as the control system attempts to drive
the robot back to the right, there is a transfer of weight to the left leg.
Figure 11.17 shows the response of the simulated robot to the same pulse, but with
alink stiffness of 20kNm/mm. In this case, the system stabilises after 5.5 seconds, a significant increase. The orange trace of the weight transfer shows significant bounce at
the extremities of oscillation, an indication that the compliance of the robot is beginning

6000
80
60
4000
20
0
0
10
-20
-2000
Position (mm)
40
2000
-40
-4000
-60
-6000
-80
-8000
-100
Time (Sec)
Pulse
Torque
Reaction Error
Position
Figure 11.17 Response of simulation to Square Pulse force input, k=20kNm/mm..

11 -15

5000
80
4000
60
3000
40
20
1000
0
0
0
10 -20
-1000
-40
Position (mm)
2000
-2000
-60
-3000
-80
-4000
-100
-5000
-6000
-120
Figure 11.18
Response of
simulation to
Square Pulse
force input,
k=10kNm/mm.
Time (Sec)
Pulse
Torque
Reaction Error
Position
System Response - Position

80
60
40
20
0
0
10
-20
-40
-60
Figure 11.19
response of
the simulated
control system
with various
values of joint
stiffness.
-80
-100
-120
Time (Sec)
45000 Nm/mm
10000 Nm/mm
20000 Nm/mm
to affect the operation of the control system. Figure 11.18 shows the simulation with
alink stiffness of 10kNm/mm. As can be seen from the graph, the increased flexibility of
the robot leads to uncontrollable oscillations which effectively make the robot
uncontrollable. The final graph of this series (figure 11.19) shows the position traces
11 -16
from the previous chart on a single graph. Here, the dramatic effect of flexibility on the
effective control of the robot can be seen.
These simulation demonstrate that while the simulated system is able to react to and to
control the robot when subject to a force input, the effectiveness of the control system is
severely degraded by an increase in the flexibility of the system.
11.6 FORCE FEEDBACK CONTROL.
Ultimately, the purpose of this MatLab simulation was to model the behaviour of the
Roboshift biped and its control system. The robot is fitted with force transducers at its
feet which were intended to measure the weight on each foot and the distribution of
weight along the foot. . As seen in previous sections, the reactions at the feet are not only
a result of the weight distribution of the robot, but are also influenced by the torque produced by the robots actuators as shown in figure 11.20.
Figure 11.20 Foot reaction

forces produced by hip torque
These reactions must be dealt with by the control system. In the case of the human, if
the Golgi Tendons detect increased weight on one leg, the body adjust hip posture to
move the centre of gravity of the body toward the other leg. This process results in a temporary increase in the foot reaction on the leg which was initially experiencing the higher foot reaction force. The human nervous system is able to distinguish between forces
produced by gravity and those produced by muscular activity through Propreoception. In
the case of the robots control system, an algorithm was developed to filter actuation
forces from those produced by the dynamics of the robot.
11 -17
Figure 11.21(a) Ground reaction from 10Nm of Hip Torque.
Figure 11.21(b) Ground reaction from 20Nm of Hip Torque.
Figure 11.21(c) Ground reaction from 40Nm of Hip Torque.
Figure 11.21(d) Ground reaction from 80Nm of Hip Torque.

11 -18
Similarly, in the case of this simulation, an algorithm was developed to distinguish

between forces generated by torque at the hips and forces generated by gravity. To determine the component of reaction force generated by hip torque, the model was run with a
range of constant torques applied to the hip joints. The torques were applied with the
robot in the vertical position after a period of one second of simulation time. The charts
of figure 11.21 show an initial oscillation as the model reacts to the step input and then
stabilizes to a constant difference before deviating as the robots center of mass displaces
causing a difference in reaction force as weight is transferred from one leg to the other.
Leg Reaction Difference v's Body Torque
Reaction Difference (N)
250
200
150
100
50
0
0
20
40
60
80
100
Torque (Nm)
Figure 11.22 Relationship between hip torque and foot ground reaction..
These forces were plotted for each value of hip torque as seen in figure 11.22 where it
can be seen that the relationship between hip force and foot reaction force is linear. The
model was then modified to incorporate an algorithm to account for these forces.
Figure 11.23 Modified Simulink

Model with torque compensation
11 -19
Using this relationship, the Simulink model was updated to detect the torque, calculate
the dynamic ground reaction and filter these forces from the static ground reaction.
Figure 11.23 shows the modified section of the model. The left and right components of
hip torque are sensed and converted to reactions which are then filtered from the ground
reaction input.
Biped Simmulation - Response to Pulse Input - Balance Control
35
10000
8000
30
6000
25
4000
20
2000
15
0
0.5
1.5
-2000
2.5
3
10
-4000
5
-6000
0
-8000
-10000
-5
Time (Sec)
Hip Torque
Reaction Error
X_Position
Figure 11.24 Plot of Torque, hip displacement and Ground reaction
Figure 11.24 shows the reaction of the system using the compensated reactions. It can
be seen that the system initially stabilises from the force input. The system then reacts to
residual cross coupled forces which can not be filtered, pushing the system into unstable
oscillation. Data from actual trials show similar characteristics.
While the use of compensation is able to tune the controller for steady state increases
in torque, the controller becomes unstable when the control system begins to hunt at the
control point where rapid changes in the size and direction of error result in unstable
feedback to the controller. This would particularly be the case where external influences
such as impulse torques from other joints and impacts from foot falls would cause significant moment increases or decreases in the joint being controlled. Based on these
results, an alternate method of control was required.
11 -20
11.7 STRAIN FEEDBACK

Previous researchers have addressed the problem of flexibility in the joints of robots
while attempting to control the position of an end effector. Hauschild (2003) describes the
use of a Passive Controller where the gain of the PD loop is modified by an observed
Flexibility Factor. The flexibility factor assumes that the relationship between input
voltage to a joint drive motor and the resultant torque is linear. Based on that torque, the
flexibility factor can be chosen. This method of control also assumes that regardless of the
other torques and forces acting on the end effector, the bending moment in thelink, and thus
the degree of flex is only proportional to the torque produced by the joint drive motor.
Vareta & Montseny (2001) presented the concept of Pseudo-Invariant Control of a
flexible beam where the feed back control is expressed under an input-output partial DE
system with an easy to implement finite-dimensional approximation. They suggest this
method produces shorter settling times than standard PID control as the oscillations from
flexible nodes are taken into account by producing an efficient decay by impedance
matching and a reduction in mechanical energy of the beam. While not relying on an
accurate position feedback of the end mass, this method still fails to take into account
forces and moments acting on the member from the rest of the system. As well, any
impedance matching will be modified by the overall geometry of the system.
Ryu and Kwon (2000, 2000, 2004) suggest that having non-collocated sensors and
actuators (such as a position sensor on the end effector) may result in an active unstable
closed loop system for all but a narrow envelope of gain of the feedback controller. They
suggest the Time-Domain Passivity Approach where the connection between trajectory
generator and controller is modified by the addition of energy based virtual feedback
conjugate variable.
Mansour and Karkoub (1997) have used m-synthesis Robust Control to model two-link
flexible manipulators. They use Timeshenko Beam Theory and assumed modes methods
to derive reference equations of motion for the flexible manipulator. The method is
claimed to be robust from high frequency dynamics. The controllers use hub angle and
tip accelerations as feedback. While this method may be of value to small high speed
flexible systems where high frequency transients are problematic, and the tip
accelerations can be directly measured, they are inappropriate for larger heavier flexible
systems where steady state and low frequency deformations exist.
11 -21
Xu et al. (1999) offer a method to model the kineto-elastodynamics of flexible links

which generates a stiffness matrix for the system based on the calculation of elastic strain
energy using Euler-Bernoulli beam theory. Initially Lagrange equations are derived for
the system elements. Differential equations for the links are then assembled and then the
system differential equations. The model takes into account gravity, Corliolis and
centrifugal effects. The method looks extremely encouraging; however the paper offers
no simulation output and presents no practical data.
Having reviewed the relevant literature, it was decided to implement a controller which
included feedback on the amount of elastic strain in the link members and the rate of rise
of link member strain. Figure 11.25 shows the modified controller. It can be seen that the
model includes lookup tables to implement the transfer functions of strain feedback. As
well, moving averages were applied to eliminate higher frequency feedback. Finally
gains were applied to control the level of feedback for each of the channels which were
summed to produce the final global control inputs. Immediately that these modification
were made the model became robust and stabilised.
The final model uses ground reaction error, left and right hip torque, the bending
moment of the left and right thigh members and the rate of change of the bending
moment of the left and right thigh members as feedback for the controller. The
performance of the modified model demonstrates the addition of strain feedback
stabilised the controller. Multiple configurations of feedback and feedback gains were
trialed until the optimum combination was found.
Figure 11.26 shows the controllers response to a step input with and without strain
feedback. The degree of unstable oscillation evident without strain feedback (as
compared to that of the unmodified force feedback controller of Figure 11.24)
demonstrates the degree the gain of the global control system was able to be increased
once stabilised by strain feedback.
Also of note is that only one of the strain signals (thigh strain or thigh strain rate) were
required to stabilise the model. The absence of either feedback signal would result in a
less damped system, but would still allow a significant increase in global controller gain
than was possible for the previous system without strain feedback.
11 -22
11 -23
Reaction of System with and without Flex Feedback.
8000
6000
Reaction Error (N)
4000
2000
0.75
1.25
1.75
2.25
2.75
3.25
3.75
4.25
4.75
-2000
-4000
-6000
Time (sec)
Step Control Input
System Resonse Without Strain Feedback
System Response with Strain Feedback
Figure 11.26 System with and without strain feedback.
Effect of Negative Feedback of Thigh Deflection

900
Reaction Error (N)
700
500
300
100
0.75
-100
1.75
2.75
3.75
4.75
5.75
6.75
7.75
-300
Time (sec)
Flex Gain = 4
Flex Gain = 2
Flex Gain = 0
Flex Gain = -2
Flex Gain = -4
Figure 11.27 Effect of Strain Feedback Gain.
11 -24
To determine the optimum values of feedback gain, a range of gains were applied to
each feedback signal while the other feedback gains were held constant. Figure 11.27
shows the effects of thigh strain feedback gain. It can be seen that as the gain increases,
the overshoot increases which should be expected in any normally damped system.
1000
Effect of Negative Feedback of Time Derivative of Thigh Deflection
800
Reaction Error (N)
600
400
200
-200
0.75
1.75
2.75
Flex Deriv. Gain = 0.0

3.75
4.75
Time
(sec)
Flex Deriv.
Gain
= 0.4
5.75
6.75

Figure 11.28 Effect of Strain Feedback Gain.
Effect of Negative Feedback of Ground Reaction
2200
1700
Reaction Error (N)
1200
700
200
-300
-800
0.75
1.75
2.75
3.75
4.75
5.75
6.75
7.75
Time (sec)
React Gain = -0.001
React Gain = -0.002
React Gain = -0.0015
React Gain = -0.00175
Figure 11.29 Effect of Ground Reaction Feedback Gain.

11 -25
Figure 11.28 shows the effect of thigh strain rate feedback gain. The stability of the
controller appeared to be extremely sensitive to this input. High values of gain produces
oscillations for high values of joint torque which is expected as high torque will produce
high rates of strain in the thigh member.
Ground reaction is the variable that is controlled by the system. Again, as feedback gain
increases Figure 11.29 shows the effects of increase gain of ground reaction feedback on
the controller. It can be seen, even with lower values of gain, the initial reaction of the
system to an unbalanced ground reaction is an increase in that reaction as the actuators
drive the robot in the opposite direction (in the frontal plane) to the leg that is
experiencing the higher force. This increase is in the vicinity of 2 to 3 times the normal
foot force. For an industrial scale biped, this force will be in the order of 2kN greater than
the normal static load. To withstand this load and to minimise resulting deflection, the
structure of the robot must be substantial which increases the weight of the robot.
Proportionally, actuators and motive force generators must be increased in size causing
additional weight in the total structure. It is suggested that there is an optimum scale and
weight for an industrial scale biped. The determination of this scale as a linear
programming problem presents an interesting challenge for future work.
Figure 11.30 shows the effect of the integral of ground reaction feedback gain on the
system. It would appear that the optimum value of gain lies in the vicinity of 0.00015.
Movement in either direction from this value (while holding all other gains constant),
increases overshoot and instability. Essentially the global controller is of the PID type
where the derivative input is not taken from the ground reaction force (the variable being
controlled). Instead, the rate limiting term is taken from the second order effect of thigh
strain. Due to the flexible nature of the structure, dynamics joint motion is absorbed by
the compliance of the link. As a result, there is no dynamic motion of the end of the link
to drive rate limiting, derivative feedback. In a flexible structure, this term can be
provided by feedback of the rate of change of deformation of the link.
The process of determining optimum values of feedback gains was an arbitrary one
based on previous observations of system behaviour. The process was repeated a number
of times as the value of each gain was determined and then held constant as other gains
were adjusted. Figure 11.31 shows the system response to the force input for a number
of combinations of feedback gains.
11 -26
Effect of Negative Feedback of Integral of Ground Reaction
1700
Reaction Error (N)
1200
700
200
-300
-800
0.75
1.75
2.75
3.75
4.75
5.75
6.75
7.75
Time (sec)
React Int Gain = -0.00015
React Int Gain = 0.0004
Figure 11.30 Effect of Integral of Ground Reaction Feedback Gain.
1100
Effect of Negative Feedback of Combinations of Input Gains
900
Reaction Error (N)
700
500
300
100
-100
0.75
1.25
1.75
2.25
2.75
3.25
Time (sec)
RI 0.00015, RG -0.0015, FD -0.055, FG -6
RI 0.0003, RG -0.0015, FD -0.075, FG -6
RI 0.0004, RG -0.0015, FD -0.055, FG -6
Disturbance (500N Step)

RI 0.0003, RG -0.0015, FD -0.055, FG -6
RI 0.0004, RG -0.0015, FD -0.055, FG -6 (35,000kn/m)
Figure 11.31 Effect of Combinations of feedback to step perturbation.
11 -27
Reaction of System to Step Input
500
Reaction Error (N)
300
100
-100
-300
-500
-700
0.75
1.25
1.75
2.25
2.75
3.25
3.75
4.25
4.75
Time (sec)
Step Control Input
RI 0.00015, RG -0.0015, FD -0.055, FG -6
"RI 0.000175, RG -0.0015, FD -0.055, FG -6"
"RI 0.0002, RG -0.0015, FD -0.055, FG -6"
Figure 11.32 System Response to Step Input
Reaction of System to Step Input
700
600
500
Reaction Error (N)
400
300
200
100
-100
-200
-300
0.75
1.75
2.75
3.75
4.75
5.75
6.75
Time (sec)
Ramp Control Input
RI 0.0002, RG -0.002, FD -0.055, FG -6
RI 0.0002, RG -0.0015, FD -0.055, FG -6
Series4
Figure 11.33 System Response to Ramp Input
11 -28
Having established a number of configurations of gains where the system behaved in a

stable manner, the model was run with both a step and ramp change to the control input.
Figures 11.32 and 11.33 show that system remain stable.
11.8 SUMMARY OF SIMULATION

This chapter has described the development of a SimMechanics model to describe and
analyse the dynamic behaviour of an industrial scale biped robot.
1. Initially, finite element analysis (FEA) was used to determine the stiffness of a
typical structural member of the robot.
2. A dynamic model of the robot has been constructed to determine the effects of
flexibility in the structure of the robot. This analysis demonstrated;
a. Increased joint stiffness produces more accurate global control.
b. Decreased joint stiffness produces more accurate local joint control.
c. High gains increase local joint control, however the system becomes unstable
with a combination of high gain and low stiffness.
d. High gain leads to high frequency transients as the system reacts to errors
accumulated during the cycle time of the global control system. These
transients were evident in the data produced from practical trials of the robot.
3. The model was then run with the stiffness calculated from the FEA to model the
dynamic behaviour of the robot to a range of inputs. This analysis was validated
by the similarity between predicted performance and that measured during
practical trials of the robot.
4. Having identified flexibility as a cause of feedback error in the simulation, the
model controller was upgraded to include non-collocated sensors to measure
deformation in links. The amount of strain and rate of change of strain were used
as negative feedback inputs to the controller.
5. The simulation showed that the optimised model was stable over a range of
feedback gains when subjected to a pulse perturbation, a step change of operating
point and a ramp change of operating point.
This is the first fully dynamic model of an industrial scale biped in frontal plane
balance to have been analysed.. The model demonstrates the most optimal means of
controlling such a device is by the use of rate of change of link deformation as the rate
limiting term in a PID controller.
11 -29
12
CONCLUSION
I think and think for months and years. Ninety-nine times, the
conclusion is false. The hundredth time I am right
- Albert Einstein
The work presented in this project has shown that the current expansion in humanoid
robot research is driven more by the 1950s vision of androids than any intent to provide
a solution to an existing industrial problem. Humanoids such as Hondas Asimo and
Toyotas trumpet playing humanoid have been used to promote those companies
technology in a spectacular way. Justification for this most demanding and expensive
research is based on philanthropic models that suggest these robots will attend to the
bedridden or would replace humans in dangerous environments. However, the cost of a
biped robot when compared to that of a wheeled or tracked vehicle restricts commercialisation for these applications. In addition, the size and working capacity of current
humanoid robots is not compatible with the heavy lifting requirements found in these
applications.
This project has shown the main commercially viable application for an autonomous
biped is the handling of materials in confined, dangerous or other environments inaccessible to traditional materials handling equipment. The project represents the first attempt
to investigate the feasibility of a fully autonomous industrial scale biped robot and the
first credible research into the challenges of controlling such a machine. As no previous
data was available to suggest operational parameters for this class of device, a set of
design criteria were determined. From these criteria, an industrial scale biped robot,
named Roboshift, has been designed, built and tested.
12.1 ACHIEVEMENT OF DESIGN CRITERIA

In terms of the design criteria, which at no stage were envisioned as operational
parameters of the first prototype, the following observations are made;
The robot must be easily maintainable.
The complexity in terms of control and mechanical systems presented by a bipedal
motion base over that of a wheeled base is considerable. This project has demonstrated
12 -1
Chapter 12 Conclusion
that the level of reliability required of these systems is compatible with that required of
avionics. However, by using commercial off the shelf (COTS) mechanical and
electronic hardware to build the robot, the project has also demonstrated that such
vehicles can be manufactured from readily obtainable and replaceable components. As
well, by grouping components such as the power unit, hydraulic and pneumatic pumps,
electrical generation and control systems, the ability to quickly swap out faulty
components makes customer level repair and maintenance a reality. This design
criterion has been integrated and demonstrated.
The robot must be able to work for long periods.
The use of a liquid petroleum gas powered internal combustion engine has shown that
alternative energy to large and expensive battery packs can be used to power industrial
scale bipeds. Practical trials of Roboshift and recording of hydraulic oil pressure have
shown that the use of a fan forced hydraulic oil cooler allows for the use of a minimum
capacity hydraulic oil reservoir making hydraulic actuation a realistic alternative. This
design criterion has been achieved.
The robot must be self contained.
All mechanical, electrical, electronic and control systems required by Roboshift are
integral to the robot. No other external components, systems or power required to start or
run the robot. As such, Roboshift was the first self contained industrial scale biped to
have been built. This design criterion was met.
The robot must be robust both physically and electronically.
Roboshift proved to be surprisingly robust, when taking into account the complexity of
the systems required and the size of the available budget. All of the on board systems
were built from COTS industrial grade components. The robot has survived in an heavy
engineering workshop for several years without the ingress of contaminants, grit
particles, oil or solvents. The robot has only suffered two mechanical failures, both of
which resulted from software errors where hydraulic cylinders were driven in opposing
directions while limbs were in contact. The structure, though flexible has proven to be
extremely robust. This design criterion has been met.
The robot must have the capacity to lift 250kg.
While this aspect was not demonstrated, the robots structure was designed for this
12 -2
capacity. The flexibility seen in the system would suggest that a load of 250kg would
increase the degree of flex, further increasing the position error exhibited. However, the
additional modeling included in Chapter 10 would indicate that the inclusion of
non-collocated sensors would stabilize the control system.
The robot must be able to traverse 500mm discontinuities.
This design criterion influenced the scale of the device and the geometry of the
structure. While the robot did not meat this criterion in terms of an operational
parameter, it was built to a scale that satisfied the criterion.
12.2 NOVEL RESEARCH

Based on research reported in the current relevant literature, this project presents the
following novel research:
A foundation set of design criteria for an industrial scale biped robot have been
determined.
A comprehensive, self contained, full scale prototype of an industrial biped robot has
been designed and constructed.
Roboshift is the first industrial scale robot to be fully self contained, carrying
on-board all power, actuation and processing systems required for continuous and
extended operation. At 2.4 metres in height and 550kg in weight, Roboshift is the
heaviest autonomous biped robot yet built.
Roboshift is the first industrial scale biped to demonstrate active balance in the
frontal and sagittal planes, and to achieve frontal sway.
The experiments conducted on the biped robot represent the first credible research
into the challenges presented by an industrial scale biped robot in terms of its design,
construction, power and control systems.
The project is the first research to identify compliance as a major issue in the
operation and control of an industrial scale biped. It is also the first research to
dynamically model a large scale biped robot and to provide solutions to the control
of a compliant biped.
The project has established the requirements of the control system of an industrial
biped.
Testing of Roboshift has established the following:
The mechanical design of the robot was successfully implemented. All joints were
12 -3
able to be moved through their full range of movement.

The control hardware and software were able to calibrate the robots joints and were
able to control the robot in active balance. While frontal sway was demonstrated,
control over this behaviour was compromised by compliance in the structure.
One major challenge in the control of an industrial scale biped is compliance in joints
and structural members. This compliance increases as the cube of the height of a
biped. Future bipeds must either optimise the structural design to decrease
compliance, or incorporate transducers to accurately measure its effects.
The use of an internal combustion engine and hydraulic power pack to drive the
motion of an industrial scale biped is feasible. By using a small hydraulic reservoir
in combination with a large fan forced oil cooler, oil temperatures can be maintained
at operational temperatures.
The incorporation of suitable vibration isolation mounts demonstrates that an
internal combustion engine can be operated without adverse effects on the
performance of control system processors or transducers.
Due to the inherent instability of bipedal locomotion, control systems of industrial
bipeds will more closely represent those of aircraft in terms of the mission critical
relationship between layers of the control system. Military or aviation standard
embedded processor and data acquisition systems would be the starting point for the
design of any control system for an industrial biped.
12.3 FUTURE WORK

Areas of future work in this project include:
The continuation of frontal balance and locomotion trials.
The formulation of a dynamic frontal sway model which incorporates compliance in
the structure of the robot.
Comparison between theoretical output of the upgraded dynamic model and
experimental data acquired from trials of the robot.
Industrial scale biped robots will be commonplace at some time in the future. Roboshift
is a major contribution towards the realisation of this vision. This work has advanced
research in the field of industrial robotics and has established the major characteristics
and challenges facing the designers of future industrial bipeds.
12 -4
REFERENCES
AIST [2002] Success in Having a Humanoid Robot Drive an Industrial Vehicle
Outdoors, www.aist.go.jp/aist_e/new_research/2003/20030120_2/20021219_2.html
[Accessed 17/01/04]
AIST [2003] Life-sized Humanoid Capable of Getting Up and Lying Down, AIST
Today, International Edition, No.7, 2003.
Android World [2004] Worlds Greatest Android Projects,
www.androidworld.com/prod01.htm, [Accessed 17/01/04].
Azevedo C., [2000] Control Architecture and Algorithms of the Anthropomorphic
Biped Robot Bip2000, www.enm.bris.ac.uk/anm/staff/
enmdb/SICONOS/D61_INRIA.pdf, [Accessed 10/11/03]
Baltes, J., McGrath, S., Anderson, J. [2004] Active Balancing Using Gyroscopes for a
Small Humanoid Robot, Proc. 2nd International Conference on Autonomous Robots ad
Agents, Palmerston North, NZ, 2004.
Bascetta, L., Rocco, P. [2004] Tip Position Control of Flexible Manipulators through
Vsiual Servoing, Proc. 6th Conference on Dynamics and Control of Systems and
Structures, Riomaggione, Italy, 2004.
Batlle, J., Hospital, E., Salellas, J., Carreras, M [1999] Development of a biped robot,
Proc. 2nd Int. Workshop on European Scientific and Industrial Control, Newport,
Wales, 1999.
BBC News [2003] Humanoid robots wow Japanese, news.bbc.co.uk/2/hi/technology
/2919345.stm. [Accessed 12/01/03]
Bosch, [2003] CAN Home page, www.can.bosch.com, [Accessed 10/11/03]
Brooks, R., [1986] A Robust Layered Control System for a Mobile Robot., IEEE
Journal of Robotics and Automation, 2 (1), 1986.
Carlo, A., Fabrizio, F., Enrico, M., Giuseppe, R., Alberto, T. [2005] The Isaac project:
development of an autonomous biped robot, Information Technology, Vol. 47. #5, 2005.
R-1
References & Bibliography
Choong, E., Chew, C. M., Poo, A. N., Hong, G. S. [2003] Mechanical Design of an
Anthropomorphic Bipedal Robot, 1st Humanoid, Nanotechnology, Information
Technology, Communications and Control Environment and Management International
Conference, Manila, Philippines, 2003.
Cronin J., Willgoss R., Ford R. [2003] Improved Joint Control Using a Genetic
Algorithm for a Humanoid Robot, Proceedings Australian Conference on Robotics and
Automation (ACRA2003)
Cronin J., Frost R B., Willgoss R. [1999] Walking Biped Robot with Distributed
Hierarchical Control System, Proceeding IEEE International Symposium on
Computatuonal Intelligence in Automation and Robotics.
DSouza A., Vijayakumar S., Schaal S. [2001] Learning Inverse Kinematics,
Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and
Systems, p298-303.
Davydova, A., [2003] Local Firm Leading Russian Robot Race, THE ST.
PETERSBURG TIMES #889, Friday, August 1, 2003.
Devy-Vareta, F. A., Montseny, G. [2001] Pseudo-invariant H2 multivariable robust
control of a flexible beam, Proc. European Control Conference, Portugal, 2001,
pp2638-pp2643.
Dubowsky, S., Gu, P. Y., Deck, J. F. [1991] The Dynamic Analysis of Flexibility in
Robotic Manipulator Systems, Proc. VIII World Congress on the Theory of Machines
and Mechanisms, Prague, Czechoslovakia, 1991.
Dunster P., [2000] F1 Controller Home Page, www.cncteknix.com/users/f1/,
[Accessed 20/03/04]
Freerepublic [2003], Russia Joins Humanoid Robot Race, 209.157.64.200/focus/fnews/958506/posts, [Accessed 17/01/04]
Fukaya N., Toyama S., Asfour T., Dillman R. [2000] Design of the TUAT/Karlsruhe
Humanoid Hand, www.sfb588.uni-karlsruhe.de/publikationen/2000_16.pdf [Accessed
10/01/04]
R-2
Furusho, J., Masabuchi, M. [1986] Control of a Dynamic Biped Locomotion System,

Journal Of Dynamic Systems Measurement and Control, (V108) p111-118.
Golliday, C.L., Hemami, H, [1977] Analysing Biped Locomotion Dynamics and
Designing Robot Locomotion Controls, IEEE Transactions on Automatic Control
Graser A. [2003] Programming an Artificial Hand for Grasping Objects Using
Cyberglove, www.iat.uni-bremen.de/InstitutsArbeiten/Ag/
Arbeiten/she002_ForceSensor_PbD_Eng.html, [Accessed 13/10/03]
Gurfinkel,V.S. [1981] Walking Robot with Supervisory Control, Mechanisms and
Machine Therory, V16,P31-36.
Hartrum, T.C. [1973] Computer Implementation of a Parametic Model of Biped
Locomotion, PhD Thesis.
Hemami, H., Weimar, F.C., Koozckanani, S.H. [1973] Aspects of Inverted Pendulum
Problem for Modelling Locomotion Systems, IEEE Trans Automatic Control,
p(ac)(18)658-661.
Hemami, H. [1977] The Inverted Pendulum and the Biped, Mathematical
Biosciences, p(34) 95-110.
Hollander, K. W., Sugar, T., G. [2004] Concepts for Compliant Actuation in Wearable
Robotic Systems, Proc. US-Korea Conference on Science, Technology and
Entrepreneurship, North Carolina, 2004.
Honda [2003] Say Hello to ASIMO, www.asimo.honda.com, [Accessed 17/01/04]
Hugel, V., Abourachid, A., Gioanni, H., Mederreg, L., Maurice, M., [2003] The
RoboCoq project: Modelling the design of a bird-like Robot Equipped with Stabilised
Vision, Proc. 2nd In. Symposium on Adaptive Motion of Animals and Machines,
Kyoto, Japan, 2003.
Honda [2003] Asimo P1 P2 P3,
http://world.honda.com/ASIMO/history/p1_p2_p3.html, [Accessed 12/01/04]
Isik, C., Meystel, A.M. [1988] Pilot Level of a Hierarchical Controller for an
Unmanned Mobile Robot, IEEE Jouranl of Robotics and Automation, (V4)p241-255.
R-3
Johnstone, B. [1985] Walking Robot Prepares for Rough Ride, New Scientist,
(V107) p32.
Kaist [2002] Kiast Humanoid Robot Platform -1,
ohzlab.kaist.ac.kr/khr_robot/khr_humanoid.html [Accessed 17/01/01]
Kato, I., Fujie, M., Yoshida, T. [1987] Development of the Legged Walking Robot,
Hitachi Review, (V36#2)p71.
Kato, I., Ohteru, S., Kobayashi, H. [1974] Information-Power Machine With Senses.
p11-24.
Kawada, [2001] HRP-2P, www.kawada.co.jp/ams/hrp-2/index.html [Accessed
17/01/04]
Kawada, [2001] Humanoid robot Isamu, www.kawada.co.jp/ams/Isamu/index.html
[Accessed 17/01/04]
Kibertron [2002] Report for fullsized self-governing humanoid Kibertron,
www.kibertron.com, [Accessed 17/01/04]
Kitamura, S., Kurematsu, Y., Iwata, M. [1990] Motion Generation for Biped Robot,
IEEE Conference on Decision and Control, p3308-3312.
Kim J. H., Park, S. W., Park, I. W., Oh, J. H., [2002] Development of a Humanoid
Biped Walking Robot Platform KHR-1 - Initial Design and Its Performance Evaluation,
Proceedings of 3rd IARP International Workshop on Humanoid and Human Friendly
Robotics, pp. 14-21, Tsukuba, Japan, Dec. 11-12, 2002.
Kumagai M., [2001] Biped Robot Monroe,
www.mechatronics.mech.tohoku.ac.jp/~kumagai/ research/monroe/biped_e.html
[Accessed 17/01/04]
Lee, W.H., Mansour, J.M. [1984] Linear Approximations for Swing Leg Motion
During Gait, Journal of Biomechanical Engineering, (V106) p137-143.
Liston, R.A. [1964] Walking Machines, Journal of Terra Mechanics, pp1-3-18-31.
R-4
McGhee, R. B. and Iswandhi, G. I. [1979] Adaptive Locomotion of a Multilegged

Robot over Rough Terrain, IEEE Transactions on Systems, Man and Cybernetics, 9(4),
1979, pp176-pp182.
McMahon, T.A. [1984] Mechanics of Locomotion, International Journal of Robotics
Research, p(3)(2)4-28.
Mansour, A. K., Tamma, K. [1997] Modelling and u- synthesis Robust Control of Twolink Flexible Manipulators, Proc. 5th IEEE Mediterranean Conference on Control and
Systems, Paplos, Cyprus, 1997.
Milper [2003] RAC-100 Rugged Computer, www.milper.com/product_rac100.html.
[Accessed 20/02/04]
MIT [1999] MIT team is building a humanoid robot,
www.mit.edu/newsoffice/1999/cog.html, acc: 20/11/05
MIT [2001] M2, www.ai.mit.edu/projects/leglab/robots/m2/m2.html , [Accessed
12/02/04]
MIT [2005] Leg laboratory Home Page, www.ai.mit.edu/projects/leglab/homemain.html, acc:24/11/05.
Morrison, R. A. [1968] Iron mule train Proceedings of Off-Road Mobility Research
Symposium. International Society for Terrain Vehicle Systems, Washington, D.C..
1968. pp. 381-400.
Murray, M.P., Drought, A.B., Kory, R.C. [1964]. Walking Patterns of Normal Men.
Journal of Bone & Joint Surgery, 46A, 335-360
Nashner, L.M. [1980] Balance Adjustments of Human Perturbed While Walking,
Journal of Neurophysiology, p(44)650-664.
NDEAA [2003] Worldwide Electroactive Polymer Actuators Webhub,
ndeaa.jpl.nasa.gov/nasa-nde/lommas/eap/EAP-web.htm, [Accessed 13/10/03]
New Scientist [2004] Artificial Exoskeleton takes the strain.,
www.newscientist.com/news/news.jsp?id=ns99994750, [Accessed 10/03/04]
R-5
Popovi_, D., Stein, R.B., Oguztreli, M.N., Lebiedowska, M., Joni_ S., [1999] Optimal
Control of Walking with Functional Electrical Stimulation: A Computer Simulation
Study, IEEE Transactions on Rehabilitation Engineering, vol. 7, no. 1, pp. 69-79,
March 1999.
Pratt J E., Pratt G A. [1999] Exploiting Natural Dynamics in the control of a 3D
Bipedal walking simulation, Proceedings 1999 International Conference on Climbing
and Walking Robots.
Raibert, M.H. [1986] Legged Robots, Communications of the ACM, V29,
p499-514.
Roberts J, Kee D, Wyeth G. [2003] Improved Joint Control using a Genetic
Algorithm for a Humanoid Robot, Proceedings Australian Conference on Robotics and
Automation (ACRA2003)
Ryu, J. H., Kwon D. S., Park Y. [2000] A Robust Controller Design Method for a
Manipulator with Large Time Varying Payload and Parameter Uncertainties, Journal
of intelligent and Robotic Systems 27, 2000, pp345-pp361.
Ryu, J. H., Kwon D. S., Hannaford, B. [2004] Control of a Flexible Manipulator with
Noncollocated Feedback: Time-Domain Passivity Approach, IEEE Trans. On Robotics,
Vol. 20, No. 4, August 2004, pp771-pp781.
Senior, A., Tosunoglu, S. [2005] Robust Bipedal Walking: The Clyon Project, Proc.
18th Florida Conference on Recent Advances in Robotics, FCRAR, 2005.
Shadow Robot Company [2003] The Shadow Biped,
www.shadow.org.uk/projects/biped.shtml, [Accessed 13/10/03]
Shahinpoor, M. [1987] A Robot Engineering Text Book, Harper and Row Publishers,
New York and London (1987).
Shigley, J., E. [1962] The Mechanics of Walking Vehicles, Proceedings 1st Int. Conf.
on Mechanics of soil-Vehicle Systems, Turin, Italy, 1962.
Shih, C.L., Gruver, W.A. [1992] Control of a Biped Robot During Support Phase,
IEEE Transactions on Systems Man and Cybernetics, (V22I4) p729-735.
R-6
Shimoyama, I., Miura, H., Kimura, H. [1985] Control System for Walking Robot ,
Dynamic, p357-363.
Sias, F., Zheng, F. [1987] Static Stability Problems in Biped Robot During Double
Support Phase, 0094-2898/87/0000/0436$01.00 1987 IEEE, p436.
Sony, [2000] Sony Develops Small Biped Entertainment Robot,
www.sony.net/SonyInfo/News/Press/200011/00-057E2/, [Accessed 18/11/03]
Sony [2003] Research and development, Sony Corporation Annual report, 2003.
Takanishi, A., Ogura, Y., Itoh, K. [2005] Some Issues in Humanoid Robot Design, Proc.
12th Int. Symposium on Robotics and Robots, San-Francisco, CA, 2005.
Time Magazine [2000] Mans Best Friend, www.time.com/time/asia/magazine
/2000/0501/japan.bestfriends.html, [Accessed 12/02/02]
Todd, D.J., [1984] Experimental Study of Pneumatic Robots, Digital Systems for
Industrial Automation.
TUM [2003], Johnnie the TUM Biped Walking Robot, www.amm.mw.tumuenchen.de/Forschung/ZWEIBEINER/johnnie_e.html, [Accessed 8/02/03]
University of Bath. [2002] History of Walking Robots,
www.bath.ac.uk/entji/history.htm, [Accessed 14/02/04]
US Dept. of Labor [1997] Injuries to Caregivers Working in Patients Homes,
www.bls.gov/opub/ils/pdf/opbils11.pdf [Accessed 12/02/04]
VUB [2002] Bipedal Walking Robot Lucy, www.lucy.vub.ac.de, [Accessed 17/01/04]
Vanderborght, B., Verrelst, B., Van Ham, R., Vermeulen, J. [2004] Control Architecture
of LUCY, a Biped with Pneumatic Artificial Muscles, 7th Int. Conf on Climbing and
Walking Robots, Madrid, Spain, 2004.
Vaughan, E. D., Di Paolo, E., Harvey, I. R. [2004] The tango of a load balancing robot,
Proc. 7th Int. Conf. on Climbing and Walking Robots, Madrid, Spain, 2004.
R-7
Vermeulen, J., Lefeber, D., Verrelst, B., Vanderborght, B. [2004] Trajectory Planning
for the walking Biped LUCY, 7th Int. Conf on Climbing and Walking Robots, Madrid,
Spain, 2004.
Vukobratovic, M. [1973] Dynamics and Control of Anthropomorphic Mechanisms,
Proc 1st Symposium on Theory and Practice of Robots and Machine Intelligence,
p313-332.
Vukobratovic, M. [1973] Legged Locomotion Robots/Math Models, Control
Algorithms and Realizations, 5th IFAC Symposium on Automatic Control in Space,
Genoa.
Vukobratovic, M. [1970] Control and Stability of One Class of Biped, ASME
Transactions on Basic Engineering, p328-332.
Wagner, N., Mulder, M.C., Hsu, M.S. [1988] A Knowledge Based Control Strategy
for a biped, CH2555-1/88/0000/1520$01.00 IEEE, p1520-1524.
Weiss P. [2001] Dances with Robots (Exoskeletons for soldiers), http://www.sciencenews.org/articles/20010630/bob8.asp [Accessed 8/02/03]
Wolfe A., [2004] Toyotas Next-Generation Computers: Robots,
www.internetnews.com/ent-news/article.php/3326221, [Accessed 10/03/04]
Wollher, D., Buss, M., Hardt, M., von Stryk, O. [2003] Research and Development
Towards an Autonomous Biped Walking Robot, proc. Int. Conf on Advanced Intelligent
Mechatronics, Kobe, Japan, 2003.
XU, X. R, Chung, W. J., Choi, Y. H. [1999] Modelling of Kineto-Elastodynamics of
Robots with Flexible Links, Proc. IEEE Internat. Conf. On Robotics and Automation,
Detroit, Michigan, USA, May 1999, pp753 pp758.
Yamaguchi G. T., Zajac, F. E. [1990] Restoring natural Gait to paraplegics with func tional neuromuscular stimulation: a computer simulation study, IEEE Transactions on
Biomedical engineering, VOL 37, No. 9, pp886-902, 1990.
Yamashita, T., Yamada, H. [1973] A study on stability of bipedal locomotion,
Proceedings 1st Ro.Man.Sy. , Udine, Italy, 1973, Volume 1 pp41 -52.
R-8
Yamashita, T. [1993] On the stability of Bipedal Locomotion, Proceedings IEEE

International Conference on Robotics and Automation, pp49-57, 1993.
Zeigler [2002] Walking Machines:2-Legged-Robots, www.cs.uni-dortmund.de/people/ziegler/RoboterLaufen2Legs.html, [Accessed 10/01/01]
Zheng, Y.F., Sias, F.R., Rao, M.N.M. [1986] A Multiprocessor System for Motion
Control of a Biped Robot, 0094-2898/86/0000/0150$01.00 IEEE, p150-153.
Zheng, Y.F., Pan, L., Rao, M.N.M. [1987] A Supervisory System for Motion Control
of a Biped Robot., 0094-2898/87/0000/0432$01.00 IEEE, p432-435.
Zhou, C., Yue, P. K., Choy, F. S., Ahmed, N. [2003] Robo-Erectus: A Soccer-Playing
Humanoid Robot, Proc. 3rd International Conference on Humanoid Robots, Karlsruhe,
Munich, 2003.
R-9
BIBLIOGRAPHY
Alexander, R. [1984] Gaits of Bipeds and Quadrupeds, International Journal of
Robotics Research, p3 49-59.
Alexander, R., Jayes, A.S. [1980] Fourier Analysis of Forces in Walk, Journal of
Biomechanics, p13:383-390.
Alexander, R. [1980] Optimum Walking Techniques for Quadrupeds and Bipeds,
Journal of Zoology, p192:97-117.
Alexander, R. [1970] Mechanics of Biped Locomotion, Perspectives in experimental
Biology, p493-504.
Amirouche, F.M.L., Ider, S.K., Trimble, J. [1990] Analytical Method of Analysis and
Simulation of Human Locomotion, ASME Journal of Biomedical Engineering,
(V112I4) p379-386.
Bakr, E.M., Shabana, A.A. [1986] Application of Timoshenko Beam Theory To
Dynamics of Flexible Leg Locomotion, Proceedings Design Engineering Technical
Conference (Ohio), 86-Det-101.
Bartos, F.J. [1992] Fuzzy Logic is Clearly Here to Stay, Control Engineering, (V39)
p45-7.
Begg, R.K., Wytch, R., Major, R.E. [1990] A Microcomputer Based Video Vector
System, Journal of Biomedical Engineering, (V12I5) p383-388.
Beletski, V.V. [1980] Motion Control of Biped Walking Robots, Proceedings 8th IFAC
Conference Automatic Control in Space, p317-312.
Berardinis, L. [1992] Building Better Models With Fuzzy Logic, Machine Design,
(V64) p72-73.
Berardinis, L. [1992] Clear Thinking On Fuzzy Logic, Machine Design, (V64) p4650.
Bernard, J A. [1988] Use of a Rule Based System for Process Control, IEEE Control
Systems Magazine, p3-12.
R - 10
Bessenov, A P. [1983] Stabilization of Position of Body in Walking, Mechanisms and

Machine Theory, p18(4)261-265.
Bessenov, A P. [1976] Geometric parameters of Walking Machine, Proceedings 2nd
CISM-IFTMM Conference.
Bessenov, A P. [1980] Features of Kinematics of Turn of Walking, Proceedings 3rd
CISM-IFTMM Conference, p87-97.
Brodland, G W., Thornton-Trump, A. [1987] Gait Reaction Reconstruction and a
Heel Strike Algorithm, Journal of Biomechanics, V20 p767-772.
Brooks, R.A. [1986] A Robust Layered Control System for a Mobile Robot, IEEE
Journal of Robotics and Automation, RA2-1-p12.
Brooks, V. [1983] Motor Control, Physical Therapy, (V63#5) p664.
Burnett, M.E., Cole, J.D., Sedgwick, E.M. [1989] Gait Analysis of Man Without
Proprioception, Journal of Physiology - London, (V417IOct)p102.
Burrows, C.R., [1972] Fluid Power Servomechanisms, Book.
Capozzo, A., Leo, T., Pedotti, A. [1975] Computing Methods for Locomotion
Analysis., Journal of Biomechanics, p8:307-320.
Carr, J.H., Shepherd, R.B. [1982] A Motor Re-Learning Programme for Stroke,
Book.
Carrand, J H., Shepherd, R B. [1982] Walking, A Motor Relearning Programme for
Stroke, p117-122.
Cavagna, C.A. [1966] Mechanics of Walking, Journal of Applied Physiology,
p21:271-278.
Channon, P.H. [1992] Derivation of Optimal Walking Motions for a Bipedal Walking
Robot, Robotica, v10 p165-172.
Chen, C.L., Chen, P.C. [1991] Application of Fuzzy Logic Controllers in Single Loop
Tuning of Multivariable Systems, Computers in Industry, (V17) p33-41.
R - 11
Chow, C.K., Jacobson, D.H. [0] An Optimal Programming Study of the Human Gait,
University paper, p164-178.
Cox, E. [1992] Fuzzy Fundamentals, IEEE Spectrum, (V29) p58-61.
Daley, S., Gill, K.F. [1987] Attitude Control of a Space Craft Using A Fuzzy Logic
Controller, Proc Inst. Mech. Engineers. Part C Mech Eng Science, (V201) p97-106.
Donner, M.D. [1983] OWL: Language for Walking, Sigplan Notices, p18(6)158-165.
Dwivedi, S.N., Mahalingham, S. [1992] Periodic Gaits for the CMU Ambler,
Journal Of Robotics Systems, (V9) p1-15.
Elftman, H. [1939] Forces & Energy Changes in the Leg During Locomotion,
American Journal of Physiology, p(125)339-356.
Elftman, H. [1967] Basic Function of the Lower Limb, Biomedical Engineering,
p342-345.
Fawcett, J R. [1970] Hydraulic Servomechanisms and Their Applications, Book, .
Fischer Braune. [1987] The Human Gait, Book.
Fischer, W. [0] Kinematic Analysis of the Hindfoot, Book, p1-26p127-172.
Fomin, S V. [1976] Movt. of the Joints of Legs During Walking, Biophysics,
p(21)572-577.
Foulston, J. [1987] Biomedical Analysis of Foot Structures, Baillieres Clinical
Rheumatology, (V1I2)p241-260.
Frank, A.A. [1971] Stability Algorithm for a Biped, Journal of Terra Mechanics,
p41-50.
Frank, A.A. [1970] Dynamic Analysis and Synthesis of Biped Locomotion,
Mechanical and Biomedical Engineering, p8:465-476.
Frost, R.B., Cass, C.A. [1981] Load Cell and Sole Assembly for Dynamic Gait
Analysis, Engineering in Medicine MEP Ltd , V10, 1 p45-49.
R - 12
Furusho, J., Masubuchi, M. [1986] Control of a Dynamic Biped Locomotion System,

Journal of Dynamic Systems Measurement and Control, (June V108) p111.
Galiana, H.L. [1968] Mathematical Model for Walking Human Leg, Proceedings of
21st ACEMB Houston.
Goldenberg, A.A., Benhabib, B., Fenton, R.G. [1985] Complete Generalized
Solution to the Inverse Kinematics of Robots, IEEE Journal of Robotics and
Automation, (V4) p14-17.
Golnaraghi, M. [1977] Stability Analysis of a Robotic Mechanism, Theoretical and
Applied Mechanics, p281-292.
GPAC. [1984] Gait Analysis Steps Into New Field, Langer Biomecha Mechanical
Engineering, (V106) p105-8.
Grunman, J. [1977] Multi Task Exoskeletal for Paraplegics, Proceedings 2nd CISMIFTMM Conf, p233-240.
Gubina, F., Hemani, H., McGhee, R B. [1974] On the Stability of Biped Locomotion,
IEEE Transactions on Biomedical Engineering, (BME21#2) p101.
Gundersen, L.A., Barr, A.E., Danoff, J.V. [1989] Bilateral Analysis of the Knee and
Ankle, Physical Therapy, (V69I8)p640-650.
Hemami, H. [1980] A Feedback on-off model of Biped Dynamics, IEEE Trans
Systems Man and Cybernetics, p(10)376-383.
Hemami, H., Chen, B.R. [1984] Stability Analysis and Input Design of a Two Link
Planar Biped., International Journal of Robotics Research, p3(2)93-100.
Hemami, H. [1982] Constrained Inverted Pendulum Model for a Biped, Journal of
Dynamic Systems Measurment and Control, p(104)343-349.
Hemami, H., Cvetkovic, V. [1974] Postural Stability of Two Biped Models via
Lyapunov Second method., Proceedings Joint Automatic Control Conference, p745-751.
Hemami, H., Hines, M.J., Goddard, R.E. [1982] Biped Sway in the Frontal Plane
With Locked Knees, IEEE Trans Systems Man and Cybernetics, p(smc)(12)577-582.
R - 13
Hemami, H. [1980] Stability Planar Bipeds Simultaneous, International Journal of

systems science, p(11)65-75.
Hemami, H. [1982] Initiation of Walk and Tiptoe of a Planar Nine Link Biped,
Mathematical Biosciences, p(61)163-189.
Hodgins, J., Koechling, J., Raibert, M.C. [1985] Running Experiments with a Planar
Biped, Proceedings 3rd Int Symposium on Robotics Research, p349-355.
Hussain, M.A. [1984] Application of Macsyma to Kinematics and Mechanical
Systems, Third Macsyma Users Conference, p262-277.
Hutchinson, A.C. [1967] Machines Can Walk, The Chartered Mechanical Engineer,
p480-484.
Infelise, N. [1991] A Clear Vision of Fuzzy Logic, Control Engineering, (V37)
p68-74.
Jones, R.L. [1941] An Experimental Study of Human Foot, American Journal of
Anatomy, p(68)1-39.
Ju, M.S., Mansour, J.M. [1988] Simulation of Double Support Phase of Human
Locomotion, Journal of Biomechanical Engineering, (V110) p223-229.
Kasvand, T., Milner, M., Rapley, L.F. [1958] Computer Based Analysis of Some
Aspects of Locomotion, Conference on Human Locomotor Engineering, p297-305.
Klein, C.A., Olson, K.W., Pugh, D.R. [1983] Use of Force and Attitude Sensors for
Locomotion of a Legged Vehicle over Irregular Terrain, International Journal of
Robotics Research, 1.
Kompass, E J. [1988] Is Control Ready For Artificial Intelligence, Control
Engineering, 47.
Kumar, V.R., Waldron, K.J. [1989] Adaptive Gait Control of a Walking Robot,
Journal of Robotic Systems, (V6I1)p49-76.
Lee, J.K., Song, S.H. [1991] Path Planning of Gait Walking Machine, Journal Of
Robotic Systems, (V8) p801-27.
R - 14
Legg, G. [1992] Special Tools and Chips Make Fuzzy Logic, EDN, (V37) p68-74.
Lundberg, A., Goldie, I., Kalin, B. [1987] An Invivo Kinetic Analysis of the Human
Foot, Acta Orthopaedica Scandinavica, (V58I2)p197.
Lundberg, A., Goldie, I. [1987] Invivo Kinematic Anaylsis of the Foot, Journal Of
Bone And Joint Surgery - British Volume, (V69I4)p678-679.
Maes, P., Brooks, A. [1990] Learning to Co-ordinate Behaviours, AAAI-90 Boston
MA 1990 796-802, p796-802.
Manleybuser, K A. [1989] A Heterochronic Study of the Human Foot, American
Journal of Physical Anthropology, (V78I2)p266-267.
Manuel, G. [1990] Group Sets Out To Promote Interest in Legged Robots, The
Engineer, (V270) p32.
McCloy, D., Martin, H.R. [1973] The Control of Fluid Power, Book, .
McConnell, H.E. [1992] A Less Fuzzy Logic, IEEE Technology and Society
Magazine, (V11) p6.
Mcgeer, T. [1990] Passive Dynamic Walking, International Journal of Robotics
Research, (V9I2) p62-82.
McGhee, R.B. [1977] Control of Legged Locomotion Systems, Proceedings 18th
Automatic Control Conf (San Francisco), p205-215.
McGhee, R.B. [1968] Some Finite State Aspects of Legged Locomotion, Mathematics
Bioscience, p(2)(1)67-84.
Miura, H. [1984] Dynamic Walk of Biped, International Journal of Robotics
Research, p(3)(2)60-74.
Miyazaki, F. [1983] Hierarchial Control for Biped Robots, International conference
of Advanced Robotics, p(b)299-306.
Miyazaki, F. [1983] Design Method of Control of Biped Walking Robot, Proceedings
4th CISM-IFTMM Conf, p317-326.
R - 15
Mochon, S., McMahon, T.A. [1980] Ballistic Walking, Journal of Biomechanics,

p(13) 49-57.
Mochon, S. [1981] A Mathematical Model of Human Walking, Lectures on mathematics in life sciences, p(14)193-213.
Mochon, S. [1981] Ballistica Walking: An Improved Model, Mathematical
Bioscience, p(52)241-260.
Muybridge, E. [1954] The Human Figure in Motion, SQ612.767/1A.
Napier, J. [1967] The Antiquity of Human Walking, Scientific American, p(216)56-66.
Nise, N.S. [1992] Control Systems Engineering, Book.
Ogo, K. [1980] Dynamic Walking of Biped Walking Machine, Proceedings 3rd
symposium on theory and practice of robots.
Olshen, R.A., Biden, E.N., Wyatt, M.P. [1989] Gait Analysis and the Bootstrap,
Annals of Statistics, (V17I4) p1419-1440.
Pearson, K. [1976] The Control of Walking, Scientific American, p72-86.
Pollock, N. [1983] Low Cost Servo-Accelerometer, Wireless World [May 1983],
p66-70.
Pollock, N. [1982] Electronic Compass Using Fluxgate Sensor, Wireless World
[October 1982], p49-54.
Raibert, M.H. [1982] Dynamically Stable Legged Locomotion, 2nd Report to DARPA
(The Robotics Institute/ Carnegie Mellon.
Raibert, M.H. [1983] Machines That Walk, Scientific American [January 1/ 1983],
p(248)32-41.
Rivin, E. [0] Mechanical Design of Robots, Book.
Rodgers, M.M., Cavanagh, P.R. [1984] Glossary of Biomechanical Terms, Physical
Therapy, (V64#12) p1886-95.
R - 16
Sangalli, A. [1992] Fuzzy Logic Goes to Market, New Scientist, 27.

Schaevitz. [0] Linear & Angular Displacement Transducers, Sensor Technology, .
Sheridan, S., Skjth, P. [1984] Automatic Kiln Control Using Fuzzy Logic, IEEE
Transactions on Industry Applications, (V20pt1) p562-568.
Shigley, J.E., Turin. [1961] The Mechanics of Walking Vehicles, Proceedings 1st Int.
Conf. on Mechanics of Soil-Vehicle Systems
Shi-Zhong, H., Shaohua, T., Chang-Chieh, H. [1992] Control of Dynamical process es using an on line rule-adaptive fuzzy control system, Fuzzy Sets and Systems,
p11-22.
Sripada, N R., Fisher, D G., Morris, A J. [1987] AI Application for Process
Regulation and Control, IEEE Proceedings. Pt D. Control Theory and Applications,
(V134) p251-259.
Stewart, D.G. [1973] Introduction to Matrix Computations, Book.
Thring, M.W. [1983] Robots and Telechirs, Book.
Todd, D.J. [0] Walking Machines, Book.
Togai, M. [1991] An Example of Fuzzy Logic Control, Computer Design, (V30)
p93-`108.
Varghese, M., Fuchs, A., Mukundan, R. [1991] Chaotic Zero Dynamics in
Kinematically Redundant Systems, IEEE Transactions on Aerospace and Electronic
Systems, (V27) p784-96.
Vohnout, V J. [1983] Structural Design of Legs for a Walking Vehicle, Proceedings
8th Applied Mechanisms Conference, p50-1-50-2.
Vukobratovic, M., Juricic, D. [1969] Contribution to the Synthesis of Biped Gait,
IEEE Transactions in Biomedical Engineering, V16, p1-6.
Vukobratovic, M., Frank, A.A., Juricic, D. [1970] On the Stability of Biped
Locomotion, IEEE Transactions on Bio-Medical Engineering, (BME17#1) p25-36.
R - 17
Wakileh, B.A.M., Gill, K.F. [1988] Use of Fuzzy Logic in Robotics, Computers In
Industry, (V10) P35-46.
Wang, H., Lee, T.T., Gruver, W.A. [1992] A Neuromorphic Controller for a 3-Link
Biped, IEEE Transactions on Systems Man and Cybernetics, (V22I1) p164-169.
West, J.C., [1958] Textbook of Servomechanisms, Book.
White, S.C., Yack, H.J., Winter, D.A. [1989] A 3D Musculoskeletal Model for Gait
Analysis, Journal of Biomechanics, (V22I8-9) p885-893.
Youngblood, R.L. [1992] Fuzzy Logic. What is it? Has it Got Legs?, Power, (V136)
p63-64.
Zadeh, L.A. [1984] Making Computers Think Like People, IEEE Spectrum, p27.
Zadeh, L.A. [1988] Fuzzy Logic, Computer, (V21) p83-93.
Zadeh, L.A. [1983] Commonsense Knowledge Representation, Computer, (V16)
p61-65.
Zamaya, A.Y., Morris, A S. [1992] Parallel Computation of Robot Inverse Dynamics,
IEEE Proceedings Part D Control Theory and Applications, (V139) p226-36.
Zhang, C.D., Song, S M. [1992] Forward Kinematics of Walking Machines, Journal
Of Robotic Systems, (V9) p955-71.
Zhang, C.D., Song, S.M. [1992] Walking Machines with Pantograph Legs, Journal
of Robotic Systems, p956-971.
R - 18
Design, Construction
and Control of an
Industrial Scale Biped Robot
Volume 2
Appendix
Joe Cronin
Supervisors
Associate Professor RA Willgoss
Associate Professor R Ford
Revision 2.0
December 2005
A thesis submitted in fulfilment of the requirements
for the degree of doctor of philosophy.
ABSTRACT
A 500Kg, self-contained biped robot, named Roboshift, has been conceived and tested
to investigate issues associated with the control of industrial scale biped robots. This
project represents the first credible attempt to build a heavy weight autonomous biped
robot. The recent expansion in humanoid robot development has highlighted advances
made in anthropomorphic biped technology. Current research into speech recognition,
vision systems, laser topography, artificial intelligence and electroactive polymers will
ultimately achieve an Android capable of human like actions and thought processes.
Justification for this most demanding and expensive research is based on
philanthropic models that suggest these robots will attend to the bedridden, or replace
humans in dangerous areas. However, the cost of a biped robot when compared to that of
a wheeled or tracked vehicle restricts commercialisation for these applications. As well,
the size and working capacity of current humanoid robots is not compatible with the
heavy lifting requirements found in such environments.
It is proposed that only biped robots of an industrial scale, possessing a capacity much
greater than that of a human, will be of commercial value in the future. Typical
applications may include the handling of materials in confined or uneven terrain, where
a forklift or other commercially available materials handling equipment would be
unsuitable. For example, field handling in military, mining or geological environments.
Minimal research has been conducted into the realisation of such a device, which
presents challenges in terms of the magnitude of dynamic forces produced and of the
systems required to control the robot in real-time.
Review of relevant literature reveals that little research has been completed in this field.
Therefore, operational characteristics for an industrial scale biped robot are defined. The
design then details the structure and integration of mechanical, hydraulic, and electrical
systems. Roboshift is powered by an internal combustion engine and is the first biped
robot with a capacity for extended operation. Modelling was conducted to determine
joint trajectories, power requirements, hydraulic flow parameters and dynamic
characteristics. The robot is controlled by a distributed, hierarchical system comprising
sixteen microprocessors, a control computer acting as the midbrain and a
communications computer acting as the central nervous system. Sensors measure attitude
Abstract
and heading (vestibular system) as well as ground reaction forces and joint angles
(propreoception). The control strategy is based on feed forward trajectories generated by
inverse kinematic analysis. Corrections to trajectories are made in real time by higher
level routines running on the main control computer. Joint position is achieved by local
feedback control. Software for the robot was written in the C language.
Experimental results are presented detailing the performance of the robot in
comparison to theoretical analysis. After construction and testing of actuators and
sensors, calibration software was tested successfully. Once calibrated, the robot was
lowered to the ground where the active balance software was able to control the robot in
the frontal and sagittal planes. Frontal sway software was tested with mixed success as
natural oscillation of the structure, which was not detectable by the control system, led
to erroneous force data. Detailed dynamic modelling was then completed to determine
the causes of oscillation in the robot. The modelling led to the formulation of a control
strategy where non-collocated sensors are used to measure link strain as a feedback to a
modified proportional controller.
The project has demonstrated that an industrial scale biped incorporating an internal
combustion engine and hydraulic power system is feasible.. Analysis presented
proposes that as the height of a biped robot increases, the expected elastic deformation
of the structure increases as the cube of the height, making control extremely
challenging. A strategy for the control of heavy-weight robots is suggested It is also
proposed that technology incorporated in current humanoid robots can not be scaled to
control industrial bipeds.
APPENDIX
Appendix 1
Paper presented at CIRA 99, Monteray, California
Appendix 2
Paper presented at ACRAA 03, Brisbane, Australia.
12
Appendix 3
CROCK.C Dynamic Simulation Software used to

calculate joint accelerations during gait cycle.
19
Appendix 4
Walk009.C Autocad ADS Software written to

animate output of dynamic simulation
42
Appendix 5
Thong1.C Main Control Program.
54
Appendix 6
MP2X.C Communications Program.
101
Appendix 7
LF1003.C Left Foot Rotation joint controller software.
121
Example of local Joint Contol Software..

Appendix 8
Roll 2.C Wheeled Robot Software.
133
Appendix 9
Roll 6.C Fuzzy Logic Wheeled Robot Software.
138
Appendix 10
PC_RS13.C PC to M68HC11
Communication Software
146
Appendix 11
HCRS_13.C M68HC11 to PC
Communications software.
148
Appendix 12
Multi_RS.C PC Software to communicate
152
with multiple M68HC11s.

Appendix 13
Download.C M68HC11 S19 code download software.
157
Appendix 14
ROBOT.H Header file.
160
Appendix 15
Local Joint Controller interrupt service routine.
168
Appendix 16
Engineering drawings of Roboshift
181
Walking Biped Robot with

Distributed Hierarchical Control System
Joe Cronin
Richard Frost
Richard Willgoss
School of Mechanical Engineering
University of New South Wales, Australia.
Abstract
2) The study of bipedal control will result in a better
understanding of the human gait and lead to devices that
will assist with the mobility of people who have lost the
use of their legs. [Todd (8), Kato (9), Hemani (5),
Yamashita (16)]
An anthropomorphic biped robot is presented. The

advantage of legged locomotion in rough terrain and in
confined spaces has been investigated extensively in
recent years. Biped robotic research has focused on
imitation of human anthropomorphic form with the
ultimate goal being the creation of the Droid or
artificial human. An alternative use for biped robots is
proposed, as materials handling platforms for use in
confined spaces or areas where irregularities in terrain
prohibit access to conventional materials handling
vehicles. The operational criteria and design strategy for
the device, which has been built, are defined. The major
components of the robots sensory, navigation, actuation
and control systems are highlighted. Finally, the control
software strategy and initial results are presented.
While these goals may be worthy, the cost of a biped

robot compared to that of a wheeled or tracked device
prohibits the commercialisation of biped robots for the
first proposition.
Significant research has been invested into the design of
robotic-type orthotic devices to aid people suffering
paraplegia. Even if such a device were to be realised, it
would be prohibitively expensive to manufacture and
maintain, placing it out of reach of all but the most
wealthy patient. Further, the requirement for an on-board
power supply would render the device bulky and
cumbersome. With the current capabilities of batteries, it
would have a very short period of operation. Current
research in biomechanics suggests that functional
electrical stimulation of nerves and muscles will be
significantly more viable.
Introduction
While mechanical walking machines have been proposed,
patented and built from the beginning of this century, it is
only since the availability of low-cost mini- and microcomputers that electronically-controlled devices have
become viable. The vast majority of bipedal walking
robots that have been built are modeled on the human
form. The geometry presented by an anthropomorphic
device, and the inherent instability of bipedal locomotion,
increase both the complexity and cost of the device in
terms of construction and control hardware. Researchers
involved in this field have tended to justify their
endeavors in philanthropic terms. Such justification
includes;
1) In the future biped devices may replace humans
performing hazardous or degrading work [(2), (12)].
The support base of a biped is an order of magnitude

smaller than that of any other vehicle. Bipeds also posses
the ability to turn in their own space and to lift heavy
objects by adjustment of posture rather than by increasing
their support base. The ideal use for a biped device is
materials handling in confined, uneven terrain where a
forklift or other lifting device would be unsuitable.
Possible situations would be field handling in a military
environment, on board a ship, or industrial applications in

the field such as geological or mining applications.
Some attempts have been made to realise a bipedal
walking materials handling robot such as the General
Electric Hardiman [Todd (8)]. In recent years however,
robots such as Hondas anthropomorphic droid have
attempted to closely imitate the human form.
This project endeavours to demonstrate that a biped
robotic materials handling device is viable. To achieve
this, the device must be;
Easily maintained
Structure
Our robot is loosely anthropomorphic, possessing all of
the significant degrees of freedom of movement exhibited
by the lower limbs. In total, the structure displays seven
degrees of freedom on each side of the body; Hip
Abduction, Hip Extension, Hip Rotation, Knee Extension,
Ankle Abduction, Ankle Extension and Toe Extension.
Each degree of freedom is controlled and actuated by a
hydraulic cylinder, except for hip rotation which is
actuated by a hydraulic motor.
Figure 1. Biped Robot

associated with carrying the power source and all control
systems on board be identified. To satisfy the
requirement that the device be easily maintained, the
overwhelming majority of components must be available
off-the-shelf. By using standard, less specialised parts
(hydraulic valves, cylinders, sensors, microprocessor
boards, etc), the cost of the project has also been reduced.
Similar in some aspects to the biped robot developed by

Waseda University and Hitachi (9), WH-11, which was
also hydraulically driven, this robot includes actuated
toes. Additionally, the body is located under the hips so
that for the same height, the stride length of the robot
would be nearly double that which would be obtained if
the body were placed on top of the hips. This
modification required the upper thighs to be separated,
swinging on either side of the body. A photograph of
the robot, is shown in Figure 1.
Biped Control
As recognised by Hemami (4), the biped gait is notionally
periodic and symmetrical, but unstable. The gait consists
of falling from one leg (accelerating in forward direction
while decreasing potential energy) onto the other leg
(decelerating in the forward direction while increasing
potential energy). If disturbed from either trajectory, the
system requires a force input to stabilise.
Unlike other larger scale robots such as WH-11 and the

biped developed by Furusho et al (10), this robot has been
designed to be completely self contained. A 16 kilowatt,
LPG powered engine powers all hydraulic and electrical
systems. Only by cutting unbilical cords can problems
Force inputs to stabilise motion are dealt with by the

human on two levels. First, as highlighted by Sias (11),
the nervous centres of the midbrain develop the feed
forward data for gait motion. Sensory organs such as the
system). It also receives force data and positional data

from local joint processors. It communicates with the rest
of the body through the communications computer (the
spinal cord) which contains a 16 port RS-232 expansion
board. 14 Motorolla M68-HC11 microprocessor and
interface boards control local joint motion
(Propreoception and fine motor control) via analogue
outputs and digital encoder position inputs. The
microprocessors communicate with each other and with
the control computer via the communications computer.
vestibular system, provide feedback error data for the

midbrain to initiate coarse posture corrections. Second, at
a lower level, fusimotor and Golgi tendons provide
propreoception and fine motor controls for precise
positional control of individual joints. Learned functions
such as walking are initiated by the midbrain, but fine
movements, such as rotations of the foot to equalise
reaction force across it, are controlled locally by twitch
muscles controlling the joint.
These features of the human control system have been
recognised by designers of biped robots.
Furusho (10), Shih (15) and Gurfinkel (3) have all used
gaits which react to disturbances or feedback errors
generated when actual trajectories fail to match computed
ones. Also recognised by biped robot researchers is the
complexity of equations governing biped motion. Furusho
(10) found that his 5 link biped model resulted in nonlinear differential equations of order 10.
Sensors
An air-driven aircraft-type artificial horizon provides
pitch and roll data, while an electronic flux-gate
compass provides yaw data on the spatial alignment of
the robots body. These vestibular signals are
conditioned by an F1 micro-controller prior to RS-232
transmission to the communications computer as an
ASCII string.
Provided with sufficient processing power, ultra-reliable

sensors and extremely fast actuation, it may be possible to
use classical control methods to coordinate a biped robot
in dynamic gait. Most researchers, however, have
attempted to reduce the order of the equations by a
number of methods. Vukobratovic (13) suggests that
designing the body so that limbs can be described as point
masses eliminates or minimises secondary inertia terms.
Using feed-forward control with local, distributed
negative feedback stabilisation has also proved popular
and effective (10), (3), (15). The configuration of the
control system employed by our biped however, addresses
the complexity of the control task by separating the
control tasks and distributing the processing of tasks.
The Control System

The control system hardware of the robot is broken into
four sections; the main control computer, the
communications computer, local joint processors, sensory
inputs and the hydraulic control valves. A schematic
diagram of the system is shown in figure 2.
The main control computer represents the midbrain. It
computes the feed-forward control data and monitors the
inputs from the artificial horizon and compass (vestibular
Figure 2. Schematic Diagram of Control System
areas of un-recoverable instability (Figure 3), it is entirely

possible that controlled joint positions may never equate
to command positions.
An essential part of balance in the human is

propreoception at the ankle and foot. Each ankle and foot
of the biped robot is fitted with eight strain gauges that
measure the reaction force distribution over the length of
the foot and the total reaction force on the foot. As well,
each foot is fitted with micro switches to signal that each
side of the foot is in contact with the ground.
The control computer also acts on data from the artificial

horizon, maintaining gross balance in the frontal and
sagital planes by adjustment to hip and knee joint
trajectories. Like the joint controllers, the main control
software is reactive to errors in measured and expected
data. The software is based on a hierarchical structure
where the higher level software functions generate gait
patterns and monitor the resulting motion. At a lower
level, continuous monitoring of hormone variables
(pitch, roll, force distribution at feet) result in reflex
An optical shaft encoder measures the angle of each joint.

The signal from the shaft encoder is fed to an interface
board that keeps the absolute position of the joint, storing
it at a single address. This relieves the micro-controller
from the task of interrupt-driven position tracking of the
joint. The interface board is fitted with a digital to
analogue converter controlled by an 8-bit address, also
mapped onto the micro-controller. Control signals to the
hydraulic valve amplifiers can be sent by the microcontroller simply by writing an integer to an address. This
also relieves the micro-controller from maintaining
interrupt-driven, pulse-width modulation output.
Control Strategy
As identified by Gurfinkel (3), ..the problem of control
seems to be the main problem of the walking robotThe
control system has been designed to minimise demand on
the control computer by distributing joint position control
and handling communications separately. As previously
discussed, the control strategy is based on the generation
of a feed-forward periodic gait trajectory
and the local feedback control of joint actuation. During
normal walk the control computer generates joint
trajectories from course kinematic and dynamic
calculations. These equate to the high level, midbrain
generated locomotion patterns in humans, governing the
movement of functionally related leg muscles (hips, legs,
and ankles).
Figure 3. Joint Trajectories

behaviour when boundary values are approached. Nashner
(7) found that muscular reactions in humans who were
perturbed while walking were rapid, large in magnitude
and movement specific ie, reflex actions. By monitoring
hormone variables in the communications computer, the
control computer and all the joint micro-controllers are
able to react to serious instability in a coordinated
behaviour without the requirement for communication.
The over-riding behaviour of the biped is the maintenance
of stability within recoverable boundaries. This is the case
whether the robot is standing or walking.
Incremental demand positions are transmitted to the joint

F1 micro-controllers every 100ms. These then control the
hydraulic valves to achieve demand positions in the
following 100ms. Given that control trajectories are
calculated using reduced-order dynamic equations, errors
between actual motion and ideal motion are to be
expected. Provided errors do not cause incursion into
Joint trajectories are generated parametrically from the

input of desired walking velocity. The amplitude of sway
required to allow complete movement of the leg in swing
phase governs the frontal sway period. All other sagital
joint trajectory periods are derived from the frontal sway
period. Given the abundance of power available, there is
no requirement for the energy saving, locked-knee
movement of the leg in support phase. This allows
minimisation of the potential/kinetic energy conversion
during the gait cycle which results in a smoother loadcarrying gait. It also allows the use of the support-phase
knee joint to quickly change the position of the centre of
gravity of the robot, enhancing balance in the sagital
plane.
Gait Generation
The literature on the analysis, modelling and simulation of
the human gait is inexhaustible. While some aspects of the
kinematics of the human gait may be relevant to biped
robotics, the same can not be said of human gait
dynamics. Industrial biped robots would be designed to
carry masses approaching their own structure mass. In the
human case, such loads produce gaits characterised by
stability optimisation rather than energy minimisation.
Also, as previously discussed, on board electrical drive is
not feasible for industrial scale bipeds. Once the decision
is made to use petrol or gas powered high-speed engines
as a power source, energy minimisation is no longer a
primary issue.
Decoupling of Feet
However, for the purposes of modelling, joint trajectories

empirically measured by Braune (17) and parameterised
by Hartrum (18) were used during the initial design phase.
The modelling showed stabilising torques generated by
the torso, head and upper limbs could be replaced by the
motion of a single mass attached below the hips. The
analysis was written as an AutoCad ADS application, the
output is shown in figure 4.
Frank (1) defines a biped as a machine in which the legs

and feet are algebraically connected. By allowing the
feet micro-controllers to automatically adjust the
orientation of the feet to available terrain, the order of
control equations is proportionally reduced. Using an
inverted pendulum analogy, this equates to moving the
pivot point of the pendulum in the opposite direction to
Figure 4. Modelling of Human gait, replacing upper torso with a single mass under the hips.
increase once desired motion has been sensed.

It can also be seen from the second graph that the actual
left hip abduction
position rarely matches
the demand position,
but remains within
required error
constraints.
that of required motion. Thus, while adding increased

stability control to a five-link biped, the feet do not
increase the complexity
of the control software
to that of a nine-link
biped.
Results
At the time of writing
this paper, the robot is
currently undergoing
initial walking trials.
All sensory, control and
communications
systems have been
tested and calibrated.
Figure 5 shows a timelapse photograph of the
robot undergoing
balance trials in which
an initial sway is
brought to rest. While
the main body and legs
are moving (mid brain
control), the feet remain
relatively motionless,
maintaining evenly
distributed force over
the ground (local
twitch control).
Conclusion
A biped robot has been
designed, constructed
and is currently under
trials. At the time of
writing of this paper,
the robot has been built
and all systems are
functional. The
communications and
joint control routines
have been calibrated
and tested. The robot
can actively balance
and react to disturbing
forces applied to it. The
biped will eventually be
completely self
contained and will
include all power
supply and processing
equipment. There will
be no umbilical cord.
Although it is a ninelink device, the feet are
de-coupled by
controlling them
locally. All joint control
is distributed to local
micro-controllers while
a central control
computer generates gait
patterns.
Figure 6 shows data of

force on each foot and
of movement of the left
hip abduction joint. It
can be seen from the
force graph that as the
robot moves to reduce
force on one foot, there
is an initial increase in
Figure 5. The biped robot balancing.
force on that foot. This
is due to the dynamic
reaction force in the frontal plane being superimposed on
the static balance force. Feedback from the artificial
Unlike contemporary research in biped robotics, our robot
horizon allows the control computer to ignore this
is designed as an industrial-scale vehicle to which
10
Dynamics and Designing Robot Locomotion Controls., IEEE

Transactions on Automatic Control, ac22, pp963-972, 1977.
3 V. S. Gurfinkel, Walking robot with Supervisory Control,
Mechanisms and Machine Theory, v16, pp31-36, 1981.
4 H. Hemami, The Inverted Pendulum, and the Biped, Mathematical
Bioscience, v(34), p95-110, 1977.
5 H. Hemami, Aspects of Inverted Pendulum Problem for Modeling of
Locomotion Systems, IEEE Transactions on Automatic Control, v24,
pp526-535, 1979.
6 R.B.McGhee, Some Finite State Aspects of Legged Locomotion
,Mathematical Bioscience, P(2)(1)67-84, 1968.
7 L. M. Nashner, Balance adjustments of the human perturbed while
walking, Journal of Neorophysiology, v44, pp650-664, 1980.
8 D. J. Todd, Walking Machines, Kogan Page, 198?.
9 I. Kato, Development of legged walking robot, Hitachi Review, v36,
pp71, 1987.
10 J.Furusho, Control of a Dynamic Biped Locomotion System,
Journal of Dynamic Systems, Measurement and Control, V108, p111,
1986.
11 F. Sias, Static Stability Problems in Biped Robot Design, IEEE
Transactions, p436, 1987.
12 . F. Zheng, Supervisory system for motion control of biped robot,
IEEE Transactions on Automatic Control, pp1520-1524, 1988.
13 M Vukobratovic, On the stability of Biped Locomotion, IEEE
Transactions on Bio-Medical Engineering, BME17#1, pp25-36.
14 M. Raibert, Legged Robots, Communications of the ACM, V29,
p499-514, 1986.
15 C. L. Shih, Control of a Biped Robot during Support Phase, IEEE
Transactions on Systems, Man and Cybernetics, V22/4, pp729-735,
1992.
16 T. Yamashita, On the stability of Bipedal Locomotion,
Proceedings IEEE International Conference on Robotics and
Automation, pp49-57, 1993.
17 T. Hartrum, Computer Implementation of a Parametric Model of
The Human Gait, Thesis, 1973.
Sagital Balance Tuning (Mass)

0910bld
400
0.7
0.6
300
200
Trim
Mass (kg)
0.5
0.4
0.3
100
0.2
0.1
15
65
115
165
215
Iteration
Mass Lef t
Mass Right
Trim
Sagital Balance Tuning (Movement)

0910bld
105
0.7
0.6
Position (counts)
100
0.5
95
0.4
0.3
90
0.2
85
15
65
115
165
0.1
215
Iteration
LHA Actual
LHA Demand
Trim
Figure 6. Balancing Trial Data

materials handling equipment can be fitted. We believe
that this is one area where a biped robot would be
commercially viable.
Gait control software has been written and is currently
being commissioned. At the time of writing , the software
that generates frontal sway is being tested. Once this has
been proven, the robot will take its first step.
References
1 A. A. Frank , Stability Algorithm for a Biped, Journal of Terra
Mechanics., pp.41-50, 1971.
2 C. L. Golliday, An Approach Analysing Biped Locomotion
11
Control of a Heavy Weight Biped Robot in the Frontal Plane

Joe Cronin
j.cronin@unsw.edu.au
Richard Willgoss
r.willgoss@unsw.edu.au
Robin Ford
r.ford@unsw.edu.au
School of Mechanical and Manufacturing Engineering
The University of New South Wales
Sydney, Australia
anthropomorphic device and the inherent instability of

bipedal locomotion increase both the complexity and cost
of the device in terms of construction and control
hardware.
Abstract
The advantage of legged locomotion in rough
terrain and in confined spaces has been
investigated extensively in recent years. It is
anticipated that in the next decade or so, the
prophecy that anthropomorphic biped robots will
work side-by-side with humans will be realised.
Recent successes in Japan have advanced the
field substantially. However, little research has
been conducted into the realisation of an
industrial scale materials handling biped robot.
Such a device presents challenges in terms of the
magnitude of dynamic forces produced and of
the systems required to control it in real-time. A
500Kg, self-contained biped robot, called
Roboshift, has been built to explore these
challenges and to identify the significant aspects
of control. This paper identifies issues associated
with the control of the biped in the frontal plane.
Results from balancing trials are presented which
demonstrate the effect of ankle torque on the
control of the biped. A model is then presented
which demonstrates the effect of control forces
and of geometry on the response of the biped in
frontal sway. It is shown that stiffness produced
by control torque, a function of the gain of the
control system and the response of the joint, is
the major factor in the dynamic characteristics of
the system.
Researchers involved in this field have tended to justify

their endeavours in philanthropic terms, such as the
suggestion that in the future biped devices may replace
humans performing hazardous or degrading work
[Golliday, 1977; Zheng, 1988]. While this goal may be
worthy, the cost of a biped robot compared to that of a
wheeled
or
tracked
vehicle
detracts
from
commercialisation. Other applications will need to be
sought.
The support base of a biped is an order of magnitude
smaller than that of any other vehicle.
Like humans, biped robots possess the capability of
turning in their own space and lifting heavy objects by
adjustment of posture rather than by increasing their
support base. Their support base can also be kept an order
of magnitude smaller than that of any other lifting
devices. Accordingly, we see the exemplar target
application for a biped as being materials handling in
confined, uneven terrain where a forklift or other lifting
device would be unsuitable. Possible situations would be
field handling in a military environment, on board a ship
or industrial applications in the field such as geological or
mining context.
1 Introduction
Some attempts have been made to realise a bipedal

walking materials-handling robot such as the General
Electric Hardiman [Todd, 1987]. Research conducted in
recent years has attempted to closely imitate the human
form. Recent successes in Japan have advanced the field
of Biped Robotics significantly. However, the ability of
Mechanical walking machines have been proposed,

patented and built from the beginning of the 20th century,
but it is only since the advent of ready access to low-cost
computing that electronically-controlled devices have
become viable. The geometry presented by an
1
12
human scale device to achieve heavy lifting is limited.

Something larger is required.
This project endeavours to demonstrate that a
biped robotic materials-handling device is feasible. It is
suggested that the design criteria for such a device to be
industrially practicable are as follows:
x Capable of lifting 500kg
x Completely self-contained
x Robust both physically and electronically
x Easily maintained
x Able to work for long periods
maintained, the overwhelming majority of components

must be available off-the-shelf. Also, by using standard,
less specialised parts, the cost of the project has been
reduced.
3 Biped Control
As recognised by Hemani [1977], the biped gait is
periodic and symmetrical but unstable. The gait consists
of falling from one leg, accelerating in the forward
direction while decreasing potential energy, onto the other
leg, decelerating in the forward direction while increasing
potential energy. If disturbed from either trajectory, the
system requires a force input to stabilise it.
Designers of biped robots have recognised these
features of a biped control system. Furusho [1986], Shih
[1992] and Gurfinkel [1981] have all used regular gaits
that react to disturbances or feedback errors generated
when actual trajectories fail to match computed ones.
Also recognised by biped robot researchers is the
complexity of equations governing biped motion. Furusho
[1986], for example, found that his 5 link biped model
resulted in non-linear differential equations of order 10.
Provided with sufficient processing power, ultra-reliable
sensors, fast actuation and a structure with minimal
deflection, it may be possible to use classical control
methods to coordinate a biped robot in dynamic gait.But
in practice it may be betterto simplify the control model
and focus available processing power on tasks that most
influence stability.
2 Structure of Roboshift
Unlike any previous or current anthropomorphic scale
biped, the body of Roboshift is located under the hips. In
this configuration (See Figure 1) the stride length of the
robot would be nearly double that if the body were placed
on top of the hips. This design requires the distance
between the hips to be substantially increased so that the
thighs can swing on either side of the body.
The robot is hydraulically driven as was the biped
robot developed by Waseda University and Hitachi [Kato,
1987], WH-11.
Researchers have attempted to reduce the

complexity of the control system by a number of methods.
Vukobratovic [1970] suggests that designing the body so
that limbs can be described as point masses eliminates or
minimises secondary inertia terms. Using feed-forward
control with local distributed negative feedback
stabilisation has also proved popular and effective
[Furushno, 1986; Gurfinkel, 1981; Shih, 1992].
Similarly, the configuration of the control system
employed by Roboshift addresses the complexity of the
control requirement by separating the processing of tasks,
in a way inspired by the characteristics of the human
system.
In the human, force inputs to stabilise motion are
generated by two systems. Firstly, as highlighted by Sias
[1987], the nervous centres of the midbrain develop the
feed forward data for gait motion. Sensory organs such as
the vestibular system provide feedback error data for the
midbrain to initiate course posture corrections. Learned
functions such as walking are initiated by the midbrain in
this way. Secondly, at a lower level, fusimotor and Golgi
tendons provide propreoception and fine motor controls
for precise positional control of individual joints. The
twitch muscles that control these joint positions do so
locally, without reference to the midbrain. An example of
this would be a rotation of the foot to equalise the reaction
force across it.
Figure 1 Schematic of Roboshift showing degrees of freedom of

the limbs.
Unlike other larger scale robots such as WH-11

and the biped developed by Furusho et al [1986],
Roboshift has been designed to be completely selfcontained. A 16 kilowatt, LPG powered engine provides
all hydraulic and electrical power. Only by cutting
umbilical cords can problems associated with carrying the
power source and all control systems on-board be
identified in a prototype.
To satisfy the requirement that the device be easily
2
13
Figure 2 shows a schematic of how this concept is

implemented in Roboshift. A primary control computer,
representing the midbrain, computes the feed-forward
control data and monitors the inputs from the artificial
horizon and compass, which represent the vestibular
system. It also receives force and positional data from
local joint processors. The primary control computer
communicates with the rest of the body through a
communications computer, representing the spinal cord,
which contains a 16-port RS232 expansion board. Local
joint motion, propreoception and fine motor control, is
controlled by 14 Motorola 68HC11 microprocessor and
interface boards via analogue outputs and digital encoder
position inputs. The microprocessors communicate with
each other and with the control computer via the
communications computer.
2.
3.
rigid stance.
Lower the robot so that it balanced on the ground,
with the control system still maintaining a rigid
stance position (see Figure 3). Note that due to
clearances and tolerances in the joints and
compliance in the joint members, the robot would
lean slightly to one side of the frontal plane and either
forward or backward in the sagital plane as it settled
under its own weight.
Activate the active balancing program and observe
the response.
The active balancing program attempts to

reposition the robots upper body so that the centre of
gravity is maintained within a specified tolerance-band of
a condition in which there would be equal load on each
foot.. The robot achieves this by abducting the hips
equally and in opposite directions. The control input to the
balancing software in the frontal plane is the proportion of
weight on the left leg, as specified by a trim term that is
4 Control Of Frontal Sway

Sway in the frontal plane is fundamental to the
walking process. The desired effect of frontal sway is to
get the robot to lean far enough to one side so that the
body is almost overbalanced. With the centre of gravity
over the supporting foot, the free foot can be moved
forward in readiness to take up the weight as the body
falls onto it. The resulting motion represents an energy
minimising gait because the system is operating at the
natural frequency of oscillation. The disadvantage of such
a gait is that fine control is required at the boundary of
stability.
To explore the behaviour of Roboshift in frontal
sway, the following test procedure was implemented.
Figure 3 Robot being lowered in stance position.

defined as:
weight _ left _ leg
Trim =
weight _ left _ leg weight _ right _ leg
Figure 2 Schematic of control System
1.
Initial results show the balancing routine to be

sensitive to three variables;
x The tolerance-band set for balancing.
x The distance between the feet
x The gain of the control system.
With the robot suspended above the ground, instruct

the control system to hold the joints in a
predetermined configuration suitable for sustaining a
3
14
lack of symmetry in the response because the robot has

limited ability to decrease angular momentum when the
centre of gravity is moving towards a position that brings
it nearly over one leg (ie trim 0 or 1). Under these
conditions, when the stance width is less relevant, the
width of the foot is likely to become an important
determinant of balancing ability.
Figure 4a shows time-history data of the left and

right hip position and the trim value as the robot actively
balances. In this case, the limits for balance were set
between 0.45 and 0.55. It can be seen that the robot
balances successfully. When the limits were set between
0.48 and 0.52, see Figure 4b, the robot became unstable,
continuously swaying in the frontal plane.
Once a stable actively-controlled stance had been

achieved, the next task was to generate controlled sway.
by varying the hip abduction positions sinusoidally at a
frequency close to that which could be expected during
locomotion. Initial attempts to generate frontal sway in
the biped have produced motion that is highly dynamic.
See Figure 7 which is taken from a video. From
observation of the motion, it appeared that the robot was
oscillating at a natural frequency. Figure 6 shows the
transfer of weight from one leg to the other as the robot
commenced frontal sway from rest. The video of the event
shows that the biped robot was swaying to the limit
conditions discussed previously where the ground reaction
The robot balanced successfully when the trial was

conducted with the same tightened trim limits as above,
but with the valve control gain reduced i.e. a reduced
proportional constant in the PI control loop. The
corresponding increase in time response can be seen in the
decreases in the slope of the joint displacement curve.
This demonstrated the relationship between balancing
limits and the gain of the control system. When the gain
was high, the robot generated substantial momentum by
the time the trim fell between the set balance limits. While
the proportional constant in the control loop might be
expected to account for any potential overshoot, there is a
(b)
(a)
Figure 4. Robot in active balance with trim limits set between (a) 0.45 and 0.55 and (b) 0.48 and 0.52.
Figure 5. Robot in active balance with wider feet.
Figure 6. Robot in frontal sway.

4
15
plane is that it provides lateral stability during the swing

phase of walking. As Hemani suggests, the disadvantage
of ankle torque is that it must be measured or estimated
and then controlled. In the case of Roboshift, the control
system has been configured so that ankle torque may be
controlled either centrally or locally.
From observation of the trials described above, it
was determined that finer control would be required at the
boundary of sway. To achieve this, it was decided that the
width of the foot should be increased to provide additional
ankle torque, which would help control the biped at the
boundary condition. The foot width was increased from
150mm to 300mm which increased the maximum
available ankle torque to approximately 667Nm1. Trials
were then conducted in the new configuration which
produced unexpected results. It was observed that an
additional frequency was now superimposed over the
fundamental frequency of motuion which had itself been
increased. Evidence of the increased torque capacity
Figure 7 Biped in frontal sway.
Figure 8 Failure of foot rotation cylinder connection
force on the non-supporting leg approaches zero.

Correspondingly, the reaction at the opposite foot
approached the total weight of the robot. At this point, the
non-supporting foot is seen to slide toward the supporting
foot. This is caused by the moment created by the weight
of the leg about the hip adducting the leg towards the
supporting leg as the lateral friction forces at the foot
approach zero as there is no weight on the non supporting
leg.. The biped then swayed back toward the nonsupporting leg, now closer to the centre of gravity of the
robot and subtending a larger angle at the hip, the
corresponding increase in torque immediately reduced the
amplitude of sway.
provided by the addition in foot width is the failure of the

foot rotation cylinder connections, see Figure 8. To
determine the cause of the changes in frequency a
dynamic analysis was conducted.
Modelling Frontal Sway of Roboshift

Various methods have been used to dynamically
model bipeds in the frontal plane. Hemani [1979, 1980]
used Lagrangian dynamics to construct a constrained
model and then linearised the system in the vicinity of the
operating point. Hemani suggested that constraint forces
could be calculated as functions of the state of the system.
Therefore, his model allowed for the control of a biped in
the frontal plane without the requirement for force sensing
5 Effect of Ankle Torque

One of the issues facing the designer of a biped control
system is the role of ankle torque in control of frontal
sway. Hemani models both the case where no ankle
torque exists and where ankle torque is available. He
concludes that control in frontal sway is achievable in
both cases. The advantage of ankle torque in the frontal
1 The maximum available ankle torque occurs at the instant

prior to the edge of the foot lifting. It is calculated as the
product of half the foot widthand the vertical ground
reaction at the foot
16
of constraints, such as ground reaction forces. Roboshift

uses an alternative approach in which these forces are
measured and used in the control routine, thereby
eliminating the need to calculate them.
A simple dynamic model of Roboshift in the
frontal plane is being developed in order to provide
insight into its dynamic characteristics. The version of the
model shown in Figure 8 was devised to explore natural,
uncontrolled, frequencies of vibration. It is of a three-link
biped with parallel legs. Simplifications made during the
derivation of the model include:
x Cos (T ) 1 for small oscillations in the vicinity of
the vertical.
x Sin (T ) T for small oscillations in the vicinity
of the vertical.
x As there can be no rotation of the body, D body 0
x
x
WB
WA
M gL
kA kB B
2
Z
M B L2
IL
2
The equation shows that the dominating
parameters of the biped are the length of the legs and the
magnitudes of torques at the ankle and body.
Data used for this model corresponded to the first
trials of the biped which had been conducted with a foot
width of 150mm. In this configuration, with full load on
one leg, these feet could provide an ankle stiffness of
approximately 7600Nm/radian with a maximum ankle
torque of 333Nm. Hip abduction is controlled by a
hydraulic cylinder mounted at the top of the body. For the
purpose of the analysis, it will be assumed that these
joints possess a similar stiffness to that of the ankles when
locked. The remaining physical properties of the biped are
listed below:
The equation of motion for the system is found to be;
M gL
D k A k B B T
Length of Legs
Mass of Body
Mass of legs
Moment of Inertia of Legs
Ankle Stiffness
Maximum Ankle Torque
Body Stiffness
M B Mass of body
L Length of legs
I L Moment of inertia of each leg about its
foot
k AT
Control torque.
Ankle torque.
The period of oscillation of the system is calculated as:
The control torques W 3 and W 4 are equal and in

the same direction.
Ankle torques W 1 and W 2 are equal and in the
same direction.
M L2
IL B
where;
k BT
2
300
80
33
7600
333
2000
m
kg
kg
Kgm2
Nm/rad
Nm
Nm/rad
Gravitational constant.
When input into the above equation, these values
predict an oscillating frequency of 0.42 Hz corresponding
to a period of 2.4 seconds. This corresponds to an
observed period of 2.8 seconds in Video 1.
As previously discussed, when the biped is first
lowered to the ground, it will lean toward one side or the
other until the ankle torque becomes equal to the moment
generated by the centre of gravity about the centre of
support. Figure 10(a) shows the response of the simulated
system as the model is allowed to lean from the vertical.
In this case no control torques are included. Ankle torques
are based on the original foot width and a damping
coefficient has been added. The damping factor has been
estimated from observed time taken for the robot to come
to rest after a step input.
Figure 8. Three link biped model
17
The simulation shows that with ankle torque only,

the biped displays a single natural frequency of oscillation
as expected. When the ankle torque is increased to values
generated by the wider feet, it can be seen that the natural
frequency of the system increases as shown inFigure
10(b). As would be expected with increased stiffness, the
frequency of oscillation has been increased and the system
has come to rest closer to the vertical.
Neither of these one degree of freedom
simulations can explain the additional frequency observed
in the motion of the biped. The model was then expanded
to include control torque at the hips. The control software
makes incremental changes to the position of the hip
cylinders based on the feedback error of the actual
position versus the target position. A time-history plot of
the control values for the hip abduction cylinders during
frontal sway can be seen in Figure 11. Control torques are
only active when the position error of the hip abduction
cylinders exceed a set value. Once active, their values are
proportional to angular displacement error. I.e.as the error
increases, the restoring force of the hip control cylinders
increase proportionally. : Effectively the control torque
acts as a tortional stiffness at the hips and can be modelled
as a torque proportional to angular displacement.
The graph in Figure 12 shows the model used in
Figure 10(a) reacting to a simulated control torque. It can
be seen that the additional stiffness of the control system

changes the frequency of the system at the times when the
control torque is acting. The greater the control torque the
shorter the period of oscillation. Once the hip abduction
torque (stiffness) is not acting, the frequency reverts to
that of the natural system. The control torque is a function
of the gain of the control system and the dynamic
response of the joint being controlled. Further work will
investigate the tuning of the gain of the control system for
the dynamics of each joint.
An inverted pendulum can only display harmonic
motion if the moment generated by gravity is overcome
by a restoring torque. In the case of a biped robot in active
control, it has been shown that the restoring torque is
primarily a function of the torque generated by the control
system. As the magnitude of that torque increases, the
period of oscillation will decrease.
The model used to analyse the system did not
explain the source of the additional frequency observed
after the addition of wider feet. An alternate explanation is
that this frequency is a function of the compliance of the
joint. As the hip abduction cylinders move to reduce the
error, the inertia of the system causes an amount of the
movement to be taken up by compliance. This compliance
then results in an increase in torque which accelerates the
joint in proportion to the inertia of the system. Effectively,
the mass of the system acts as a high-pass filter for the
(a)
(b).
Figure 10. Response of biped dynamic model when biped is allowed to lean uncontrolled from the vertical with maximum
ankle torque = (a)333Nm, (b) 667Nm
Frontal Sway Trials
0706awlk
15
110
15 00
10 00
200
10
Degrees
50 0
Control
Position
100
90
-50 0
100
-10 00
-5
80
-15 00
-10
-20 00
0
70
10
15
20
25
30
10
15
20
Seconds
Time
Angular Position
Left Hip Abduction
Left Hip Control
Right Hip Abduction
Control Torque
Right Hip Control
Figure 12. Simulation with control torque.
Figure 11 Hip control values
18
control system which runs at a frequency of 7 Hz. The

additional torque created by the addition of the wider feet
increases the frequency response of the system decreasing
the damping of the control frequency. Thus, as the width
of the feet increases the stiffness of the system, higher
order responses will become apparent. This aspect of the
system will be investigated in future work.
[Kato, 1987] I. Kato. Development of legged walking

robot, Hitachi Review, v36, pp71.
[Furusho, 1986] J.Furusho. Control of a Dynamic Biped
Locomotion System. Journal of Dynamic Systems,
Measurement and Control, V108, p111.
[Sias, 1987] F. Sias, Static Stability Problems in Biped
Robot Design. IEEE Transactions, p436.
Conclusions
Widespread
research
being
conducted
into
anthropomorphic service robots is certainly fascinating
but these robots are not cost effective for the roles on
which the research has been justified. It is as an industrial
scale materials handling platform that biped robots are
more likely to be commercially viable.
An industrial scale biped robot has been built which is
completely self-contained with on-board motive and
electrical power.
The research presented in this paper
demonstrates the large forces involved with the control of
an industrial scale biped robot in sway in the frontal
plane. It demonstrates that the major control factors are:
x The geometry of the device
x The stiffness of its connection to the ground
x The stiffness of the control system at hip joints
The effect of ankle torque stiffness is demonstrated
in data obtained from balancing trials of the robot and
from a dynamic model of the system.
More trials are being conducted. Further areas of
research will include an examination of the manipulation
of the dynamic response of the system by controlling the
active and passive stiffness factors.
Once the robot has been stabilised in frontal sway,
walking will be attempted.
[Zheng, 1988] F. Zheng, Supervisory system for motion

control of biped robot. IEEE Transactions on
Automatic Control, pp1520-1524.
[Vukobratovic, 1970] M Vukobratovic. On the stability of
Biped Locomotion, IEEE Transactions on BioMedical Engineering, BME17#1, pp25-36.
[Raibert, 1986] M. Raibert, Legged Robots.
Communications of the ACM, V29, p499-514.
[Shih, 1992] C. L. Shih. Control of a Biped Robot during
Support Phase. IEEE Transactions on Systems, Man
and Cybernetics, V22/4, pp729-735.
[Yamashita, 1993] T. Yamashita. On the stability of
Bipedal Locomotion. Proceedings IEEE International
Conference on Robotics and Automation, pp49-57
[Golliday, 1977] I Golliday. An Approach Analysing
Biped Locomotion Dynamics and Designing Robot
Locomotion Controls. IEEE Transactions on
Automatic Control, ac22, pp963-972
References
[Gurfinkle, 1981] V. S. Gurfinkel. Walking robot with

Supervisory Control. Mechanisms and Machine
Theory, v16, pp31-36
[Hemani, 1979]
H. Hemani, Aspects of Inverted
Pendulum Problem for Modeling of Locomotion
Systems. IEEE Transactions on Automatic Control,
v24, pp526-535.
[Hemani, 1977] H. Hemani, The Inverted Pendulum, and

the Biped. Mathematical Bioscience, v(34), p95-110.
[Hartrum, 1977] T. Hartrum, Computer Implementation of
a Parametric Model of The Human Gait, Thesis.
[McGhee, 1968] R.B.McGhee, Some Finite State Aspects

of Legged Locomotion Mathematical Bioscience,
P(2)(1)67-84
[Honda,
2003]
Honda
Motor
Corp
www.honda.com/ASIMO/index.html accessed Sept
2003
[Nasluner, 1980] L. M. Nashner, Balance adjustments of

the human perturbed while walking. Journal of
Neurophysiology, v44, pp650-664.
[Todd, 1982]D. J. Todd, Walking Machines, Kogan Press.
19
CROCK.C
1 / 23
/* Program to calculate joint kinetics
*/
#include <stdio.h>
#include <math.h>
#include <dos.h>
#include <stdlib.h>
#include <errno.h>
#define WSPACES(A) ( (A)=='\r' || (A)=='\n' || (A)=='\t' || (A)==' ' )
#define FOPEN(fp,filename,mode)\
{if((fp = fopen(filename,mode)) == NULL)\
{printf("Cannot open file:",filename);\
goto dummyspit;}}/* open file with error control */
#define FCLOSE(fp) {if(fp){fclose(fp); fp=NULL;}}
/*
char *sys_errlist[];
static int
*/
calculated;
int strtods(char*, double*); /* Function to read in rows

of input file and convert
them to numbers
*/
double absd(double);
/* One of Sweeney's bodgie,
macro functions
*/
double trutan(double, double, double, double); /*trutan function */
void head_print(FILE*); /* header printing routine */
int main(void)
{
static char *wet,
*infile,
*outfile,
*hip_out,
*box_ang_pos_out,
*box_lin_pos_out,
*box_ang_acc_out,
*box_lin_acc_out,
*comment_out;
static FILE *inpos,
*outpos,
/*
/*
/*
/*
/*
/*
/*
/*
/* Check pointer */
Input file containing joint positions */
Output file containing converted box positions */
Output file of new config joint positions */
Box angular positions output file */
Box linear positions output file */
Box angular accelerations output file */
Box linear accelerations output file */
Comment File */
/* Joint positions input */

/* Box positions output */
20
CROCK.C
2 / 23
*hip_mtn,
*box_ap_out,
*box_lp_out,
*box_aa_out,
*box_la_out,
*comm_out;
/* Joint positions output */

/* Box angular positions out */
/* Box linear positions out */
/* Box angular accelerations output */
/* Box linear accelerations output */
/* Comments out */
static double hip[35][3][2],

kne[35][3][2],
foo[35][3][2],
ank[35][3][2],
toe[35][3][2],
hel[35][3][2],
knk[35][3][2],
sho[35][3][2],
elb[35][3][2],
wri[35][3][2],
hed[35][3][2],
hic[35][3][2],
chi[35][3][2];
static int
X = 0,
Y = 1,
Z = 2,
L = 0,
R = 1,
side = 0,
port = 0,
star = 1;
/*
/*
/*
/*
/*
/*
/*
/*
/*
/*
/*
/*
/* Hip Coords
Knee Coords
Foot Coords
Ankle Coords
Toe Coords
Heel Coords
Neck and Head Coords
Shoulder Coords
Elbow Coords
Wrist Coords
Chin and Pelvis Coords
Centre of hip coord
Chin Cooordiantes
*/
*/
*/
*/
*/
*/
*/
*/
*/
*/
*/
*/
*/
/* Co-ordinate indecies
*/
/* Auxilary counters
*/
static int
frame = 0,
/* Counter input frames
page = 0;
/* Auxilary Counter
*/
*/
static char linedat[600];

static
double jline[66]; /* array for reading the values of the

line read statement
*/
static double
cog_head[35][3][2],
/* COG of Head
cog_trnk[35][3][2],
/* COG of Trunk
*/
*/
21
CROCK.C
3 / 23
cog_uarm[35][3][2],
cog_larm[35][3][2];
/* COG of Upper Arm

/* COG of lower Arm
static double
acc_head[35][3][2],
acc_trnk[35][3][2],
/* Acc
acc_uarm[35][3][2],
/* Acc
acc_larm[35][3][2],
/* Acc
acc_hic[35][3];
/* Acc
*/
*/
/* Acc of Head
of Trunk
of Upper Arm
of lower Arm
of hic
*/
*/
*/
*/
*/
static double vel_head[35][3][2],

/* Vel of Head
vel_trnk[35][3][2],
/* Vel of Trunk
vel_uarm[35][3][2],
/* Vel of Upper Arm
vel_larm[35][3][2],
/* Vel of lower Arm
vel_hic[35][3];
/* Vel of hic
*/
*/
*/
*/
*/
static double theta_head[35][2],

/* Theta of Head
theta_trnk[35][2],
/* Theta of Trunk
theta_uarm[35][2],
/* Theta of Upper Arm
theta_larm[35][2];
/* Theta of lower Arm
*/
*/
*/
*/
static double aacc_head[35][2],

aacc_trnk[35][2],
aacc_uarm[35][2],
aacc_larm[35][2];
/* Aacc of Head
/* Aacc of Trunk
/* Aacc of Upper Arm
/* Aacc of lower Arm
*/
*/
*/
static double avel_head[35][2],

avel_trnk[35][2],
avel_uarm[35][2],
avel_larm[35][2];
/* avel of Head
/* avel of Trunk
/* avel of Upper Arm
/* avel of lower Arm
*/
*/
*/
static double length = 2.0,

weight = 100.0;
static
static
*/
*/
/*body length */
/* body weight
*/
double mass_head,
/* Mass of Head
mass_trnk,
/* Mass of Trunk
mass_uarm,
/* Mass of Upper Arm
mass_larm,
/* Mass of lower Arm
mass_box;
/* Mass of Box
double length_head, /* length of Head
length_trnk, /* length of Trunk
*/
*/
*/
*/
*/
*/
*/
22
CROCK.C
4 / 23
length_uarm,
length_larm,
length_box,
sides_box;
/*
/*
/*
/*
length of Upper Arm

length of lower Arm
width of box
sides of box
static double rmi_head,

rmi_trnk,
rmi_uarm,
rmi_larm,
rmi_box;
/*
/*
/*
/*
/*
rmi
rmi
rmi
rmi
static
rmi of Head
of Trunk
of Upper Arm
of lower Arm
of Box
*/
*/
*/
*/
*/
double tim_inc = 0.1;
static int board, j, k, wind, grind;

static
static
static
static
static
static
static
*/
*/
*/
*/
/* Utility Counters
*/
/* Declare reaction calculation variables */

double grav = 9.81, vconst;
double lx_larm_L, lx_larm_R, lx_head, lx_trnk, nx_trnk, nx_uarm_L, lx_uarm_L, nx_uarm_R, lx_uarm_R;
double lz_larm_L, lz_larm_R, lz_head, lz_trnk, nz_trnk, nz_uarm_L, lz_uarm_L, nz_uarm_R, lz_uarm_R;
double mom_head, mom_trnk, mom_larm_L, mom_larm_R, mom_uarm_L, mom_uarm_R, crx;
double fz_head, fz_trnk, fz_larm_L, fz_larm_R, fz_uarm_L, fz_uarm_R, lx_box, fg_trnk;
double fx_head, fx_trnk, fx_larm_L, fx_larm_R, fx_uarm_L, fx_uarm_R, lz_box;
double Ax[35], Az[35], Am[35], Vx[35], Vz[35], Vm[35], Dx[35], Dz[35], Dm[35], Dd[35], lx_box_0, lz_box_0;
infile = "c:\\alien\\cicords.prn";
/* Joints In */
outfile = "c:\\alien\\bocords.prn"; /* joints Out */
hip_out = "c:\\alien\\hip_pos.prn";
box_ang_pos_out = "c:\\alien\\box_a_p.prn"; /* Ang pos
box_lin_pos_out = "c:\\alien\\box_l_p.prn"; /* Lin pos
box_ang_acc_out = "c:\\alien\\box_a_a.prn"; /* Ang acc
box_lin_acc_out = "c:\\alien\\box_l_a.prn"; /* Lin acc
comment_out = "c:\\alien\\comm.prn";
/* Comment
out */
out */
out */
out */
File */
hip_mtn = NULL;
box_ap_out = NULL;
box_lp_out = NULL;
box_aa_out = NULL;
box_la_out = NULL;
comm_out = NULL;
inpos = NULL;
outpos = NULL; /* Open the file forreading input */
23
CROCK.C
5 / 23
FOPEN( hip_mtn, hip_out, "w");

FOPEN(box_ap_out, box_ang_pos_out, "w");
FOPEN(box_lp_out, box_lin_pos_out, "w");
FOPEN(box_aa_out, box_ang_acc_out, "w");
FOPEN(box_la_out, box_lin_acc_out, "w");
FOPEN(comm_out, comment_out, "w");
FOPEN(outpos, outfile, "w");
FOPEN(inpos, infile, "r"); /* Open the file forreading input */
/*=========================================================================*/
/*
Calculate Mass Moments Of Inertia
*/
/*=========================================================================*/
mass_head = 0.080 * weight;
/* Mass of Head
*/
mass_trnk = 0.480 * weight;
/* Mass of Trunk
*/
mass_uarm = 0.027 * weight;
/* Mass of Upper Arm
*/
mass_larm = 0.022 * weight;
/* Mass of lower Arm
*/
mass_box = 0.658 * weight;
/* Mass of Box
*/
length_head
length_trnk
length_uarm
length_larm
length_box
sides_box =
= 0.156 * length;
= 0.346 * length;
= 0.173 * length;
= 0.260 * length;
= 0.150 * length;
0.07 * length;
/* length of Head
/* length of Trunk
/* length of Upper Arm
/* length of lower Arm
/* width of box
/* sides of box
rmi_head = (length_head * length_head * mass_head)

rmi_trnk = (length_trnk * length_trnk * mass_trnk)
rmi_uarm = (length_uarm * length_uarm * mass_uarm)
rmi_larm = (length_larm * length_larm * mass_larm)
rmi_box = (mass_box / 12.0) * (sides_box*sides_box
/
/
/
/
+
*/
*/
*/
*/
*/
*/
12.0;
12.0;
12.0;
12.0;
length_box*length_box);
/*------------------------------------------------------------------*/
/* Scan the co-ordinates then transfer them to the
*/
/* array coordinates via all these ridiculous equations
*/
/*------------------------------------------------------------------*/
printf("\n");
printf("\n");
printf("\n");
printf("\n");
24
CROCK.C
6 / 23
frame = 2;
/* initialise first frame pointer */
while ((wet = fgets( linedat, 600, inpos)) != NULL)

{
jline[0] = 66.0;
if(strtods( linedat, jline) == -1)
hip[frame][X][L]
hip[frame][Z][L]
hip[frame][Y][L]
hip[frame][X][R]
hip[frame][Z][R]
hip[frame][Y][R]
=
=
=
=
=
=
jline[0];
jline[1];
jline[2];
jline[3];
jline[4];
jline[5];
kne[frame][X][L]
kne[frame][Z][L]
kne[frame][Y][L]
kne[frame][X][R]
kne[frame][Z][R]
kne[frame][Y][R]
=
=
=
=
=
=
jline[6];
jline[7];
jline[8];
jline[9];
jline[10];
jline[11];
ank[frame][X][L]
ank[frame][Z][L]
ank[frame][Y][L]
ank[frame][X][R]
ank[frame][Z][R]
ank[frame][Y][R]
=
=
=
=
=
=
jline[12];
jline[13];
jline[14];
jline[15];
jline[16];
jline[17];
foo[frame][X][L]
foo[frame][Z][L]
foo[frame][Y][L]
foo[frame][X][R]
foo[frame][Z][R]
foo[frame][Y][R]
=
=
=
=
=
=
jline[18];
jline[19];
jline[20];
jline[21];
jline[22];
jline[23];
printf(" Input file loading error. \n");
toe[frame][X][L] = jline[24];
toe[frame][Z][L] = jline[25];
25
CROCK.C
7 / 23
toe[frame][Y][L]
toe[frame][X][R]
toe[frame][Z][R]
toe[frame][Y][R]
=
=
=
=
jline[26];
jline[27];
jline[28];
jline[29];
hel[frame][X][L]
hel[frame][Z][L]
hel[frame][Y][L]
hel[frame][X][R]
hel[frame][Z][R]
hel[frame][Y][R]
=
=
=
=
=
=
jline[30];
jline[31];
jline[32];
jline[33];
jline[34];
jline[35];
knk[frame][X][L] = jline[36];
knk[frame][Z][L] = jline[37];
knk[frame][Y][L] = jline[38];
hic[frame][X][L] = jline[39];
hic[frame][Z][L] = jline[40];
hic[frame][Y][L] = jline[41];
sho[frame][X][L]
sho[frame][Z][L]
sho[frame][Y][L]
sho[frame][X][R]
sho[frame][Z][R]
sho[frame][Y][R]
=
=
=
=
=
=
jline[42];
jline[43];
jline[44];
jline[45];
jline[46];
jline[47];
elb[frame][X][L]
elb[frame][Z][L]
elb[frame][Y][L]
elb[frame][X][R]
elb[frame][Z][R]
elb[frame][Y][R]
=
=
=
=
=
=
jline[48];
jline[49];
jline[50];
jline[51];
jline[52];
jline[53];
wri[frame][X][L]
wri[frame][Z][L]
wri[frame][Y][L]
wri[frame][X][R]
wri[frame][Z][R]
wri[frame][Y][R]
=
=
=
=
=
=
jline[54];
jline[55];
jline[56];
jline[57];
jline[58];
jline[59];
26
CROCK.C
8 / 23
chi[frame][X][L] = jline[60];
chi[frame][Z][L] = jline[61];
chi[frame][Y][L] = jline[62];
hed[frame][X][L] = jline[63];
hed[frame][Z][L] = jline[64];
hed[frame][Y][L] = jline[65];
printf("
Loading input file line
##
%d \r", frame);
frame = frame + 1;
delay(20);
}
crx = toe[30][X][R] - toe[2][X][R];
vconst = crx / (31*tim_inc);
printf("\n ");
/*----------------------------------------------------------------------*/
/* Initialise first and second frame as second last and last frames read*/
/*----------------------------------------------------------------------*/
knk[0][X][L] = knk[frame-2][X][L] - crx;
knk[0][Z][L] = knk[frame-2][Z][L];
knk[0][Y][L] = knk[frame-2][Y][L];
hic[0][X][L] = hic[frame-2][X][L] - crx;
hic[0][Z][L] = hic[frame-2][Z][L];
hic[0][Y][L] = hic[frame-2][Y][L];
sho[0][X][L]
sho[0][Z][L]
sho[0][Y][L]
sho[0][X][R]
sho[0][Z][R]
sho[0][Y][R]
=
=
=
=
=
=
sho[frame-2][X][L] - crx;
sho[frame-2][Z][L];
sho[frame-2][Y][L];
sho[frame-2][X][R] - crx;
sho[frame-2][Z][R];
sho[frame-2][Y][R];
elb[0][X][L]
elb[0][Z][L]
elb[0][Y][L]
elb[0][X][R]
elb[0][Z][R]
=
=
=
=
=
elb[frame-2][X][L] - crx;
elb[frame-2][Z][L];
elb[frame-2][Y][L];
elb[frame-2][X][R] - crx;
elb[frame-2][Z][R];
27
CROCK.C
9 / 23
elb[0][Y][R] = elb[frame-2][Y][R];
wri[0][X][L]
wri[0][Z][L]
wri[0][Y][L]
wri[0][X][R]
wri[0][Z][R]
wri[0][Y][R]
=
=
=
=
=
=
wri[frame-2][X][L] - crx;
wri[frame-2][Z][L];
wri[frame-2][Y][L];
wri[frame-2][X][R] - crx;
wri[frame-2][Z][R];
wri[frame-2][Y][R];
chi[0][X][L] = chi[frame-2][X][L] - crx;

chi[0][Z][L] = chi[frame-2][Z][L];
chi[0][Y][L] = chi[frame-2][Y][L];
hed[0][X][L] = hed[frame-2][X][L] - crx;
hed[0][Z][L] = hed[frame-2][Z][L];
hed[0][Y][L] = hed[frame-2][Y][L];
/*----------------------------------------------------------------------*/
knk[1][X][L] = knk[frame-1][X][L] - crx;
knk[1][Z][L] = knk[frame-1][Z][L];
knk[1][Y][L] = knk[frame-1][Y][L];
hic[1][X][L] = hic[frame-1][X][L] - crx;
hic[1][Z][L] = hic[frame-1][Z][L];
hic[1][Y][L] = hic[frame-1][Y][L];
sho[1][X][L]
sho[1][Z][L]
sho[1][Y][L]
sho[1][X][R]
sho[1][Z][R]
sho[1][Y][R]
=
=
=
=
=
=
sho[frame-1][X][L] - crx;
sho[frame-1][Z][L];
sho[frame-1][Y][L];
sho[frame-1][X][R] - crx;
sho[frame-1][Z][R];
sho[frame-1][Y][R];
elb[1][X][L]
elb[1][Z][L]
elb[1][Y][L]
elb[1][X][R]
elb[1][Z][R]
elb[1][Y][R]
=
=
=
=
=
=
elb[frame-1][X][L] - crx;
elb[frame-1][Z][L];
elb[frame-1][Y][L];
elb[frame-1][X][R] - crx;
elb[frame-1][Z][R];
elb[frame-1][Y][R];
wri[1][X][L] = wri[frame-1][X][L] - crx;

wri[1][Z][L] = wri[frame-1][Z][L];
28
CROCK.C
10 / 23
wri[1][Y][L]
wri[1][X][R]
wri[1][Z][R]
wri[1][Y][R]
=
=
=
=
wri[frame-1][Y][L];
wri[frame-1][X][R] - crx;
wri[frame-1][Z][R];
wri[frame-1][Y][R];
chi[1][X][L] = chi[frame-1][X][L] - crx;

chi[1][Z][L] = chi[frame-1][Z][L];
chi[1][Y][L] = chi[frame-1][Y][L];
hed[1][X][L] = hed[frame-1][X][L] - crx;
hed[1][Z][L] = hed[frame-1][Z][L];
hed[1][Y][L] = hed[frame-1][Y][L];
/*=========================================================================*/
/*
*/
/*
Main Loop For Kinetic Calculations
*/
/*
*/
/*=========================================================================*/
/*----------------------------------------------------------------------*/
/*
Calculate Co-Ordinates Of Centres Of Gravity
*/
/*----------------------------------------------------------------------*/
page = 0;
for( page = 0; page < frame; page ++)
{
j = page;
side = 0;
for( side = port; side < (star+1); side ++)
{
/* Loop To Calculate Each Side
*/
cog_uarm[page][X][side] = sho[page][X][side] + 0.43 * ( elb[page][X][side] - sho[page][X][side] );

cog_uarm[page][Z][side] = elb[page][Z][side] + 0.57 * ( sho[page][Z][side] - elb[page][Z][side] );
cog_larm[page][X][side] = elb[page][X][side] + 0.50 * ( wri[page][X][side] - elb[page][X][side] );
cog_larm[page][Z][side] = wri[page][Z][side] + 0.50 * ( elb[page][Z][side] - wri[page][Z][side] );
/* Calculate Middle Coordinates */
cog_head[page][X][port] = knk[page][X][port] + 0.75 * ( hed[page][X][port] - knk[page][X][port] );
29
CROCK.C
11 / 23
cog_head[page][Z][port] = knk[page][Z][port] + 0.75 * ( hed[page][Z][port] - knk[page][Z][port] );

cog_trnk[page][X][port] = knk[page][X][port] + 0.43 * ( hic[page][X][port] - knk[page][X][port] );
cog_trnk[page][Z][port] = hic[page][Z][port] + 0.57 * ( knk[page][Z][port] - hic[page][Z][port] );
}
printf("
delay(10);
}
Centre of Gravity Calculation ** %5d \r", page);
printf("\n ");
/*----------------------------------------------------------------------*/
/*
Calculate Linear Velocities and Accelerations
*/
/*----------------------------------------------------------------------*/
for ( j = 2; j < frame; j++ )
{
printf("
Linear Position Calculation
||
%d \r", j);
for( board = X; board < (Z+1); board = board + 2)

{
{
/* hip centre */
vel_hic[j-1][board] = ( hic[j-1][board][port] - hic[j-2][board][port] ) / (tim_inc * 1000);
vel_hic[j][board] = ( hic[j][board][port] - hic[j-1][board][port] ) / (tim_inc * 1000);
acc_hic[j][board] = ( vel_hic[j][board] - vel_hic[j-1][board] );
/* head */
vel_head[j-1][board][port] = (cog_head[j-1][board][port] - cog_head[j-2][board][port]) / (tim_inc * 1000);
vel_head[j][board][port] =
(cog_head[j][board][port] cog_head[j-1][board][port]) / (tim_inc * 1000);
acc_head[j][board][port] =
(vel_head[j][board][port] - vel_head[j-1][board][port]) / tim_inc;
/* trnk */
vel_trnk[j-1][board][port] = (cog_trnk[j-1][board][port] - cog_trnk[j-2][board][port]) / (tim_inc * 1000);
vel_trnk[j][board][port] =
(cog_trnk[j][board][port] cog_trnk[j-1][board][port]) / (tim_inc* 1000);
acc_trnk[j][board][port] =
(vel_trnk[j][board][port] - vel_trnk[j-1][board][port]) / tim_inc;
/* larm */
vel_larm[j-1][board][side] = (cog_larm[j-1][board][side] - cog_larm[j-2][board][side]) / (tim_inc * 1000);
vel_larm[j][board][side] =
(cog_larm[j][board][side] cog_larm[j-1][board][side]) / (tim_inc * 1000);
acc_larm[j][board][side] =
(vel_larm[j][board][side] - vel_larm[j-1][board][side]) / tim_inc;
/* uarm */
30
CROCK.C
12 / 23
vel_uarm[j-1][board][side] = (cog_uarm[j-1][board][side] - cog_uarm[j-2][board][side]) / (tim_inc * 1000);
vel_uarm[j][board][side] =
(cog_uarm[j][board][side] cog_uarm[j-1][board][side]) / (tim_inc * 1000);
acc_uarm[j][board][side] =
(vel_uarm[j][board][side] - vel_uarm[j-1][board][side]) / tim_inc;
delay(10);
}
}
}
printf("\n");
/*----------------------------------------------------------------------*/
/*
Calculate Angular Velocities and Accelerations
*/
/*----------------------------------------------------------------------*/
for( grind = 0; grind < frame; grind++ )
{
printf("
Angular Position Calculation
^^ %5d \r", grind);
for( side = L; side < R+1; side ++)

{
theta_uarm[grind][side] = trutan( sho[grind][X][side],
side] );
theta_larm[grind][side] = trutan( elb[grind][X][side],
side] );
theta_trnk[grind][port] = trutan( knk[grind][X][port],
port] );
theta_head[grind][port] = trutan( hed[grind][X][port],
port] );
sho[grind][Z][side],elb[grind][X][side], elb[grind][Z][
elb[grind][Z][side],wri[grind][X][side], wri[grind][Z][
knk[grind][Z][port],hic[grind][X][port], hic[grind][Z][
hed[grind][Z][port],knk[grind][X][port], knk[grind][Z][
}
delay(10);
}
printf(" \n");
/*----------------------------------------------------------------------*/
/*
Calculate Angular Velocities and Accelerations
*/
/*----------------------------------------------------------------------*/
for ( j = 2; j < frame; j++ )
{
printf("
Angular Acc Calculation
$$ %5d \r", j);
31
CROCK.C
13 / 23

{
/* head */
avel_head[j-1][port]
avel_head[j][port] =
aacc_head[j][port] =
/* trnk */
avel_trnk[j-1][port]
avel_trnk[j][port] =
aacc_trnk[j][port] =
/* larm */
avel_larm[j-1][side]
avel_larm[j][side] =
aacc_larm[j][side] =
/* uarm */
avel_uarm[j-1][side]
avel_uarm[j][side] =
aacc_uarm[j][side] =
= (theta_head[j-1][port] - theta_head[j-2][port]) / tim_inc;

(theta_head[j][port] theta_head[j-1][port]) / tim_inc;
(avel_head[j-1][port] - avel_head[j-1][port]) / tim_inc;
= (theta_trnk[j-1][port] - theta_trnk[j-2][port]) / tim_inc;
(theta_trnk[j][port] theta_trnk[j-1][port]) / tim_inc;
(avel_trnk[j][port] - avel_trnk[j-1][port]) / tim_inc;
= (theta_larm[j-1][side] - theta_larm[j-2][side]) / tim_inc;
(theta_larm[j][side] theta_larm[j-1][side]) / tim_inc;
(avel_larm[j][side] - avel_larm[j-1][side]) / tim_inc;
= (theta_uarm[j-1][side] - theta_uarm[j-2][side]) / tim_inc;
(theta_uarm[j][side] theta_uarm[j-1][side]) / tim_inc;
(avel_uarm[j][side] - avel_uarm[j-1][side]) / tim_inc;
}
}
printf("\n");
/*----------------------------------------------------------------------*/
/*
Correction for initial accelerations.
*/
/*----------------------------------------------------------------------*/
/* 1st Frame */
acc_hic[3][X] = acc_hic[frame-2][X];
acc_hic[3][Z] = acc_hic[frame-2][Z];
acc_head[3][X][L] = acc_head[frame-2][X][L];
acc_head[3][Z][L] = acc_head[frame-2][Z][L];
acc_trnk[3][X][L] = acc_trnk[frame-2][X][L];
acc_trnk[3][Z][L] = acc_trnk[frame-2][Z][L];
acc_larm[3][X][L] = acc_larm[frame-2][X][L];
acc_larm[3][Z][L] = acc_larm[frame-2][Z][L];
32
CROCK.C
14 / 23
acc_larm[3][X][R] = acc_larm[frame-2][X][R];
acc_larm[3][Z][R] = acc_larm[frame-2][Z][R];
acc_uarm[3][X][L]
acc_uarm[3][Z][L]
acc_uarm[3][X][R]
acc_uarm[3][Z][R]
=
=
=
=
acc_uarm[frame-2][X][L];
acc_uarm[frame-2][Z][L];
acc_uarm[frame-2][X][R];
acc_uarm[frame-2][Z][R];
aacc_head[3][L] = aacc_head[frame-2][L];
aacc_trnk[3][L] = aacc_trnk[frame-2][L];
aacc_larm[3][L] = aacc_larm[frame-2][L];
aacc_larm[3][R] = aacc_larm[frame-2][R];
aacc_uarm[3][L] = aacc_uarm[frame-2][L];
aacc_uarm[3][R] = aacc_uarm[frame-2][R];
/*----------------------------------------------------------------------*/
/* 2nd Frame */
acc_hic[2][X] = acc_hic[frame-1][X];
acc_hic[2][Z] = acc_hic[frame-1][Z];
acc_head[2][X][L] = acc_head[frame-1][X][L];
acc_head[2][Z][L] = acc_head[frame-1][Z][L];
acc_trnk[2][X][L] = acc_trnk[frame-1][X][L];
acc_trnk[2][Z][L] = acc_trnk[frame-1][Z][L];
acc_larm[2][X][L]
acc_larm[2][Z][L]
acc_larm[2][X][R]
acc_larm[2][Z][R]
=
=
=
=
acc_larm[frame-1][X][L];
acc_larm[frame-1][Z][L];
acc_larm[frame-1][X][R];
acc_larm[frame-1][Z][R];
acc_uarm[2][X][L]
acc_uarm[2][Z][L]
acc_uarm[2][X][R]
acc_uarm[2][Z][R]
=
=
=
=
acc_uarm[frame-1][X][L];
acc_uarm[frame-1][Z][L];
acc_uarm[frame-1][X][R];
acc_uarm[frame-1][Z][R];
33
CROCK.C
15 / 23
aacc_head[2][L] = aacc_head[frame-1][L];
aacc_trnk[2][L] = aacc_trnk[frame-1][L];
aacc_larm[2][L] = aacc_larm[frame-1][L];
aacc_larm[2][R] = aacc_larm[frame-1][R];
aacc_uarm[2][L] = aacc_uarm[frame-1][L];
aacc_uarm[2][R] = aacc_uarm[frame-1][R];
/*-------------------------------------------------------------------------*/
/*
Routines to print out cog data
*/
/*-------------------------------------------------------------------------*/
fprintf(box_lp_out, "Centre Of Gravity Co-Ordinates \n");
fprintf(box_lp_out, "------------------------------\n");
head_print(box_lp_out);
wind = 0;
for( wind = 0; wind < frame; wind ++)
{
fprintf( box_lp_out, " %8.1f %8.1f %8.1f %8.1f",
[X][R], cog_larm[wind][Z][R]);
fprintf( box_lp_out, " %8.1f %8.1f %8.1f %8.1f",
[X][R], cog_uarm[wind][Z][R]);
fprintf( box_lp_out, " %8.1f %8.1f %8.1f %8.1f\n",
[X][L], cog_head[wind][Z][L]);
cog_larm[wind][X][L], cog_larm[wind][Z][L],
cog_larm[wind]
cog_uarm[wind][X][L], cog_uarm[wind][Z][L],
cog_uarm[wind]
cog_trnk[wind][X][L], cog_trnk[wind][Z][L],
cog_head[wind]
printf("
Centre of Gravity Printing
>> %5d \r", wind);
delay(10);
}
/*-------------------------------------------------------------------------*/
/*
Routines to print out angularinpos data
*/
/*-------------------------------------------------------------------------*/
fprintf(box_ap_out, "Angular Positions Of Segments \n");
fprintf(box_ap_out, "--------------------------------\n");
head_print(box_ap_out);
{
34
CROCK.C
16 / 23
fprintf( box_ap_out, " %17.3f %17.3f",

theta_larm[wind][L],
theta_larm[wind][R]);
fprintf( box_ap_out, " %17.3f %17.3f",
theta_uarm[wind][L],
theta_uarm[wind][R]);
fprintf( box_ap_out, " %17.3f %17.3f\n", theta_trnk[wind][L],
theta_head[wind][L]);
}
/*-------------------------------------------------------------------------*/
/*
Routines to print out aacc data
*/
/*-------------------------------------------------------------------------*/
fprintf(box_aa_out, "Angular Accelerations Of Segments \n");
fprintf(box_aa_out, "--------------------------------\n");
head_print(box_aa_out);
{
fprintf( box_aa_out, " %17.3f %17.3f",
aacc_larm[wind][L],
aacc_larm[wind][R]);
fprintf( box_aa_out, " %17.3f %17.3f",
aacc_uarm[wind][L],
aacc_uarm[wind][R]);
fprintf( box_aa_out, " %17.3f %17.3f\n", aacc_trnk[wind][L],
aacc_head[wind][L]);
}
/*-------------------------------------------------------------------------*/
/*
Routines to print out linear position data
*/
/*-------------------------------------------------------------------------*/
fprintf(box_la_out,"Linear Accelerations Of Cog's \n");
fprintf(box_la_out, "-----------------------------\n");
head_print(box_la_out);
{
fprintf( box_la_out, " %8.4f %8.4f %8.4f %8.4f",
acc_larm[wind][X][L], acc_larm[wind][Z][L],
[X][R], acc_larm[wind][Z][R]);
fprintf( box_la_out, " %8.4f %8.4f %8.4f %8.4f",
acc_uarm[wind][X][L], acc_uarm[wind][Z][L],
[X][R], acc_uarm[wind][Z][R]);
fprintf( box_la_out, " %8.4f %8.4f %8.4f %8.4f \n", acc_trnk[wind][X][L], acc_trnk[wind][Z][L],
[X][L], acc_head[wind][Z][L]);
}
/*-------------------------------------------------------------------------*/
/*
Routine to print out hip centre position data
*/
/*-------------------------------------------------------------------------*/
{
fprintf( hip_mtn, " %3d %8.5f %8.5f %8.5f %8.5f %8.5f %8.5f \n",
wind, hic[wind][X][L]/1000, hic[wind][Z][L]/1000, vel_hic[wind][X],
acc_larm[wind]
acc_uarm[wind]
acc_head[wind]
35
CROCK.C
17 / 23
vel_hic[wind][Z], acc_hic[wind][X], acc_hic[wind][Z]);
}
printf(" \n");
printf(" \n");
printf(" \n");
printf("
printf("
printf("
Reaction Calculations\n");
---------------------\n");
cyc Moment
Fx
Fz
Dx
Dz
Dm\n");
/*=========================================================================*/
/*=========================================================================*/
/*=========================================================================*/
/*
*/
/*
Main Loop For Box Calculations
*/
/*
*/
/*=========================================================================*/
/*=========================================================================*/
/*=========================================================================*/
/*Initialise boundary conditions
*/
/* These values obrtained from graphical */
/* analysis of COG motion.
*/
lx_box_0
lz_box_0
lx_box =
lz_box =
Dx[0]
Ax[0]
Vx[1]
Dz[0]
Az[0]
Vz[0]
Dm[1]
Am[1]
Vm[1]
Dd[1]
=
=
=
=
=
=
=
=
=
=
= 0.03 * length;
= - 0.005 * length + 0.315 /* Average distance between COG & hip_c */;
lx_box_0;
lz_box_0;
lx_box_0;
0.0;
-0.02;
lz_box_0;
0.0;
vel_hic[1][Z];
-2.0;
0.0;
-16;
hic[1][X][L] + 28;
36
CROCK.C
18 / 23
for ( k = 2; k < frame; k++ )
{
/* Calculate head action radiuses */
lx_head = (knk[k][X][L] - cog_head[k][X][L]) / 1000;
lz_head = (cog_head[k][Z][L] - knk[k][Z][L]) / 1000;
/* Calculate forces transferred at neck */
fx_head = mass_head * acc_head[k][X][L];
fz_head = mass_head * ( acc_head[k][Z][L] + grav);
mom_head = fx_head * lz_head - fz_head * lx_head + aacc_head[k][L] * rmi_head;
/* Calculate lower arm action radiuses */
lx_larm_L = (elb[k][X][L] - cog_larm[k][X][L])
lx_larm_R = (elb[k][X][R] - cog_larm[k][X][R])
lz_larm_L = (elb[k][Z][L] - cog_larm[k][Z][L])
lz_larm_R = (elb[k][Z][L] - cog_larm[k][Z][L])
/
/
/
/
1000;
1000;
1000;
1000;
/* Calculate forces transferred at elbows */

fx_larm_L = mass_larm * acc_larm[k][X][L];
fx_larm_R = mass_larm * acc_larm[k][X][R];
fz_larm_L = mass_larm * ( acc_larm[k][Z][L] + grav);
fz_larm_R = mass_larm * ( acc_larm[k][Z][R] + grav);
mom_larm_L = aacc_larm[k][L] * rmi_larm - fx_larm_L * lz_larm_L + fz_larm_L * lx_larm_L;
mom_larm_R = aacc_larm[k][R] * rmi_larm - fx_larm_R * lz_larm_R + fz_larm_R * lx_larm_R;
/* Calculate upper arm action radiuses */
lx_uarm_L = (sho[k][X][L] - cog_uarm[k][X][L])
lx_uarm_R = (sho[k][X][R] - cog_uarm[k][X][R])
lz_uarm_L = (sho[k][Z][L] - cog_uarm[k][Z][L])
lz_uarm_R = (sho[k][Z][R] - cog_uarm[k][Z][R])
/
/
/
/
1000;
1000;
1000;
1000;
nx_uarm_L
nx_uarm_R
nz_uarm_L
nz_uarm_R
/
/
/
/
1000;
1000;
1000;
1000;
=
=
=
=
(cog_uarm[k][X][L]
(cog_uarm[k][X][R]
(cog_uarm[k][Z][L]
(cog_uarm[k][Z][R]
elb[k][X][L])
elb[k][X][R])
elb[k][Z][L])
elb[k][Z][R])
/* calculate reactions at shoulders

fx_uarm_L = fx_larm_L + mass_uarm *
fx_uarm_R = fx_larm_R + mass_uarm *
fz_uarm_L = fz_larm_L + mass_uarm *
fz_uarm_R = fz_larm_R + mass_uarm *
*/
acc_uarm[k][X][L];
acc_uarm[k][X][R];
( acc_uarm[k][Z][L] + grav );
( acc_uarm[k][Z][R] + grav );
37
CROCK.C
19 / 23
mom_uarm_L = aacc_uarm[k][L] * rmi_uarm + mom_larm_L - fx_larm_L * nz_uarm_L + fz_larm_L
* nx_uarm_L - fx_uarm_L * lz_uarm_L + fz_uarm_L * lx_uarm_L;
mom_uarm_R = aacc_uarm[k][L] * rmi_uarm + mom_larm_R - fx_larm_R * nz_uarm_R + fz_larm_R
* nx_uarm_R - fx_uarm_R * lz_uarm_R + fz_uarm_L * lx_uarm_R;
/* Calculate trunk action radiuses */
lx_trnk = (sho[k][X][L] - cog_trnk[k][X][L]) / 1000;
lz_trnk = (sho[k][Z][L] - cog_trnk[k][Z][L]) / 1000;
nx_trnk = (cog_trnk[k][X][L] - ((hip[k][X][L] + hip[k][X][R]) / 2)) / 1000;
nz_trnk = (cog_trnk[k][Z][L] - ((hip[k][Z][L] + hip[k][Z][R]) / 2)) / 1000;
/* calculate reactiions at hips */
fx_trnk = fx_uarm_L + fx_uarm_R + fx_head + mass_trnk * acc_trnk[k][X][L];
fz_trnk = fz_uarm_L + fz_uarm_R + fz_head + mass_trnk * ( acc_trnk[k][Z][L] + grav );
fg_trnk = fz_trnk - grav * mass_box;
mom_trnk = mom_head + mom_uarm_L + mom_uarm_R + aacc_trnk[k][L] * rmi_trnk
- fz_trnk * nx_trnk + fx_trnk * nz_trnk
- (fz_uarm_L + fz_uarm_R + fz_head) * lx_trnk
+ (fx_uarm_L + fx_uarm_R + fx_head) * lz_trnk;
/* calculate accelerations of box */
Az[k] = fz_trnk/mass_box - 9.81;
Ax[k] = fx_trnk/mass_box;
Vz[k] = Vz[k-1] + Az[k-1]*tim_inc;
Vx[k] = Vx[k-1] + Ax[k-1]*tim_inc;
Dd[k] = Dd[k-1] + vconst*tim_inc;
Dx[k] = Dx[k-1] + Vx[k-1]*tim_inc + 0.5*Ax[k-1]*tim_inc*tim_inc;
Dz[k] = Dz[k-1] + Vz[k-1]*tim_inc + 0.5*Az[k-1]*tim_inc*tim_inc;
lx_box = hic[k][X][L]/1000 - Dx[k] - Dd[k]/1000;
lz_box = hic[k][Z][L]/1000 - Dz[k];
Am[k] = (mom_trnk - fz_trnk*lx_box - fx_trnk*lz_box) / rmi_box;
Vm[k] = Vm[k-1] + Am[k-1]*tim_inc;
Dm[k] = Dm[k-1] + Vm[k-1]*tim_inc + 0.5*Am[k-1]*tim_inc*tim_inc;
printf("
);
%3d %7.2f %7.2f %7.2f %7.2f %7.2f %7.2f\n",k, fx_trnk, fz_trnk, mom_trnk, Dx[k], Dz[k], Dm[k]
38
CROCK.C
20 / 23
fprintf(comm_out," %3d %7.2f %7.2f %7.2f %7.2f %7.2f %7.2f\n", k, lx_box, lz_box, fx_trnk, fz_trnk - 9.81*
mass_box, mom_trnk, Am[k]);
/* :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: */
/*
Output joint coords
*/
/* :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: */
fprintf( outpos, "%6.1f %6.1f %6.1f %6.1f %6.1f %6.1f %6.1f %6.1f %6.1f %6.1f %6.1f %6.1f %6.1f %6.1f %6.1f %
6.1f %6.1f %6.1f %6.1f %6.1f %6.1f %6.1f %6.1f %6.1f %6.1f %6.1f %6.1f %6.1f %6.1f %6.1f %6.1f %6.1f %6.1f
%6.1f %6.1f %6.1f %6.1f %6.1f %6.1f \n",
hip[k][X][L], hip[k][Z][L], hip[k][Y][L], hip[k][X][R], hip[k][Z][R], hip[k][Y][R],
kne[k][X][L], kne[k][Z][L], kne[k][Y][L], kne[k][X][R], kne[k][Z][R], kne[k][Y][R],
ank[k][X][L], ank[k][Z][L], ank[k][Y][L], ank[k][X][R], ank[k][Z][R], ank[k][Y][R],
foo[k][X][L], foo[k][Z][L], foo[k][Y][L], foo[k][X][R], foo[k][Z][R], foo[k][Y][R],
toe[k][X][L], toe[k][Z][L], toe[k][Y][L], toe[k][X][R], toe[k][Z][R], toe[k][Y][R],
hel[k][X][L], hel[k][Z][L], hel[k][Y][L], hel[k][X][R], hel[k][Z][R], hel[k][Y][R],
Dx[k]*1000, Dz[k]*1000, Dm[k]);
/* :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: */
}
printf(" \n");
printf("
Normal Program Termination Achieved !!! ");
/*=========================================================================*/
/*
*/
/*
End Of Main Loop For Box Calculations
*/
/*
*/
/*=========================================================================*/
dummyspit:
FCLOSE(inpos);
FCLOSE(outpos);
FCLOSE(hip_mtn);
FCLOSE(box_ap_out);
FCLOSE(box_lp_out);
FCLOSE(box_aa_out);
FCLOSE(box_la_out);
FCLOSE(comm_out);
39
CROCK.C
21 / 23
return(0);
}
/* *********************************************************************** */
/* This function takes a pointer to a character string containing substrings
seperated by whitespace characters to be converted to doubles. The double
array is pointed to by dblp and has dblp[0] number of elements. strtods()
attempts to fill the array with doubles and returns the array size or -1
for invalid input. */
int strtods(char* strp, double* dblp)
{
int i=0,maxelt;
char* endptr=strp;
while(WSPACES(*endptr)) endptr++; /* move to next non whitespace ch'cter */
if (*endptr==NULL)
return(0); /* return 0 if empty string */
maxelt=(int)dblp[0];
/* number of array elts */
while ( (absd(dblp[i++]=strtod(endptr,&endptr))) != HUGE_VAL)

{
/* incr i ok in absd fn call */
if (*endptr==NULL)
return(i);
if (!WSPACES(*endptr))return(-1);
/* illegal character */
while(WSPACES(*endptr)) endptr++; /* move to next non whitespace ch'cter*/
if (*endptr==NULL)
return(i);
if (i>=maxelt)
return(-1);
/* more elts than expected */
}
return(-1);
/* illegal out of range value */
}
/* *********************************************************************** */
double absd(double d)
/* abs function for double */
{
/* (avoids double calling of nested functions */
if(d>=0) return(d);
/* passed to macros as arguments) */
return(-d);
}
40
CROCK.C
22 / 23
/* *********************************************************************** */
/*-------------------------------------------------------------------------*/
/*
Subroutine to calculate angles using true tangents
*/
/*-------------------------------------------------------------------------*/
double trutan(s, t, x, y)
double x, y, s, t;
{
double angle, rise, run;
rise = t - y;
run = s - x;
/* test = fabs(run);
if( test < 0.000001 ) run = 0.0000001;
*/
angle = atan2(rise, run);
/* if ( quote > 0 && rise < 0 ) angle = angle + 22/7;
if ( quote < 0 && rise < 0 ) angle = angle + 22/7;
*/
return(angle);
}
/*-------------------------------------------------------------------------*/
/* Routine to print file headers
*/
/*-------------------------------------------------------------------------*/
void head_print(fname)
FILE* fname;
{
fprintf(fname," \n");
fprintf(fname,"
fprintf(fname,"
fprintf(fname,"
fprintf(fname,"
fprintf(fname,"
fprintf(fname,"
Left
Left
Lower Arm");
Upper Arm");
Trunk");
Head \n");
Right");
Right");
41
CROCK.C
fprintf(fname,"
fprintf(fname,"
fprintf(fname,"
fprintf(fname,"
}
23 / 23
X
X
X
Left
Z
Z
Z
X
X
X
Right\n");
Z");
Z");
Z\n");
42
WALK1009.C
/*
walk
1 / 11
*/
#include <stdio.h>
#include <math.h>
#include <dos.h>
#include "adslib.h"
#include <stdlib.h>
#define WSPACES(A) ( (A)=='\r' || (A)=='\n' || (A)=='\t' || (A)==' ' )
#define ExtFuncCount
1
/* Must increase this count if new
external functions are added */
int funcload _((void));

int funcunload _((void));
int dofun _((void));
int strods(char*, double*);
char *exfun[] = {/*MSG0*/"walk"};
/* No "C:" -- to be called as an AutoLISP
function, not as an AutoCAD command */
/* More functions could be added to this table. For example:
char *exfun[] = {"C:sqr", "C:shaft", "PIN"}; */
/*-----------------------------------------------------------------------*/
/* MAIN - the main routine */
void main(argc,argv)
int argc; char *argv[];
{
short scode = RSRSLT;
int stat;
/* Normal result code (default) */
ads_init(argc, argv);
/* Initiate communication with AutoLISP */
for ( ;; ) {
/* Request/Result loop */
if ((stat = ads_link(scode)) < 0) {

printf(/*MSG1*/"WALK: bad status from ads_link() = %d\n", stat);
43
WALK1009.C
2 / 11
fflush(stdout);
exit(1);
}
scode = RSRSLT;
/* Reset result code */
switch (stat) {
case RQXLOAD:
/* Load & define functions */
scode = funcload() ? -RSRSLT : -RSERR;
break;
case RQXUNLD:
/* Unload functions */
scode = funcunload() ? -RSRSLT : -RSERR;
ads_printf(/*MSG2*/"Unloading.\n");
break;
case RQSUBR:
/* Handle request for external function */
scode = dofun() ? RSRSLT : RSERR;
break;
default:
break;
}
}
}
/*-----------------------------------------------------------------------*/
/* FUNCLOAD -- Define this application's external functions */
int funcload()
{
int i;
for (i = 0; i < ExtFuncCount; i++) {
if (!ads_defun(exfun[i], i))
return RTERROR;
}
return RTNORM;
}
/*-----------------------------------------------------------------------*/
/* FUNCUNLOAD -- Unload external functions */
44
WALK1009.C
3 / 11
int funcunload()
{
int i;
/* Undefine each function we defined */
for (i = 0; i < ExtFuncCount; i++) {
ads_undef(exfun[i],i);
}
return RTNORM;
}
/*-----------------------------------------------------------------------*/
/* DOFUN -- Execute external function (called upon an RQSUBR request) */
int dofun()
{
struct resbuf *rb;
int val;
ads_real x;
extern ads_real rwalk _((ads_real));
if ((val = ads_getfuncode()) < 0)
return 0;
if ((rb = ads_getargs()) == NULL)
return 0;
switch (val) {
/* The 'walk' function was defined

to have a funcode of 0 */
case 0:
if (rb->restype == RTSHORT) {
/* Save in local variable */
x = (ads_real) rb->resval.rint;
} else if (rb->restype == RTREAL) {
x = rb->resval.rreal;
/* Can accept either real
or integer */
} else {
ads_fail(/*MSG3*/"Argument should be a real or integer value.");
return 0;
}
if (x < 0) {
/* Check argument range */
45
WALK1009.C
4 / 11
ads_fail(/*MSG4*/"Argument should be positive.");

return 0;
}
ads_retreal(rwalk(x));
/* Call the function itself, and
return the value to AutoLISP */
return 1;
default:
ads_fail(/*MSG5*/"Received nonexistent function code.");
return 0;
}
}
/*-----------------------------------------------------------------------*/
/* This is the implementation of the actual external function */
ads_real rwalk(x)
ads_real x;
/* Variable argument for the function, will one day be a string */
{
ads_real y;
/* dummy arguement */
int count, space;
ads_point hip_l, hip_r,
kne_l, kne_r,
foo_r, foo_l,
ank_r, ank_l,
toe_r, toe_l,
hel_r, hel_l,
knk_m, hed_m,
sho_r, sho_l,
elb_r, elb_l,
wri_r, wri_l,
hip_m, chi_m;
/*
/*
/*
/*
/*
/*
/*
/*
/*
/*
/* Hip Coords
Knee Coords
Foot Coords
Ankle Coords
Toe Coords
Heel Coords
Neck and Head Coords
Shoulder Coords
Elbow Coords
Wrist Coords
Chin and Pelvis Coords
/* declare the points of ankle, knee, foot, hip
*/
*/
*/
*/
*/
*/
*/
*/
*/
*/
*/
*/
46
WALK1009.C
5 / 11
/* declaration of points, see ADS manual page 30 */

char linedat[500];
struct resbuf genrb;
/* No Idea What This Does */
ads_real jline[66];
/* array for reading the values of the line read statement */
char line[] =
"line",
/* Declaration of command
nullstring[] =
"",
/* strings to be used in
layer[] =
"layer",
/* the ads_command() function
set[] =
"set",
/* for generating lines etc.
RED[] =
"red",
YELLOW[] =
"yellow",
WHITE[] =
"white",
GREEN[] =
"green",
BLUE[] =
"blue",
CYAN[] =
"cyan",
MAGENTA[] =
"magenta",
circle[] =
"circle",
erase[] =
"erase",
all[] =
"all",
redraw[] =
"redraw";
*/
*/
*/
*/
char *jointfile, *textout;

/* Declare our input file and */
FILE *position, *verify;
/* give it a pointer to a name */
/* which is a dos file name.
*/
jointfile = "c:\\alien\\cicords.prn";
/* The DOS filename
textout = "c:\\alien\\joints.out";
*/
genrb.restype = RTSHORT;
genrb.resval.rint = 0;
verify = fopen(textout, "w");
position = fopen(jointfile, "r");
if (position == NULL)
{
return(999);
}
/* Open the file.
*/
/* Return a "1" in the event

*/
/* the file could not be found.*/
47
WALK1009.C
6 / 11
if (x == 0.0) return (0.0);

/* Avoid the old floating point overflow */
ads_setvar("cmdecho", &genrb);
/* scan the co-ordinates then transfer them to the */
/* array coordinates via all these ridiculous equations */
count = 0.0;
while (fgets( linedat, 510, position) != NULL)
{
jline[0] = 66.0;
strtods( linedat, jline);
hip_r[X]
hip_r[Y]
hip_r[Z]
hip_l[X]
hip_l[Y]
hip_l[Z]
=
=
=
=
=
=
jline[0];
jline[1];
jline[2];
jline[3];
jline[4];
jline[5];
kne_r[X]
kne_r[Y]
kne_r[Z]
kne_l[X]
kne_l[Y]
kne_l[Z]
=
=
=
=
=
=
jline[6];
jline[7];
jline[8];
jline[9];
jline[10];
jline[11];
ank_r[X]
ank_r[Y]
ank_r[Z]
ank_l[X]
ank_l[Y]
ank_l[Z]
=
=
=
=
=
=
jline[12];
jline[13];
jline[14];
jline[15];
jline[16];
jline[17];
48
WALK1009.C
7 / 11
foo_r[X]
foo_r[Y]
foo_r[Z]
foo_l[X]
foo_l[Y]
foo_l[Z]
=
=
=
=
=
=
jline[18];
jline[19];
jline[20];
jline[21];
jline[22];
jline[23];
toe_r[X]
toe_r[Y]
toe_r[Z]
toe_l[X]
toe_l[Y]
toe_l[Z]
=
=
=
=
=
=
jline[24];
jline[25];
jline[26];
jline[27];
jline[28];
jline[29];
hel_r[X]
hel_r[Y]
hel_r[Z]
hel_l[X]
hel_l[Y]
hel_l[Z]
=
=
=
=
=
=
jline[30];
jline[31];
jline[32];
jline[33];
jline[34];
jline[35];
knk_m[X] = jline[36];
knk_m[Y] = jline[37];
knk_m[Z] = jline[38];
hip_m[X] = jline[39];
hip_m[Y] = jline[40];
hip_m[Z] = jline[41];
sho_r[X]
sho_r[Y]
sho_r[Z]
sho_l[X]
sho_l[Y]
sho_l[Z]
=
=
=
=
=
=
jline[42];
jline[43];
jline[44];
jline[45];
jline[46];
jline[47];
elb_r[X]
elb_r[Y]
elb_r[Z]
elb_l[X]
elb_l[Y]
=
=
=
=
=
jline[48];
jline[49];
jline[50];
jline[51];
jline[52];
49
WALK1009.C
8 / 11
elb_l[Z] = jline[53];
wri_r[X]
wri_r[Y]
wri_r[Z]
wri_l[X]
wri_l[Y]
wri_l[Z]
=
=
=
=
=
=
jline[54];
jline[55];
jline[56];
jline[57];
jline[58];
jline[59];
chi_m[X] = jline[60];
chi_m[Y] = jline[61];
chi_m[Z] = jline[62];
hed_m[X] = jline[63];
hed_m[Y] = jline[64];
hed_m[Z] = jline[65];
/* draw the lines */
if( x < 10.00)
{
delay( x * 100);
ads_command(RTSTR, erase, RTSTR, all, RTSTR, nullstring, NULL);
ads_command(RTSTR, redraw, NULL);
}
/* Change layer to GREEN and draw toes */
ads_command(RTSTR, layer, RTSTR, set, RTSTR, GREEN, RTSTR, nullstring, NULL);
ads_command(RTSTR, line, RT3DPOINT, toe_r, RT3DPOINT, foo_r, RTSTR, nullstring, NULL);
line, RT3DPOINT, toe_l, RT3DPOINT, foo_l, RTSTR, nullstring, NULL);
ads_command(RTSTR, line, RT3DPOINT, toe_l, RT3DPOINT, foo_l, RTSTR, nullstring, NULL);
line, RT3DPOINT, toe_l, RT3DPOINT, foo_l, RTSTR, nullstring, NULL);
ads_command(RTSTR,
ads_command(RTSTR,
/*change layer to yellow an draw right foot, lower leg and upper leg */
ads_command(RTSTR, layer, RTSTR, set, RTSTR, YELLOW, RTSTR, nullstring, NULL);
ads_command(RTSTR, line, RT3DPOINT, ank_r, RT3DPOINT, kne_r, RTSTR, nullstring, NULL);
ads_command(RTSTR, line, RT3DPOINT, kne_r, RT3DPOINT, hip_r, RTSTR, nullstring, NULL);
/* Change layer to white and draw left foot, lower leg and upper leg */
50
WALK1009.C
9 / 11
ads_command(RTSTR, layer, RTSTR, set, RTSTR, WHITE, RTSTR, nullstring, NULL);

ads_command(RTSTR, line, RT3DPOINT, ank_l, RT3DPOINT, kne_l, RTSTR, nullstring, NULL);
ads_command(RTSTR, line, RT3DPOINT, kne_l, RT3DPOINT, hip_l, RTSTR, nullstring, NULL);
/* change layer to
ads_command(RTSTR,
ads_command(RTSTR,
ads_command(RTSTR,
ads_command(RTSTR,
ads_command(RTSTR,
ads_command(RTSTR,
red and blue and draw feet */

layer, RTSTR, set, RTSTR, RED, RTSTR, nullstring, NULL);
line, RT3DPOINT, foo_l, RT3DPOINT, ank_l, RTSTR, nullstring,
line, RT3DPOINT, hel_l, RT3DPOINT, ank_l, RTSTR, nullstring,
layer, RTSTR, set, RTSTR, BLUE, RTSTR, nullstring, NULL);
line, RT3DPOINT, foo_r, RT3DPOINT, ank_r, RTSTR, nullstring,
line, RT3DPOINT, hel_r, RT3DPOINT, ank_r, RTSTR, nullstring,
NULL);
NULL);
NULL);
NULL);
/* Change layer to green and draw pelvis */

ads_command(RTSTR, line, RT3DPOINT, hip_l, RT3DPOINT, hip_r, RTSTR, nullstring, NULL);
/* Change Layer to Magenta and draw trunk */
ads_command(RTSTR, layer, RTSTR, set, RTSTR, MAGENTA, RTSTR, nullstring, NULL);
ads_command(RTSTR, line, RT3DPOINT, hip_m, RT3DPOINT, knk_m, RTSTR, nullstring, NULL);
/* Change Layer to Cyan and draw Shoulders */
ads_command(RTSTR, layer, RTSTR, set, RTSTR, CYAN, RTSTR, nullstring, NULL);
ads_command(RTSTR, line, RT3DPOINT, sho_r, RT3DPOINT, sho_l, RTSTR, nullstring, NULL);
/* Change Layer to
ads_command(RTSTR,
ads_command(RTSTR,
ads_command(RTSTR,
ads_command(RTSTR,
Red and Blue and draw upper arms */

line, RT3DPOINT, sho_l, RT3DPOINT, elb_l, RTSTR, nullstring, NULL);
line, RT3DPOINT, sho_r, RT3DPOINT, elb_r, RTSTR, nullstring, NULL);
/* Change Layer to
ads_command(RTSTR,
ads_command(RTSTR,
ads_command(RTSTR,
ads_command(RTSTR,
Red and Blue and draw lower arms */

line, RT3DPOINT, elb_l, RT3DPOINT, wri_l, RTSTR, nullstring, NULL);
line, RT3DPOINT, elb_r, RT3DPOINT, wri_r, RTSTR, nullstring, NULL);
/* Change Layer to green and draw Neck */

ads_command(RTSTR, line, RT3DPOINT, knk_m, RT3DPOINT, chi_m, RTSTR, nullstring, NULL);
/* Change Layer to Magenta and draw Head */
51
WALK1009.C
10 / 11
ads_command(RTSTR, layer, RTSTR, set, RTSTR, CYAN, RTSTR, nullstring, NULL);

ads_command(RTSTR, circle, RT3DPOINT, hed_m, RTREAL, 101.6, NULL);
fprintf ( verify, "hip_r[X] %f hip_r[Y] %f hip_r[Z] %f hip_l[X] %f hip_l[Y] %f hip_l[Z] %f \n",
jline[60], jline[61], jline[62], jline[63], jline[64], jline[65]);
}
fclose(position);
fclose(verify);
y = x;
return (y);
}
/* *********************************************************************** */
/* This function takes a pointer to a character string containing substrings
seperated by whitespace characters to be converted to doubles. The double
array is pointed to by dblp and has dblp[0] number of elements. strtods()
attempts to fill the array with doubles and returns the array size or -1
for invalid input. */
int strtods(char* strp, double* dblp)
{
int i=0,maxelt;
char* endptr=strp;
while(WSPACES(*endptr)) endptr++; /* move to next non whitespace ch'cter */
if (*endptr==NULL)
return(0); /* return 0 if empty string */
maxelt=(int)dblp[0];
/* number of array elts */
while ((absd(dblp[i++]=strtod(endptr,&endptr)))!=HUGE_VAL)
{
/* incr i ok in absd fn call */
if (*endptr==NULL)
return(i);
52
WALK1009.C
11 / 11
if (!WSPACES(*endptr))return(-1);
/* illegal character */
while(WSPACES(*endptr)) endptr++; /* move to next non whitespace ch'cter*/
if (*endptr==NULL)
return(i);
if (i>=maxelt)
return(-1);
/* more elts than expected */
}
return(-1);
/* illegal out of range value */
}
/* *********************************************************************** */
double absd(double d)
/* abs function for double */
{
/* (avoids double calling of nested functions */
if(d>=0) return(d);
/* passed to macros as arguments) */
return(-d);
}
53
THONG_1.C
1 / 48
/*??????????????????????????????????????????????????????????????????????????*/
/*
*/
/*
jello.c
*/
/*
*/
/*
*/
/*??????????????????????????????????????????????????????????????????????????*/
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
int
char
int
int
char
float
<dos.h>
<stdio.h>
<conio.h>
<stdlib.h>
<string.h>
<time.h>
<robio.h>
<math.h>
<ctype.h>
<mem.h>
<setjmp.h>
<bios.h>
/* robot header file */
all_ok;
buf[50];
key_in;
log_count = 0;
logname[20];
comp,
comp_old = 250.0;
/*flag for checking all channel read */

/*buff for reading file data
*/
/* KB buffer
*/
/* number of iterations for log print */
/* filename for log */
/* compass value */
/*dummy compass output variable */
int analog_med[4];
int pit[13];
int rol[13];
int analog[4][3];
int pit_m, rol_m;
float pitch_trim, roll_trim;
int pitch_old = 250, roll_old = 250;
int total;
int status;
/* dummy pitch and roll variables

/* counter to delay print */
/* response from receive routine
long t_zero, t_now, t_run;
/* timers for sway function */
/* median filtered values for pitch and roll */
int anal_old_1, anal_old_2, anal_old_3, anal_old_4;
*/
*/
/* temp analog data */
54
THONG_1.C
2 / 48
float right_trim,
front_trim,
ft_rate,
left_trim,
mass_left,
mass_right;
/* load distribution on right foot */

/* load distribution accross legs */
/* rate of change of front trim */
/*load distribution left leg */
/* load on left leg */
/* load on right leg */
float pitchd, rolld;
/* pitch and roll in degrees */
int
/* enable balance in the frontal plane */

/* enable longitudinal balance routine */
SAG_ENABLE = 0,
LON_ENABLE = 0;
int roll_max, roll_min, /*minimum and maximum roll values */

trim_max, trim_min, /*minimum and maximum trim values */
trim_d;
/* integer trim percent
*/
time_t a_time,
b_time,
c_time;
/* current time */
int
/* elapsed time in biosticks */
bios_ticks;
double lower_leg = 1057,

upper_leg = 1045;
/*lower leg length */

/* upper leg length */
double deg_rad = 0.0175,

rad_deg = 57.273;
/*degrees to radians factor */

/* radians to degree factor */
double phase = 0.0;
/* gait phase */
double right_hip_ext,
left_hip_ext,
right_knee_ext,
left_knee_ext,
right_foot_ext,
left_foot_ext;
double beta_right,
beta_left,
epsilon_left,
epsilon_right,
/* Right hip extension angle in (deg). */

/* Left hip extension angle in (deg). */
/* right knee extesnion angle in deg */
/* left knee extension angle in deg */
/* right foot extension angle in deg */
/* left foot extension angle in deg */
/* calculated knee angle */
/* intermediate anngles */
55
THONG_1.C
3 / 48
psi_left,
psi_right;
double right_hip_height,
left_hip_height,
abs_dist_right,
abs_dist_left,
right_hip_displ,
left_hip_displ;
/* right hip height in mm */

/* left hip height in mm
*/
/*absolute distane right */
/*absolute distance left */
/*x displacement of hip */
int
/* bios_tick rate */
bios_rate;
int logg = 0;
int period = 180,
ab_adj = 0;
/* Gait period */
/* Hip abduction offset */
void allstop(void);
/* reset CMD for all channels (cmd = 6) */
void cycle(void);
/* set all CMD to flush all valves
*/
void stagger (void);
/* balancing routine */
void setcmd(struct HC11_DATA *hp, char *data);
int dump(void);
/* data output routine */
void Screend(void);
void Screens(void);
int median(int n,int* array)
{
int copy[13];
int i;
int med, cum;
cum = 0;
for(i=0;i<n;i++)
{
copy[i]=array[i];
cum = cum + copy[i];
}
med = cum / (i);
56
THONG_1.C
4 / 48
return med;
}
void next(int n,int* array,int new_val)
{
int i;
for(i=0;i<n-1;i++) {
array[i]=array[i+1];
}
array[n-1]=new_val;
return;
}
/*
Convert
byte 0:
byte 1:
byte 2:
byte 3:
byte 4:
byte 5:
byte 6:
P --C --S --V --CRC ---
all data
'X'
1 C3 C2
P2
P3
1 S4 S3
V2
CRC
Position
Command
Status
Velocity
Checksum
into the Communication package format

C1 Ps P13 P12 P11
S2 S1 Vs V13 V12 V11
( -500 ( 0 - 7)
( 0 - 15
( -300 for byte
500 )
)
300 )
1 to 5
*/
void cov_data1(char *cp,struct HC11_DATA input_data)
{
int temp1,temp2,sign;
*cp = 'X';
/* byte 0
= 'X'*/
if (input_data.position < 0 )
{
sign = 1;
input_data.position *= -1;
}
else
{
//chech the sign of position
57
THONG_1.C
5 / 48
sign = 0;
};
/* byte 1 */
temp1 = (int) (input_data.position /100);
//extract P1
*(cp+1) = ((input_data.cmd & 0x7) << 4) | temp1 | 0x80;
//combine with Command
if (sign != 0)
*(cp+1) = *(cp+1) | 0x08;
else
*(cp+1) = *(cp+1) & 0xf7;
// combine with Position sing
/* byte 2 and byte 3 */

temp1 = temp1 * 100;
temp2 = (int)(input_data.position - temp1) /10;
*(cp+2) = temp2 + 0x30;
*(cp+3) = (int)(input_data.position - temp1- temp2*10)+ 0x30;
/* byte 4 and 5 */
if (input_data.velocity < 0 )
// check sign for velocity
{
sign = 1;
input_data.velocity *= -1;
}
else
{
sign = 0;
};
temp1 = input_data.velocity / 100;
// extract V1
temp2 = input_data.velocity - temp1 * 100;
//extract V2
*(cp+4) = ((input_data.status & 0xf) << 3) | (temp1 & 0x3) | 0x80; // V1 combine with Status
if ( sign == 1 )
{ *(cp+4) = (*(cp+4) | 0x4) & 0xff;}
else
{ *(cp+4) = *(cp+4) & 0xfb; };
*(cp+5) = (temp2 | 0x80) & 0xff;
*(cp+6) = crc(cp+1,5);
if (*(cp+6) == 0xd || *(cp+6) == 0xa)
return ;
// byte 4 = Status | Vs | V1
// calculate Checksum
(*(cp+6))++;
58
THONG_1.C
6 / 48
// Convert all output data to the right package format

void To_HC11(void)
{
// call cov_data1 for channel 7 to 20
cov_data1(RTE_OUT, RTE_O);
cov_data1(LTE_OUT, LTE_O);
cov_data1(RFO_OUT, RFO_O);
cov_data1(LFO_OUT, LFO_O);
cov_data1(RFI_OUT, RFI_O);
cov_data1(LFI_OUT, LFI_O);
cov_data1(RKE_OUT, RKE_O);
cov_data1(LKE_OUT, LKE_O);
cov_data1(RHE_OUT, RHE_O);
cov_data1(LHE_OUT, LHE_O);
cov_data1(RHA_OUT, RHA_O);
cov_data1(LHA_OUT, LHA_O);
cov_data1(RHR_OUT, RHR_O);
cov_data1(LHR_OUT, LHR_O);
/* convert data to analog channel */
*analog_out = 'X';
*(analog_out + 1) = 0x30 | (( analog_out1 & 0xf0 ) >>
*(analog_out + 2) = 0x30 | ( analog_out1 & 0xf );
*(analog_out + 9) = crc(analog_out+1,8);
if ( *(analog_out + 9) == 0xd || *(analog_out + 9) ==
(*(analog_out +9)) ++;
}
4);
4);
4);
4);
0xa )
/* Extract data from the communcation pack ( refer to description above) */

void cov_data2(char *cp,struct HC11_DATA *output_data)
{
char a;
output_data->status = (*(cp+4) >> 3) & 0xf ;
// reading status
59
THONG_1.C
// reading position with sign
output_data->position = (*(cp+1) & 0x07)* 100 + ( *(cp+2) - 0x30 ) * 10 + ( *(cp+3)
if ( (*(cp+1)&0x8) != 0 ) output_data->position *=-1;
7 / 48
- 0x30 );
// reading position with sign

output_data->velocity = ( *(cp+4) & 3 )* 100 + (*(cp+5)& 0x7f);
if ( (*(cp+4) & 4) != 0 ) output_data->velocity *=-1;
// reading Command. If chechsum error, the command is "e"
output_data->cmd = (*(cp+1) & 0x70) >> 4;
a = crc(cp+1,5);
if (a==0xd || a == 0xa) a++;
if ( *(cp+6) != a )
{
output_data->cmd = 'e';
all_ok=0;
};
return;
};
// unpack data by calling cov_data2 for channel 7 - 20
// unpack data for channel 21 ( analog )
void From_HC11(void)
{
int cc;
all_ok=1;
cov_data2(RTE_IN, &RTE_I);
cov_data2(LTE_IN, &LTE_I);
cov_data2(RFO_IN, &RFO_I);
cov_data2(LFO_IN, &LFO_I);
cov_data2(RFI_IN, &RFI_I);
cov_data2(LFI_IN, &LFI_I);
cov_data2(RKE_IN, &RKE_I);
cov_data2(LKE_IN, &LKE_I);
cov_data2(RHE_IN, &RHE_I);
cov_data2(LHE_IN, &LHE_I);
cov_data2(RHA_IN, &RHA_I);
cov_data2(LHA_IN, &LHA_I);
cov_data2(RHR_IN, &RHR_I);
cov_data2(LHR_IN, &LHR_I);
if (_fstrlen(analog_in) > 9)
{
60
THONG_1.C
8 / 48
anal_old_1
anal_old_2
anal_old_3
anal_old_4
=
=
=
=
analog_in1;
analog_in2;
analog_in3;
analog_in4;
analog_in1
analog_in2
analog_in3
analog_in4
}
else
{
analog_in1
analog_in2
analog_in3
analog_in4
}
=
=
=
=
(*(analog_in+1)
(*(analog_in+3)
(*(analog_in+5)
(*(analog_in+7)
=
=
=
=
anal_old_1;
anal_old_2;
anal_old_3;
anal_old_4;
0x30)
0x30)
0x30)
0x30)
*
*
*
*
16
16
16
16
+
+
+
+
(*(analog_in+2)
(*(analog_in+4)
(*(analog_in+6)
(*(analog_in+8)
0x30);
0x30);
0x30);
0x30);
remove("ip07.dat");
};
// reading one line, return if 0x0d/0x0a found or read 13 chars
char *Line_input(FILE *file1)
{
char *a;
int char_count=0,
done = 0;
memset(buf,0,15);
a= buf;
/*loop to read char until either
char count more than 13
or
0x0d or 0x0a read
*/
while ( !done && (char_count < 13))
{
*a=fgetc(file1) & 0xff;
if (
(*a == 0x0d) || (*a == 0x0a)
{
if (char_count != 0 )
61
THONG_1.C
9 / 48
{
done = -1;
*a = '\0';
}
}
else
{
char_count ++;
a++;
};
};
return buf;
};
// This routine reads a data file from HC11. ( channel 7 to 21 )
// the data is in the communication format
int get_input()
{
FILE *f;
int error = 1;
struct find_t ffblk;
if (_dos_findfirst("ip07.dat", FA_ARCH, &ffblk) != 0 ) return 0;
if ((f=fopen("ip07.dat","r+")) == NULL ) return 0;
_fstrcpy(LHE_IN, Line_input(f));
if (_fstrlen(LHE_IN) == 0) error = -2;
_fstrcpy(RKE_IN, Line_input(f));
if (_fstrlen(RKE_IN) == 0) error = -3;
_fstrcpy(LHA_IN, Line_input(f));
if (_fstrlen(LHA_IN) == 0) error = -4;
_fstrcpy(LKE_IN, Line_input(f));
if (_fstrlen(LKE_IN) == 0) error = -5;
_fstrcpy(LTE_IN, Line_input(f));
if (_fstrlen(LTE_IN) == 0) error = -6;
_fstrcpy(LFI_IN, Line_input(f));
62
THONG_1.C
if (_fstrlen(LFI_IN) == 0)
10 / 48
error = -7;
_fstrcpy(RFI_IN, Line_input(f));
if (_fstrlen(RFI_IN) == 0) error = -8;
_fstrcpy(RHE_IN, Line_input(f));
if (_fstrlen(RHE_IN) == 0) error = -9;
_fstrcpy(RHA_IN, Line_input(f));
if (_fstrlen(RHA_IN) == 0) error = -10;
_fstrcpy(RHR_IN, Line_input(f));
if (_fstrlen(RHR_IN) == 0) error = -11;
_fstrcpy(LHR_IN, Line_input(f));
if (_fstrlen(LHR_IN) == 0) error = -12;
_fstrcpy(RTE_IN, Line_input(f));
if (_fstrlen(RTE_IN) == 0) error = -13;
_fstrcpy(LFO_IN, Line_input(f));
if (_fstrlen(LFO_IN) == 0) error = -14;
_fstrcpy(RFO_IN, Line_input(f));
if (_fstrlen(RFO_IN) == 0) error = -15;
_fstrcpy(analog_in, Line_input(f));
if (_fstrlen(analog_in) == 0) error = -16;
fclose(f);
return error;
};
// output data, which is in communcation format, to a file
int send_output()
{
FILE *f;
if ( (f=fopen("outtemp","w+")) == NULL )
{
printf( "Error writing to temporary output file ");
63
THONG_1.C
11 / 48
return -1;
}
fprintf(f,"p07%7s\n",LHE_OUT);
fprintf(f,"p08%7s\n",RKE_OUT);
fprintf(f,"p09%7s\n",LHA_OUT);
fprintf(f,"p10%7s\n",LKE_OUT);
fprintf(f,"p11%7s\n",LTE_OUT);
fprintf(f,"p12%7s\n",LFI_OUT);
fprintf(f,"p13%7s\n",RFI_OUT);
fprintf(f,"p14%7s\n",RHE_OUT);
fprintf(f,"p15%7s\n",RHA_OUT);
fprintf(f,"p16%7s\n",RHR_OUT);
fprintf(f,"p17%7s\n",LHR_OUT);
fprintf(f,"p18%7s\n",RTE_OUT);
fprintf(f,"p19%7s\n",LFO_OUT);
fprintf(f,"p20%7s\n",RFO_OUT);
fprintf(f,"p21%7s\n",analog_out);
fclose(f);
rename ("outtemp","op.dat") ;
return 1;
}
// CheckSum routine
int crc(char *cp,int NUM)
{
int count=1,SUM;
SUM = *cp;
while (count < NUM)
{
SUM ^= *(cp+count);
count++;
};
return SUM;
};
// reads the compass data file and returns the compass value
float compass()
{
FILE *fp;
64
THONG_1.C
12 / 48

char *cm;
float comp_t;
// check if the compass file exist.
if (_dos_findfirst("ip06.dat", FA_ARCH, &ffblk) != 0 ) return comp_old;
if (_dos_findfirst("ip06.old", FA_ARCH, &ffblk) == 0 ) remove("ip06.old");
rename("ip06.dat","ip06.old");
memset (compass_data ,0,30);
fp = fopen("ip06.old","r+");
if (fp == NULL) return 999;
cm = fgets( compass_data,30,fp);
fclose ( fp);
// if file exist, rename and read it
if (_fstrlen(compass_data) < 15) return comp_old;
//ignore if the data is not right
compass_data[12] = 0;
comp_t = atof(compass_data + 7);
comp_old = comp_t;
return comp_t;
}
// return the compass value
//read Horizontal data

int horizon()
{
FILE *fp;
char *cm;
//read the data if file exists
if (_dos_findfirst("ip08.dat", FA_ARCH, &ffblk) != 0 )
{
pp = pitch_old;
rr = roll_old;
return 0;
}
65
THONG_1.C
13 / 48
if (_dos_findfirst("ip08.old", FA_ARCH, &ffblk) == 0 ) remove("ip08.old");

rename("ip08.dat","ip08.old");
memset (compass_data ,0,30);
// clear a data buf to read char
fp = fopen("ip08.old","r+");
// open data file
if (fp == NULL) return 999;
// if error return
cm = fgets( compass_data,30,fp);
// read char to buf
fclose ( fp);
if (_fstrlen(compass_data) < 12) return 0;
// close file after finish
cm= strstr(compass_data,"P");
// search for PP ( pitch dat )
if (cm == NULL) return 0;
cm++;
if (*cm == 'P') cm++;
// ignore second P exist;
if ( *cm >= 0x41)
// read pitch value to pp
pp= (*cm- 0x37)*16;
else
pp= (*cm-0x30) * 16;
cm++;
if ( *cm >= 0x41)
pp += (*cm- 0x37);
else
pp += (*cm-0x30) ;
/* check rr(roll)*/
cm++;
cm++;
cm++;
if ( *cm >= 0x41)
// read the ROLL value
rr= (*cm- 0x37)*16;
else
rr= (*cm-0x30) * 16;
cm++;
if ( *cm >= 0x41)
rr += (*cm- 0x37);
else
rr += (*cm-0x30) ;
/* for check aa(pressure) */
cm++;
cm++;
cm++;
if ( *cm >= 0x41)
// read the Presure value
aa= (*cm- 0x37)*16;
else
aa= (*cm-0x30) * 16;
66
THONG_1.C
14 / 48
cm++;
if ( *cm >= 0x41)
aa += (*cm- 0x37);
else
aa += (*cm-0x30) ;
cm++;
/*if (pp> 256)
{
printf("\n%s %d \n ",compass_data,pp);
exit(0);
}*/
roll_old = rr;
pitch_old = pp;
return -1 ;
}
void allstop(void)
{
// send a stop command to all HC11s
RTE_O.cmd = 6;
LTE_O.cmd = 6;
RFO_O.cmd = 6;
LFO_O.cmd = 6;
RFI_O.cmd = 6;
LFI_O.cmd = 6;
RKE_O.cmd = 6;
LKE_O.cmd = 6;
RHE_O.cmd = 6;
LHE_O.cmd = 6;
RHA_O.cmd = 6;
LHA_O.cmd = 6;
RHR_O.cmd = 6;
LHR_O.cmd = 6;
}
// flush all valves until ESC hit
void cycle()
{ int a,status,group,kb_temp;
group= 1;
clrscr();
printf("Commencing valve cycling !!!!\n");
67
THONG_1.C
15 / 48
printf("\n");
printf("Press any key to continue or ESC to cancel.....\n");
while (kbhit() == 0) ;
// check KB key hit
kb_temp = getch();
if (kb_temp == 27 ) return;
// if ESC hit exit
kb_temp = 0;
while ( kb_temp != 27 )
// if ESC hit exit otherwise loop to flush valve
{
if ( kbhit() != 0 ) kb_temp = getch();
allstop();
switch (group)
// flush vale by group
{ case 1:
LTE_O.cmd=0; LFI_O.cmd=0; LFO_O.cmd=0; break;
case 2:
RTE_O.cmd=0; RFI_O.cmd=0; RFO_O.cmd=0; break;
case 3:
RHR_O.cmd=0; LHR_O.cmd=0; RHA_O.cmd=0; LHA_O.cmd=0; break;
case 4:
RHE_O.cmd=0; LHE_O.cmd=0; RKE_O.cmd=0; LKE_O.cmd=0; break;
};
group++;
if (group > 4) group = 1;
To_HC11();
//send command to HC11
a = send_output();
while ((a <350) && ((status =get_input()) == 0))
{ delay(5);
a++;
}
From_HC11();
delay(1500);
clrscr();
printf("Valve cycling lte.cmd is %d %d !!!!\n",LTE_I.cmd,RTE_I.cmd);
printf("\n");
printf("Press ESC to cancel.....\n");
printf("It may take two seconds to stop.");
};
allstop();
// stop all valve before back to main loop
To_HC11();
a = send_output();
{ delay(5);
a++;
68
THONG_1.C
16 / 48
}
From_HC11();
}
/* Perform the balancing act */
void stagger()
{
int
kb_temp, a, status, i, j;
float RIGHT_POINT = 0.5,
LEFT_POINT = 0.5,
LAT_POINT = 0.5,
FOOT_ZONE = 0.2,
LAT_ZONE = 0.05,
low_lim_left,
low_lim_right,
up_lim_left,
up_lim_right,
low_lat_lim_1,
up_lat_lim_1,
low_lat_lim_2,
up_lat_lim_2,
ft_new = 0.5,
ft_old;
/* desired right foot balance point */

/* desired left foot balance point */
/* desired distribution between feet */
/* longitudinal foot balance zone */
/* sagital balance zone */
/* left foot lower limit for long balance */
/* left foot upper limit for long balance */
/* right foot lower limit for long balance */
/* right foot upper limit for long balance */
/* limits of lateral adjustment zone 1 */
/* New Historiocal value for trim
/* Old Historiocal value for trim
*/
*/
float lat_bal_inc,
/* lateral balance compenstion increment */
lat_bal_float,
/* lateral balance floating number compensation */
height_adj_right = 0.0, /*left hip height adjustment value */
/*
height_inc,
/*height adjustment increment */
height_adj_left = 0.0; /*left hip height adjustment value */
int
r_pre, r_pst,
/* variables for screen display */
l_pre, l_pst,
f_pre, f_pst,
pi_pre, pi_pst,
ro_pre, ro_pst;
int
LAT_ENABLE = 0, /* enable lateral balance routine */

GYR_ENABLE = 0, /* enable gyro balance routine */
SWAY_ENABLE = 0, /* enable swaying routine */
69
THONG_1.C
17 / 48
LEFT_CAL_POS,
/* store left foot extension calibraytion position */
RIGHT_CAL_POS,
/* store right foot extension calibration position */
LON_COUNT_L,
/* delay count for left foot balance adjustment */
LON_COUNT_R,
/* delay count for right foot balance adjustment */
RHA_CAL,
/* Right hip abduction calibrated positrion */
LHA_CAL,
/* Left hip abduction calibrated position */
/*
L_HIP_CAL,
/* Right hip abduction calibrated positrion */
/*
R_HIP_CAL,
/* Left hip abduction calibrated position */
hip_slew = 0;
/* Hip slew rate */
int
foot_lift = 0,
/* generic foot lift height */
right_foot_lift = 0, /* right foot lift height
*/
left_foot_lift = 0; /* left foot lift height
*/
long old_bios_ticks,
new_bios_ticks;
int
lat_bal_comp;
int screen_count = 0,
body = 0;
int
R_LIM_IN,
R_LIM_OUT,
L_LIM_IN,
L_LIM_OUT;
/* previous biostick value */

/* current bios time */
/* lateral balance position compensation */
/* body geometry adjustment flag */
/* contact limit switch settings */
float temp_float;
int SURGE_SET = 0;
/* Reset flag for sway */
float f_t_run,
/* intrmediate variables for sway calcs */
f_period,
frac,
/* floating variables */
d_offset,
/* float offset value */
f_lift,
/* float foot lift value */
sway_amp = 10.0,
/* Sagital sway amplitute */
lift_amp = 75.0;
/* Foot lift amplitute */
// send stance command to command to all HC11s
RTE_O.cmd = 3;
/* set HC11 output commands to stance routine */
70
THONG_1.C
LTE_O.cmd
RFO_O.cmd
LFO_O.cmd
RFI_O.cmd
LFI_O.cmd
RKE_O.cmd
LKE_O.cmd
RHE_O.cmd
LHE_O.cmd
RHA_O.cmd
LHA_O.cmd
RHR_O.cmd
LHR_O.cmd
18 / 48
=
=
=
=
=
=
=
=
=
=
=
=
=
3;
3;
3;
3;
3;
3;
3;
3;
3;
3;
3;
3;
3;
RHE_O.position = 270;
LHE_O.position = 285;
/* initial stance joint positions */
RKE_O.position = 180;
LKE_O.position = 160;
RFO_O.position = 30 ;
LFO_O.position = 30;
RTE_O.position =
LTE_O.position =
15;
15;
RFI_O.position =
LFI_O.position =
-17;
-29;
RHA_O.position = 97;
LHA_O.position = 82 ;
RHR_O.position = -15;
LHR_O.position = 0;
kb_temp = 0;
LON_COUNT_L = 0;
LON_COUNT_R = 2;
/* flags to switch longitudinal control */
RHA_CAL = RHA_O.position;
LHA_CAL = LHA_O.position;
/* set calibrated (post bal) hip ab posns */
71
THONG_1.C
19 / 48
/*R_HIP_CAL = RHA_O.position; */
/* L_HIP_CAL = LHA_O.position; */
RIGHT_CAL_POS = RFO_O.position;
LEFT_CAL_POS = LFO_O.position;
lat_bal_inc = 0.1;
lat_bal_float = 0.0;
/* loop inc for sagital balance */

/* initial compensation for sagital balance */
logg = 0;
/* calculate balance limits */
/* __________________________________________________________________________________*/
/*|
Main Balancing Act Control Loop
|*/
/*|_________________________________________________________________________________|*/
/* .................................................................................*/
/* Calculate the length of time of a bios tick
*/
/* .................................................................................*/
time(&a_time);
time(&b_time);
while( a_time == b_time)
{
time (&b_time);
}
time(&a_time);
new_bios_ticks = biostime(0, 0L);
old_bios_ticks = new_bios_ticks;
while ( (time(&b_time)) < (a_time + 10) )
{
72
THONG_1.C
20 / 48
}
bios_rate = ( new_bios_ticks - old_bios_ticks) / 10;

while ( kb_temp != 27 )
{
/* if ESC hit exit */
old_bios_ticks = new_bios_ticks;
bios_ticks = new_bios_ticks - old_bios_ticks;
if (bios_ticks == 0) bios_ticks = 123;
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
/*
Check for keyboard Input
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
*/
kb_temp = 0;
if ( kbhit() != 0 )
{
kb_temp = getch();
if (kb_temp == 39)
{
LON_ENABLE = 0;
LAT_ENABLE = 0;
GYR_ENABLE = 0;
SAG_ENABLE = 0;
SWAY_ENABLE = 0;
}
if
if
if
if
(kb_temp
(kb_temp
(kb_temp
(kb_temp
==
==
==
==
60)
62)
63)
58)
/* ' key */
LAT_ENABLE
LON_ENABLE
SAG_ENABLE
GYR_ENABLE
if (kb_temp == 124)
{
SWAY_ENABLE = 4;
SAG_ENABLE = 0;
}
=
=
=
=
4;
4;
4;
4;
/*
/*
/*
/*
<
>
?
:
key
key
key
Key
*/
*/
*/
*/
/* | Key */
73
THONG_1.C
21 / 48
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*
Adjust body geometry
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*
*/
Adjust lateral hip position */

if (kb_temp == 57)
{
RHA_CAL = RHA_CAL - 1;
LHA_CAL = LHA_CAL + 1;
}
if (kb_temp == 55)
{
RHA_CAL = RHA_CAL + 1;
LHA_CAL = LHA_CAL - 1;
}
/* 9 Key */
/* 7 Key */
/* Move Individual joints */

if (kb_temp == 45 )
{
/* - key Toggle Body Adjustment */
if( body > 0 )

{
body = 0;
}
else
{
body = 1;
}
}
if( body > 0 )
{
if (kb_temp == 99)
if (kb_temp == 67)
RFO_O.position = RFO_O.position + 1; /* c Key */

RFO_O.position = RFO_O.position - 1; /* C Key */
if (kb_temp == 100) LFO_O.position = LFO_O.position + 1; /* d Key */
74
THONG_1.C
22 / 48
if (kb_temp == 68)
LFO_O.position = LFO_O.position - 1; /* D Key */
if (kb_temp == 120) RFI_O.position = RFI_O.position + 1; /* x Key */

if (kb_temp == 88) RFI_O.position = RFI_O.position - 1; /* X Key */
if (kb_temp == 115) LFI_O.position = LFI_O.position + 1; /* s Key */
if (kb_temp == 83) LFI_O.position = LFI_O.position - 1; /* S Key */
if (kb_temp == 122) RTE_O.position = RTE_O.position + 1; /* z Key */
if (kb_temp == 90) RTE_O.position = RTE_O.position - 1; /* Z Key */
if (kb_temp == 97)
if (kb_temp == 65)
LTE_O.position = LTE_O.position + 1; /* a Key */

LTE_O.position = LTE_O.position - 1; /* A Key */
if (kb_temp == 118) RKE_O.position = RKE_O.position + 1; /* v Key */

if (kb_temp == 86) RKE_O.position = RKE_O.position - 1; /* V Key */
if (kb_temp == 102) LKE_O.position = LKE_O.position + 1; /* f Key */
if (kb_temp == 70) LKE_O.position = LKE_O.position - 1; /* F Key */
if (kb_temp == 98)
if (kb_temp == 66)
RHR_O.position = RHR_O.position + 1; /* b Key */

RHR_O.position = RHR_O.position - 1; /* B Key */
if (kb_temp == 103) LHR_O.position = LHR_O.position + 1; /* g Key */

if (kb_temp == 71) LHR_O.position = LHR_O.position - 1; /* G Key */
if (kb_temp == 110) RHA_O.position = RHA_O.position + 1; /* n Key */
if (kb_temp == 78) RHA_O.position = RHA_O.position - 1; /* N Key */
if (kb_temp == 104) LHA_O.position = LHA_O.position + 1; /* h Key */
if (kb_temp == 72) LHA_O.position = LHA_O.position - 1; /* H Key */
if (kb_temp == 109) RHE_O.position = RHE_O.position + 1; /* m Key */
if (kb_temp == 77) RHE_O.position = RHE_O.position - 1; /* M Key */
if (kb_temp == 106) LHE_O.position = LHE_O.position + 1; /* j Key */
if (kb_temp == 74) LHE_O.position = LHE_O.position - 1; /* J Key */
}
75
THONG_1.C
23 / 48
/* set lat limits */

if (kb_temp == 53) /* 5 key */
{
LAT_POINT = front_trim;
}
/* Toggle debug logging */
if (kb_temp == 61 )
{
/* calculate balance limits */
/* = key */
if( logg > 0 )

{
logg = 0;
}
else
{
logg = 1;
}
}
}
screen_count++;
LON_COUNT_L++;
if ( LON_COUNT_L > 2) LON_COUNT_L = 0;
LON_COUNT_R++;
if ( LON_COUNT_R > 2) LON_COUNT_R = 0;
comp = compass();
if (comp > 370) comp = 444; /* get the compass value, if it's > 370 */
/* send an error value */
horizon();
/* Get the horizon values */
76
THONG_1.C
24 / 48
analog_out1
analog_out2
analog_out3
analog_out4
=
=
=
=
1;
10;
100;
255;
To_HC11();
a = send_output();
//send command to HC11
a = 0;
while ((a <300) && ((status = get_input()) == 0))
{
delay(5);
a++;
}
From_HC11();
/*---------------------------------------------------------------------------*/
/*
Data Aquisition and Smoothing
*/
/*---------------------------------------------------------------------------*/
next(13,pit,pp);
next(13,rol,rr);
next(3,analog[0],analog_in1);
pit_m = median(13,pit);
temp_float = pit_m;
pitch_trim = ( temp_float / 255.0);
pitchd = (temp_float / 5.186 ) - 18.4;
rol_m = median(13,rol);
temp_float = rol_m;
roll_trim = (temp_float / 255.0);
rolld = (temp_float / 7.06) - 16.54;
for(i=0;i<4;i++)
{
analog_med[i]=median(3,analog[i]);
}
77
THONG_1.C
25 / 48
mass_left = (float) ( analog_med[1] * 1.6 );

if (mass_left < 1 ) mass_left = 1;
mass_right = (float) ( analog_med[0] * 1.6 );
if (mass_right < 1 ) mass_right = 1;
front_trim = (mass_right / (mass_left + mass_right));
trim_d = 100 * front_trim;
/*---------------------------------------------------------------------------*/
/*
Sway Routine
*/
/*---------------------------------------------------------------------------*/
if( SWAY_ENABLE > 0 )
{
if ( SURGE_SET < 1)
{
SURGE_SET = 1;
RHA_CAL = RHA_I.position;
LHA_CAL = LHA_I.position;
SAG_ENABLE = 0;
lat_bal_float = 0.0;
t_zero = biostime(0, 0L);
trim_min = 50;
trim_max = 50;
roll_min = 127;
roll_max = 127;
}
t_now = biostime(0, 0L);
t_run = t_now - t_zero;
if ( t_run > period )
{
t_run = t_run - period;
t_zero = t_zero + period;
}
78
THONG_1.C
26 / 48
f_t_run = (float) (t_run);

f_period = (float) (period);
frac = (f_t_run/f_period);
phase =
frac * 6.28319;
d_offset = (sway_amp * sin(phase));

f_lift = (lift_amp * sin(phase));
ab_adj = (int) (d_offset);
foot_lift = (int) (f_lift);
if ( phase <= 3.143)
{
left_foot_lift = foot_lift;
right_foot_lift = 0;
}
if ( phase > 3.143 )
{
left_foot_lift = 0;
right_foot_lift = -1 * foot_lift;
}
if
if
if
if
( rol_m < roll_min ) roll_min = rol_m;

( rol_m > 127 ) roll_min = 127;
(rol_m > roll_max) roll_max = rol_m;
(rol_m < 127) roll_max = 127;
if
if
if
if
( trim_d < trim_min ) trim_min = trim_d;

( trim_d > 50 ) trim_min = 50;
(trim_d > trim_max) trim_max = trim_d;
(trim_d < 50) trim_max = 50;
}
/*---------------------------------------------------------------------------*/
/*
Dimensional calculations
*/
/*---------------------------------------------------------------------------*/
79
THONG_1.C
27 / 48
if( GYR_ENABLE > 0 )

{
height_adj_left = left_foot_lift;
height_adj_right = right_foot_lift;
right_hip_height = 2070.0 - height_adj_right;
left_hip_height = 2070.0 - height_adj_left;
right_hip_displ = 10.0;
left_hip_displ = 10.0;
abs_dist_right = sqrt( right_hip_height * right_hip_height + right_hip_displ * right_hip_displ );
abs_dist_left = sqrt( left_hip_height * left_hip_height + left_hip_displ * left_hip_displ );
beta_right = rad_deg * ( acos( ( (upper_leg * upper_leg) + (lower_leg * lower_leg)
- (abs_dist_right * abs_dist_right) ) / ( 2 * upper_leg * lower_leg) ));
beta_left = rad_deg * ( acos( ( (upper_leg * upper_leg) + (lower_leg * lower_leg)
- (abs_dist_left * abs_dist_left) ) / ( 2 * upper_leg * lower_leg) ));
epsilon_right = rad_deg * atan( right_hip_displ / right_hip_height);
epsilon_left = rad_deg * atan( left_hip_displ / left_hip_height);
psi_right = rad_deg * ( acos( ( (lower_leg * lower_leg) - (upper_leg * upper_leg)
- (abs_dist_right * abs_dist_right) ) / ( -2 * upper_leg * abs_dist_right) ));
psi_left = rad_deg * ( acos( ( (lower_leg * lower_leg) - (upper_leg * upper_leg)
- (abs_dist_left * abs_dist_left) ) / ( -2 * upper_leg * abs_dist_left) ));
right_hip_ext = 90.0 - psi_right - epsilon_right;
left_hip_ext = 90.0 - psi_left - epsilon_left;
right_foot_ext = beta_right - right_hip_ext;
left_foot_ext = beta_left - left_hip_ext;
RHE_O.position = (int) ((right_hip_ext - 52.5) * 12.3);
LHE_O.position = (int) ((left_hip_ext - 55.5) * 12.3);
80
THONG_1.C
28 / 48
RKE_O.position = (int) ((beta_right - 122.5) * 5.69);

LKE_O.position = (int) ((beta_left - 126.5) * 5.69);
RFO_O.position = (int) ((right_foot_ext - 70.0) * 3.0);
LFO_O.position = (int) ((left_foot_ext - 70.0) * 3.0);
}
/*:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::*/
/*::
Balancing routine
::*/
/*:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::*/
R_LIM_IN
R_LIM_OUT
L_LIM_IN
L_LIM_OUT
=
=
=
=
RFO_I.status
RFO_I.status
LFO_I.status
LFO_I.status
&
&
&
&
1;
2;
2;
1;
/* Set foot contact limit */

/* switch flags
*/
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*
Left Foot
*/
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
left_trim = (float) analog_med[3] / 254.0;
if ( left_trim > 1.0 ) left_trim = 1.0;
if ( left_trim < 0.0 ) left_trim = 0.0;
low_lim_left = LEFT_POINT - FOOT_ZONE;
up_lim_left = LEFT_POINT + FOOT_ZONE;
if ( (LON_ENABLE == 4) & (LON_COUNT_L == 2) )
{
if ( left_trim > up_lim_left )
{
LFO_O.position--;
if ( LFO_O.position < LEFT_CAL_POS - 20 ) LFO_O.position = LEFT_CAL_POS - 20;
}
else if ( left_trim < low_lim_left )
{
LFO_O.position++;
if ( LFO_O.position > LEFT_CAL_POS + 20 ) LFO_O.position = LEFT_CAL_POS + 20;
81
THONG_1.C
29 / 48
}
}
else if ( LON_ENABLE == 0 )
{
/* LFO_O.position = LEFT_CAL_POS; */
}
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*
Right Foot
*/
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
right_trim = (float) analog_med[2] / 254.0;
if ( right_trim > 1.0 ) right_trim = 1.0;
if ( right_trim < 0.0 ) right_trim = 0.0;
low_lim_right = RIGHT_POINT - FOOT_ZONE;
up_lim_right = RIGHT_POINT + FOOT_ZONE;
if ( (LON_ENABLE == 4) & (LON_COUNT_R == 2) )
{
if ( right_trim > up_lim_right )
{
RFO_O.position--;
if ( RFO_O.position < RIGHT_CAL_POS - 20 ) RFO_O.position = RIGHT_CAL_POS - 20;
}
else if ( right_trim < low_lim_right )
{
RFO_O.position++;
if ( RFO_O.position > RIGHT_CAL_POS + 20 ) RFO_O.position = RIGHT_CAL_POS + 20;
}
}
else if ( LON_ENABLE == 0 )
{
/* RFO_O.position = RIGHT_CAL_POS;*/
}
82
THONG_1.C
30 / 48
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*
Sagital Balance
*/
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
ft_old = ft_new;
ft_new = front_trim;
ft_rate = 100 * (ft_new - ft_old) / (float) bios_ticks;
hip_slew = abs(RHA_I.velocity - LHA_I.velocity);
if ( SAG_ENABLE == 0)
{
lat_bal_float= 0.0;
}
if
( ( SAG_ENABLE == 4) & (hip_slew < 75 ) )

{
if ( front_trim > up_lat_lim_1 ) lat_bal_float = lat_bal_float - 1*lat_bal_inc;
if ( front_trim < low_lat_lim_1 ) lat_bal_float = lat_bal_float + 1*lat_bal_inc;
if ( front_trim < low_lat_lim_2 ) lat_bal_float = lat_bal_float + 2 * lat_bal_inc;
}
lat_bal_comp = (int) (lat_bal_float);

if ( lat_bal_comp > 15 ) lat_bal_comp = 15;
if ( lat_bal_comp < -15 ) lat_bal_comp = -15;
RHA_O.position = RHA_CAL + lat_bal_comp + ab_adj;
LHA_O.position = LHA_CAL - lat_bal_comp - ab_adj;
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*
Lateral Balance
*/
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
RFI_O.status = 0;
LFI_O.status = 0;
/* send status */
/* to hc11 with enable bit */
83
THONG_1.C
31 / 48
if (LAT_ENABLE == 4)
{
if ( (front_trim < LAT_POINT + 2 * LAT_ZONE) & (front_trim >
{
RFI_O.status = 4;
/* send status */
LFI_O.status = 4;
/* to hc11 with enable bit */
}
}
LAT_POINT - 2 * LAT_ZONE))
/*:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::*/
if( logg == 1) dump();
r_pre = (70 * right_trim) - 1;
r_pst = 70 - (r_pre + 1);
l_pre = (70 * left_trim) - 1;
l_pst = 70 - (l_pre + 1);
f_pre = (70 * front_trim) - 1;
f_pst = 70 - (f_pre + 1);
pi_pre = (70 * pitch_trim) - 1;
pi_pst = 70 - (pi_pre + 1);
ro_pre = (70 * roll_trim) - 1;
ro_pst = 70 - (ro_pre + 1);
if (screen_count > 3 )
{
screen_count = 0;
clrscr();
Screens();
cprintf("\n\r");
textcolor (DARKGRAY);
cprintf("
_____________________________________________________________________\n\r");
cprintf("
| 0
| 1
| 2
| 3
| 4
| 5
| 6
| 7
| 8
| 9
|\n\r");
cprintf("\n\r");
cprintf("R_Foot ");
for (j=0; j<r_pre; j++) { cprintf("."); }
textcolor (BLACK);
84
THONG_1.C
32 / 48
textbackground(GREEN);
cprintf("^");
textbackground(BLACK);
for (j=0; j<r_pst; j++) { cprintf("."); }
cprintf("\n\r");
cprintf("L_Foot ");
for (j=0; j<l_pre; j++) { cprintf("."); }
textcolor (BLACK);
textbackground(RED);
cprintf("^");
for (j=0; j<l_pst; j++) { cprintf("."); }
cprintf("\n\r");
cprintf("Front
");
for (j=0; j<f_pre; j++) { cprintf("."); }
textcolor (BLACK);
textbackground(BLUE);
cprintf("^");
for (j=0; j<f_pst; j++) { cprintf("."); }
cprintf("\n\r");
cprintf("Pitch
");
for (j=0; j<pi_pre; j++) { cprintf("."); }
textcolor (BLACK);
textbackground(MAGENTA);
cprintf("^");
for (j=0; j<pi_pst; j++) { cprintf("."); }
cprintf("\n\r");
cprintf("Roll
");
85
THONG_1.C
33 / 48
for (j=0; j<ro_pre; j++) { cprintf("."); }

textcolor (BLACK);
textbackground(CYAN);
cprintf("^");
for (j=0; j<ro_pst; j++) { cprintf("."); }
cprintf("\n\r");
cprintf("
Left Foot
");
if (L_LIM_OUT == 0) textbackground(RED);
cprintf(" ");
cprintf("
");
if (L_LIM_IN == 0) textbackground(RED);
cprintf(" ");
cprintf("
");
if (R_LIM_IN == 0) textbackground(RED);
cprintf(" ");
cprintf("
");
if (R_LIM_OUT == 0) textbackground(RED);
cprintf(" ");
cprintf("
");
cprintf("Right Foot
");
cprintf("\n\r");
textcolor (LIGHTRED);
cprintf("\n\r");
cprintf (" RiTri = %3.2f LeTri = %3.2f MaLe = %5.2f MaRi = %5.2f Fr_tri = %4.2f BIO= %5.2f",
right_trim,
left_trim,
mass_left,
mass_right, front_trim,
bios_rate);
cprintf("\n\r");
textcolor (YELLOW);
86
THONG_1.C
34 / 48
cprintf("LAT = %d LON = %d GYR = %d SAG = %d Log = %d offd = %6.3f ab = %d",

LAT_ENABLE, LON_ENABLE, GYR_ENABLE, SAG_ENABLE, logg, d_offset, ab_adj);
}
}
/*___________________________________________________________________________________*/
allstop();
}
void setcmd(struct HC11_DATA *hp, char *data)
{
int val, sign;
char *cp;
cp = strstr(data,"CMD:") ;
val = 0;
if ( cp != NULL )
{
cp += 5;
while ( (*cp >= '0') && (*cp <= '9'))
{
val = val * 10 + *cp -48;
cp++;
};
hp->cmd = val;
};
cp =strstr(data,"Pos:");
val = 0;
if ( cp != NULL )
{
cp += 5;
sign = 1;
if (*cp == '-')
{
sign = -1;
cp++;
}
while ( (*cp >= '0') && (*cp <= '9'))
{
val = val * 10 + *cp -48;
87
THONG_1.C
35 / 48
cp++;
};
hp->position= val*sign;
};
}
/* output file for dumping degug data */
int dump()
{
FILE *out;
if ( (out=fopen("dump.dat","a")) == NULL ) return -1;
time(&c_time);
fprintf(out," %ld %5.2f %5.2f %4.3f %4.3f %4.3f %5d %5d %5d %5d %5d %5d %6.3f %5d %5d %5d %5d %5d %5d %5d %
5d %5d %5d %5d %5d %5d %5d %5d %5d %5d %5d %5d %5d %5.3f %4d %4d %5.2f %6.3f %5d %5d %5d %5d\n",
c_time,
mass_left, mass_right, front_trim, left_trim, right_trim,
LHA_O.position, LHA_I.position, LHA_I.velocity,
RHA_O.position, RHA_I.position, RHA_I.velocity, ft_rate,
LTE_I.position, LTE_O.position, RTE_I.position, RTE_O.position,
LFI_I.position, LFI_O.position, RFI_I.position, RFI_O.position,
LFO_I.position, LFO_O.position, RFO_I.position, RFO_O.position,
LHE_I.position, LHE_O.position, RHE_I.position, RHE_O.position,
LKE_I.position, LKE_O.position, RKE_I.position, RKE_O.position,
front_trim, ab_adj, rol_m, comp, phase, roll_max, roll_min, trim_max, trim_min);
fclose (out);
return 1;
}
void Screend (void)
{
textcolor(LIGHTGREEN);
cprintf( "Right toe extension > pos:%3d CMD:%1d Status:%1d Vel: %3d \n\r",
RTE_I.position,RTE_I.cmd,RTE_I.status,RTE_I.velocity);
cprintf( "Right foot extension > pos:%3d CMD:%1d Status:%1d Vel: %3d \n\r",
RFO_I.position,RFO_I.cmd,RFO_I.status,RFO_I.velocity);
cprintf( "Right foot rotation > pos:%3d CMD:%1d Status:%1d Vel: %3d \n\r",
RFI_I.position,RFI_I.cmd,RFI_I.status,RFI_I.velocity);
88
THONG_1.C
36 / 48
cprintf( "\n");
textcolor(LIGHTRED);
cprintf( "Left toe extension
> pos:%3d CMD:%1d Status:%1d Vel: %3d \n\r",
LTE_I.position,LTE_I.cmd,LTE_I.status,LTE_I.velocity);
cprintf( "Left foot extension > pos:%3d CMD:%1d Status:%1d Vel: %3d \n\r",
LFO_I.position,LFO_I.cmd,LFO_I.status,LFO_I.velocity);
cprintf( "Left foot rotation
LFI_I.position,LFI_I.cmd,LFI_I.status,LFI_I.velocity);
cprintf( "\n");
cprintf( "Right knee extension > pos:%3d CMD:%1d Status:%1d Vel: %3d \n\r",
RKE_I.position,RKE_I.cmd,RKE_I.status,RKE_I.velocity);
cprintf( "Left knee extension > pos:%3d CMD:%1d Status:%1d Vel: %3d \n\r",
LKE_I.position,LKE_I.cmd,LKE_I.status,LKE_I.velocity);
cprintf( "\n");
cprintf( "Right hip extension > pos:%3d CMD:%1d Status:%1d Vel: %3d \n\r",
RHE_I.position,RHE_I.cmd,RHE_I.status,RHE_I.velocity);
cprintf( "Right hip abduction > pos:%3d CMD:%1d Status:%1d Vel: %3d \n\r",
RHA_I.position,RHA_I.cmd,RHA_I.status,RHA_I.velocity);
cprintf( "Right hip rotation
RHR_I.position,RHR_I.cmd,RHR_I.status,RHR_I.velocity);
cprintf( "\n");
cprintf( "Left hip extension > pos:%3d CMD:%1d Status:%1d Vel: %3d \n\r",
LHE_I.position,LHE_I.cmd,LHE_I.status,LHE_I.velocity);
cprintf( "Left hip abduction > pos:%3d CMD:%1d Status:%1d Vel: %3d \n\r",
LHA_I.position,LHA_I.cmd,LHA_I.status,LHA_I.velocity);
cprintf( "Left hip rotation
LHR_I.position,LHR_I.cmd,LHR_I.status,LHR_I.velocity);
cprintf( "\n");
textcolor(YELLOW);
cprintf( "Analog output: Channel 1: %3d
2: %3d 3: %3d 4: %3d\n\r",
analog_in1, analog_in2, analog_in3, analog_in4);
89
THONG_1.C
textcolor(CYAN);
cprintf( "Nav Data: Pitch %3d
pp, rr, aa, comp);
37 / 48
Roll %3d
Air %3d
Head %5f\n\r",
}
void Screens(void)
{
textcolor(YELLOW);
cprintf( " RTOEX > pos: %3d Vel: %3d Stat: %2d
LTOEX > pos:%3d Vel: %3d Stat: %2d \n\r",
RTE_I.position, RTE_I.velocity, RTE_I.status, LTE_I.position, LTE_I.velocity, LTE_I.status);
cprintf( " RFOEX > pos: %3d Vel: %3d Stat: %2d
LFOEX > pos:%3d Vel: %3d Stat: %2d \n\r",
RFO_I.position, RFO_I.velocity, RFO_I.status, LFO_I.position, LFO_I.velocity, LFO_I.status);
textcolor(YELLOW);
cprintf( " RFORO > pos: %3d Vel: %3d Stat: %2d
LFORO > pos:%3d Vel: %3d Stat: %2d \n\r",
RFI_I.position, RFI_I.velocity, RFI_I.status, LFI_I.position, LFI_I.velocity, LFI_I.status);
cprintf( " RKNEX > pos: %3d Vel: %3d Stat: %2d
LKNEX > pos:%3d Vel: %3d Stat: %2d \n\r",
RKE_I.position, RKE_I.velocity, RKE_I.status, LKE_I.position, LKE_I.velocity, LKE_I.status);
textcolor(YELLOW);
cprintf( " RHIEX > pos: %3d Vel: %3d Stat: %2d
LHIEX > pos:%3d Vel: %3d Stat: %2d \n\r",
RHE_I.position, RHE_I.velocity, RHE_I.status, LHE_I.position, LHE_I.velocity, LHE_I.status);
cprintf( " RHIAB > pos: %3d Vel: %3d Stat: %2d
LHIAB > pos:%3d Vel: %3d Stat: %2d \n\r",
RHA_I.position, RHA_I.velocity, RHA_I.status, LHA_I.position, LHA_I.velocity, LHA_I.status);
textcolor(YELLOW);
cprintf( " RHIRO > pos: %3d Vel: %3d Stat: %2d
LHIRO > pos:%3d Vel: %3d Stat: %2d \n\r",
RHR_I.position, RHR_I.velocity, RHR_I.status, LHR_I.position, LHR_I.velocity, LHR_I.status);
cprintf( "\n\r");
textcolor(LIGHTGRAY);
cprintf( "Left_trim %3d Left_mass %3d Right_trim %3d Right_mass %3d\n\r",
analog_med[3], analog_med[1], analog_med[2], analog_med[0]);
90
THONG_1.C
38 / 48
textcolor(RED);
cprintf( "Nav Data: Pitch %4d -> %5.2f
Roll %4d -> %5.2f
pit_m, pitchd, rol_m, rolld, comp);
Head %5.2f\n\r",
}
/*##########################################################################*/
/*
*/
/*
Main Control Loop
*/
/*
*/
/*##########################################################################*/
int main()
{
int a,
/* response from comms functions
i,j,
/* basic counter
e_level_s = 0,
// error counter for status
e_level_c = 0,
// error counter for command
e_level_p = 0,
// error counter for position
e_level_v = 0,
// error counter for velocity
loop_count = 0,
// total loops
c1 =0,
// if delay is longer 20< and <40
c2= 0,
c3 = 0,
c4 = 0,
c5=0 ;
// if delay is longer 120<
int ESC = 0;
/*
*/
*/
// ESC flag
char test[7];
time_t t_tmp1, t_tmp2;
/*Test character string array
*/
// time var for timing
int logger = 0;
float loop_speed;
int total_time;
/* Initialise median filters variables */
for (i=0;i<13;i++)
{
pit[i]=0;
rol[i]=0;
91
THONG_1.C
39 / 48
for (j=0;j<4;j++)
{
analog[j][i]=0;
analog_med[j]=0;
}
}
/* Initialise test array
RTE_O.status = 0x03; RTE_O.cmd
LTE_O.status = 0x02; LTE_O.cmd
RFO_O.status = 0x03; RFO_O.cmd
LFO_O.status = 0x04; LFO_O.cmd
RFI_O.status = 0x05; RFI_O.cmd
LFI_O.status = 0x06; LFI_O.cmd
RKE_O.status = 0x07; RKE_O.cmd
=
=
=
=
=
=
=
0;
0;
0;
0;
0;
0;
0;
*/
RTE_O.position=500; RTE_O.velocity =300;
LTE_O.position=-500; LTE_O.velocity = 300;
RFO_O.position=-123; RFO_O.velocity = -5;
LFO_O.position=256; LFO_O.velocity = 5;
RFI_O.position=-256; RFI_O.velocity =100;
LFI_O.position=0; LFI_O.velocity = -100;
RKE_O.position=-1; RKE_O.velocity = -5;
LKE_O.status = 0x08; LKE_O.cmd = 0; LKE_O.position=-2; LKE_O.velocity = 5;

RHE_O.status = 0x9; RHE_O.cmd = 0; RHE_O.position=500; RHE_O.velocity = 5;
LHE_O.status = 0xa; LHE_O.cmd = 0; LHE_O.position=146; LHE_O.velocity = 5;
RHA_O.status = 0xb; RHA_O.cmd = 0; RHA_O.position=146; RHA_O.velocity = 5;
LHA_O.status = 0xc; LHA_O.cmd = 0; LHA_O.position=146; LHA_O.velocity = 5;
RHR_O.status = 0xd; RHR_O.cmd = 0; RHR_O.position=146; RHR_O.velocity = 5;
LHR_O.status = 0xe; LHR_O.cmd = 0; LHR_O.position=146; LHR_O.velocity = 5;
allstop();
printf(" \n the compass value is %6.2f", compass());
if (horizon() != 0 ) printf("\n Pitch: %d
Roll: %d
Press: %d", pp,rr,aa);
remove("ip07.dat");
/* Reset system time storage variables
*/
time(&t_tmp1);
time(&t_tmp2);
/* Loop until seconds clock over */
while ( (t_tmp2 - t_tmp1) < 1) time(&t_tmp2);
/* Reset time variables
*/
time(&t_tmp1);
time(&t_tmp2);
while (t_tmp2 == t_tmp1) time(&t_tmp2);
time(&t_tmp1);
t_tmp2= t_tmp1;
92
THONG_1.C
40 / 48
a=0;
while ( t_tmp2 == t_tmp1)
{
status =get_input();
a++; delay(5); time(&t_tmp2);
} ;
printf("\nloop %d for 1 second",a);
/*>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>X<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<*/
/* Main transmission loop
*/
/*>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>X<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<*/
while ( ESC == 0 )
{
loop_count ++;
/* Increment Loop count */
comp = compass();
if (comp > 370) comp = 444;
horizon();
/* get the compass value, if it's > 370 */

/* send an error value */
/* Get the horizon values */
if (kbhit() != 0)
{
key_in = getch();
};
printf( "Input = %d\n", key_in);
/*:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::*/
/* Keyboard command functions
*/
/*:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::*/
/* Exit Program */
if (key_in == 27) ESC ++;
/* Stop all valve functions */
93
THONG_1.C
41 / 48
if (key_in == 32)
allstop();
/* Go into stagger mode */

if (key_in == 76)
{
stagger();
}
/* Toggle debug logging */
if (key_in == 61 )
{
if( logger > 0 )
{
logger = 0;
}
else
{
logger = 1;
}
key_in = 0;
};
/* Cycle valves */
if (key_in == 0x70 )
{
cycle();
key_in = 0;
};
/*............................................................................*/
/* Air flush command */
if (key_in == 49
)
{
LHE_O.cmd = 1; /* flush lhe cylinder
RHE_O.cmd = 1; /* flush lhe cylinder
LHA_O.cmd = 1; /* flush lhe cylinder
RHA_O.cmd = 1; /* flush lhe cylinder
RHA_O.position = LHE_I.position;
*/
*/
*/
*/
94
THONG_1.C
42 / 48
LHA_O.position = LHE_I.position;
RHE_O.position = LHE_I.position;
if (LHE_I.status > 3) key_in = 73;
}
/* Left Hip Abduction */
if (key_in == 0x48 ) LHA_O.cmd
if (key_in == 84
) LHA_O.cmd
=
=
=
=
7;
4;
2;
6;
/* Right Hip
if (key_in
if (key_in
if (key_in
if (key_in
*/
RHA_O.cmd
RHA_O.cmd
RHA_O.cmd
RHA_O.cmd
=
=
=
=
7;
4;
2;
6;
/* Left Hip Extension */

if (key_in == 0x4A ) LHE_O.cmd
if (key_in == 0x6A ) LHE_O.cmd
if (key_in == 0x75 ) LHE_O.cmd
if (key_in == 85
) LHE_O.cmd
=
=
=
=
7;
4;
2;
6;
/* Right Hip
if (key_in
if (key_in
if (key_in
if (key_in
*/
RHE_O.cmd
RHE_O.cmd
RHE_O.cmd
RHE_O.cmd
=
=
=
=
7;
4;
2;
6;
/* M/m for RHE */
*/
) LHR_O.cmd
) LHR_O.cmd
) LHR_O.cmd
) LHR_O.cmd
=
=
=
=
7;
4;
2;
6;
/* G/g for LHR */
Rotation */
== 0x42 ) RHR_O.cmd
== 0x62 ) RHR_O.cmd
== 0x77 ) RHR_O.cmd
== 87
) RHR_O.cmd
=
=
=
=
7;
4;
2;
6;
/* B/b for RHR */
Abduction
== 0x4e )
== 0x6E )
== 0x79 )
== 89
)
Extension
== 0x4D )
== 0x6D )
== 0x69 )
== 73
)
/* Left Hip Rotation

if (key_in == 0x47
if (key_in == 0x67
if (key_in == 0x71
if (key_in == 81
/* Right Hip
if (key_in
if (key_in
if (key_in
if (key_in
/*H/h for LHA */

/* t for lhA cal */
/* t for lhA cal */
/* N/n for RHA */
/* y for RHA cal */
/* y for RHA cal */
/* J/j FOR LHE */
/* u for lhE cal */
/* u for lhE cal */
/* i for RHE cal */

/* i for RHE cal */
/* q for lhr cal */

/* q for lhr cal */
/* w for Rhr cal */

/* w for Rhr cal */
95
THONG_1.C
/* Left Knee
if (key_in
if (key_in
if (key_in
if (key_in
43 / 48
Extension
== 0x46 )
== 0x66 )
== 0x65 )
== 69
)
*/
LKE_O.cmd
LKE_O.cmd
LKE_O.cmd
LKE_O.cmd
=
=
=
=
7;
4;
2;
6;
/* F/f for LKE */
/* Right Knee Extension */

if (key_in == 0x56 ) RKE_O.cmd
if (key_in == 82
) RKE_O.cmd
=
=
=
=
7;
4;
2;
6;
/* F/f for rKE */
/* e for lhr cal */

/* e for lhr cal */
/* e for rhr cal */

/* e for rhr cal */
/* Left Toe Extension */

if (key_in == 0x41 ) LTE_O.cmd = 7;
if (key_in == 0x61 ) LTE_O.cmd = 4;
/* A/a for lto */
/* Left Foot Rotation */

if (key_in == 0x53 ) LFI_O.cmd = 7;
if (key_in == 0x73 ) LFI_O.cmd = 4;
/* S/s for LFI */
/* Left Foot Extension */

if (key_in == 0x44 ) LFO_O.cmd = 7;
if (key_in == 0x64 ) LFO_O.cmd = 4;
/* D/d for LFO */
/* Right Toe Extension */

if (key_in == 0x5a ) RTE_O.cmd = 7;
if (key_in == 0x7a ) RTE_O.cmd = 4;
/* Z/z for rto */
/* Right Foot Rotation */

if (key_in == 0x58 ) RFI_O.cmd = 7;
if (key_in == 0x78 ) RFI_O.cmd = 4;
/* X/x for rFI */
/* Right Foot Extension */

if (key_in == 0x43 ) RFO_O.cmd = 7;
if (key_in == 0x63 ) RFO_O.cmd = 4;
/* C/c for RFO */
/*............................................................................*/
/* Left Foot Calibration */
if (key_in == 123)
{
96
THONG_1.C
44 / 48
LTE_O.cmd = 6;
LFO_O.cmd = 6;
LFI_O.cmd = 6;
/* Set all foot cylinders to stop */
}
if (key_in == 91)
{
/* Begin calibration */
if (LTE_I.cmd != 2)
{
LTE_O.cmd = 2;
LFO_O.status = 0;
LFI_O.status = 0;
LTE_O.status = 0;
}
/* send calibrate command until */

/* toe HC11 sends back "2" to indicate */
/* toe calibration is complete */
else if (LFI_I.cmd != 2)
{
LFI_O.cmd = 2;
}
else if ( (LFO_I.cmd ==2)
{
LFO_O.status =
LFI_O.status =
LTE_O.status =
}
/* once toe calibratrion is complete */

/* begin inside cylinder calibration */
/* wait for a "2" to indicate completion */
&& ((LTE_I.status&1) == 0) )
9;
9;
9;
/* indicate calibration is complete */

/* if outside cylinder hc11 returns "2" */
/* and foot extension limit switch is on */
else
{
LFO_O.cmd = 2;
/* send calibration command to foot extension */
}
}
/*............................................................................*/
/* Right Foot Calibration */
if (key_in == 125)
{
RTE_O.cmd = 6;
RFO_O.cmd = 6;
RFI_O.cmd = 6;
}
97
THONG_1.C
45 / 48
if (key_in == 93)
{
if (RTE_I.cmd != 2)
{
RTE_O.cmd = 2;
RFO_O.status = 0;
RFI_O.status = 0;
RTE_O.status = 0;
}
else if (RFI_I.cmd != 2)
{
RFI_O.cmd = 2;
}
else if ( (RFO_I.cmd
{
RFO_O.status =
RFI_O.status =
RTE_O.status =
}
==2) && ((RTE_I.status&1) == 0) )

9;
9;
9;
else
{
RFO_O.cmd = 2;
}
}
/*............................................................................*/
/* if key in is anything but foot calibration or bleed, set it to 0 */
if ( (key_in != 91) && (key_in != 93) && (key_in != 48)) key_in =0;
total ++;
analog_out1
analog_out2
analog_out3
analog_out4
=
=
=
=
1;
10;
100;
255;
To_HC11();
98
THONG_1.C
46 / 48
a = send_output();
//
clrscr();
if ( a != 1)
{
printf(" write error %d",a);
exit(0);
}
else
{
//printf ("Data written");
}
a = 0;
{ delay(5);
a++;
}
From_HC11();
if ( a == 350)
{
printf("\ntime out, please increase the wait value , loops : %d",a);
exit(0);
};
if (status != 1)
{
printf("\nerror in receiver %d loops %d",status, a);
exit(0);
};
if (a<20)
{
}
else
{
if (a < 40) c1++;
99
THONG_1.C
47 / 48
else
{
if (a < 60) c2++;
else
{
if (a < 90) c3++;
else
{
if (a< 120) c4++;
else
}
c5++;
}
}
}
clrscr();
if( loop_count > 2 && logger > 0)
{
dump();
loop_count = 0;
}
Screend();
next(13,pit,pp);
next(13,rol,rr);
pit_m=median(13,pit);
rol_m=median(13,rol);
for(i=0;i<4;i++)
{
analog_med[i]=median(3,analog[i]);
100
THONG_1.C
48 / 48
}
time(&t_tmp2);
total_time = t_tmp2 - t_tmp1;
loop_speed = total/total_time;
printf("looped %d times\tspeed %3.2f\tlogging = %d\n", a, loop_speed, logger);
}
/*>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>X<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<*/
printf("\nTotal pack is %d.\n",total);

printf("\nTotal errors are status:%d cmd:%d position:%d velocity:%d\n"
,e_level_s,e_level_c,e_level_p,e_level_v);
printf("\n loop 20< and <40: %d",c1);
printf("\n loop 40< and < 60: %d",c2);
printf("\n loop 60< and < 90: %d",c3);
printf("\n loop 90< and > 120: %d",c4);
printf("\n loop 120<
: %d",c5);
time(&t_tmp2);
printf("\nTotal time is %d ", t_tmp2 - t_tmp1);
/*
fclose(fff);
*/
return 0;
}
101
MP2X.C
1 / 19
#include <stdio.h>
#include <conio.h>
#include <PROCESS.H>
#include <STDLIB.h>
#include <time.h>
#include <io.h>
#include <dos.h>
//#include <graph.h>
#include <string.h>
#include <head-c.h>
#include <errno.h>
#include <fcntl.h>
#include <share.h>
#define ArraySize 100
FILE
FILE
FILE
FILE
*ip_handle;
*op_handle;
* (log_handle[16]);
*sys_handle;
char ip_filename[16][10];
/*name for input file like ip01.dat .. ip16.dat*/
char op_filename[16][10];
/*name for output file like op01.dat .. op16.dat*/
char log_file[16][10];
/*log file for each port data in and out */
char sys_log_file[]= "syslog.txt"; /*System log file to keep track the system fail information*/
int ip_count[16],op_count[16];
//package counter displayed by init_screen
unsigned int read_finish=0 ;
//Flag - finished reading sixteen ports
int Max_Retry = 5; //If file exists but is unavailable, will try to open it five times.
char READ[]="r";
char APPEND[] = "a";
char WRITE[] = "w+";
int log_sign = 0;
char input_buf[16][100]; /*Data input buffer*/
char output_buf[16][100]; /*data outpur buffer*/
int open_retry[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; //error vector counts errors on each port
102
MP2X.C
char data_buf[400];
int Port_No_Array[ArraySize];
int
int
int
int
Total_No_Port = 0;
Total_com = 0;
fatal_error =0;
system_time_var=0;
void Init_MP();
void Initialize();
void Init_Screen();
void WriteTime(FILE *);
void System_Log(char *);
void CloseAllFile(void);
void CloseAllPort(void);
int readdata();
void Rece_and_Write();
void Port6_WR();
void Port1_WR();
void hang(int);
void check_com_line(char *);
2 / 19
/*I have no idea why at this moment, just do
what the book says*/
/* Total number of P747 Port*/
/* Total number of normal com port;*/
//fatal error flag - halts execution if true
//timing variable for delay routine delay(int n)
// Declaration of multiport initialisation routine
// Declaration of system variable initialisation routine
// Declaration of routine to update screen
// Declaration of routine to write time stamp to log files
// Declaration of routine to write text into log file
// Declaration of routine to close log files
// Declaration of routine to close ports
// Declaration of routine to read output data and write to HC11
// Declaration of routine to read data from hc11 and write
// Declaration of routine to read and write compas data
// Declaration of routine to read and write COM1 data
// Declaration of routine to set a hang
// Check the command line for
// 0 for no log
// 1 for log
// Routine to write time to "file_handle", sets a new line and writes the time.
void WriteTime(FILE *file_handle)
{
long tmp;
struct tm *system_time;
time(&tmp);
system_time = localtime(&tmp);
fprintf(file_handle,"\n%d%d%d ",system_time->tm_hour,
system_time->tm_min,system_time->tm_sec);
return;
}
// Routine to check the command line if log is wanted. default: log off
103
MP2X.C
3 / 19
void check_com_line(char *a)

{
if (*a == '1') log_sign =1;
}
// routine to set a hang. Will loop for n milliseconds

void hang(int n)
/*hang n
{ time_t time1;
long tmp;
tmp = n * system_time_var ;
while (tmp)
{
tmp --;
time(&time1);
};
}
ms*/
// routine to initialise system variables

void Initialize()
{ time_t tmp_time1,tmp_time2;
int tmp;
/*name for input file */
_fstrcpy(ip_filename[0],"ip06.dat");
_fstrcpy(ip_filename[1],"ip07.dat");
/*name for output file */
_fstrcpy(op_filename[0],"op06.dat");
_fstrcpy(op_filename[1],"op.dat");
/*name for log file
*/
_fstrcpy(log_file[0],"datalog.dat");
/*init the input and output counter*/
for (tmp = 0; tmp < 16; tmp ++)
{
ip_count[tmp] = 0;
104
MP2X.C
4 / 19
op_count[tmp] = 0;
memset(input_buf[tmp],0,50);
};
/*initialize the system time var.*/

time(&tmp_time1);
time(&tmp_time2);
while (( tmp_time2 -
tmp_time1) ==0){time(&tmp_time2);};
time(&tmp_time1);
time(&tmp_time2);
while ( tmp_time2 == tmp_time1)
{
system_time_var++;
time(&tmp_time2);
};
system_time_var = system_time_var / 1000;
// wait until end of second
/*hang parameter for hang function*/
/*open the system log file*/

sys_handle=fopen(sys_log_file,WRITE);
if (sys_handle == NULL)
{ printf("Can not open System LOG File, System shuts down.");
exit(0);
};
WriteTime(sys_handle);
System_Log("System File created");
/*open data log file*/
if ((log_handle[0]=fopen(log_file[0],WRITE)) == NULL)
{ WriteTime(sys_handle);
fprintf(sys_handle,"Can not open log file %d %s\n",tmp + 6,log_file[tmp]);
fatal_error++;
System_Log("System shuts down");
}
printf("Initialize finish");
return;
};
105
MP2X.C
5 / 19
void Init_Screen()
{ int i;
printf("
Multi Port control Module \n");
printf("\tPort Number
TX
RX ");
if (log_sign )
printf("
logging\n");
else
printf("\n");
for (i = 0 ; i< 16; i ++)
{ printf("\t
%2d
%4d
%4d %4d %s\n", i+6, op_count[i], ip_count[i],_fstrlen(input_buf[i]),
input_buf[i]);
};
for (i = 0 ;i <6; i++)
printf("\n");
}
void Init_MP()
{
/*initialize the com port */
int port,tmp1=ArraySize;
char buf[]="Port
Error";
char inttochar[4] = {0,0,0,0};
printf("Initialising Driver\n");
delay(3000);
tmp1 = sio_reset();
printf("Friver = %d \n", tmp1);
delay(3000);
if (tmp1 < 0)
{ fatal_error ++;
printf("Driver not found\n");
System_Log("Driver not found\n");
106
MP2X.C
6 / 19
return;
};
if ( (Total_No_Port=sio_getports(Port_No_Array,tmp1)) <= 0 )
{ fatal_error ++;
System_Log("Com Port not found");
};
fprintf(sys_handle,"\n %d port found ", Total_No_Port);
printf("\n %d port found ", Total_No_Port);
if (Total_No_Port == 18)
{
Total_No_Port -= 2;
Total_com +=2;
};
if (Total_No_Port != 16)
{
fatal_error ++;
fprintf(sys_handle,"Warning:%d Found ports\n",Total_No_Port);
System_Log("System shut down");
};
System_Log("Setting all ports 9600/4800, N, 8, 1....");
if (Total_com != 0 )
{
if ((tmp1 =sio_ioctl(1,B9600,BIT_8| P_NONE| STOP_1)) != 0)
{
fprintf(sys_handle, "Port %d error %d",port,tmp1);
fatal_error ++;
System_Log("System Shut down,");
}
sio_lctrl(1, 3);
/* set DTR RTS on
sio_flowctrl(1,0);
/* set NO flow control */
sio_open(1);
};
/*set all port to 9600,8 bits,None, 1 stop bit*/
*/
107
MP2X.C
7 / 19
if ((tmp1 =sio_ioctl(6,B4800,BIT_8| P_NONE| STOP_1)) != 0)

{
fprintf(sys_handle, "Port %d error %d",6,tmp1);
fatal_error ++;
}
sio_lctrl(6, 3);
/* set DTR RTS on
sio_flowctrl(6,0);
sio_open(6);
/*set port 6 to 4800,8 bits,None, 1 stop bit*/
*/
for (port =7 ; port < Total_No_Port+6; port++)

/*initialize all ports from port 7 to 21) */
{
if ((tmp1 =sio_ioctl(port,B9600,BIT_8| P_NONE| STOP_1)) != 0) /*set all port to 9600,8 bits,None, 1 stop bit*/
{
fprintf(sys_handle, "Port %d error %d",port,tmp1);
fatal_error ++;
}
sio_lctrl(port, 3);
/* set DTR RTS on
sio_flowctrl(port,0);
sio_open(port);
}
System_Log("Ports opened!\n");
printf("\ninit mp finish");
return;
}
*/
void CloseAllFile()
{
fclose(log_handle[0]);
if (fclose(sys_handle) != 0)
{printf("Can not Close the system log file, System shuts down");
exit (0);
108
MP2X.C
8 / 19
};
return;
}
void CloseAllPort()
{ int port;
if (Total_com) { sio_close(1);};
for (port = 6 ; port < Total_No_Port + 6; port ++)
{
if (sio_close(port) != 0 )
{
fprintf(sys_handle, "Can not close port %d",port);
}
else
{
fprintf(sys_handle, "Port %d closed",port);
};
}
}
void System_Log(char *ErrDesc)

{
if (fprintf(sys_handle,ErrDesc) < 0 ) fatal_error = 1;
if (fatal_error!=0)
{
if (Total_No_Port > 0) CloseAllPort();
CloseAllFile();
exit (31);
};
}
void Rece_and_Write()
{ int R_data=0,tmp=0,count=0;
int port=0;
int Max_Retry;
int retry_read=0;
FILE *data_file;
109
MP2X.C
9 / 19
//printf("
Write port start
");
Max_Retry = 10000;
read_finish = 0;
while (read_finish != 0x7fff && retry_read < Max_Retry)
{
for (port = 1 ; port < 16; port++)
{
count = _fstrlen(input_buf[port]);
while ( ((R_data = sio_getch(port+6)) >=0)
&& ((read_finish & (1<<(port-1))) == 0))
{ input_buf[port][count] = R_data;
count++;
if ( read_finish != 0) Max_Retry = 1000;
if (R_data == 0x0d || count > 30)
{
read_finish |=1<<(port-1) ;
ip_count[port]++;
}
}
if (R_data < 0 && R_data != -4)
{ fprintf(log_handle[0],"read something %d in port %d",R_data, port+ 6);
//return;
read_finish |=1<<(port-1) ;
ip_count[port]++;
}
};
retry_read++;
};
// printf("
Write data file
");
if ((data_file=fopen("temp",APPEND)) != NULL)
{
//write data to output file
for (port = 1 ; port < 16; port++)
{
if ((read_finish & (1<<(port-1))) != 0)
{
fprintf(data_file,"%s\n",input_buf[port]);
if (log_sign) fprintf(log_handle[0],"Port%2d r %s\n",port + 6,input_buf[port]
);
}
else
110
MP2X.C
10 / 19
{
fprintf(data_file,"ERROR\n");
fprintf(log_handle[0],"p%2dERROR\n",port + 6);
};
memset(input_buf[port],0,30);
}
fclose(data_file);
remove(ip_filename[1]);
rename("temp",ip_filename[1]);
}
else
{ fclose(data_file);
fprintf(sys_handle, "Can not open
};
data file");
}
int readdata()
{
int data_file;
int Data_len,ch=0;
unsigned int error=0;
int retry=0;
char *cp;
struct find_t temp;
// printf("
read file
");
if (_dos_findfirst(op_filename[1] ,_A_NORMAL ,&temp) != 0 )
{
file not found
");
// printf("
return 0;
};
//printf("
file found
");
while (retry < Max_Retry )
{
if ((error = _dos_open(op_filename[1],O_DENYWRITE,&data_file)) != 0 )
{if (error ==ENOENT) return 0;
else retry++;
}
111
MP2X.C
11 / 19
else
{
break;
};
};
if (error != 0 )
return 0;
//printf("
file open
");
error = _dos_read(data_file,data_buf,400,&Data_len);
_dos_close(data_file);
//printf("
outputfile close
");
if (error != 0 ) return 0;
remove(op_filename[1]);
//printf("
file delete
");
data_buf[Data_len] =0;
//if ((cp=_fstrstr(data_buf,"p06"))!= NULL){_fstrncpy(output_buf[0],cp +=3,9);op_count[0]++;};
if ((cp=strstr(data_buf,"p07"))!= NULL){strncpy(output_buf[1],(cp+=3),8);op_count[1]++;};
if ((cp=strstr(data_buf,"p08"))!= NULL){strncpy(output_buf[2],(cp+=3),8);op_count[2]++;};
if ((cp=strstr(data_buf,"p09"))!= NULL){strncpy(output_buf[3],(cp +=3),8);op_count[3]++;};
output_buf[1][7]= 0xd; output_buf[2][7]= 0xd; output_buf[3][7]= 0xd; output_buf[4][7]= 0xd;
output_buf[13][7]= 0xd; output_buf[14][7]= 0xd; output_buf[15][10]= 0xd;
// printf("
communicating
");
/* send all queues together*/
for (Data_len = 0; Data_len < 8; Data_len++)
112
MP2X.C
12 / 19
{ for (ch = 1 ; ch < 16; ch++)
{
error = sio_putch(ch+6, output_buf[ch][Data_len]);
if (error < 0)
fprintf(log_handle[0],"Port %d sends error. Code: %d",ch,error);
};
};
for (Data_len = 8; Data_len < 11; Data_len++)
{
error = sio_putch(21, output_buf[15][Data_len]);
if (error < 0)
fprintf(log_handle[0],"Port %d sends error. Code: %d",21,error);
};
if (log_sign)
for (ch = 1 ; ch < 16; ch++)
{
fprintf(log_handle[0],"Port %d s %s\n",ch + 6,output_buf[ch]);
}
//printf("
return 1;
communicating finish
");
}
void Port1_WR()
{
int Num_byte,R_data,tmp;
FILE *data_file;
int error;
struct find_t temp;
if (_dos_findfirst("op08.dat" ,_A_NORMAL ,&temp) == 0 )
{
memset(data_buf,0,50);
remove("op08.dat");
error = sio_putch(1, 0xd);
113
MP2X.C
13 / 19
hang(3000);
Num_byte = (int)sio_iqueue(1);
while (log_sign && ((int)sio_iqueue(1) != 0))
{
R_data = sio_getch(1);
fprintf(log_handle[0],"%c",R_data);
printf("%c",R_data);
hang(100);
}
_fstrcpy(data_buf,"g b60e");
data_buf[6] = 0xd;
for (Num_byte = 0; Num_byte < 7;Num_byte ++)
{
error = sio_putch(1, data_buf[Num_byte]);
if (error < 0)
{
fprintf(sys_handle, "Can not write to port %d,error %d\n",1,error);
break;
};
fputc(data_buf[Num_byte],log_handle[0]);
};
hang(1000);Num_byte = 0;
while (log_sign && ((int)sio_iqueue(1) != 0) && (Num_byte < 30))
{
fprintf(log_handle[0],"%c",R_data);
printf("%c",R_data);
hang(100); Num_byte++;
}
fprintf( log_handle[0],"\n");
}
//if (Num_byte < 30) return;
114
MP2X.C
14 / 19
// while (R_data != 'P' && R_data >= 0)
//
if (R_data < 0) return;
tmp = 0;
do
{ data_buf[tmp]=R_data;
tmp++;
} while (R_data != 0x0d && tmp < 40 && R_data>= 0);
sio_flush(1,2);
data_buf[tmp] = 0;
if ((data_file=fopen("tmp",WRITE)) != NULL)
{
/*write data to output file*/
fputs(data_buf,data_file);
fclose(data_file);
remove("ip08.dat");
rename("tmp","ip08.dat");
}
else
{
open_retry[0]++;
fprintf(sys_handle, "Can not open Number %d input data file",6);
};
// printf("
end of write Com1 file
");
void Port6_WR()
{
int Num_byte,R_data,tmp;
115
MP2X.C
15 / 19
FILE *data_file;
int data_file_handle,error;
struct find_t temp;
if (Num_byte < 30) return;
while (R_data != '$' && R_data >= 0)R_data = sio_getch(6);
if (R_data < 0) return;
tmp = 0;
do
{ data_buf[tmp]=R_data;
tmp++;
} while (R_data != '$' && tmp < 40 && R_data>= 0);
sio_flush(6,2);
data_buf[tmp] = 0;
if ((data_file=fopen(ip_filename[0],WRITE)) != NULL)
{
/*write data to output file*/
fputs(data_buf,data_file);
fclose(data_file);
//remove(ip_filename[0]);
//rename("tmp",ip_filename[0]);
}
else
{
open_retry[0]++;
fprintf(sys_handle, "Can not open Number %d input data file",6);
};
//
printf("
end of write campass file
");
if (_dos_findfirst(op_filename[0] ,_A_NORMAL ,&temp) != 0 )

{
116
MP2X.C
16 / 19
return ;
};
error = _dos_open(op_filename[0],O_DENYWRITE,&data_file_handle);
if (error != 0) return;
if ( _dos_read(data_file_handle,data_buf,100,&R_data) != 0 )
{ _dos_close(data_file_handle);
return;
};
_dos_close(data_file_handle);
if (R_data == 0 ) return;
if ((error = sio_flush(6,2))!=0)
fprintf(sys_handle, "Can not FLUSH port %d ",6);
return;
};
if (sio_close(6) != 0) return;
if (error = (sio_ioctl(6,B9600,BIT_8| P_NONE| STOP_1)) != 0) /*set all port to 9600,8 bits,None, 1 stop bit*/
fprintf(sys_handle, "Port %d error %d",6,error);
return;
};
sio_lctrl(6, 3);
/* set DTR RTS on
*/
sio_flowctrl(6,0);
sio_open(6);
fprintf( log_handle[0],"port %d s",6);
for (Num_byte = 0; Num_byte < R_data ;Num_byte ++)
{
error = sio_putch(6, data_buf[Num_byte]);
if (error < 0)
{
fprintf(sys_handle, "Can not write to port %d,error %d\n",6,error);
break;
};
fputc(data_buf[Num_byte],log_handle[0]);
117
MP2X.C
17 / 19
};
op_count[0]++;
fprintf( log_handle[0],"\n");
Num_byte = (int)sio_oqueue(6);
hang(10);
while ( Num_byte != (int)sio_oqueue(6)) {Num_byte = (int)sio_oqueue(6);hang(20);};
sio_close(6);
if (error = (sio_ioctl(6,B4800,BIT_8| P_NONE| STOP_1)) != 0) /*set all port to 9600,8 bits,None, 1 stop bit*/
fprintf(sys_handle, "Port %d error %d",6,error);
return;
};
sio_lctrl(6, 3);
/* set DTR RTS on
*/
sio_flowctrl(6,0);
sio_open(6);
sio_flush(6,2);
}
//*********************************************************************************************
// Main module to call communications subroutines
//*********************************************************************************************
void main( int argc, char *argv[ ], char *envp[ ] )
{ int ESC = 0;
int j,k;
//FILE *com_file;
struct find_t temp;
//Screen initialisation - writes data-count columns to the screen
printf(" initialising screen \n");
Init_Screen();
// Parameter initialisation - declares file names etc.
printf(" initialising parameters \n");
Initialize();
// Initialise ports on P747 module - port6 is set to 4800
//
- port 7 - 21 set to 9600
printf(" initialising IO \n");
118
MP2X.C
18 / 19
delay(3000);
Init_MP();
if (Total_com) Port1_WR();
// start the Horizotal HC11
j=0; // counter to update screen
k=0; // counter to read compass
if (argc > 1 ) check_com_line(argv[1]);
sio_flush(6,2); // Flush compass
while (!ESC)
{
if (kbhit() != 0)
{
if (getch()==27) ESC = 1;
};
//
//
//
if
readdata checks for op.dat and reads the data if the file is valid and
exists. It thens sends the data to the HC11s. Rece_and _Write then waits
for the response from the HC11's and writes it to the file IP07.dat.
(readdata() == 1)
{ Rece_and_Write();
}
// Reads and writes compass data to file every 5 loops. File = IP06.dat
// Also checks for file OP06.dat and displays data if available.
else
if (k > 10)
{ //printf("
read port 6
");
Port6_WR();
if (Total_com) Port1_WR();
if (_dos_findfirst("Flush.dat" ,_A_NORMAL ,&temp) == 0 )
{
remove("Flush.dat");
for (k= 6;k<22;k++) sio_flush(k,2);
printf("\n******************Flush all the Ports*********************
};
k=0;
hang(2);
}
\n");
119
MP2X.C
19 / 19
else
{
if (j > 100)
{
Init_Screen();
j=0;
}
else
{
if (ip_count[5] > 30000)
for (k = 0; k < 16 ; k++)
{ip_count[k]=0; op_count[k]=0; };
};
hang(5);
} ;
j++;
k++;
// Updates screen every 100 counts.
}
CloseAllPort(); // Closes the multiport ports.
CloseAllFile(); // Closes the log files - System.log, Data.Log.
return;
}
120
lfi003.c
1 / 12
/*##########################################################################*/
/*#
#*/
/*#
HC11 communications and control routine
#*/
/*#
#*/
/*#
Left Foot Rotation - Channel #
#*/
/*#
#*/
/*##########################################################################*/
#define WANT_PACKET
#include <f1board.h>
#include <f1inttes.h>
extern unsigned int mcount,
/* millisecond counter
sec,
/* seconds byte of counter
msec;
/* Timing variables
*/
*/
*/
long int count1,

/* High byte of encoder
count2,
/* Low byte of encoder
posn[4];
/* Historical position vector
/* pos[3] = current position to PC */
int
run = 0;
long int time[4],

sex,
msex;
*/
*/
*/
/* Main loop flag

/* Historical time vector
/* Temporary seconds storage
/* temporary millisecond storage
*/
*/
*/
*/
/* Communications variables
int pos_diff_1,
pos_diff_2,
ROLLI,
ROLLO;
int
int
i;
term = 0,
step[4],
chron[4],
*/
/* POS2 - POS1
*/
/* POS3 - POS1
*/
/* Inner roll switch status 1 = Off */
/* Outer roll switch status 1 = Off */
/* temp counter
/* velocity flag
/* temp position vector
/* temp time vector
*/
*/
*/
*/
121
lfi003.c
2 / 12
rate;
/* temp velocity variable
*/
int wait = 0,
/* thinking */
zero = 1,
/* Position Zero Command */
cal = 2,
/* Calibration Command
*/
stay = 3,
/* Rough position holding command - no integral */
hold = 4,
/* Fine position holding routine - integral */
move = 5,
/* Velocity control command */
stop = 6,
/* Stop */
crash = 7;
/* Going to crash command
*/
int contr = 127,
contr_o = 127,
pos_e = 0,
pos_t = 0;
int min_up = 165,
min_dn = 90,
ZEDS = 0,
LAT_ENABLE = 0,
FOOT_LIM = 3,
roll_comp = 0;
/* calculated control value */

/* output integer control value */
/* position error */
/* target position */
/*minium value to go up and down */
/*####################################################################*/
/*#
Update Global Variables
#*/
void update(void)
{
msex = msec;
sex = sec;
count1 = peek(0x1820);
ROLLO = peek(0x1823) & 0x02;
ROLLI = peek(0x1823) & 0x01;
/*
/*
/*
/*
Read
Read
Read
Read
high count byte */

low count byte */
outer roll switch */
inner roll switch */
posn[1] = posn[2];
/* Update position vector */
posn[2] = posn[3];
posn[3] = count2 + count1 * 256;
POS_OUT = posn[3];
122
lfi003.c
time[1] = time[2];
time[2] = time[3];
time[3] = sex + msex * 1000;
3 / 12
/* Update time vector */
pos_diff_1 = posn[3] - posn[2];

/* Calculate position diffs */
STAT_OUT = ROLLI + ROLLO + ZEDS;

if (LAT_ENABLE == 4) STAT_OUT = STAT_IN;
}
/*####################################################################*/
void cycle()
{
poke (0x1821, 0x01);
while (CMD_IN == wait)
{
poke (0x1820, 0x00);
msleep(500);
poke (0x1820, 0xff);
msleep(500);
}
poke (0x1820, 0x7f);
}
/*####################################################################*/
void reset_c()
{
poke (0x1822, 0x01);
}
/*####################################################################*/
void calibrate()
{
ZEDS = 0;
CMD_OUT = wait;
poke(0x1821,0x01);
VEL_OUT = 01;
123
lfi003.c
4 / 12
/* Move up until limit switch goes off */

if ( (ROLLI < 1) && (CMD_IN == cal) )
{
while ( (ROLLI < 1) && (CMD_IN == cal) )
{
poke(0x1820, 198);
update();
}
msleep(1000);
poke(0x1820, 127);
}
/* Move down until it comes on */
while (( ROLLI > 0) && (CMD_IN == cal) )
{
poke(0x1820, 65);
update();
}
/* Zero position counter
*/
poke (0x1822, 0x01);

poke(0x1820, 127);
/*----------------------------------------------------------------------*/
/* Find minimum value for valve control
in down direction
*/
min_dn = 90;
term = 0;
update();
step[1] = 0;
step[2] = 0;
step[3] = 0;
124
lfi003.c
5 / 12
chron[1] = 0;
chron[2] = 0;
chron[3] = 0;
while ( (posn[3] < 20) && (CMD_IN == cal))

{
update();
step[1] = step[2];
step[2] = step[3];
chron[1] = chron[2];
step[3] = peek(0x1821) + peek(0x1820) * 256;
chron[3] = sex * 1000 + msex;
if ( term < 1 )
{
term = 1;
rate = ((step[3] - step[1]) * 1000) / (chron[3] - chron[1]);
if ( rate < 3 )
{
min_dn = min_dn - 2;
poke (0x1820, min_dn);
VEL_OUT = min_dn;
term = 0;
msleep(200);
}
}
}
125
lfi003.c
6 / 12
poke (0x1820, 0x7f);

poke (0x1822, 0x01);
/*----------------------------------------------------------------------*/
/* Find minimum value for valve control
in up direction
*/
min_up = 155;
update();
step[1] = 0;
step[2] = 0;
step[3] = 0;
chron[1] = 0;
chron[2] = 0;
chron[3] = 0;
term = 0;
while ( (posn[3] > -20) && (CMD_IN == cal))
{
update();
step[1] = step[2];
step[2] = step[3];
step[3] = peek(0x1821) + peek(0x1820) * 256;
chron[3] = sex * 1000 + msex;
if ( term < 1 )
{
term = 1;
126
lfi003.c
7 / 12
rate = ((step[3] - step[1]) * 1000) / (chron[3] - chron[1]);
if ( rate > -3 )
{
min_up = min_up + 2;
term = 0;
poke (0x1820, min_up);
VEL_OUT = min_up;
msleep(200);
}
}
}
poke (0x1820, 0x7f);
poke (0x1822, 0x01);
/*----------------------------------------------------------------------*/
poke(0x1820, 0x7f);
pos_t = -45;
/* Hold at pos_t */
while (CMD_IN == cal)
{
update();
if((STAT_IN == 9) && (ZEDS == 0) )
{
poke (0x1822, 0x01);
pos_t = -12;
ZEDS = 9;
}
contr = 127;
if (posn[3] == -40) CMD_OUT = cal;
update();
127
lfi003.c
8 / 12

if
if
if
if
if
if
if
if
(
(
(
(
(
(
(
(
pos_e
pos_e
pos_e
pos_e
pos_e
pos_e
pos_e
pos_e
<
<
<
<
>
>
>
>
-1
-3
-6
-9
1
3
6
9
)
)
)
)
)
)
)
)
contr
contr
contr
contr
contr
contr
contr
contr
=
=
=
=
=
=
=
=
min_dn;
min_dn min_dn min_dn min_up;
min_up +
min_up +
min_up +
2;
4;
8;
2;
4;
8;
contr_o = contr;
VEL_OUT = contr_o;
poke (0x1820, contr_o);
}
poke(0x1820, 0x7f);
}
/*####################################################################*/
void stance()
{
while (CMD_IN == stay)
{
update();
contr = 127;
LAT_ENABLE = STAT_IN & 4;
FOOT_LIM = STAT_IN & 3;
if ( (FOOT_LIM < 3) & (FOOT_LIM > 0) )
{
if (FOOT_LIM < 2 & LAT_ENABLE == 4)
{
128
lfi003.c
9 / 12
roll_comp = roll_comp + 1;
if (roll_comp > 15) roll_comp = 15;
}
else
{
roll_comp = roll_comp - 1;
if (roll_comp > 15) roll_comp = 15;
}
}
pos_t = POS_IN;
if ( LAT_ENABLE == 4) pos_t = POS_IN + roll_comp;
if ( LAT_ENABLE == 0 ) roll_comp = 0;
if ( pos_e < -1 ) contr = min_dn;
if ( pos_e > 1 ) contr = min_up;
contr_o = contr;
poke (0x1820, contr_o);
CMD_OUT = CMD_IN;
VEL_OUT = /* POS_IN */
STAT_OUT = STAT_IN;
LAT_ENABLE;
}
poke(0x1820, 0x7f);
}
/*####################################################################*/
void place()
{
129
lfi003.c
10 / 12
poke(0x1821,0x01);
poke(0x1820, 64);
VEL_OUT = 64;
}
/*####################################################################*/
void speed()
{
}
/*####################################################################*/
void die()
{
poke(0x1821,0x01);
poke(0x1820, 199);
VEL_OUT = 199;
}
/*####################################################################*/
void cut()
{
poke (0x1820, 0x7f);
VEL_OUT = 0x7f;
ZEDS = 0;
}
/*####################################################################*/
main()
{
/* Initialise primary variables */
run = 1;
mcount = 0;
STAT_OUT = 0;
VEL_OUT = 128;
CMD_IN = 6;
sec = 0; /* Reset seconds timer */
msec = 0; /* Reset millisecond timer */
130
lfi003.c
11 / 12
time[0]
time[1]
time[2]
time[3]
=
=
=
=
0;
0;
0;
0;
posn[0]
posn[1]
posn[2]
posn[3]
=
=
=
=
0;
0;
0;
0;
/* Initialise HC11 */
poke (0x1820, 0x7f);
poke (0x1821, 0x01);
poke (0x1822, 0x01);
/* Set analogue outputto zero */

/* Enable analogue output */
/* Set encoder count to zero */
/**********************************************************************/
/* Start - Main Loop - while run > 0
*/
/**********************************************************************/
while( run > 0)
{
update();
/* updates position and other standard information */
if ( CMD_IN == wait )
if ( CMD_IN == zero )
if ( CMD_IN == cal )
if ( CMD_IN == stay )
if ( CMD_IN == hold )
if ( CMD_IN == move )
if ( CMD_IN == crash)
if ( CMD_IN == stop )
CMD_OUT = CMD_IN;
cycle();
reset_c();
calibrate();
stance();
place();
speed();
die();
cut();
}
/* Check to see if command is available and determine what command is */
/**************************************************************************/
/* End - Main Loop
*/
131
lfi003.c
12 / 12
/**************************************************************************/
poke(0x1820, 0x7f);
}
132
roll2.c
1 / 5
/*
* Wheeled Robot Control Software
*
*
*/
#include <f1intsvc.h>
extern unsigned int mcount, sec;
main()
{
int i, tim1, tim2;
int
posn[3][5], count1_1, count1_2, count2_2, count2_1, acc[3];
int switches, sw1, sw2, sw3, sw4, sw5, dirn, gear;
int yaw_diff = 0, yaw_sum = 0, contr = 0, yaw_rate = 0, yaw_old;
int
acc1_save, event, left = 0, right = 0, straight = 0, reset = 0;
int out, zero[1, 2, 4, 8];
mcount = 0;
sec = 0;
tim1 = 0;
tim2 = 0;
acc[1] = 0;
acc[2] = 0;
posn[1][1] = 0;
posn[1][2] = 0;
posn[1][3] = 0;
posn[2][1] = 0;
posn[2][2] = 0;
posn[2][3] = 0;
poke(0x1822, 0x01);
poke(0x1812, 0x01);
poke(0x1811, 0x01);
poke(0x1821, 0x01);
133
roll2.c
2 / 5
poke(0x1810, 0xa0);
poke(0x1820, 0xa0);
poke(0x1813, 0x00);
poke(0x1823, 0x00);
switches = 255;
while ( switches == 255 )
{
switches = peek(0x1823);
}
msleep (2000);
poke(0x1813, 0x03);
poke(0x1823, 0x03);
poke(0x1822, 0x01);
poke(0x1812, 0x01);
acc[1] = 0;
acc[2] = 0;
gear = 0x60;
dirn = 1;
for( i=0; i < 300; i++)
{
/***********************************************************************************************/
/*
*/
/*
Data Aquisition
*/
/*
*/
/***********************************************************************************************/
count1_1
count1_2
count2_1
count2_2
=
=
=
=
peek(0x1810);
peek(0x1811);
peek(0x1820);
peek(0x1821);
/*
/*
/*
/*
Read
Read
Read
Read
high count byte */

low count byte */
high count byte */
low count byte */
134
roll2.c
3 / 5
posn[1][1] = posn[1][2];
posn[1][2] = posn[1][3];
posn[1][3] = count1_2 + count1_1 * 256;
posn[2][1] = posn[2][2];
posn[2][2] = posn[2][3];
posn[2][3] = count2_2 + count2_1 * 256;
switches = peek(0x1823);
sw1 = switches & 0x01;
if (posn[1][3] > 10000)
{
acc[1] = acc[1] + 1;
poke(0x1812, 0x01);
posn[1][3] = posn[1][3] - 10000;
}
if (posn[1][3] < -10000)
{
acc[1] = acc[1] - 1;
poke(0x1812, 0x01);
posn[1][3] = posn[1][3] + 10000;
}
if (posn[2][3] > 10000)
{
acc[2] = acc[2] + 1;
poke(0x1822, 0x01);
posn[2][3] = posn[2][3] - 10000;
}
if (posn[2][3] < -10000)
{
135
roll2.c
4 / 5
acc[2] = acc[2] - 1;
poke(0x1822, 0x01);
posn[2][3] = posn[2][3] + 10000;
}
yaw_old = yaw_diff;
yaw_diff = dirn * (acc[1]*10000
+ posn[1][3]) + (acc[2]*10000 + posn[2][3]);
yaw_rate = yaw_diff - yaw_old;

yaw_sum = yaw_sum + yaw_diff;
if (yaw_diff > 28000 ) yaw_diff = 28000;
if (yaw_diff < -28000 ) yaw_diff = -28000;
if (yaw_sum > 28000 ) yaw_sum = 28000;
if (yaw_sum < -28000 ) yaw_sum = -28000;
/***********************************************************************************************/
/*
*/
/*
Behaviour of Open Loop Wheel
*/
/*
*/
/***********************************************************************************************/
if( i > 150 ) gear = 0xdd;
/***********************************************************************************************/
/*
*/
/*
PID Control
*/
/*
*/
/***********************************************************************************************/
contr = 130 + (yaw_diff)/2 + (yaw_sum)/10
+ 1.5*(yaw_rate);
if (contr < 0 ) contr = 0;

if (contr > 255 ) contr = 255;
poke(0x1810, gear);
poke(0x1820, contr);
136
roll2.c
5 / 5
printf( "%d,%d,%d,%d,%d,%d,%d,%d\r",
acc[1], posn[1][3], acc[2], posn[2][3], yaw_diff, contr, yaw_sum, yaw_rate, out);
msleep(5);
}
/***********************************************************************************************/
/*
*/
/*
Shut-down
*/
/*
*/
/***********************************************************************************************/
poke(0x1810, 0xa0);
poke(0x1820, 0xa0);
msleep (2000);
poke(0x1823, 0);
}
137
PC_RS13.C
1 / 6
/*
*
HC11 Communications Package
*/
#include <stdio.h>
#include <stdlib.h>
#include <conio.h>
/*#include <math.h>*/
#include <dos.h>
#include <head-c.h>
/* P747 file with sio prototypes */
int
char
int
port_no[100];
/* Port number vector */
buffer[1000];
/* Input Buffer */
buf_ptr,
/* Pointer to the buffer */
p,
/* Port number pointer */
D_POS = 0,
/* Desired Position */
D_VEL = 0,
/* Desired Velocity */
A_POS = 0,
/* Actual position */
LIM = 0;
void
{
int
int
char
char
FILE
char
char
main()
i, c, port, n, k, l, j, t, x;
total_port;
/* total number of ports */
data[100], q, key; /* input buffer */
rawin[100];
/* read back hc11 response */
*output;
/* output file */
TX_pack[15] = "PTX000000"; /*Transmit Packet */
RX_pack[15] = "000000000"; /* Incomming Packet */
char
DP_C1, DP_C2, DP_C3, DP_C4,

/* Temp characters for pos */
DV_C1, DV_C2, DV_C3, DV_C4,
/* Temp characters for vel */
LL_C;
/* Temp character for lim */
int
DP_I1, DP_I2, DP_I3, DP_I4,

/* Temp integers for pos */
DV_I1, DV_I2, DV_I3, DV_I4,
/* Temp integers for vel */
LL_I;
/* Temp integer for lim */
int
split[5],
block[5],
chk_sum,
TX_ERR,
/* temporary storage */
138
PC_RS13.C
2 / 6
ABS_ERR;
output = fopen ("c:\\develop\\robot\\out.dat", "w");
/* Open the output file to check what comes out */
if ( output == NULL )
{
printf( " Could not open output file !! \n\r");
printf( " Program terminated.....\n\r");
exit(0);
}
clrscr();
/* Clears the text screen */
printf("Reset the serial ports....\n");

if ( sio_reset() <= 0 )
{
printf("No Serial Driver Found !\n");
exit(0);
}
/* sio_reset() resets the serial ports, the
result should be a positive integer. If not
it exits the program */
total_port = sio_getports(port_no,99);
if( total_port <= 0 )
{
printf( " Could not find any ports !! \n\r");
exit(0);
}
/* checks for the number of ports on the
pc747 module and displays the result */
printf("Total ports = %d \n", total_port);
p = port_no[0];
139
PC_RS13.C
3 / 6
/* Before using, each port should be OPENed first */
printf("Setting all ports 9600, N, 8, 1....\n");

for (port = 0; port < total_port; port++)
{
n = port_no[port];
if (sio_ioctl(n,B9600,BIT_8| P_NONE| STOP_1) != 0)
{
printf("Port #%d IOCTL error !\n", n);
exit(0);
}
/* set line settings for all ports,
exit if there is a line setting error */
sio_lctrl(n, 3);
/* set DTR RTS on
*/
sio_flowctrl(n,0);
printf("Open Port #%d\n\r", port);
sio_open(n);
/* open the port */
}
/*::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::*/
k = 0;
A_POS = 0;
TX_ERR = 0;
ABS_ERR = 0;
while ( kbhit() == 0 && TX_ERR <= 1)
{
k = k+ 1;
140
PC_RS13.C
4 / 6
/* . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .*/
D_POS = k;
D_VEL = k;
if ( k >= 200 ) LIM = 1;
split[3]
split[2]
split[1]
split[0]
split[4]
=
=
=
=
=
( D_POS / 1000 );
( D_POS - split[3] * 1000) / 100;
( D_POS - split[3] * 1000 - split[2] * 100) / 10;
( D_POS - split[3] * 1000 - split[2] * 100 - split[1] * 10);
split[0] + split[1] + split[2] + split[3];
/* printf("%d
%d %d %d %d
", k, split[3], split[2], split[1], split[0]); */
i = 10;
TX_pack[3]
TX_pack[4]
TX_pack[5]
TX_pack[6]
TX_pack[7]
=
=
=
=
=
split[3]
split[2]
split[1]
split[0]
split[4]
+
+
+
+
+
48;
48;
48;
48;
33;
for ( j = 0; j <= 7; j++)

{
sio_putch(p, TX_pack[j]);
/*printf( "%c", TX_pack[j]);*/
delay (2);
}
sio_putch(p, '\r');
delay (20);
l = -1;
/*
while ( i >= 0 )
{
l = l + 1;
delay (2);
i = sio_getch(p);
RX_pack[l] = i;
if( RX_pack[l] >= 0 ) printf("%c", RX_pack[l]); */
141
PC_RS13.C
5 / 6
}
/* read in characters from port "p" one

at a time and print them to the screen. sio_getch
returns the character 0 to 255 if a character is
available */
if ( RX_pack[0] == 'P' && RX_pack[1] == 'T')
{
block[0] = ( RX_pack[3] - 48 );
block[1] = ( RX_pack[4] - 48 );
block[2] = ( RX_pack[5] - 48 );
block[3] = ( RX_pack[6] - 48 );
chk_sum = block[0] + block[1] + block[2] + block[3];
if (chk_sum == RX_pack[7] - 33)
{
A_POS = block[0] * 1000 + block[1] * 100 + block[2] * 10 + block[3];
}
else
{
TX_ERR = TX_ERR + 1;
A_POS = 9999;
}
if( (2*A_POS-D_POS) >= 3 )
{
ABS_ERR = ABS_ERR+1;
}
}
printf("
PRX%d \n ", A_POS);
}
getch();
printf(" Loops = %d\n\r", k);
printf(" Error = %d\n\r", ABS_ERR);
/*:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::*/
142
PC_RS13.C
sio_timeout(18 * 5);
6 / 6
/* set timeout value = 5 sec */
printf("\nFlush all ports input/output queue....\n");

{
n = port_no[port];
sio_flush(n, 2);
/* clears the input/output queue */
sio_close(n);
}
sio_reset();
fclose(output);
}
143
HCRS_13.C
1 / 4
/*##########################################################################*/
/*#
#*/
/*#
HC11 communications and control routine
#*/
/*#
#*/
/*##########################################################################*/
#define WANT_PACKET
#include <f1intsvc.h>
extern unsigned int mcount,
/* millisecond counter
sec,
/* seconds byte of counter
msec;
/* Timing variables
main()
{
long int count1,
count2,
posn[4];
int
int
int
/* High byte of encoder

/* Low byte of encoder
/* Historical position vector
run;
long int time[4],

sex,
msex;
*/
*/
*/
*/
*/
*/
/* Main loop flag

/* Historical time vector
/* Temporary seconds storage
/* temporary millisecond storage
POS1,
POS2,
POS3,
STAT_IN,
STAT_OUT,
CHKSM,
ROLLI,
ROLLO;
/*
/*
/*
/*
/*
/*
/*
pos_diff_1,
pos_diff_2;
/* POS2 - POS1
/* POS3 - POS1
*/
*/
*/
*/
/* Position / 100
Position / 10
*/
Position / 1
*/
Input Status
*/
Out Status
*/
TX Checksum
*/
Inner roll switch status 1 = Off */
Outer roll switch status 1 = Off */
*/
*/
*/
/* Initialise primary variables */

run = 1;
144
HCRS_13.C
2 / 4
mcount = 0;
STATUS = 0;
sec = 0; /* Reset seconds timer */
msec = 0; /* Reset millisecond timer */
time[0]
time[1]
time[2]
time[3]
=
=
=
=
0;
0;
0;
0;
posn[0]
posn[1]
posn[2]
posn[3]
=
=
=
=
0;
0;
0;
0;
/* Initialise HC11 */
poke (0x1820, 0x7f);
poke (0x1821, 0x01);
poke (0x1822, 0x01);
/* Set analogue outputto zero */

/* Enable analogue output */
/* Set encoder count to zero */
/**********************************************************************/
/* Start - Main Loop - while run > 0
*/
/**********************************************************************/
while( run > 0)
{
msex = msec;
sex = sec;
/* Read high count byte */
/* Read low count byte */
ROLLO = peek(0x1823) & 0x02; /* Read outer roll switch */
ROLLI = peek(0x1823) & 0x01; /* Read inner roll switch */
posn[1] = posn[2];
posn[2] = posn[3];
posn[3] = count2 + count1 * 256;
time[1] = time[2];
/* Update time vector */
145
HCRS_13.C
3 / 4
time[2] = time[3];
time[3] = sex + msex * 1000;
STATUS = ROLLI
POS1 = posn[3]
POS2 = posn[3]
POS3 = posn[3]
CHKSM = STATUS
+
/
^
/* Calculate position diffs */
ROLLO;
100;
POS1 * 100;
POS1 - POS2 * 10;
POS1 ^ POS2 ^ POS3;
/* Check to see if command is available and determine what command is */

if(packet)
{
INCHK = inbuffer[1]înbuffer[2]înbuffer[3]înbuffer[4];
OUTPUT[0]
OUTPUT[1]
OUTPUT[2]
OUTPUT[3]
OUTPUT[4]
OUTPUT[5]
=
=
=
=
=
=
69;
STATUS;
POS1 + 48;
POS2 + 48;
POS3 + 48;
CHKSM + 48;
if ( INCHK != inbuffer[6] )
{
OUTPUT[0] = 69;
};
for(i = 0; i < 6; i++)
{
printf("%c", OUTPUT[i]);
}
printf("%c%c", 0x0d,0x0a);
clrbuff();
inbuffer[0] = 0x00;
}
}
146
HCRS_13.C
4 / 4
/**************************************************************************/
/* End - Main Loop
*/
/**************************************************************************/
poke(0x1820, 0x7f);
}
147
MULTI_RS.C
#include
#include
#include
#include
#include
#include
1 / 4
<stdio.h>
<conio.h>
<stdlib.h>
<string.h>
<time.h>
<io.h>
int main()
{
int a,
i,
status,
e_level,
total = 0;
/* response from comms functions

/* basic counter
/* response from receive routine
/* error level on reception
char test[7];
/*Test character string array
*/
*/
*/
*/
*/
time_t t_tmp1,
t_tmp2;
/* Initialise test array
test[0]
test[1]
test[2]
test[3]
test[4]
test[5]
=
=
=
=
=
=
88;
/* X */
83;
/* S */
49;
/* 1 */
50;
/* 2 */
51;
/* 3 */
test[1]^test[2]^test[3]^test[4];
/* Initialise joint output strings
*/
/* Checksum */
*/
for (i = 0; i < 6; i++)

{
RTE_OUT[i]
LTE_OUT[i]
RFO_OUT[i]
LFO_OUT[i]
RFI_OUT[i]
LFI_OUT[i]
=
=
=
=
=
=
test[i];
test[i];
test[i];
test[i];
test[i];
test[i];
148
MULTI_RS.C
RKE_OUT[i]
LKE_OUT[i]
RHE_OUT[i]
LHE_OUT[i]
RHA_OUT[i]
LHA_OUT[i]
RHR_OUT[i]
LHR_OUT[i]
2 / 4
=
=
=
=
=
=
=
=
test[i];
test[i];
test[i];
test[i];
test[i];
test[i];
test[i];
test[i];
}
/* Reset system time storage variables
*/
time(&t_tmp1);
time(&t_tmp2);
/* Loop until seconds clock over */
while ( (t_tmp2 - t_tmp1) < 1) time(&t_tmp2);
/* Reset time variables
time(&t_tmp1);
time(&t_tmp2);
*/
/* Main transmission loop

while ( (t_tmp2 - t_tmp1) < 10)
{
total ++;
*/
a =send_output();
clrscr();
if ( a != 1)
{
printf(" write error %d",a);
exit(0);
}
else
{
149
MULTI_RS.C
3 / 4
printf ("Data written");
}
a = 0;
while ((a < 2000) && ((status =get_input()) == 0)) a++;
if ( a == 2000)
{ printf("time out, please increase the wait value");
exit(0);
};
if (status != 1)
{
printf("error in receiver %d ",status);
exit(0);
};
for (i = 0; i < 6; i++)
{
if(
if(
if(
if(
if(
if(
if(
if(
if(
if(
if(
if(
if(
if(
RTE_OUT[i]
LTE_OUT[i]
RFO_OUT[i]
LFO_OUT[i]
RFI_OUT[i]
LFI_OUT[i]
RKE_OUT[i]
LKE_OUT[i]
RHE_OUT[i]
LHE_OUT[i]
RHA_OUT[i]
LHA_OUT[i]
RHR_OUT[i]
LHR_OUT[i]
!=
!=
!=
!=
!=
!=
!=
!=
!=
!=
!=
!=
!=
!=
test[i]
test[i]
test[i]
test[i]
test[i]
test[i]
test[i]
test[i]
test[i]
test[i]
test[i]
test[i]
test[i]
test[i]
)
)
)
)
)
)
)
)
)
)
)
)
)
)
e_level
e_level
e_level
e_level
e_level
e_level
e_level
e_level
e_level
e_level
e_level
e_level
e_level
e_level
=
=
=
=
=
=
=
=
=
=
=
=
=
=
e_level
e_level
e_level
e_level
e_level
e_level
e_level
e_level
e_level
e_level
e_level
e_level
e_level
e_level
+
+
+
+
+
+
+
+
+
+
+
+
+
+
1;
1;
1;
1;
1;
1;
1;
1;
1;
1;
1;
1;
1;
1;
}
/*
printf("Rte is %s
printf("Lte is %s
printf("Rfo is %s
CRC is %d",RTE_IN, crc(RTE_IN+1,5) - RTE_IN[6]);

CRC is %d",LTE_IN, crc(LTE_IN+1,5) - LTE_IN[6]);
CRC is %d",RFO_IN, crc(RFO_IN+1,5) - RFO_IN[6]);
150
MULTI_RS.C
printf("Lfo
printf("Rfi
printf("Lti
printf("Rke
printf("Lke
4 / 4
is
is
is
is
is
%s
%s
%s
%s
%s
CRC
CRC
CRC
CRC
CRC
is
is
is
is
is
%d",LFO_IN,
%d",RFI_IN,
%d",LFI_IN,
%d",RKE_IN,
%d",LKE_IN,
crc(LFO_IN+1,5)
crc(RFI_IN+1,5)
crc(LFI_IN+1,5)
crc(RKE_IN+1,5)
crc(LKE_IN+1,5)
LFO_IN[6]);
RFI_IN[6]);
LFI_IN[6]);
RKE_IN[6]);
LKE_IN[6]);
printf("Rhe is %s
CRC is %d",RHE_IN, crc(RHE_IN+1,5) - RHE_IN[6]);
printf("Lhe is %s
CRC is %d",LHE_IN, crc(LHE_IN+1,5) - LHE_IN[6]);
printf("Rha is %s
CRC is %d",RHA_IN, crc(RHA_IN+1,5) - RHA_IN[6]);
printf("Lha is %s
CRC is %d",LHA_IN, crc(LHA_IN+1,5) - LHA_IN[6]);
printf("Rhr is %s
CRC is %d",RHR_IN, crc(RHR_IN+1,5) - RHR_IN[6]);
printf("Lhr is %s
CRC is %d",LHR_IN, crc(LHR_IN+1,5) - LHR_IN[6]);
*/
//printf("Rte is %d\n",( RTE_IN[3]-30)* 100 +(RTE_IN[4]-30)*10 + RTE_IN[5] - 30);
time(&t_tmp2);
}
printf("\nTotal pack is %d.",total);
return 0;
}
151
DOWNLOAD.C
#include
#include
#include
#include
#include
#include
1 / 8
/*
*
HC11 Communications Package
*/
<stdio.h>
<stdlib.h>
<conio.h>
<string.h>
<dos.h>
<head-c.h>
/* P747 file with sio prototypes */
int
char
int
port_no[100];
/* Port number vector */
buffer[1000];
/* Input Buffer */
buf_ptr,
/* Pointer to the buffer */
p;
int jj,kk;
char
file_name[20];
void
main()
{
int
i, c, port, n, k, j, t, z;
int
total_port, h, g;
/* total number of ports */
int
done;
/* flag for successful load */
char
data[100], key, q;
/* input buffer */
char
rawin[100];
/* read back hc11 response */
FILE
*input, *output,
/* input file */
*analog;
int
char
int
GAS;
tome[10];
tyme;
int
char
/* port number for gas plasma display */

/* result of itoa conversion */
/* integer which is converted to character */
messlen1, messlen2;
*message1, *message2;
/* message lengths of GAS outputs */

/* gas outputs */
z = 0;
message1
message2
messlen1
messlen2
=
=
=
=
"\rLoad S19 card ";

"\r
16;
22;
";
/*
/*
/*
/*
header for gas display announcement */

blank string for gas display */
length of header */
length of blank string */
output = fopen ("c:\\f1_dev\\out.dat", "w");
152
DOWNLOAD.C
2 / 8
/* Open the output file to check what comes out */
if ( output == NULL )
{
printf( " Could not open output file !! \n\r");
exit(0);
}
clrscr();
/* Clears the text screen */
printf("Reset the serial ports....\n");

if ( sio_reset() <= 0 )
{
printf("No Serial Driver Found !\n");
exit(0);
}
/* sio_reset() resets the serial ports, the
result should be a positive integer. If not
it exits the program */
total_port = sio_getports(port_no,99);
if( total_port <= 0 )
{
printf( " Could not find any ports !! \n\r");
exit(0);
}
/* checks for the number of ports on the
pc747 module and displays the result */
printf("Total ports = %d \n", total_port);
GAS = 6;
/* sets the port number for the gas display */
153
DOWNLOAD.C
3 / 8
/*
printf("Which port do you want to down load to (0/1/2/3...)? ");
scanf("%d", &port);
p = port_no[port];
*/
/* prompts and scans for the download port */
/* Before using, each port should be OPENed first */
printf("Setting all ports 9600, N, 8, 1....\n");
{
n = port_no[port];
if (sio_ioctl(n,B9600,BIT_8| P_NONE| STOP_1) != 0)
{
printf("Port #%d IOCTL error !\n", n);
exit(0);
}
/* set line settings for all ports,
exit if there is a line setting error */
sio_lctrl(n, 3);
/* set DTR RTS on
*/
sio_flowctrl(n,0);
printf("Open Port #%d\n\r", port);
sio_open(n);
/* open the port */
}
if (sio_putb(GAS, "\nPorts Reset !", 15) != 15)
{
printf("GAS Display write error !\r");
}
154
DOWNLOAD.C
4 / 8
/* Send "Ports Reset" message to front Panel
display */
printf("S19 Downloader Version 102a.2cc.....Press any key to download\n");

key = NULL;
while ( kbhit() == 0)
{
if( (i = sio_getch(p)) >= 0 ) printf("%c", i );
}
/* read in characters from port "p" one
at a time and print them to the screen. sio_getch
returns the character 0 to 255 if a character is
available */
key = getch();
/* clear keyboard buffer */
/***************************************************************************/
/*
*/
/*
Loop to download s19 code
*/
/*
*/
/***************************************************************************/
for (p = 7; p < 22; p++)
{
h = 0;
switch (p)
{
case 7:strcpy(file_name,"lhe005.s19"); break;
case 8: strcpy(file_name,"rke005.s19"); break;
case 9: strcpy(file_name,"lha005.s19"); break;
case 10: strcpy(file_name,"lke005.s19"); break;
case 11: strcpy(file_name,"lte005.s19"); break;
case 12: strcpy(file_name,"lfi005.s19"); break;
case 13: strcpy(file_name,"rfi005.s19"); break;
case 14: strcpy(file_name,"rhe005.s19"); break;
case 15: strcpy(file_name,"rha005.s19"); break;
155
DOWNLOAD.C
5 / 8
case 16:
case 17:
case 18:
case 19:
case 20:
case 21:
default:
};
printf("\n
strcpy(file_name,"rhr005.s19");
strcpy(file_name,"lhr005.s19");
strcpy(file_name,"rte005.s19");
strcpy(file_name,"lfo005.s19");
strcpy(file_name,"rfo005.s19");
strcpy(file_name,"analog.s19");
exit(0);
break;
break;
break;
break;
break;
break;
Loading %s to channel %d\n ",file_name,p);
input = fopen ( file_name, "r" );

/* Open the input file */
if ( input == NULL )
{
printf( " Could not open input file !! \n\r");
exit(0);
}
tyme = p;
tome[9] = '\0';
itoa (tyme, tome, 10);
printf ("Loading port %s
\n", tome);
k = (sio_putb(GAS,message2,messlen2));
k = (sio_putb(GAS,message1,messlen1));
k = (sio_putb(GAS,tome,3));
/*printf( "Return value was %d \n", k);*/
/*
if (sio_putb(GAS, "\rLoading s19 code

", 19) != 19)
{
printf("GAS Display write error !\n");
}
/*
p = 20;
/*
q = 13;
*/
*/
156
DOWNLOAD.C
6 / 8
sio_write(p, "\rlooad t\r", 9 );

fprintf( output, "\rload t\r");
delay (500);
do {
c = fgetc(input);
/* Read teh character form the input file */
h = h + 1;
if ( h > 13) h = 1;
i = sio_putch(p, c);
/* Write the next character to the comms ports */
if
if
if
if
(
(
(
(
h
h
h
h
==
==
==
==
1) printf( "\r-");
5) printf( "\r\\");
9) printf( "\r|");
13) printf( "\r/");
/*putchar(c);*/
q = fputc(c, output);
delay(1);
for(jj=0; jj<100; jj++)
/* Display the character actually sent to the screen */

/* Store the character to the output check file */
{
for (kk=0; kk<1000; kk++)
{
}
}
} while (c != EOF);
i = sio_putch(p, EOF);
c =
j =
t =
for
13;
0;
0;
(g = 0; g < 100; g++) rawin[g] = ' ';
do {
157
DOWNLOAD.C
7 / 8
c = sio_getch(p);
/*putchar(c);*/
delay(25);
rawin[j] = c;
j++;
if (j >= 98)
{
printf(" \r\nOutput buffer overflow error ! ");
printf(" \r\nProgram terminated.....");
exit(0);
}
} while (c >= 0);
printf(" \rChecking feedback");

for (j = 0; j < 40; j++)
{
/* Check through */
/*putc(rawin[j], stdout);*/
/* feedback searching */
/* for the "done" response */
t = 0;
/* from the HC11 */
if(
if(
if(
if(
rawin[j+0]
rawin[j+1]
rawin[j+2]
rawin[j+3]
==
==
==
==
'd')
'o')
'n')
'e')
t
t
t
t
=
=
=
=
t+1;
t+1;
t+1;
t+1;
if( t == 4)
{
printf( " \rProgram successfully loaded !\n");
done = 1;
z = z + 1;
j = 41;
}
}
if (done != 1)
{
printf( " \rProgram load failed !\n");
158
DOWNLOAD.C
8 / 8
}
fclose (input);
}
/***************************************************************************/
/*
*/
/*
LEnde of loop to download s19 code
*/
/*
*/
/***************************************************************************/
fclose (output);
if (sio_putb(GAS, "\rS19 load complete", 19) != 19)
{
printf("GAS Display write error !\r");
}
sio_timeout(18 * 5);
/* set timeout value = 5 sec */
printf("\n %d ports loaded successfully. \n", z);

{
n = port_no[port];
sio_flush(n, 2);
/* clears the input/output queue */
sio_close(n);
}
sio_reset();
}
159
robio.H
1 / 2
/****************************************************
This is the input/output header
*****************************************************/
struct HC11_DATA
{ int status;
int cmd;
int position;
int velocity;
};
struct
struct
struct
struct
struct
struct
struct
HC11_DATA
HC11_DATA
HC11_DATA
HC11_DATA
HC11_DATA
HC11_DATA
HC11_DATA
RTE_O,RTE_I,LTE_O,LTE_I;
RFO_O,RFO_I,LFO_O,LFO_I;
RFI_O,RFI_I,LFI_O,LFI_I;
RKE_O,RKE_I,LKE_O,LKE_I;
RHE_O,RHE_I,LHE_O,LHE_I;
RHA_O,RHA_I,LHA_O,LHA_I;
RHR_O,RHR_I,LHR_O,LHR_I;
char RTE_OUT[8],RTE_IN[12],LTE_OUT[8],LTE_IN[12];
char RFO_OUT[8],RFO_IN[12],LFO_OUT[8],LFO_IN[12];
char RFI_OUT[8],RFI_IN[12],LFI_OUT[8],LFI_IN[12];
char RKE_OUT[8],RKE_IN[12],LKE_OUT[8],LKE_IN[12];
char RHE_OUT[8],RHE_IN[12],LHE_OUT[8],LHE_IN[12];
char RHA_OUT[8],RHA_IN[12],LHA_OUT[8],LHA_IN[12];
char RHR_OUT[8],RHR_IN[12],LHR_OUT[8],LHR_IN[12];
char analog_in[20];
char analog_out[20];
unsigned int analog_in1;
unsigned int analog_out1;
160
robio.H
2 / 2
char compass_data[20];
/* Char buff for reading Compass file */
char *Line_input(FILE *);
/* Read one line for a file*/
int get_input(void);
int send_output(void);
int crc(char *,int);
/* store all data to a file and pass to mp*/

/* CRC Check*/
/*copy all Data to output buffer and call send_output to write to a output file.*/
void To_HC11();
/* pack the data into the right format*/
void cov_data1(char *,struct HC11_DATA );
/* Convert data format*/
/* return all data into the input buffers from HC11 */
void From_HC11();
/* unpack data from HC11 */
void cov_data2(char *,struct HC11_DATA *);
/* Convert data format */
float compass(void);
/* Extract Compass data from a file*/
int horizon(void);
/* Extract Horizontal data from a file*/
int pp,rr,aa;
/* Vari. for Pitch,Roll,Press */
161
F1INTTES.H
/*
*
*
*
*
*
*
*
*
*
*
*
*
*/
1 / 10
f1intsvc.h
This include file contains the interrupt service routines
Created 07/25/94 by Chuck McManis from the mboard.h file.
Updated 07/26/94 to use the interrupt_handler pragma
08/04/94 Added ic1_svc, ic2_svc, do_userint().
ic1/ic2 are used for encoders.
do_userint() is used for pid drivers.
Modified and Updated 04/95 for the F1 board by Pete Dunster to
include LCD, Keypad and Serial packet handling
Modified and updated 07/09/97 for Robot development
/*
* This is a kind of really gross hack, but it has tremendous value on
* the limited memory of the miniboard. In this file I've put the
* interrupt service routines for the motors, beeper, and servos. I've
* also put in "stub" routines. If you define variables WANT_xxx where
* xxx is SERVOS, MOTORS, or BEEPER then the "real" interrupt service
* routine gets compiled in, otherwise a stub routine gets compiled in
* that saves space. This allows you to write a program that doesn't
* need the servos for example and not have to carry around the code
* for controlling them.
*/
#include <hc11.h>
#ifdef WANT_SERVOS
/* servo variables */
unsigned int
servo_position[3];
#endif
/* MAX of 3 servos */
#ifdef WANT_KEYBD
/* keyboard variables */
static unsigned
{
0x55, 0x50,
0x56, 0x51,
0x57, 0x52,
0x58, 0x53,
char keypatch[70]
0x4B, 0x46, 0x41,
0x4C, 0x47, 0x42,
0x4D, 0x48, 0x43,
0x4E, 0x49, 0x44,
=
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
162
F1INTTES.H
0x59,
0x5A,
0x20,
0x0A,
2 / 10
0x54,
0x30,
0x2E,
0x1A,
0x4F,
0x31,
0x32,
0x33,
0x4A,
0x34,
0x35,
0x36,
0x45,
0x37,
0x38,
0x39,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
};
#endif
#ifdef WANT_LCD
/* lcd screen variables
*/
unsigned char linechar;

#endif
#ifdef WANT_PACKET
/* SCI packet protocol variables */
unsigned char inbuffer[100], inptr, packet;
/* Variable for robot communication*/
int
int
int
int
int
int
int
POS_IN;
POS_OUT;
VEL1;
VEL2;
VEL_IN;
VEL_OUT;
CMD_IN,
CMD_OUT,
STAT_IN,
STAT_OUT,
CHKSM,
INCHK;
/* Demand position from PC

/* Current position in HC11
/* velocity /100
/* velocity - velocity / 100
/* Demand velocity from PC
/* Current velocity to PC
/* Command input 0 - 5 from PC
/* Command output 0 - 5 to PC
/* Input Status from PC
*/
/* Out Status to PC
*/
/* TX Checksum to PC
*/
/* RX Checksum from PC
char
int
int
int
OUTPUT[7];
POS1;
POS2;
POS3;
/*
/*
/*
/*
Output string
Position / 100
Position / 10
Position / 1
int
int
sign;
comtemp;
/* sign variable
/*temp variable
*/
*/
*/
*/
*/
*/
*/
*/
*/
*/
*/
*/
*/
*/
*/
163
F1INTTES.H
3 / 10
#endif
#ifdef WANT_MOTORS
/* Motor variables */
unsigned char
motor_ctl;
unsigned int
speed[4];
#endif
#ifdef WANT_STEPPERS
/* Stepper motor routines
/* Motor control high nybble on/off, low nybble dir */

/* Motor speed masks */
TBA */
#endif
/*
* initialize requested options. Note this is called from mb_init() before
* you're in "main()" so don't try to be clever and put your own stuff here.
* Note we only declare variables for the options that have been selected.
*/
void
opt_init()
{
#ifdef WANT_SERVOS
servo_position[0] = servo_position[1] = servo_position[2] = 0;
#endif
#ifdef USER_INIT
user_init();
#endif
#ifdef WANT_PACKET
SCCR2 = 0x2C;
#endif
return;
};
#ifdef WANT_SERVOS
struct servo_param {
unsigned int
neutral_value;
164
F1INTTES.H
unsigned char
4 / 10
step_value;
};
#ifndef
#define
#endif
#ifndef
#define
#endif
#ifndef
#define
#endif
#ifndef
#define
#endif
#ifndef
#define
#endif
#ifndef
#define
#endif
const
{
{
{
};
SERVO1_NEUTRAL
SERVO1_NEUTRAL
3050
SERVO1_STEP
SERVO1_STEP 17
/* Defines step size */
SERVO2_NEUTRAL
SERVO2_NEUTRAL
3050
SERVO2_STEP
SERVO2_STEP 17
SERVO3_NEUTRAL
SERVO3_NEUTRAL
3050
SERVO3_STEP
SERVO3_STEP 17
/* Defines 1525 uS neutral position */
struct servo_param parms[3]

SERVO1_NEUTRAL, SERVO1_STEP
= {
},
},
}
/*
* If requested, compile in the servo interrupt service handler routine.
*
* Position up to three servos on OC1, OC2, and OC3 (OC4 is used as the
* system tick interrupt). I've written this code a couple of times and
* this seems to be the most space efficient, in part because accessing
* OC1 stuff versus OC2/OC3 stuff is asymmetric. The code trys to not
* disturb any other bits in TCTL1 if a particular servo isn't being
* used so that the user can do whatever they want with the pin if it
* isn't driving a servo.
*/
void
do_servos()
165
F1INTTES.H
5 / 10
{
if (servo_position[0]) {
asm("
ldx #0x1000"); /* Point at I/O */
asm("
ldaa
#0x80");
/* Forcing stuff */
asm("
staa
0x0c,x");
/* OC1M = 0x80 */
asm("
staa
0x0d,x");
/* OC1D = 0x80 */
asm("
staa
0x0b,x");
/* Force it */
asm("
clra
");
/* Clear A */
asm("
staa
0x0d,x");
/* Clear the bit */
asm("
ldd 0x0e,x");
/* Setup to switch it */
asm("
addd
_servo_position");
asm("
std 0x16,x");
/* At T + n mSec */
asm("
tsx");
/* Restore X */
}
/*
* Check this out, we set both bits in TCTL1 which
* tell the system that effect of a compare is to
* set the pin to '1', then we reset just one bit
* changing TCTL1 so that the operation on compare is
* to clear the pin.
*/
asm("
ldx #0x1000"); /* Point at I/O */
asm("
bset
0x20,x,#0xc0"); /* TCTL1 = 0xc0 */
asm("
ldaa
#0x40");
/* Force only OC2 */
asm("
staa
0x0b,x");
/* Force it */
asm("
bclr
0x20,x,#0x40"); /* Clear bit */
asm("
ldd 0x0e,x");
asm("
addd
_servo_position+2");
std 0x18,x");
asm("
asm("
tsx");
/* Restore X */
}
asm("
ldx #0x1000"); /* Point at I/O */
asm("
bset
0x20,x,#0x30"); /* TCTL1 = 0x30 */
asm("
ldaa
#0x20");
/* Force only OC3 */
asm("
staa
0x0b,x");
/* Force it */
asm("
bclr
0x20,x,#0x10"); /* Clear bit */
asm("
ldd 0x0e,x");
asm("
addd
_servo_position+4");
166
F1INTTES.H
asm("
asm("
6 / 10
std 0x1a,x");
tsx");
/* Restore X */
}
return;
}
#else
/* if the interrupt handler isn't selected, compile in this stub routine */
void do_servos() { return; }
#endif
#ifdef WANT_MOTORS
/*
* If motors are selected, then compile in the PWM generator function.
*
* This is the PWM function for the motor. It uses PORT B but could be
* easily modified to use a different port. Since it clocks out 1 bit
* every system tick (1Khz system tick) the frequency at 50% is 500 hz.
*/
void
do_motors()
{
unsigned char
motor_mask,
motors;
int
i,j;
if ((motor_ctl & 0xf0) == 0) {
poke(MOTOR_ADR, 0); /* eliminates race condition by forcing off */
return; /* nothing to do */
}
motor_mask = 0x10; /* Mask information */
motors = motor_ctl;
/*
* For each motor we check to see if it is enabled in
* the motor_ctl variable, and if it is we then take
* its "speed" bitmask and effectively do a rotate right
* on it. We preserve the state of the low order bit so
* that we can add it back in after the shift (thus making
* it a rotation).
*/
for (i = 0; i < 4; i++, motor_mask = motor_mask << 1) {
167
F1INTTES.H
7 / 10
if ((motor_ctl & motor_mask) == 0)

continue;
j = speed[i] & 0x1; /* check the bit */
speed[i] = speed[i] >> 1; /* shift speed */
if (j)
speed[i] |= 0x8000; /* complete the rotate */
else
motors &= ~motor_mask; /* turn off the motor */
}
poke(MOTOR_ADR, motors);
return;
/* actually do it */
}
#else
/* else if motors aren't selected then compile in this stub routine */
void do_motors() { return; }
#endif
#pragma interrupt_handler irqi_svc
#ifdef WANT_KEYBD
/* The interrupt service routine that deals with the keypad */
void
irqi_svc()
{
extern unsigned char keypress;
unsigned char key;
asm("
pshx");
asm("
ldx #0x400");
asm("keyloop:: dex");
asm("
bne keyloop");
asm("
pulx");
key = peek(0x180C);
/* read character */
if((key & 0x40)== 0){
/* check valid */
key = (key & 0x3f);
keypress = keypatch[key];
/* get ASII char into global mem */
}
poke(0x180d,0);
/* clear interrupt
*/
}
#else
168
F1INTTES.H
8 / 10
/* stub routine for the button */

void irqi_svc() { }
#endif
#pragma interrupt_handler sci_svc
#ifdef WANT_PACKET
void sci_svc()
{
if(peek(0x102e) & 0x20){
inbuffer[inptr] = peek(0x102f);
inptr++;
if(inbuffer[inptr-1] == 0x0d || inptr > 8)
{
packet = 1;
/*receiving*/
INCHK = inbuffer[1]înbuffer[2]înbuffer[3]înbuffer[4]înbuffer[5];
if ( INCHK == 0xa || INCHK == 0xd ) INCHK ++;
if ( INCHK != inbuffer[6] )
{
CMD_IN = 101;
};
CMD_IN = (inbuffer[1] >> 4 ) & 0x7;
POS_IN = (inbuffer[1] & 7) * 100 + (inbuffer[2]
if ( (inbuffer[1] & 8) != 0 ) POS_IN = POS_IN *
VEL_IN = (inbuffer[4]&3) * 100 + (inbuffer[5] &
if (( inbuffer[4] & 4) != 0 ) VEL_IN = VEL_IN *
- 48 ) * 10 + (inbuffer[3] - 48 );
-1;
0x7f);
-1;
STAT_IN = (inbuffer[4]>>3) & 0xf;

/* comm -- only for test */
169
F1INTTES.H
9 / 10
/* sending*/
sign = 0;
if ( POS_OUT < 0 )
{
sign = -1;
POS_OUT = POS_OUT * -1;
}
POS1
POS2
POS3
if (
{
= POS_OUT / 100;
= ( POS_OUT - POS1 * 100)/10 ;
= POS_OUT - POS1*100 - POS2 * 10;
sign == -1 )
POS1 = POS1 | 0x8;

}
OUTPUT[0] =
if (CMD_IN
OUTPUT[1] =
OUTPUT[2] =
OUTPUT[3] =
88;
== 101) OUTPUT[0] = 101;
POS1 | ((CMD_OUT & 0x7) <<4 ) | 0x80 ;
POS2+ 48;
POS3+ 48;
sign = 0;
if (VEL_OUT < 0 )
{
sign = -1;
VEL_OUT = VEL_OUT * -1;
};
VEL1 = VEL_OUT / 100;
VEL2 = VEL_OUT - VEL1 * 100;
OUTPUT[4] = ((STAT_OUT & 0xf)<<3) | ( VEL1 & 3 ) | 0x80;
if ( sign != 0 )
{ OUTPUT[4] = OUTPUT[4] | 0x4;}
else
{ OUTPUT[4] = OUTPUT[4] & 0xfb;};
OUTPUT[5] = VEL2 | 0x80;
170
F1INTTES.H
10 / 10
OUTPUT[6] = OUTPUT[1] ^ OUTPUT[2] ^ OUTPUT[3] ^ OUTPUT[4] ^ OUTPUT[5];
if ( OUTPUT[6] == 0xd || OUTPUT[6] == 0xa ) OUTPUT[6]++;
for(comtemp = 0; comtemp < inptr; comtemp++)
{
printf("%c", OUTPUT[comtemp]);
}
printf("%c", 0x0d);
clrbuff();
inbuffer[0] = 0x00;
};
}
}
#else
void sci_svc() { }
#endif
#pragma interrupt_handler ic1_svc
#ifndef IC1_SVC
/* stub routine for ic1_svc() */
void ic1_svc() { }
#endif
#pragma interrupt_handler ic2_svc
#ifndef IC2_SVC
/* stub routine for ic2_svc() */
void ic2_svc() { }
#endif
#ifndef DOUSERINT
void do_userint() { }
#endif
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199

Design Construction and Control of An in

Загружено:

Сведения о документе

Оригинальное название

Авторское право

Доступные форматы

Поделиться этим документом

Поделиться или встроить документ

Параметры публикации

Этот документ был вам полезен?

Это неприемлемый материал?

Авторское право:

Доступные форматы

Design Construction and Control of An in

Загружено:

Авторское право:

Доступные форматы

Design, Construction

List of Figures, Tables and Flowcharts

1.2 Embodiments of Bipedal Motion

1.3 About this Project

1.4 Structure of this Document

2.0 Walking Robots

2.1.1 Wheels versus Legs

2.1.2 Legged Robots

2.2 Biped Robots

2.3 Control of Biped Robots

2.4 Design Criteria of an Industrial Biped

3.0 Mechanical Design of Roboshift

3.2 Mechanical Configuration

3.2.1 Basic Structure

3.2.2 Configuration of Joints

3.2.3 Range of Movement

3.2.4 Mechanical Design for Aspects of Control System

Power and Electrical Systems

3.3 Detail Mechanical Design

3.3.2 Knees, Lower and Upper Legs

4.0 Power and Electrical Design

4.1.1 Flow Calculation

4.1.2 Hydraulic Circuit Design

4.1.3 Conclusions on Hydraulic Design

4.2.1 Electrical Power Requirements

4.2.2 Circuit Protection

4.2.3 Power Distribution

4.2.5 Conclusions on Electrical Power

4.3 The Engine

4.3.1 Engine Power

4.3.2 Engine Selection

5.0 Control System Philosophy

5.2 Control System Overview

5.3 Control Strategy

5.4 Walking Control

5.5 Software Structure

6.0 Control System Hardware

6.3 Local Joint Controllers

6.4.1 Joint Movement (Opto Encoders & Limit Switches)

6.4.2 Attitude (Gyro & Flux Gate Compass)

6.4.3 Force (Strain Gauges and Amplifiers)

6.4.4 Foot Contact Switches

6.5 Conclusions on Control Hardware

7.0 Control System Modelling

7.2 Dynamic Simulation

7.3 Formulation of Joint Trajectories

8.0 Control System Software

8.1.1 Data Acquisition Routine

8.1.2 Maintenance Routines

8.1.3 Calibration Routine

8.1.4 Locomotion Routine

8.2 Communications Program

8.2.1 Receiving Data from The Microprocessors

8.2.2 Sending Data

8.3 Local Joint Control Software

8.3.1 Main Control Loop

8.3.2 Interrupt Service Routine

8.3.3 Calibration Routine

8.3.4 Motion Control Routine

8.4 Summary of Control Software Implementation

8.5 Software Development and Testing

8.6 Starting the Robot

9.0 Operational Trials on Roboshift