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 I
List of Figures, Tables and Flowcharts VI
List of Special Names and Abbreviations 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 - 10
3.2.4.1 Transducers 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 - 10
4.2 Electrical Design 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 7 - 12
Conclusion 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 - 5
10.3.1 Sensors 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 11 - 10 Local Joint Trajectories using a Calculated Member Stiffness
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
V LIST OF FIGURES
Figure Name Page
1.1 Robby the Robot 1 - 1 1.2 Kawada’s HRP2, Honda’s Asimo and 1 - 2 Toyota’s trumpet playing humanoid
1.3 Roboshift 1 - 7
2.1 Japanese toy Android 2 - 6
2.2 Rygg’s 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 University’s WH 11 2 - 19
2.8 Honda’s 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 3 - 9 from Braume (b) from the modified Hartrum model
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 Roboshift’s foot 3 - 17
3.9(b) Sideview of a Roboshift’s foot 3 - 17
3.10 Range of movement of foot 3 -18
3.11 Initial design of ankle frame 3 - 19
VI List of Figures, Tables and Flowcharts
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 List of Figures, Tables and Flowcharts
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 7 - 9 motion of upper body segments
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 List of Figures, Tables and Flowcharts
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 9 - 18 between 0.48 and 0.52 and reduced gain
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 List of Figures, Tables and Flowcharts
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
9.28 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 v’s 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 Response of simulation to square pulse force input 11 - 15
11.18 Response of simulation to square pulse force input 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
11.21a-d Ground reaction from hip torque 11 - 18
X List of Figures, Tables and Flowcharts
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 2 - 14
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
XI List of Figures, Tables and Flowcharts
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 Motion control routine 8 - 13
8.4c Motion control routine 8 - 15
8.4d Motion control routine 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 List of Special Names and Abbreviations
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 Processing data sufficiently rapidly to be able to control Control the source of the data. (Word Reference.com).
Resolved The frequency at which the control system is able to compute one Motion Rate complete control iteration.
RMR Resolved Motion Rate.
ROM Read Only Memory.
Unimate First industrial robot developed by Unimate in 1961.
XIII List of Special Names and Abbreviations
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 Right Foot Extension
LeFoRo Left Foot Rotation
RiToEx Right Toe Extension
LeToEx Left Toe Extension
RHA Right Hip Abduction
XIV List of Special Names and Abbreviations
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 Right Foot Extension
LFE Left Foot Extension
RFE Right Foot Extension
LFR Left Foot Rotation
RTE Right Toe Extension
LTE Left Toe Extension
XV 1INTRODUCTION
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. Japan’s fascination with androids and the personification of electronic devices has driv- en 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. “It’s more than just owning them. They are our friends, and we want to integrate them into society.” (Time, 2000)
More recently, the development of Sony’s SD R - 3 x , H o n d a ’s A s i m o a n d To y o t a ’s trumpet playing humanoid (Figure 1.2) demon- strated 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 SDR-3X as an “entertainment robot” as its small movie Forbidden Planet
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 robot’s significant processing power and human-like qualities either satisfy the developers’ craving for creation or the market’s 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 Honda’s Asimo, Kawada’s HRP2 and Toyota’s trumpet playing humanoid 1 - 2 Chapter 1 - Introduction
• 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 Honda’s 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 a device which would replicate some human characteristics. Sony (2003) justify their biped robot as a proving ground for the demon- stration of new technology.;
“next generation technology is functional device technology that correspond to the five senses”.
1 - 3 Chapter 1 - Introduction
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 Chapter 1 - Introduction
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 on- board all power, actuation and processing systems required for continuous and extended operation. 1 - 5 Chapter 1 - Introduction
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 Chapter 1 - Introduction
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 Electric’s 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 Chapter 1 - Introduction
Originally intending to complete a master’s 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 Chapter 1 - Introduction 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 Electric’s 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 1 - Introduction
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 robot’s 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 1 - Introduction
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 con- trol strategy using non-collocated sensors is then modelled.
C h a p t e r 1 2 discusses the major accomplishments and failures of the project. The project’s 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 Chapter 1 - Introduction
• 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 presen- tation of strategies to deal with compliance by teh use of non collocat- ed 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 2 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 biomechan- ics. 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 e n d e a v o u r, th e f i e ld o f bi p e d r o b o t i c s h a s i n s a t i a b l y a d a p te d e me rg i n g 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 Chapter 2 - Walking Robots 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 Rossum’s 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, today’s industrial robot is simply an intelligently controlled machine, a machine designed to carry out repetitive and laborious tasks as highlighted in Kapek’s 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 Asimov’s 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 Chapter 2 - Walking Robots 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 Chapter 2 - Walking Robots 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 Chapter 2 - Walking Robots
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 Japan’s 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. Rygg’s pedal- powered mechanical horse, patented in 1893 (see Figure 2-2).
2 - 6 Chapter 2 - Walking Robots
Figure 2-2. Rygg’s 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 Chapter 2 - Walking Robots 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 1960’s was a volatile time for robotics as teams working in many parts of the world developed new ideas and prototypes. During this period, the USA’s 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 driver’s hands and feet. This was part of an ongoing experiment in force feedback, and the driver was able to “feel” the vehicle’s 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 Chapter 2 - Walking Robots 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 today’s desk-top personal computers, units such as Digital Electronic Corporation’s 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 vehicle’s 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 Chapter 2 - Walking Robots
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 Chapter 2 - Walking Robots 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 600.0
500.0
400.0
300.0
200.0
Robot Weight (Kg) 100.0
0.0
H6
KDW Guru Lucy Kaist
Ninjya Isamu Azimo CURBI SAICO Monroe Johnnie Bart-UH
Roboshift WL-12RV1 BIP 2000 HITBWR-III 3D Biped Wabot - 1
UWCC Biped Robo Erectus Arne & Arnea Spring Turkey Biped Robot #6 Biped Robot #7 Robot Bipede 1 Biped RobotBiped #2 Robot #3 Biped Robot #1 Shadow Walker
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 Chapter 2 - Walking Robots 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 Chapter 2 - Walking Robots
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 Machine Name Institution Height Mass (kg) DOF Control Structure Year Site
Biped robot #3 UCLA Commotion lab 0.6 * 7 * 1995 www.muster.cs.ucla.edu
Biped Robot #5 Nagoya University * * 13 GA & Recurrent Neural Networks 1997 www.hp73.nagaokaut.ac.jp
Biped-Bike Hori Laboratories 0.8# * 6 Virtual Inverted Pendulum 1997
Geekbot MIT Leg Lab * * 6 Hierarchial - state control 1994-1995 www.ai.mit.edu/ projects/leglab/
M2 MIT Leg Lab * * 12 * www.ai.mit.edu/ projects/leglab/
Passive Walker Uni of Michigan * * * Dynamic - non powered 1996
Piernuda Robtica, Mexico 0.9# * * 1996-2000 www.132.248.59.55/ piernuda/piernuda.html
Planar Biped MIT Leg Lab * * 6 Constrained motion planar 1985-1990 www.ai.mit.edu/ projects/leglab/
Smooth Walker Harvard Robotics Lab * * * Smooth Walking - Constrained 1996 hrl.harvard.edu
Idaten-II Osaka University 0.8 * 7 Hierarchial -reactive 1981 www.dyna.ccm.eng. osaka-u.ac.jp
Biped Robot #6 Koube University 0.02 0.67 6 Neuro Oscillator 1994 ziong.cs.kobe-u.ac.jp
Robo Erectus Singapore Polytechnic 0.5 4 22 central * www.robo-erectus.org
Biped Robot #7 Keio University 0.6# 8 6 Impedence Control 1997 www.yamazaki. mech.keio.ac.jp
# Estimated from photograph. * No data available Table 2.1 Survey of biped robot research Machine Name Institution Height Mass (kg) DOF Control Structure Year Site CURBI Ohio State 0.89 9.16 8 Jacobian Motion control 1995 www.ee.eng.ohio-state.edu
Spring Turkey MIT Leg Lab * 10 4 Constrained planar biped 1994-1996 www.ai.mit.edu/ projects/leglab/
Ninjya Miyazaki University 0.7 12 5 * 1980-1996 *
SAICO Mexico DF 1.1 12 12 * 1997 *
Robot Bipede 1 LSHT GRAVIR 0.8 15 5 * 1991-1994 *
KDW Changsha Institute 0.8 16.3 12 Sequential control 1987-1995 *
Biped Robot #2 Kobe University 0.85 18 7 Inverted Pendulum Neural Network 1989 www.ziong.cs.kobe-u.ac.jp
Biped Robot #3 Osaka University 1.1 22 5 Hierarchial - Low order 1986 www.dyna.ccm. eng.osaka-u.ac.jp
Monroe Tohoku University 1.2 22 6 Hierarchical 2003 www.mechatronics.mech. tohoku.ac.jp/research/biped/
Guroo Uni of Queensland 1.2 30 23 Neural / Heirarchical 2003 www.csee.uq.edu.au
Lucy VUB Uni Brussells 1.5 30 8 Hierarchical 1990 www.lucy.vub.ac.be
UWCC Biped Uni of Wales, Cardiff 1.0 30 6 * 1995 *
HITBWR-III Harbin Institute of Tech. 1.0 40 12 Gait Control 1988-1995 *
Johnnie Munich Technical Uni 1.8 40 17 Central * www.amm.mw.tumuenchen. de/forschung/zweibeiner/ johnnie_e.html
# Estimated from photograph. * No data available Table 2.1 Survey of biped robot research Machine Name Institution Height Mass (kg) DOF Control Structure Year Site Bart-UH Hannover University 1.2# 40 6 Central 2000 *
Shadow Walker Shadow Robot Company 1.4 40 12 Central 1987 www.shadow.org.uk/ projects/biped.shtml
Kaist Korea Advanced Institute 1.2 48 21 Hierarchical 2003 www.Ohzlab.kaist.ac.kr/khr_ robot/khr_humanoid.html
3D Biped MIT Leg Lab 1.4# 50 6# 1989-1995 www.ai.mit.edu/ projects/leglab/
WL-12RV1 Waseda University 1.4# 50 8# * 1996 www.humanoid.waseda.ac.jp
H6 Tokyo University 1.37 55 24 * 2003 www.jsk.t.u-tokyo.ac.jp/ research/h6/h6.html
Isamu Kawada Industries 1.5 55 32 Heirarchical 2003 www.kawada.co.jp/ams/isamu/
Biped Robot #1 University of Kentucky 0.7 60 12 Reduced order dynamic Jacobian 1987 www.crms.engr.uky.edu
Arne & Arnea New Era, Russia 1.23 61 * * 1996 www.robotarena.com
BIP 2000 BIP team 1.8 105 15 * 1999 www.inria.fr/rrrt/rt-0243.html
Wabot - 1 Waseda University 1.5# 130 9 Kinematic - Static 1988 www.humanoid.waseda.ac.jp
Asimo Honda 1.2 210 * * 1997 www.honda.com.jp
Roboshift UNSW 2.4 500 14 Hybrid Classic/Hormonal 1994-2004 www.mech.unsw.edu.au/mech/ Mechlab/mechatronics.htm
# Estimated from photograph. * No data available Table 2.1 Survey of biped robot research Chapter 2 - Walking Robots
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 CAL’s man amplifier was designed which used the natural movements of the operator to actuate servo-drives powering the exoskeleton’s 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 The second point highlighted by the GEC Hardiman Exoskeleton exoskeleton research, is that it was the control of
2 -17 Chapter 2 - Walking Robots 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 operator’s 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 1970’s 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 Chapter 2 - Walking Robots
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 University’s WH 11 2 -19 Chapter 2 - Walking Robots 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 robot’s 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 Honda’s humanoid robot Asimo
2 -20 Chapter 2 - Walking Robots
Figure 2.9 Scatter plot of mass v’s 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 robot’s 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 Chapter 2 - Walking Robots 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. Kawasaki’s 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 walk- ing 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 Chapter 2 - Walking Robots 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 con- trol 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 Wa r. 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 o ff 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 manip- ulation of the resultant reaction vector at the foot/feet in contact with the ground. For this
2 -23 Chapter 2 - Walking Robots 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. GE’s walking quadruped truck was controlled by a human operator tele-operating the vehicle’s 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 machine’s 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.
Waldron’s 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
1 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, cer- tainly not an intelligently controlled machine. For the purposes of this project, the word robot will be taken to mean a microprocessor-controlled machine. 2 -24 Chapter 2 - Walking Robots 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 project’s 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 Chapter 2 - Walking Robots 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 con- trol 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 biped’s joints via reduction drives or screw–crank systems. The BIP team use an external Unix workstation running a generic robot control system to dynamically model the robot’s 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 Chapter 2 - Walking Robots 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 modifica- tions 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 predeter- mined 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 num- ber 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 Chapter 2 - Walking Robots 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 kinemat- ics. Recursive multi-body algorithms are then use to evaluate reduced body dynamics without actually extracting them. This reduced order processing can be conducted in real- time 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 stabi- lize 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 “nat- ural” 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 Chapter 2 - Walking Robots 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 robot’s 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 robot’s 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 Chapter 2 - Walking Robots 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 disad- vantage of a fuzzy system lies in the unpredictability of the response to inputs not previ- ously 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 Chapter 2 - Walking Robots 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 Chapter 2 - Walking Robots
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 robot’s 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 robot’s 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 Chapter 2 - Walking Robots 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 Chapter 2 - Walking Robots 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. (Design criterion #2)
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 Honda’s 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 Chapter 2 - Walking Robots 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. (Design criterion #3)
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. U n f o r t u n a t e l y, 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 Chapter 2 - Walking Robots
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. (Design Criterion #4) • Completely self contained. (Design Criterion #5)
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 Honda’s ASIMO:
I don’t know how useful a robot like ASIMO is, but I wouldn’t mind having one at home for the kids.
2 -36 3 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.
• Robust both physically and electronically (1st Criterion) • Easily maintained (2nd Criterion) • Capable of lifting 250kg (3rd Criterion) • Able to work for long periods (4th Criterion) • Able to traverse 500mm discontinuities (5th Criterion) • Completely self contained (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 robot’s design that could be improved. However, the majority of these issues would only have 3 - 2 Chapter 3 - Mechanical design of Roboshift 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 Chapter 3 - Mechanical design of Roboshift
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 robot’s 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 Mosher’s 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 Chapter 3 - Mechanical design of Roboshift
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 robot’s 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 mathemati- cal 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 Chapter 3 - Mechanical design of Roboshift 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 para- digms 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 Chapter 3 - Mechanical design of Roboshift 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 Hinge Thigh abduction/adduction Hinge Thigh rotation Axial rotation
Table 3.3 Simulation of human lower limb joints
3 - 7 Chapter 3 - Mechanical design of Roboshift
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 subject’s gait. In 1973, Hartrum (1973) used Fourier Transforms to generate a parametric model from Braune and Fischer’s 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 Hartrum’s 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 Braune’s data and from the modified Hartrum model are shown in Figure 3.3 (a) and (b).
3 - 8 Chapter 3 - Mechanical design of Roboshift
Hip, Knee & Ankle Angle Iterpolated from Braune 1.4 2
1.8 1.2 1.6
1 1.4
1.2 0.8 1 0.6 0.8 Angle (Radians) 0.4 0.6 0.4 0.2 0.2
0 0 012345
Phase (Radians) Knee Extension Ankle Flexion Hip Extension (a)
Hip, Knee, Ankle & Foot Angles Parametric Model 2.5 6
5 2
4 1.5 3 1 2 Angle (Radians) 0.5 1
0 0 012345 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 Chapter 3 - Mechanical design of Roboshift
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 Fischer’s 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 Chapter 3 - Mechanical design of Roboshift
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 Chapter 3 - Mechanical design of Roboshift
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 Chapter 3 - Mechanical design of Roboshift
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. G e n e r a l l y, 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 robot’s 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 Chapter 3 - Mechanical design of Roboshift 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 robot’s 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 struc-
3 - 14 Chapter 3 - Mechanical design of Roboshift 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 Chapter 3 - Mechanical design of Roboshift
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 incorpo- rate 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 Chapter 3 - Mechanical design of Roboshift
Figure 3.9 (a) Rear view of Roboshift’s foot
Figure 3.9 (b) Side view of Roboshift’s foot
3 - 17 Chapter 3 - Mechanical design of Roboshift ankle, the rear of the heel has been radiused to allow for ankle abduction without the requirement for a separate joint. Photographs of Roboshift’s feet can be seen in the pho- tographs 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: = MIN LXX Sin )( (1) += MAX LXX 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; = + MIN LX Fixedcomponents S The length of the extended cylinder is equal to that of the fixed cylinder plus the stroke length: += MAX MIN SXX Then: + +== MAX MIN LXSXX Sin )( LX Sin )( (2) - (1)
= [] + troke SinSinLS )()(
Figure 3.10 Range of movement of foot 3 - 18 Chapter 3 - Mechanical design of Roboshift
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 robot’s 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 Chapter 3 - Mechanical design of Roboshift 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 Chapter 3 - Mechanical design of Roboshift
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 consid- ered before a parallel swing arm arrangement was selected. This design offered the addi- tional advantage of allowing for accurate indication of the load on the feet by measure- ment 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 incor- porated into the joint with the ability to control spring force via adjustment of air pres- sure. 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 par- allel. Figure 3.13 shows the parallel swing-arm arrangement which can also be seen in the photograph of Figure 3.14. 3 - 21 Chapter 3 - Mechanical design of Roboshift
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 Chapter 3 - Mechanical design of Roboshift
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 Chapter 3 - Mechanical design of Roboshift
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 Chapter 3 - Mechanical design of Roboshift
3.3.4 BODY While General Electric’s 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 operator’s own control abilities to manoeuvre it. As discussed previously, the device was controlled by the movement of the pilot’s 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 operator’s requests being the lowest priority. Such a system would no longer require the operator to coordinate the movement of the Robot’s 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. Eff e c t i v e l y, the ultimate conclusion from such discussions is that industrial scale exoskeletons would require such a level of control to convert the operator’s 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 Chapter 3 - Mechanical design of Roboshift
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 togeth- er. Once the stitched fabrications had been checked for accuracy, they were them MIG
3 - 26 Chapter 3 - Mechanical design of Roboshift 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 4 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 3 2.5 2 1.5 1 Angle (Radians) 0.5 0 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 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 Chapter 4 - Power and Electrical Design 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
2
1.5
1 Litres/Second
0.5
0 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 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 Chapter 4 - Power and Electrical Design
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 6 5 4 3 2 1 Flow (Litres/Second) 0 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 2.2 2.4 Time (Sec)
Total Hydraulic Oil Flow Total Flow with Accumulators Figure 4.3 Total hydraulic flow 4 - 4 Chapter 4 - Power and Electrical Design unpredictable. As an example of this in robot design, unqualified accounts such as reported by Wiess (2001) would indicate that General Electric’s 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 machine’s 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 Chapter 4 - Power and Electrical Design 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 system’s 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 robot’s legs
4 - 6
Chapter 4 - Power and Electrical Design
Figure 4.6 Hydraulic power pack 4 - 8 Chapter 4 - Power and Electrical Design
• 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 pos- sible from the body of the robot, under the amplifier box so that the lengths of valve con- trol 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 Chapter 4 - Power and Electrical Design 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 Chapter 4 - Power and Electrical Design 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 Chapter 4 - Power and Electrical Design
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 robot’s 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
Chapter 4 - Power and Electrical Design
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 pro- viding 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 Chapter 4 - Power and Electrical Design 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 Chapter 4 - Power and Electrical Design
• 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 Chapter 4 - Power and Electrical Design to the toe cylinder supporting half of the total load of the robot and accelerating that load 600 F = ()g = + 4.315.01 kN Cyl 2 by 0.15g. The cylinder force would then be, with an estimated total weight of 600kg:
F = Cyl = 3400 == PCyl Mpa bar 500303 psi ACyl 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
CpQ P = Average System Duty Re 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 analy- sis was not possible, nor would such an analysis be completely accurate. Having access to a number of highly respect- ed 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 sys- tem to be modelled was small compared to other systems that have been modelled using proprietary hydraulic soft- ware 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 pres- sures 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 Chapter 4 - Power and Electrical Design
Q 6.2 === Displ. 0624.0 l 3000 () ispl. out in CppD Duty = 60 hydromechanical efficiency of 0.85 at 3000rpm, this gives a displacement of: This equates to a torque of:
= /7.030000624.0
= 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 Chapter 4 - Power and Electrical Design
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 Curves
KW 16.0 16.1 15.5 @3000 Torque 41 42 41 @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 Chapter 4 - Power and Electrical Design
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 with- out 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 5 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 robot’s 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 Chapter 5 - Control System Philosophy 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 Chapter 5 - Control System Philosophy 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 Chapter 5 - Control System Philosophy swing leg. Using McMahon’s 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 Chapter 5 - Control System Philosophy
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 robot’s 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 Chapter 5 - Control System Philosophy
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 Chapter 5 - Control System Philosophy
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 Chapter 5 - Control System Philosophy 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 robot’s 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 Chapter 5 - Control System Philosophy
• 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 Data Acquisition Subroutine horizon and compass.
Proprioception HC11 Data program Data acquisition HC11 Data Acquisition from Strain Data Acquisition and Gauges Conditioning Subroutine
Proprioception HC11 Joint Joint control HC11s Data acquisition from control software joint shaft encoders Data acquisition subroutine
Proprioception HC11 Joint Joint control HC11s Local control of joint movement control Software Motion control subroutine Cerebral system Communications program Communications computer Communications between supervisory computer and distributed systems Cerebral system Supervisory program Supervisory computer Supervisory control Movement subroutine Balance control Movement control Cerebral system Supervisory program Supervisory computer Balance control Balance monitoring routine
Table 5.1 Control tasks 5 - 10 Chapter 5 - Control System Philosophy
• 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
Balance Monitoring Joint Trajectory Status Monitoring Software Subroutine Software Vestibular & Ground Error Status R eacti on D ata
Main Control Computer
Air Pump Communications Software Program Communications Computer
Joint Contr ol & Instr umentation HC11s
Left Foot Left Foot Right Foot Right Foot Left Toe Right Toe Rotation Extension Extension Rotation Program Software Software Software Software Software
Left Left Left Right Right Right Hip Hip Hip Hip Hip Hip Rotation Extension Abduction Abduction Extension Rotation Software Software Software Software Software Software
Actual Position Velocity
Demand Position / Velocity
Artificial Analogue Left Knee Right Knee Horizon Input Software Software LVDT Output Software Software
Strain Guage Amplifier Output Hormone Variables
Figure 5.4 Data network 5 - 11 Chapter 5 - Control System Philosophy
• 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 Computer’s RAM
Figure 5.5 Flow of data
5 - 12 Chapter 5 - Control System Philosophy
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 Computer’s 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 Computer’s 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 6 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 devel- oped 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 writer’s 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 Chapter 6 - Control System Hardware
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 Chapter 6 - Control System Hardware
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 Chapter 6 - Control System Hardware 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 robot’s 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 robot’s 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 Chapter 6 - Control System Hardware 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 robot’s 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 Chapter 6 - Control System Hardware
(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 Chapter 6 - Control System Hardware
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 earth’s 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 availabil- ity 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 Chapter 6 - Control System Hardware
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 or electromagnetic
Figure 6.5 Fluxgate compass mounting. 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 aircraft’s 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 Chapter 6 - Control System Hardware
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 aircraft’s 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 Chapter 6 - Control System Hardware
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 robot’s 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 Chapter 6 - Control System Hardware
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 robot’s 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 Chapter 6 - Control System Hardware
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 Chapter 6 - Control System Hardware
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 robot’s feet. Contact with the ground has proved difficult to detect. The control system is able to calculate the orientation of the robot’s 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 non- volatile methods such as potentiometers 6 - 15 Chapter 6 - Control System Hardware
• 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 7 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 robot’s 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 Fischer’s 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 Murray’s 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 AutoCAD’s 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):
W ()+ + + = X AN FCos HI KN AN W W == AN YY FO 0 W ()+ + + = Z AN FSin HI KN AN
where:HI = Hip Angle KN = Knee Angle AN = Ankle Angle
7 - 2 Chapter 7 - Control System Modelling
Figure 7.2. Modified foot model.
Equations for the heel coordinates are: W () + + = X HE FCos HI KN AN W W == HE YY FO 0 W () + + = Z HE FSin HI KN AN
Simarly, in swing phase, the equations for the ankle coordinates are: W W () + += AN KN LXX LCos HI KN W W == AN YY KN 0 W W () + += AN KN LZZ L Sin HI KN
Equations for the heel coordinates are: W () + + = X HE FCos HI KN AN W W == HE YY FO 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 FO SinSin KN () ()() ()() Sin FO Cos FO Cos KN Cos FO Sin KN () () 0 Sin KN Cos KN
To give the joint coordinate equations for the stance phase:
() ( + + + += ) AN FO FCosXX HI KN AN Cos FO () ()( + + + ) FSin HI KN AN FO SinSin KN
7 - 3 Chapter 7 - Control System Modelling
() ( + + + += ) AN FO FCosYY HI KN AN Sin FO () ()( + + + ) FSin HI KN AN Cos FO Sin KN () ( + + + = ) AN FO FSinZZ HI KN AN Cos KN Where; F=length of foot.
() ( + + + += ) HE FO HCosXX HI KN AN Cos FO () ()( + + + ) HSin HI KN AN FO SinSin KN
() ( + + + += ) HE FO HCosYY HI KN AN Sin FO () ()( + + + ) HSin HI KN AN Cos FO Sin KN
() ( + + + = ) HE FO HSinZZ HI KN AN Cos KN Where H = length of heel:
()() + += AN KN LLCosLXX HI KN Cos FO ()()() + LL SinL HI KN FO SinSin KN
()() + += AN KN LLCosLYY HI KN Sin FO ()()() + LLSinL HI KI Cos FO Sin KN ()() + = AN KN LL SinLZZ HI KN Cos KN = Where;LLL length of lower leg.
()() + + += HE FO FCosXX HI KN AN Cos FO ()()() + + FSin HI KN AN FO SinSin KN ()() + + += HE FO FCosYY HI KN AN Sin FO ()()() + + FSin HI KN AN Cos FO Sin KN
()() + + = HE FO FSinZZ 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 Chapter 7 - Control System Modelling
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 Chapter 7 - Control System Modelling
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 XX v = CH 2 CH1 CH 2x T
v Where;CH 2 x = 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 CH1 = Initial X coordinate of the Centre of Gravity of the head T = Time increment between frames
vv CH 2 CH1 a = x x CH 2 x T
a Where;CH 2x = the acceleration of the centre of gravity of the head in the X direction
Therefore; the horizontal force produced by the head on the hip is:
+ = aMFx H H CH 2x
7 - 6 Chapter 7 - Control System Modelling
Vertical Forces
YY v = CH 2 CH1 CH 2Y T
v Where;CH 2Y = Average velocity of the Centre of Gravity of the head in the Y direction
YCH 2 = Final Y coordinate of the Centre of Gravity of the head
YCH1 = Initial Y coordinate of the Centre of Gravity of the head T = Time increment between frames
vv CH 2 CH1 a = y y g CH 2 y T
a Where;CH 2 y = The acceleration of the centre of gravity of the head in the Y direction. g = Gravitational constant
Therefore; the vertical force produced by the head on the hip is:
+ = () gaMFy H H CH 2 y
Torques
The torque generated at the hip by the motion of the head is produced by a combina- tion of translational and angular accelerations. The angular accelerations are calculated as follows; The angular forces are given by: = H H12 H T Where;H1 = Initial angular velocity of head H 2 = Final angular velocity of head and is given by: = H H12 H 2 T
7 - 7 Chapter 7 - Control System Modelling
Where;H1 = Initial angular position of head H 2 = Final angular position of head
= []()() H1 ,2tan YYYYa HHH 0101
= []()() H 2 2tan , YYYYa HHHH 1212
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
1.5
1.0
0.5
0.0
(Rad/s^2) -0.5
-1.0 Angular Acelerations -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) Lower Arm (R) Upper Arm (L) Upper Arm (R) Trunk 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:
= HHH
H Where; = Torque produced by the head H = Moment of Inertia of the head H = Angular Acceleration of the head 7 - 8 Chapter 7 - Control System Modelling
The torque produced at the hip by the head is:
+ = + + H HH .. RxFyRyFxT HHHH
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
300
200
100
0
-100
-200 Force (N) Torque (Nm) -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 deter- mine 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 Chapter 7 - Control System Modelling
The motion of the mass is calculated as follows: X Displacement
+ = aMFx
Fx a = M
vv 12 = Fx t M
xx xx 12 01 t t = Fx t M
()+ 2 xxx 012 = Fx t 2 M
Fx x = 2 2 + xxt 2 M 01
Similarly, the Y displacement is calculated as:
Fx y = 2 2 + yyt 2 M 01
The rotational displacement is more complex to calculate as the translational motion produces additional torques. Rearranging the above equations:
+ xx Fx = 02 M t 2
+ yy Fy = 02 M t 2
Given: + + + = XFyYFxM H then: = XFyYFxM
[]1 ()() + + XyyYxxM = 02 02 2 0
7 - 10 Chapter 7 - Control System Modelling
The displacements produced by the analysis are shown in Figure 7.9.
Box Motion Vertical And Horizontal Displacement 0.6 8.0
0.5 6.0
0.4 4.0 0.3 2.0 0.2 0.0 0.1 Displacement (m)
-2.0 Angular Displ. (Rad) 0.0
-0.1 -4.0
-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 Chapter 7 - Control System Modelling
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 it’s 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:
1 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. 7 - 12 Chapter 7 - Control System Modelling
From Figure 7.11;
+= yxr 22
x = asin r
12 rLL 222 = a cos 12 rL
LLr 21 222 = a cos LL 212 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 8 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
M68HC11 Main Control Communications Microprocessors Program Program 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 robot’s 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 Chapter 8 - Control System Software
Flowchart 8.1 Main control program 8 - 3 Chapter 8 - Control System Software
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 Chapter 8 - Control System Software
MCP_112.C Data Aquisition Module
Increment Theloopcounterisusedto Loop Count monitor the subroutine loop speed as well as to monitor the number of loops with errors.
Read Compass Thedatafromthecompass and and artificial horizon is read. Artificial Horizon
Check for keyboard input Keyboard during loop. Input ? No
Yes
Initiate The logging function writes position Logging? and instrumentation data to a file No for later analysis or debugging. Yes
This function converts the double Convert integers and single integers and Joint/Glob al Data floating point numbers to 7 bit bytes To Output Form. for transmitting to the Communications Computer.
Write Data file to RAM Drive Transmit Output Data 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 Update LoopError to optimise timing parameters, however it and Timing Counters. is an excellent indication of a problem with any part of the system.
All aquired data such as artificial Update Moving horizon and compass data as well Averages and asstrainguagedataareheldina recalculate Medians moving average matrix. The matrix and median values are updated on each loop. Joint position, instrumenttion Update and other data are constantly On-Screen displayed on the screen. Display.
Flowchart 8.2 Data acquisition routine
8 - 5 Chapter 8 - Control System Software
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 robot’s 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 Chapter 8 - Control System Software 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 robot’s joints once power is lost. Each time the robot control system is started, the shaft encoders must be synchronised with the robot’s 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 robot’s 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 Chapter 8 - Control System Software
Keyboard Input ? No
Yes Send All All Stop Comand To Stop ? All Joint Controllers Yes
Send Stop Comand Stop Current To Joint Controllers Calibration? In Calibration Mode Yes
Send Move Move Limb Positive Command to Positive Drn? Joint Controller. Yes No
Send Move Move Limb Negative Command NegativeDrn? to Joint Controller. Yes No
Send Calibrate Calibrate Command to Limb ? Joint Controller Yes No
Set Foot Calibrate Calibration Foot ? Flag Yes No
Foot Calibration Toe Cylinder Foot Rotation Foot Extension Active? Flag Set ? Flag Set ? Flag Set ? Yes Yes Yes Yes No No No No
Yes Send Calibrate Send Calibrate Send Calibrate Command to Toe Command to Foot Command to Foot Exit ? Rotation Extension Joint Controller Joint Controller Joint Controller No End
Communicate Commands To Joint Controllers MCP_112.C Calibration Module
Flowchart 8.3 calibration module 8 - 8 Chapter 8 - Control System Software
Key Function Key Function
Esc Exit program Space All stop
+ Toggle logging
H Left hip abduction positive N Right hip abduction positive
h Left hip abduction negative n Right hip abduction negative
T Left hip abduction calibration y Right hip abduction calibration
t Stop left hip abduction calibration Y Stop right hip abduction calibration
J Left hip extension positive M Right hip extension positive
j Left hip extension negative m Right hip extension negative
U Left hip extension calibration i Right hip extension calibration
u Stop left hip extension calibration I Stop right hip extension calibration
G Left hip rotation positive B Right hip rotation positive
g Left hip rotation negative b Right hip rotation negative
q Left hip rotation calibration w Right hip rotation calibration
Q Stop left hip rotation calibration W Stop right hip rotation calibration
f Left knee extension positive V Right knee extension positive
F Left knee extension negative v Right knee extension negative
e Left knee extension calibration R Right knee extension calibration
E Stop left knee extension calibration r Stop right knee extension calibration
A Left toe extension positive S Right toe extension positive
a Left toe extension negative s Right toe extension negative
D Left foot rotation positive Z Right foot rotation positive
d Left foot rotation negative z Right foot rotation negative
S Left foot extension positive X Right foot extension positive
s Left foot extension negative x 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 Chapter 8 - Control System Software
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 Chapter 8 - Control System Software
Flowchart 8.4(a) Motion control routine 8 - 11 Chapter 8 - Control System Software
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 computer’s RAM drive. Once found
it is read into memory and deleted by the subroutine get_output().
8 - 12 Chapter 8 - Control System Software
Flowchart 8.4(b) Motion control routine
8 - 13 Chapter 8 - Control System Software
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 robot’s 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 Chapter 8 - Control System Software
Flowchart 8.4(c) Motion control routine 8 - 15 Chapter 8 - Control System Software
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 robot’s 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 Chapter 8 - Control System Software
Flowchart 8.4(d) Motion control routine 8 - 17 Chapter 8 - Control System Software
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 Computer’s 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 Chapter 8 - Control System Software
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 robot’s 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 Chapter 8 - Control System Software 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 > 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; }
8 - 20 Chapter 8 - Control System Software
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 computer’s 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 Chapter 8 - Control System Software
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 Chapter 8 - Control System Software 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 Chapter 8 - Control System Software
8.2 COMMUNICATIONS PROGRAM As discussed in previous sections, the communications program runs on the communications computer controlling the distribution of data throughout the robot’s 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 Chapter 8 - Control System Software
Flowchart 8.5 Main communications program
8 - 25 Chapter 8 - Control System Software
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 Chapter 8 - Control System Software
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 Chapter 8 - Control System Software
3. Within the main loop, an additional loop steps through each of the RS232 channels. While the data can be received into the P747’s 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 computer’s 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 computer’s 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 Chapter 8 - Control System Software
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 Chapter 8 - Control System Software
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 Chapter 8 - Control System Software 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 Chapter 8 - Control System Software
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 joint’s 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 robot’s 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 robot’s main control software. 7. The motion function controls the movement of the joint as commanded by the robot’s 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 robot’s 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 Chapter 8 - Control System Software 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 Chapter 8 - Control System Software
Flowchart 8.9. Local joint controller interrupt service routine 8 - 34 Chapter 8 - Control System Software
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 joint’s position. On power up, no memory of the joint’s 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 Chapter 8 - Control System Software
HC11 Calibration Subroutine
Initialise
Set control output to zero Set Output to Zero before disabling it. Zero the Enable Output encoder count. Set Encoder to Zero
Has the minimum up control Min_Up = 0 ? value been determined ? Yes No
While up_rate < 3 While the velocity is less than && 3, and the position has not Count < 20 changed by 20, increase the && control value. Calibrate is enabled No Yes Ifthejointmovesmorethan 20 counts without achieveing Increase Output Rate < 3 minimum velocity stop Communicate calibration
Disable Output Rate < 3 ? Return Error Yes Stop Mode No Once the joint has achieved minimum velocityrecord the No control value and disable Min_Up = Output the output. Output = 0
Has the minimum down control value been determined ?
Min_Dwn = 0 ? Yes While the velocity is less than No 3, and the position has not changed by 20, decrease the control value. While Dwn_rate < 3 && Count < 20 && No Calibrate is enabled Checkfornewdatafromthe control PC in the middle of every loop. Yes Rate < 3
Decrease Output Communicate
Disable Output Return Error Once the joint has achieved No Stop Mode minimum velocityrecord the Rate < 3 ? control value and disable the output. Yes
Min_Up = Output Output = 0 Flowchart 8.10(a) Local joint controller calibration software
8 - 36 Chapter 8 - Control System Software
Flowchart 8.10(b) Local joint controller calibration software
8 - 37 Chapter 8 - Control System Software joint control calibration software determines the values of V _ N e g a t i v e _ M i n a n d 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 < -3 ) contr = min_dn - 10; if ( pos_e < -5 ) contr = min_dn - 16; if ( pos_e > 1 ) contr = min_up + 4;
8 - 38 Chapter 8 - Control System Software
Flowchart 8.11 Position control routine
if ( pos_e > 3 ) contr = min_up + 8; if ( pos_e > 5 ) contr = min_up + 16; 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 Chapter 8 - Control System Software
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 Chapter 8 - Control System Software
Flowchart 8.12 Flowchart showing path for joint movement.
8 - 41 Chapter 8 - Control System Software
Hardware/Software Functionality 1 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. 2 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. 3 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 4 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. 5 Packet Protocol • Development of packet protocol for communication between communications PC and Microcontrollers. 6 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 Chapter 8 - Control System Software
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 Chapter 8 - Control System Software
Wheeled Robot Error Trials 1 0 2250
2200
5 150
100 0 Error 50 Thousands Shaft Count
0 - 5
--50
- 1 0 --100 0 5 0 1 0 0 1 5 0 2 0 0 2 5 0 3 0 0
X - A x i s 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 Chapter 8 - Control System Software 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 robot’s 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
Wheeled Robot Error Trials 250 300
200 250
150
200 100 Y-Axis 50 150 Control Output
0
100 -50
-100 50 0 50 100 150 200 250 300 Control Error
Figure 8.11 Error and control signals from PID control
Wheeled Robot Error Trials 1 0 2 5 0
2 0 0
5 1 5 0
1 0 0
0 5 0 Error Position Thousands 0
- 5 - 5 0
- 1 0 0
- 1 0 - 1 5 0 0 5 0 1 0 0 1 5 0 2 0 0 2 5 0 3 0 0
Right Wheel Left Wheel Error
Figure 8.12 Error and control signals of Fuzzy Control 8 - 45 Chapter 8 - Control System Software 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 HC11’s 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 Chapter 8 - Control System Software 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 Chapter 8 - Control System Software
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 repeat- edly 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 Chapter 8 - Control System Software
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 9 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 robot’s 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 calibra- tion 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 Chapter 9 - Operational Trials on Roboshift
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 calculat- ed 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 Chapter 9 - Operational Trials on Roboshift
Strain Gauge Calibration Left foot - Heel loading
Figure 9.3 Calibration of left foot strain gauge (heel loading)
Balance Trials SM1508b
400 2
300 0
200 -2 Mass (Kg)
100 -4
0 -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 Chapter 9 - Operational Trials on Roboshift 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: weight left __ leg trim = 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 StrainStrain GuageGauge Trial #3 30/06/98 106 0.6
104
0.5
102 % Weight 0.4 Position (Counts) 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 Chapter 9 - Operational Trials on Roboshift
Frontal Balance Sagital Balance Tuning (Mass) 0801bla
500
400
300 Mass (Kg) 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 Chapter 9 - Operational Trials on Roboshift
Artificial Horizon Calibration
200
150
100 Output (Bits)
50
0 -15 -5 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 Chapter 9 - Operational Trials on Roboshift
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 robot’s 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 Chapter 9 - Operational Trials on Roboshift
Figure 9.8 Spikes on shaft encoder output Figure 9.9 Spikes on shaft encoder supply
Figure 9.10 High frequency interference 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 robot’s 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 Chapter 9 - Operational Trials on Roboshift
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 robot’s 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 temper- ature 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 40
8 30 6
20 4 Current (Amps)
10 Temperature (Deg. C) 2
0 0
-2 -10 0 60 120 180 240 300 360 420 480 540 600 660 720 780 840
Time (Sec)
Main battery current Temperature
Figure 9.12 Hydraulic oil temperature and 12V current draw during engine test 9 - 10 Chapter 9 - Operational Trials on Roboshift 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 Roboshift’s 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 Chapter 9 - Operational Trials on Roboshift
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
Sagital BalanceFrontal Tuning Balance (Movement) 1507vlf
120 0.56
0.555 100
0.55 80
0.545 Mass 60 0.54
40 0.535
20 0.53 0 20 40 60 80 100 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 Chapter 9 - Operational Trials on Roboshift
Sagital BalanceFrontal Tuning Balance (Movement) 1507po
120 0.535
0.53 100
0.525 80
0.52 Mass 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 Chapter 9 - Operational Trials on Roboshift
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 Chapter 9 - Operational Trials on Roboshift 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 robot’s 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 robot’s 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 Chapter 9 - Operational Trials on Roboshift
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 robot’s shape so that the trim value fell between the set limits. While the
9 - 16 Chapter 9 - Operational Trials on Roboshift
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 Chapter 9 - Operational Trials on Roboshift
Sagital BalanceFrontal Tuning Balance (Movement) Trial# 1107blb
110 0.52
0.5
0.48 105
0.46
Position 0.44
100 0.42
0.4
95 0.38 0 50 100 150 200 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 sub- stantial 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 Chapter 9 - Operational Trials on Roboshift
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 robot’s feet were set apart so that
9 - 19 Chapter 9 - Operational Trials on Roboshift
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 Chapter 9 - Operational Trials on Roboshift
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 Chapter 9 - Operational Trials on Roboshift
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 expecta- tions, the robot’s frontal sway behaviour became more unpredictable and unstable with the widened feet. However, it was
Figure 9.24 Failure of foot rotation cylinder apparent that the foot rotation cylinders connection 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 Chapter 9 - Operational Trials on Roboshift
Figure 9.26 Three link biped model
Simplifications made during the derivation of the model include: • Cos = 1)( for small oscillations in the vicinity of the vertical. • Sin )( = 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;
LM 2 M gL + B ++ B = I L kk BA 0 2 2
M = where: B Mass of body L = Length of legs = I L Moment of inertia of each leg about its foot g = Gravitational constant. == k BB Control torque. == k AA Ankle torque.
9 - 23 Chapter 9 - Operational Trials on Roboshift
The period of oscillation of the system is calculated as:
M gL kk + B BA = 2 LM 2 I + B L 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 2 m 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 Chapter 9 - Operational Trials on Roboshift
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 Chapter 9 - Operational Trials on Roboshift
Frontal Sway Simulation
Figure 9.28 Simulation of robot coming to rest when released from central position with 1800nm/rad ankle torque
FrontalFrontal Sway SimulationSimmulation
15 1000
10 500
5 0 Degrees
0 -500 Control Value
-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 Chapter 9 - Operational Trials on Roboshift
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 respons- es 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 Chapter 9 - Operational Trials on Roboshift
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 Chapter 9 - Operational Trials on Roboshift
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 Roboshift’s body is slung from the hips, the length of the legs is as long as the robot is high. Effectively, Roboshift’s 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 Roboshift’s 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 Chapter 10 Discussion 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 Toyota’s (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. Honda’s Asimo, and now Toyota’s 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 Toyota’s 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 Chapter 10 Discussion 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 robot’s
10 -4 Chapter 10 Discussion 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 Chapter 10 Discussion 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 robot’s 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 robot’s 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 Chapter 10 Discussion
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 Chapter 10 Discussion 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 Chapter 10 Discussion
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 commu- nications 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 Chapter 10 Discussion
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 robot’s 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 biped’s 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 Chapter 10 Discussion 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. S e c o n d l y, th e p o s it i o n o f th e r o b o t ’s jo i n ts a r e m e a s u r e d e i th e r b y a n a l o g ue 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 Chapter 10 Discussion show the same values in each diagram, the curvature of the robot’s 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 v’s height of biped
10 -12 Chapter 10 Discussion 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 11ADDITIONAL 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 self- propelled 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 dynam- ic 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 con- ducted 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 robot’s 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 tor- sional stiffness of the simulated joint connections; = Total *WnW n
Where; n = Number of links used to simulate the joint. W W = Total n n
585.3 W == 2.1 mm n 3
11 -2 Chapter 11 Additional Modelling
Figure 11.2 FEA of robotlink.
Given the length of each link, the angle subtended by the deflection Q is calculated as; 2.1 = aQ tan = = rad.01.0.deg51.0 133
The torque produced at the joint is constant and equal to that of the original joint, it is calculated as;
dFT == = 4004.0*000,1* Nm
The stiffness of the springs is then calculated as; 400Nm K == /40 radkNm t 01.0 rad
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 Chapter 11 Additional Modelling that the only articulated joints of the robot in the frontal plane are those of the robot’s hips and ankles.
Figure11.3 Machine Model of Biped
The joint driver subsystems (Figure 11.4) are based as closely as possible on the indi- vidual 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 Chapter 11 Additional Modelling
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 Chapter 11 Additional Modelling
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
6
4
2
0 5 7 9 1113151719
-2 Change in Hip Position (Deg)
-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 v’s Joint Stiffness.
It can be seen that as stiffness increases position error decreases immediately indicat- ing 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 robot’s links are flexing as the mass of the robot is still decelerating towards the right of
11 -6 Chapter 11 Additional Modelling 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 robot’s 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 Chapter 11 Additional Modelling 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 over- shoot. As the stiffness of the joint actuators ( torque produced from control system gain) exceeds that of the link stiffness, the robot’s 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 Chapter 11 Additional Modelling 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 hyper- redundant, 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 Chapter 11 Additional Modelling
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
6 200
4 100
2
0
0 5 7 9 1113151719 -100 Change in Hip Position (Deg) Change in Ankle Reaction (N) -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 Chapter 11 Additional Modelling
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 Chapter 11 Additional Modelling
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 vehicle’s 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 Chapter 11 Additional Modelling
The chart found in figure 11.14 records the system’s 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 robot’s balance and will be discussed in later sections.
Biped Simmulation - Response to Step Input
10000 2150
2100
5000 2050
2000
0 01234561950
1900
-5000 position (mm)
1850 Torque Nm | Ground Reaction N
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 Chapter 11 Additional Modelling
Biped Simmulation - Response to Step Input - High Gain
15000 2100
2050 10000
2000
5000 1950
0 1900 0123456
1850 -5000 Torque Nm | Ground Reaction N 1800 Figure 11.15 -10000 Step response 1750 simulation with high -15000 1700 gain. 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 robot’slinks 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 2000 20
0 0 012345678910
-2000 -20
-40 Position (mm) -4000
-60 Torque (Nm) | Reaction Error (N) -6000 -80
-8000 Figure 11.16 -100 Response of simulation to -10000 -120 Square Pulse Time (Sec) force input.
Pulse Torque Reaction Error Position
11 -14 Chapter 11 Additional Modelling
11.5 SYSTEM RESPONSE TO SQUARE WAVE FORCE INPUT. Having established the system’s response to a step input, the next phase of the simula- tion 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 accelera- tion 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 sec- onds. 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 sig- nificant 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
System Response - 2000Nm pulse - K=20kNm/mm
6000 80
60 4000
40
2000 20
0 0 012345678910
-20 -2000 Position (mm)
-40
Torque (Nm) | Reaction Error (N) -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 Chapter 11 Additional Modelling
System Response - 2000Nm pulse - K=10kNm/mm
5000 80
4000 60
3000 40
2000 20
1000 0
0 012345678910-20 -1000
-40 Position (mm) -2000
-60 Torque (Nm) | Reaction Error (N) -3000 Figure 11.18 -80 -4000 Response of simulation to -5000 -100 Square Pulse force input, -6000 -120 k=10kNm/mm. Time (Sec)
Pulse Torque Reaction Error Position
System Response - Position
80
60
40
20
0 012345678910
-20
-40
-60 Torque (Nm) | Reaction Error (N) Figure 11.19 response of -80 the simulated control system -100 with various values of joint -120 stiffness. 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 Chapter 11 Additional Modelling 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 pro- duced by the robot’s 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 tem- porary increase in the foot reaction on the leg which was initially experiencing the high- er 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 robot’s control system, an algorithm was developed to filter actuation forces from those produced by the dynamics of the robot.
11 -17 Chapter 11 Additional Modelling
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 Chapter 11 Additional Modelling
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 deter- mine 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 robot’s 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
250
200
150
100
50 Reaction Difference (N) 0 0 20406080100 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 Chapter 11 Additional Modelling
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
10000 35
8000 30
6000 25 4000
20 2000
0 15 0 0.5 1 1.5 2 2.5 3
-2000 10
-4000 Torque Nm | Ground Reaction N 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 sig- nificant moment increases or decreases in the joint being controlled. Based on these results, an alternate method of control was required.
11 -20 Chapter 11 Additional Modelling
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 Chapter 11 Additional Modelling
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 Chapter 11 Additional Modelling
11 -23 Chapter 11 Additional Modelling
Reaction of System with and without Flex Feedback. 8000
6000
4000
2000
0 Reaction Error (N) 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
700
500
300 Reaction Error (N)
100
0.75 1.75 2.75 3.75 4.75 5.75 6.75 7.75 -100
-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 Chapter 11 Additional Modelling
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
600
400 Reaction Error (N) 200
0
-200 0.75 1.75 2.75 3.75 4.75 5.75 6.75 Flex Deriv. Gain = 0.0 Flex Deriv.Time Gain (sec) = 0.4 Flex Deriv. Gain = 0.55 Flex Deriv. Gain = 0.6 Flex Deriv. Gain = 0.7 Flex Deriv. Gain = 0.9
Figure 11.28 Effect of Strain Feedback Gain.
2200 Effect of Negative Feedback of Ground Reaction
1700
1200
700 Reaction Error (N) 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 Chapter 11 Additional Modelling
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 Chapter 11 Additional Modelling
1700 Effect of Negative Feedback of Integral of Ground Reaction
1200
700
200 Reaction Error (N)
-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 React Int Gain = 0.00015 React Int Gain = 0.0003
Figure 11.30 Effect of Integral of Ground Reaction Feedback Gain.
1100 Effect of Negative Feedback of Combinations of Input Gains
900
700
500 Reaction Error (N) 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 Disturbance (500N Step) RI 0.0003, RG -0.0015, FD -0.075, FG -6 RI 0.0003, RG -0.0015, FD -0.055, FG -6 RI 0.0004, 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 Chapter 11 Additional Modelling
Reaction of System to Step Input
500
300
100
-100 Reaction Error (N)
-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
400
300
200
100 Reaction Error (N)
0
-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 Chapter 11 Additional Modelling
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 Honda’s Asimo and Toyota’s 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 commercial- isation 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 inacces- sible 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 robot’s structure was designed for this
12 -2 Chapter 12 Conclusion 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 Chapter 12 Conclusion
able to be moved through their full range of movement. • The control hardware and software were able to calibrate the robot’s 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] World’s 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.
D’Souza 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/f- news/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 References & Bibliography
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 References & Bibliography
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 References & Bibliography
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 Two- link 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/home- main.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 References & Bibliography
Popovi_, D., Stein, R.B., Oguztöreli, 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 References & Bibliography
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] Man’s 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.tu- muenchen.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 References & Bibliography
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.science- news.org/articles/20010630/bob8.asp [Accessed 8/02/03]
Wolfe A., [2004] Toyota’s 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 References & Bibliography
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/peo- ple/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 References & Bibliography 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) p46- 50.
Bernard, J A. [1988] Use of a Rule Based System for Process Control, IEEE Control Systems Magazine, p3-12.
R - 10 References & Bibliography
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 References & Bibliography
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 References & Bibliography
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 CISM- IFTMM 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 References & Bibliography
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 References & Bibliography
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 References & Bibliography
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 mathe- matics 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 References & Bibliography
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 References & Bibliography
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
2 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.
3 APPENDIX
Appendix 1 Paper presented at CIRA 99, Monteray, California 5
Appendix 2 Paper presented at ACRAA ‘03, Brisbane, Australia. 12
Appendix 3 CROCK.C Dynamic Simulation Software used to 19 calculate joint accelerations during gait cycle.
Appendix 4 Walk009.C Autocad ADS Software written to 42 animate output of dynamic simulation
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 146 Communication Software
Appendix 11 HCRS_13.C M68HC11 to PC 148 Communications software.
Appendix 12 Multi_RS.C PC Software to communicate 152 with multiple M68HC11’s.
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
4 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 An anthropomorphic biped robot is presented. The 2) The study of bipedal control will result in a better advantage of legged locomotion in rough terrain and in understanding of the human gait and lead to devices that confined spaces has been investigated extensively in will assist with the mobility of people who have lost the recent years. Biped robotic research has focused on use of their legs. [Todd (8), Kato (9), Hemani (5), imitation of human anthropomorphic form with the Yamashita (16)] ultimate goal being the creation of the “Droid” or artificial human. An alternative use for biped robots is While these goals may be worthy, the cost of a biped proposed, as materials handling platforms for use in robot compared to that of a wheeled or tracked device confined spaces or areas where irregularities in terrain prohibits the commercialisation of biped robots for the prohibit access to conventional materials handling first proposition. vehicles. The operational criteria and design strategy for the device, which has been built, are defined. The major Significant research has been invested into the design of components of the robot’s sensory, navigation, actuation robotic-type orthotic devices to aid people suffering and control systems are highlighted. Finally, the control paraplegia. Even if such a device were to be realised, it software strategy and initial results are presented. would be prohibitively expensive to manufacture and maintain, placing it out of reach of all but the most Introduction wealthy patient. Further, the requirement for an on-board While mechanical walking machines have been proposed, power supply would render the device bulky and patented and built from the beginning of this century, it is cumbersome. With the current capabilities of batteries, it only since the availability of low-cost mini- and micro- would have a very short period of operation. Current computers that electronically-controlled devices have research in biomechanics suggests that functional become viable. The vast majority of bipedal walking electrical stimulation of nerves and muscles will be robots that have been built are modeled on the human significantly more viable. form. The geometry presented by an anthropomorphic device, and the inherent instability of bipedal locomotion, The support base of a biped is an order of magnitude increase both the complexity and cost of the device in smaller than that of any other vehicle. Bipeds also posses terms of construction and control hardware. Researchers the ability to turn in their own space and to lift heavy involved in this field have tended to justify their objects by adjustment of posture rather than by increasing endeavors in philanthropic terms. Such justification their support base. The ideal use for a biped device is includes; materials handling in confined, uneven terrain where a 1) In the future biped devices may replace humans forklift or other lifting device would be unsuitable. performing hazardous or degrading work [(2), (12)]. Possible situations would be field handling in a military
1 5 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 Honda’s 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;
Capable of lifting 500kg Completely self contained Robust both physically and electronically Able to work for long periods 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 Figure 1. Biped Robot degrees of freedom on each side of the body; Hip Abduction, Hip Extension, Hip Rotation, Knee Extension, associated with carrying the power source and all control Ankle Abduction, Ankle Extension and Toe Extension. systems “on board” be identified. To satisfy the Each degree of freedom is controlled and actuated by a requirement that the device be easily maintained, the hydraulic cylinder, except for hip rotation which is overwhelming majority of components must be available actuated by a hydraulic motor. “off-the-shelf”. By using standard, less specialised parts (hydraulic valves, cylinders, sensors, microprocessor Similar in some aspects to the biped robot developed by boards, etc), the cost of the project has also been reduced. 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 Biped Control that for the same height, the stride length of the robot As recognised by Hemami (4), the biped gait is notionally would be nearly double that which would be obtained if periodic and symmetrical, but unstable. The gait consists the “body” were placed on top of the hips. This of falling from one leg (accelerating in forward direction modification required the upper thighs to be separated, while decreasing potential energy) onto the other leg swinging on either side of the “body”. A photograph of (decelerating in the forward direction while increasing the robot, is shown in Figure 1. 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 Force inputs to stabilise motion are dealt with by the designed to be completely self contained. A 16 kilowatt, human on two levels. First, as highlighted by Sias (11), LPG powered engine powers all hydraulic and electrical the nervous centres of the midbrain develop the feed systems. Only by cutting “unbilical cords” can problems forward data for gait motion. Sensory organs such as the
2 6 vestibular system, provide feedback error data for the system). It also receives force data and positional data midbrain to initiate coarse posture corrections. Second, at from local joint processors. It communicates with the rest a lower level, fusimotor and Golgi tendons provide of the “body” through the communications computer (the propreoception and fine motor controls for precise spinal cord) which contains a 16 port RS-232 expansion positional control of individual joints. Learned functions board. 14 Motorolla M68-HC11 microprocessor and such as walking are initiated by the midbrain, but fine interface boards control local joint motion movements, such as rotations of the foot to equalise (Propreoception and fine motor control) via analogue reaction force across it, are controlled locally by “twitch” outputs and digital encoder position inputs. The muscles controlling the joint. microprocessors communicate with each other and with the control computer via the communications computer. These features of the human control system have been recognised by designers of biped robots. Sensors Furusho (10), Shih (15) and Gurfinkel (3) have all used An air-driven aircraft-type artificial horizon provides gaits which react to disturbances or feedback errors “pitch” and “roll” data, while an electronic flux-gate generated when actual trajectories fail to match computed compass provides “yaw” data on the spatial alignment of ones. Also recognised by biped robot researchers is the the robot’s “body”. These vestibular signals are complexity of equations governing biped motion. Furusho conditioned by an F1 micro-controller prior to RS-232 (10) found that his 5 link biped model resulted in non- transmission to the communications computer as an linear differential equations of order 10. 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 Figure 2. Schematic Diagram of Control System inputs from the artificial horizon and compass (vestibular
3 7 areas of un-recoverable instability (Figure 3), it is entirely An essential part of balance in the human is possible that controlled joint positions may never equate propreoception at the ankle and foot. Each ankle and foot to command positions. of the biped robot is fitted with eight strain gauges that measure the reaction force distribution over the length of The control computer also acts on data from the artificial the foot and the total reaction force on the foot. As well, horizon, maintaining gross balance in the frontal and each foot is fitted with micro switches to signal that each sagital planes by adjustment to hip and knee joint side of the foot is in contact with the ground. trajectories. Like the joint controllers, the main control software is reactive to errors in measured and expected An optical shaft encoder measures the angle of each joint. data. The software is based on a hierarchical structure The signal from the shaft encoder is fed to an interface where the higher level software functions generate gait board that keeps the absolute position of the joint, storing patterns and monitor the resulting motion. At a lower it at a single address. This relieves the micro-controller level, continuous monitoring of “hormone” variables from the task of interrupt-driven position tracking of the (pitch, roll, force distribution at feet) result in “reflex” 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 micro- controller 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 robot…The 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 Figure 3. Joint Trajectories and the local feedback control of joint actuation. During normal walk the control computer generates joint behaviour when boundary values are approached. Nashner trajectories from course kinematic and dynamic (7) found that muscular reactions in humans who were calculations. These equate to the high level, midbrain perturbed while walking “were rapid, large in magnitude generated locomotion patterns in humans, governing the and movement specific” ie, reflex actions. By monitoring movement of functionally related leg muscles (hips, legs, “hormone” variables in the communications computer, the and ankles). control computer and all the joint micro-controllers are able to react to serious instability in a coordinated Incremental demand positions are transmitted to the joint behaviour without the requirement for communication. F1 micro-controllers every 100ms. These then control the The over-riding behaviour of the biped is the maintenance hydraulic valves to achieve demand positions in the of stability within recoverable boundaries. This is the case following 100ms. Given that control trajectories are whether the robot is standing or walking. calculated using reduced-order dynamic equations, errors between actual motion and ideal motion are to be expected. Provided errors do not cause incursion into
4 8 Gait Generation Joint trajectories are generated parametrically from the The literature on the analysis, modelling and simulation of input of desired walking velocity. The amplitude of sway the human gait is inexhaustible. While some aspects of the required to allow complete movement of the leg in swing kinematics of the human gait may be relevant to biped phase governs the frontal sway period. All other sagital robotics, the same can not be said of human gait joint trajectory periods are derived from the frontal sway dynamics. Industrial biped robots would be designed to period. Given the abundance of power available, there is carry masses approaching their own structure mass. In the no requirement for the energy saving, locked-knee human case, such loads produce gaits characterised by movement of the leg in support phase. This allows stability optimisation rather than energy minimisation. minimisation of the potential/kinetic energy conversion Also, as previously discussed, on board electrical drive is during the gait cycle which results in a smoother load- not feasible for industrial scale bipeds. Once the decision carrying gait. It also allows the use of the support-phase is made to use petrol or gas powered high-speed engines knee joint to quickly change the position of the centre of as a power source, energy minimisation is no longer a gravity of the robot, enhancing balance in the sagital primary issue. plane.
However, for the purposes of modelling, joint trajectories Decoupling of Feet empirically measured by Braune (17) and parameterised Frank (1) defines a biped as “a machine in which the legs by Hartrum (18) were used during the initial design phase. and feet are algebraically connected”. By allowing the The modelling showed stabilising torques generated by feet micro-controllers to automatically adjust the the torso, head and upper limbs could be replaced by the orientation of the feet to available terrain, the order of motion of a single mass attached below the hips. The control equations is proportionally reduced. Using an analysis was written as an AutoCad ADS application, the inverted pendulum analogy, this equates to moving the output is shown in figure 4. 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.
5 9 that of required motion. Thus, while adding increased increase once desired motion has been sensed. stability control to a five-link biped, the feet do not It can also be seen from the second graph that the actual increase the complexity left hip abduction of the control software position rarely matches to that of a nine-link the demand position, biped. but remains within required error Results constraints. At the time of writing this paper, the robot is Conclusion currently undergoing A biped robot has been initial walking trials. designed, constructed All sensory, control and and is currently under communications trials. At the time of systems have been writing of this paper, tested and calibrated. the robot has been built Figure 5 shows a time- and all systems are lapse photograph of the functional. The robot undergoing communications and balance trials in which joint control routines an initial sway is have been calibrated brought to rest. While and tested. The robot the main body and legs can actively balance are moving (mid brain and react to disturbing control), the feet remain forces applied to it. The relatively motionless, biped will eventually be maintaining evenly completely self distributed force over contained and will the ground (local include all power “twitch” control). supply and processing equipment. There will Figure 6 shows data of be no umbilical cord. force on each foot and Although it is a nine- of movement of the left link device, the feet are hip abduction joint. It de-coupled by can be seen from the controlling them force graph that as the locally. All joint control robot moves to reduce is distributed to local force on one foot, there micro-controllers while is an initial increase in a central control Figure 5. The biped robot balancing. force on that foot. This computer generates gait is due to the dynamic patterns. 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
6 10 Dynamics and Designing Robot Locomotion Controls.”, IEEE Sagital Balance Tuning (Mass) Transactions on Automatic Control, ac22, pp963-972, 1977. 0910bld 3 V. S. Gurfinkel, “Walking robot with Supervisory Control”, 400 0.7 Mechanisms and Machine Theory, v16, pp31-36, 1981. 4 H. Hemami, “The Inverted Pendulum, and the Biped”, Mathematical 0.6 Bioscience, v(34), p95-110, 1977. 300 5 H. Hemami, “Aspects of Inverted Pendulum Problem for Modeling of 0.5 Locomotion Systems”, IEEE Transactions on Automatic Control, v24,
200 0.4 pp526-535, 1979. Trim 6 R.B.McGhee, “Some Finite State Aspects of Legged Locomotion” Mass (kg) 0.3 ,Mathematical Bioscience, P(2)(1)67-84, 1968. 100 7 L. M. Nashner, “Balance adjustments of the human perturbed while 0.2 walking”, Journal of Neorophysiology, v44, pp650-664, 1980. 8 D. J. Todd, Walking Machines, Kogan Page, 198?. 0 0.1 15 65 115 165 215 9 I. Kato, “Development of legged walking robot”, Hitachi Review, v36, Iteration pp71, 1987. 10 J.Furusho, Control of a Dynamic Biped Locomotion System”, Mass Lef t Mass Right Trim Journal of Dynamic Systems, Measurement and Control, V108, p111, 1986. 11 F. Sias, “Static Stability Problems in Biped Robot Design”, IEEE Sagital Balance Tuning (Movement) Transactions, p436, 1987. 0910bld 12 . F. Zheng, “Supervisory system for motion control of biped robot”, 105 0.7 IEEE Transactions on Automatic Control, pp1520-1524, 1988. 13 M Vukobratovic, “On the stability of Biped Locomotion”, IEEE 0.6 Transactions on Bio-Medical Engineering, BME17#1, pp25-36. 100
0.5 14 M. Raibert, “Legged Robots”, Communications of the ACM, V29, p499-514, 1986. 95 0.4 15 C. L. Shih, “Control of a Biped Robot during Support Phase”, IEEE Transactions on Systems, Man and Cybernetics, V22/4, pp729-735, 0.3
Position (counts) 1992. 90 16 T. Yamashita, ”On the stability of Bipedal Locomotion”, 0.2 Proceedings IEEE International Conference on Robotics and
85 0.1 Automation, pp49-57, 1993. 15 65 115 165 215 17 T. Hartrum, Computer Implementation of a Parametric Model of Iteration The Human Gait, Thesis, 1973. 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
7 11 Control of a Heavy Weight Biped Robot in the Frontal Plane
Joe Cronin [email protected]
Richard Willgoss [email protected]
Robin Ford [email protected]
School of Mechanical and Manufacturing Engineering The University of New South Wales Sydney, Australia
Abstract anthropomorphic device and the inherent instability of bipedal locomotion increase both the complexity and cost The advantage of legged locomotion in rough of the device in terms of construction and control terrain and in confined spaces has been hardware. investigated extensively in recent years. It is anticipated that in the next decade or so, the Researchers involved in this field have tended to justify prophecy that anthropomorphic biped robots will their endeavours in philanthropic terms, such as the work side-by-side with humans will be realised. suggestion that in the future biped devices may replace Recent successes in Japan have advanced the humans performing hazardous or degrading work field substantially. However, little research has [Golliday, 1977; Zheng, 1988]. While this goal may be been conducted into the realisation of an worthy, the cost of a biped robot compared to that of a industrial scale materials handling biped robot. wheeled or tracked vehicle detracts from Such a device presents challenges in terms of the commercialisation. Other applications will need to be magnitude of dynamic forces produced and of sought. the systems required to control it in real-time. A 500Kg, self-contained biped robot, called The support base of a biped is an order of magnitude Roboshift, has been built to explore these smaller than that of any other vehicle. challenges and to identify the significant aspects of control. This paper identifies issues associated Like humans, biped robots possess the capability of with the control of the biped in the frontal plane. turning in their own space and lifting heavy objects by Results from balancing trials are presented which adjustment of posture rather than by increasing their demonstrate the effect of ankle torque on the support base. Their support base can also be kept an order control of the biped. A model is then presented of magnitude smaller than that of any other lifting which demonstrates the effect of control forces devices. Accordingly, we see the exemplar target and of geometry on the response of the biped in application for a biped as being materials handling in frontal sway. It is shown that stiffness produced confined, uneven terrain where a forklift or other lifting by control torque, a function of the gain of the device would be unsuitable. Possible situations would be control system and the response of the joint, is field handling in a military environment, on board a ship the major factor in the dynamic characteristics of or industrial applications in the field such as geological or the system. mining context.
1 Introduction Some attempts have been made to realise a bipedal walking materials-handling robot such as the General Mechanical walking machines have been proposed, Electric Hardiman [Todd, 1987]. Research conducted in patented and built from the beginning of the 20th century, recent years has attempted to closely imitate the human but it is only since the advent of ready access to low-cost form. Recent successes in Japan have advanced the field computing that electronically-controlled devices have of Biped Robotics significantly. However, the ability of become viable. The geometry presented by an 1 12 human scale device to achieve heavy lifting is limited. maintained, the overwhelming majority of components Something larger is required. must be available off-the-shelf. Also, by using standard, This project endeavours to demonstrate that a less specialised parts, the cost of the project has been biped robotic materials-handling device is feasible. It is reduced. suggested that the design criteria for such a device to be industrially practicable are as follows: 3 Biped Control x Capable of lifting 500kg x Completely self-contained As recognised by Hemani [1977], the biped gait is periodic and symmetrical but unstable. The gait consists x Robust both physically and electronically of falling from one leg, accelerating in the forward x Easily maintained direction while decreasing potential energy, onto the other x Able to work for long periods leg, decelerating in the forward direction while increasing potential energy. If disturbed from either trajectory, the 2 Structure of Roboshift system requires a force input to stabilise it. Designers of biped robots have recognised these Unlike any previous or current anthropomorphic scale features of a biped control system. Furusho [1986], Shih biped, the body of Roboshift is located under the hips. In [1992] and Gurfinkel [1981] have all used regular gaits this configuration (See Figure 1) the stride length of the that react to disturbances or feedback errors generated robot would be nearly double that if the body were placed when actual trajectories fail to match computed ones. on top of the hips. This design requires the distance Also recognised by biped robot researchers is the between the hips to be substantially increased so that the complexity of equations governing biped motion. Furusho thighs can swing on either side of the body. [1986], for example, found that his 5 link biped model The robot is hydraulically driven as was the biped resulted in non-linear differential equations of order 10. robot developed by Waseda University and Hitachi [Kato, Provided with sufficient processing power, ultra-reliable 1987], WH-11. 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.
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 Figure 1 Schematic of Roboshift showing degrees of freedom of the vestibular system provide feedback error data for the the limbs. midbrain to initiate course posture corrections. Learned functions such as walking are initiated by the midbrain in Unlike other larger scale robots such as WH-11 this way. Secondly, at a lower level, fusimotor and Golgi and the biped developed by Furusho et al [1986], tendons provide propreoception and fine motor controls Roboshift has been designed to be completely self- for precise positional control of individual joints. The contained. A 16 kilowatt, LPG powered engine provides twitch muscles that control these joint positions do so all hydraulic and electrical power. Only by cutting locally, without reference to the midbrain. An example of umbilical cords can problems associated with carrying the this would be a rotation of the foot to equalise the reaction power source and all control systems on-board be force across it. 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 rigid stance. implemented in Roboshift. A primary control computer, 2. Lower the robot so that it balanced on the ground, representing the midbrain, computes the feed-forward with the control system still maintaining a rigid control data and monitors the inputs from the artificial stance position (see Figure 3). Note that due to horizon and compass, which represent the vestibular clearances and tolerances in the joints and system. It also receives force and positional data from compliance in the joint members, the robot would local joint processors. The primary control computer lean slightly to one side of the frontal plane and either communicates with the rest of the body through a forward or backward in the sagital plane as it settled communications computer, representing the spinal cord, under its own weight. which contains a 16-port RS232 expansion board. Local 3. Activate the active balancing program and observe joint motion, propreoception and fine motor control, is the response. controlled by 14 Motorola 68HC11 microprocessor and interface boards via analogue outputs and digital encoder position inputs. The microprocessors communicate with The active balancing program attempts to each other and with the control computer via the reposition the robot’s upper body so that the centre of communications computer. gravity is maintained within a specified tolerance-band of a condition in which there would be equal load on each 4 Control Of Frontal Sway foot.. The robot achieves this by abducting the hips equally and in opposite directions. The control input to the Sway in the frontal plane is fundamental to the balancing software in the frontal plane is the proportion of walking process. The desired effect of frontal sway is to weight on the left leg, as specified by a trim term that is 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: Trim = __ legleftweight __ weightlegleftweight __ legright
Figure 2 Schematic of control System Initial results show the balancing routine to be sensitive to three variables; 1. With the robot suspended above the ground, instruct x The tolerance-band set for balancing. the control system to hold the joints in a x The distance between the feet predetermined configuration suitable for sustaining a x The gain of the control system. 3 14 lack of symmetry in the response because the robot has Figure 4a shows time-history data of the left and limited ability to decrease angular momentum when the right hip position and the trim value as the robot actively centre of gravity is moving towards a position that brings balances. In this case, the limits for balance were set it nearly over one leg (ie trim § 0 or 1). Under these between 0.45 and 0.55. It can be seen that the robot conditions, when the stance width is less relevant, the balances successfully. When the limits were set between width of the foot is likely to become an important 0.48 and 0.52, see Figure 4b, the robot became unstable, determinant of balancing ability. continuously swaying in the frontal plane. Once a stable actively-controlled stance had been The robot balanced successfully when the trial was achieved, the next task was to generate controlled sway. conducted with the same tightened trim limits as above, by varying the hip abduction positions sinusoidally at a but with the valve control gain reduced i.e. a reduced frequency close to that which could be expected during proportional constant in the PI control loop. The locomotion. Initial attempts to generate frontal sway in corresponding increase in time response can be seen in the the biped have produced motion that is highly dynamic. decreases in the slope of the joint displacement curve. See Figure 7 which is taken from a video. From This demonstrated the relationship between balancing observation of the motion, it appeared that the robot was limits and the gain of the control system. When the gain oscillating at a natural frequency. Figure 6 shows the was high, the robot generated substantial momentum by transfer of weight from one leg to the other as the robot the time the trim fell between the set balance limits. While commenced frontal sway from rest. The video of the event the proportional constant in the control loop might be shows that the biped robot was swaying to the limit expected to account for any potential overshoot, there is a conditions discussed previously where the ground reaction
(a) (b)
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 provided by the addition in foot width is the failure of the approached the total weight of the robot. At this point, the foot rotation cylinder connections, see Figure 8. To non-supporting foot is seen to slide toward the supporting determine the cause of the changes in frequency a foot. This is caused by the moment created by the weight dynamic analysis was conducted. 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 Modelling Frontal Sway of Roboshift leg.. The biped then swayed back toward the non- supporting leg, now closer to the centre of gravity of the robot and subtending a larger angle at the hip, the Various methods have been used to dynamically corresponding increase in torque immediately reduced the model bipeds in the frontal plane. Hemani [1979, 1980] amplitude of sway. used Lagrangian dynamics to construct a constrained model and then linearised the system in the vicinity of the 5 Effect of Ankle Torque 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 One of the issues facing the designer of a biped control the frontal plane without the requirement for force sensing system is the role of ankle torque in control of frontal sway. Hemani models both the case where no ankle 1 The maximum available ankle torque occurs at the instant torque exists and where ankle torque is available. He prior to the edge of the foot lifting. It is calculated as the concludes that control in frontal sway is achievable in product of half the foot widthand the vertical ground both cases. The advantage of ankle torque in the frontal reaction at the foot 5 16 of constraints, such as ground reaction forces. Roboshift W k T Control torque. uses an alternative approach in which these forces are BB k Ankle torque. measured and used in the control routine, thereby W AA T 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 The period of oscillation of the system is calculated as: model shown in Figure 8 was devised to explore natural, § B gLM · uncontrolled, frequencies of vibration. It is of a three-link ¨ kk BA ¸ © 2 ¹ biped with parallel legs. Simplifications made during the Z 2 derivation of the model include: B LM I L x Cos T 1)( for small oscillations in the vicinity of 2 the vertical. The equation shows that the dominating x Sin T )( T for small oscillations in the vicinity parameters of the biped are the length of the legs and the magnitudes of torques at the ankle and body. of the vertical. Data used for this model corresponded to the first x As there can be no rotation of the body, D body 0 trials of the biped which had been conducted with a foot x The control torques W and W are equal and in width of 150mm. In this configuration, with full load on 3 4 one leg, these feet could provide an ankle stiffness of the same direction. approximately 7600Nm/radian with a maximum ankle x Ankle torques W 1 and W 2 are equal and in the torque of 333Nm. Hip abduction is controlled by a same direction. hydraulic cylinder mounted at the top of the body. For the purpose of the analysis, it will be assumed that these The equation of motion for the system is found to be; joints possess a similar stiffness to that of the ankles when locked. The remaining physical properties of the biped are 2 listed below: § LM · § gLM · ¨ I B ¸ kk B 0 ¨ L ¸D ¨ BA ¸T © 2 ¹ © 2 ¹ Length of Legs 2 m where; Mass of Body 300 kg Mass of legs 80 kg M B Mass of body Moment of Inertia of Legs 33 Kgm2 L Length of legs Ankle Stiffness 7600 Nm/rad I L Moment of inertia of each leg about its Maximum Ankle Torque 333 Nm foot Body Stiffness 2000 Nm/rad g 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 6 17 The simulation shows that with ankle torque only, be seen that the additional stiffness of the control system the biped displays a single natural frequency of oscillation changes the frequency of the system at the times when the as expected. When the ankle torque is increased to values control torque is acting. The greater the control torque the generated by the wider feet, it can be seen that the natural shorter the period of oscillation. Once the hip abduction frequency of the system increases as shown inFigure torque (stiffness) is not acting, the frequency reverts to 10(b). As would be expected with increased stiffness, the that of the natural system. The control torque is a function frequency of oscillation has been increased and the system of the gain of the control system and the dynamic has come to rest closer to the vertical. response of the joint being controlled. Further work will Neither of these one degree of freedom investigate the tuning of the gain of the control system for simulations can explain the additional frequency observed the dynamics of each joint. in the motion of the biped. The model was then expanded An inverted pendulum can only display harmonic to include control torque at the hips. The control software motion if the moment generated by gravity is overcome makes incremental changes to the position of the hip by a restoring torque. In the case of a biped robot in active cylinders based on the feedback error of the actual control, it has been shown that the restoring torque is position versus the target position. A time-history plot of primarily a function of the torque generated by the control the control values for the hip abduction cylinders during system. As the magnitude of that torque increases, the frontal sway can be seen in Figure 11. Control torques are period of oscillation will decrease. only active when the position error of the hip abduction The model used to analyse the system did not cylinders exceed a set value. Once active, their values are explain the source of the additional frequency observed after the addition of wider feet. An alternate explanation is proportional to angular displacement error. I.e.as the error that this frequency is a function of the compliance of the increases, the restoring force of the hip control cylinders joint. As the hip abduction cylinders move to reduce the increase proportionally. : Effectively the control torque error, the inertia of the system causes an amount of the acts as a tortional stiffness at the hips and can be modelled movement to be taken up by compliance. This compliance as a torque proportional to angular displacement. then results in an increase in torque which accelerates the The graph in Figure 12 shows the model used in joint in proportion to the inertia of the system. Effectively, Figure 10(a) reacting to a simulated control torque. It can 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 Frontal Sway Simulation 15 1500 110
1000 200 10
100 500
5 0
90
Degrees -50 0
Control 0 Position 100 -10 00
80 -5 -15 00
-10 -20 00 70 0 5 10 15 20 10 15 20 25 30 Seconds Time Angular Position Control Torque Left Hip Abduction Le ft H ip Control Right Hip Abduction Right Hip Control Figure 12. Simulation with control torque. Figure 11 Hip control values
7 18 control system which runs at a frequency of 7 Hz. The additional torque created by the addition of the wider feet [Kato, 1987] I. Kato. Development of legged walking increases the frequency response of the system decreasing robot, Hitachi Review, v36, pp71. the damping of the control frequency. Thus, as the width of the feet increases the stiffness of the system, higher [Furusho, 1986] J.Furusho. Control of a Dynamic Biped order responses will become apparent. This aspect of the Locomotion System. Journal of Dynamic Systems, system will be investigated in future work. Measurement and Control, V108, p111. 6 Conclusions [Sias, 1987] F. Sias, Static Stability Problems in Biped Robot Design. IEEE Transactions, p436. Widespread research being conducted into anthropomorphic service robots is certainly fascinating [Zheng, 1988] F. Zheng, Supervisory system for motion but these robots are not cost effective for the roles on control of biped robot. IEEE Transactions on which the research has been justified. It is as an industrial Automatic Control, pp1520-1524. scale materials handling platform that biped robots are more likely to be commercially viable. [Vukobratovic, 1970] M Vukobratovic. On the stability of An industrial scale biped robot has been built which is Biped Locomotion, IEEE Transactions on Bio- completely self-contained with on-board motive and Medical Engineering, BME17#1, pp25-36. electrical power. The research presented in this paper [Raibert, 1986] M. Raibert, Legged Robots. demonstrates the large forces involved with the control of Communications of the ACM, V29, p499-514. an industrial scale biped robot in sway in the frontal plane. It demonstrates that the major control factors are: [Shih, 1992] C. L. Shih. Control of a Biped Robot during x The geometry of the device Support Phase. IEEE Transactions on Systems, Man x The stiffness of its connection to the ground and Cybernetics, V22/4, pp729-735. x The stiffness of the control system at hip joints The effect of ankle torque stiffness is demonstrated [Yamashita, 1993] T. Yamashita. On the stability of in data obtained from balancing trials of the robot and Bipedal Locomotion. Proceedings IEEE International from a dynamic model of the system. Conference on Robotics and Automation, pp49-57 More trials are being conducted. Further areas of research will include an examination of the manipulation [Golliday, 1977] I Golliday. An Approach Analysing of the dynamic response of the system by controlling the Biped Locomotion Dynamics and Designing Robot active and passive stiffness factors. Locomotion Controls. IEEE Transactions on Once the robot has been stabilised in frontal sway, Automatic Control, ac22, pp963-972 walking will be attempted. [Gurfinkle, 1981] V. S. Gurfinkel. Walking robot with References Supervisory Control. Mechanisms and Machine Theory, v16, pp31-36 [Hemani, 1979] H. Hemani, Aspects of Inverted Pendulum Problem for Modeling of Locomotion [Hemani, 1977] H. Hemani, The Inverted Pendulum, and Systems. IEEE Transactions on Automatic Control, the Biped. Mathematical Bioscience, v(34), p95-110. v24, pp526-535. [Hartrum, 1977] T. Hartrum, Computer Implementation of [McGhee, 1968] R.B.McGhee, Some Finite State Aspects a Parametric Model of The Human Gait, Thesis. of Legged Locomotion Mathematical Bioscience, P(2)(1)67-84 [Honda, 2003] Honda Motor Corp www.honda.com/ASIMO/index.html accessed Sept [Nasluner, 1980] L. M. Nashner, Balance adjustments of 2003 the human perturbed while walking. Journal of Neurophysiology, v44, pp650-664.
[Todd, 1982]D. J. Todd, Walking Machines, Kogan Press.
8 19 CROCK.C 1 / 23
/* Program to calculate joint kinetics */
#include
/* 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, /* Check pointer */ *infile, /* Input file containing joint positions */ *outfile, /* Output file containing converted box positions */ *hip_out, /* Output file of new config joint positions */ *box_ang_pos_out, /* Box angular positions output file */ *box_lin_pos_out, /* Box linear positions output file */ *box_ang_acc_out, /* Box angular accelerations output file */ *box_lin_acc_out, /* Box linear accelerations output file */ *comment_out; /* Comment File */
static FILE *inpos, /* Joint positions input */ *outpos, /* Box positions output */
20 CROCK.C 2 / 23
*hip_mtn, /* Joint positions output */ *box_ap_out, /* Box angular positions out */ *box_lp_out, /* Box linear positions out */ *box_aa_out, /* Box angular accelerations output */ *box_la_out, /* Box linear accelerations output */ *comm_out; /* Comments out */ static double hip[35][3][2], /* Hip Coords */ kne[35][3][2], /* Knee Coords */ foo[35][3][2], /* Foot Coords */ ank[35][3][2], /* Ankle Coords */ toe[35][3][2], /* Toe Coords */ hel[35][3][2], /* Heel Coords */ knk[35][3][2], /* Neck and Head Coords */ sho[35][3][2], /* Shoulder Coords */ elb[35][3][2], /* Elbow Coords */ wri[35][3][2], /* Wrist Coords */ hed[35][3][2], /* Chin and Pelvis Coords */ hic[35][3][2], /* Centre of hip coord */ chi[35][3][2]; /* Chin Cooordiantes */ static int X = 0, /* Co-ordinate indecies */ Y = 1, Z = 2, L = 0, R = 1, side = 0, port = 0, /* Auxilary counters */ star = 1; 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 of Upper Arm */ cog_larm[35][3][2]; /* COG of lower Arm */ static double acc_head[35][3][2], /* Acc of Head */ acc_trnk[35][3][2], /* Acc of Trunk */ acc_uarm[35][3][2], /* Acc of Upper Arm */ acc_larm[35][3][2], /* Acc of lower Arm */ acc_hic[35][3]; /* Acc 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 of Head */ aacc_trnk[35][2], /* Aacc of Trunk */ aacc_uarm[35][2], /* Aacc of Upper Arm */ aacc_larm[35][2]; /* Aacc of lower Arm */ static double avel_head[35][2], /* avel of Head */ avel_trnk[35][2], /* avel of Trunk */ avel_uarm[35][2], /* avel of Upper Arm */ avel_larm[35][2]; /* avel of lower Arm */ static double length = 2.0, /*body length */ weight = 100.0; /* body weight */ static 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 */ static double length_head, /* length of Head */ length_trnk, /* length of Trunk */
22 CROCK.C 4 / 23
length_uarm, /* length of Upper Arm */ length_larm, /* length of lower Arm */ length_box, /* width of box */ sides_box; /* sides of box */ static double rmi_head, /* rmi of Head */ rmi_trnk, /* rmi of Trunk */ rmi_uarm, /* rmi of Upper Arm */ rmi_larm, /* rmi of lower Arm */ rmi_box; /* rmi of Box */ static double tim_inc = 0.1; static int board, j, k, wind, grind; /* Utility Counters */
/* Declare reaction calculation variables */ static double grav = 9.81, vconst; static 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; static 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; static double mom_head, mom_trnk, mom_larm_L, mom_larm_R, mom_uarm_L, mom_uarm_R, crx; static double fz_head, fz_trnk, fz_larm_L, fz_larm_R, fz_uarm_L, fz_uarm_R, lx_box, fg_trnk; static double fx_head, fx_trnk, fx_larm_L, fx_larm_R, fx_uarm_L, fx_uarm_R, lz_box; static 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 out */ box_lin_pos_out = "c:\\alien\\box_l_p.prn"; /* Lin pos out */ box_ang_acc_out = "c:\\alien\\box_a_a.prn"; /* Ang acc out */ box_lin_acc_out = "c:\\alien\\box_l_a.prn"; /* Lin acc out */ comment_out = "c:\\alien\\comm.prn"; /* Comment 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 = 0.156 * length; /* length of Head */ length_trnk = 0.346 * length; /* length of Trunk */ length_uarm = 0.173 * length; /* length of Upper Arm */ length_larm = 0.260 * length; /* length of lower Arm */ length_box = 0.150 * length; /* width of box */ sides_box = 0.07 * length; /* sides of box */
rmi_head =(length_head * length_head * mass_head)/12.0; rmi_trnk =(length_trnk * length_trnk * mass_trnk)/12.0; rmi_uarm =(length_uarm * length_uarm * mass_uarm)/12.0; rmi_larm =(length_larm * length_larm * mass_larm)/12.0; rmi_box =(mass_box / 12.0)*(sides_box*sides_box + 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) printf(" Input file loading error. \n");
hip[frame][X][L]=jline[0]; hip[frame][Z][L]=jline[1]; hip[frame][Y][L]=jline[2]; hip[frame][X][R]=jline[3]; hip[frame][Z][R]=jline[4]; hip[frame][Y][R]=jline[5];
kne[frame][X][L]=jline[6]; kne[frame][Z][L]=jline[7]; kne[frame][Y][L]=jline[8]; kne[frame][X][R]=jline[9]; kne[frame][Z][R]=jline[10]; kne[frame][Y][R]=jline[11];
ank[frame][X][L]=jline[12]; ank[frame][Z][L]=jline[13]; ank[frame][Y][L]=jline[14]; ank[frame][X][R]=jline[15]; ank[frame][Z][R]=jline[16]; ank[frame][Y][R]=jline[17];
foo[frame][X][L]=jline[18]; foo[frame][Z][L]=jline[19]; foo[frame][Y][L]=jline[20]; foo[frame][X][R]=jline[21]; foo[frame][Z][R]=jline[22]; foo[frame][Y][R]=jline[23];
toe[frame][X][L]=jline[24]; toe[frame][Z][L]=jline[25];
25 CROCK.C 7 / 23
toe[frame][Y][L]=jline[26]; toe[frame][X][R]=jline[27]; toe[frame][Z][R]=jline[28]; toe[frame][Y][R]=jline[29];
hel[frame][X][L]=jline[30]; hel[frame][Z][L]=jline[31]; hel[frame][Y][L]=jline[32]; hel[frame][X][R]=jline[33]; hel[frame][Z][R]=jline[34]; hel[frame][Y][R]=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]=jline[42]; sho[frame][Z][L]=jline[43]; sho[frame][Y][L]=jline[44]; sho[frame][X][R]=jline[45]; sho[frame][Z][R]=jline[46]; sho[frame][Y][R]=jline[47];
elb[frame][X][L]=jline[48]; elb[frame][Z][L]=jline[49]; elb[frame][Y][L]=jline[50]; elb[frame][X][R]=jline[51]; elb[frame][Z][R]=jline[52]; elb[frame][Y][R]=jline[53];
wri[frame][X][L]=jline[54]; wri[frame][Z][L]=jline[55]; wri[frame][Y][L]=jline[56]; wri[frame][X][R]=jline[57]; wri[frame][Z][R]=jline[58]; wri[frame][Y][R]=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[frame-2][X][L]-crx; sho[0][Z][L]=sho[frame-2][Z][L]; sho[0][Y][L]=sho[frame-2][Y][L]; sho[0][X][R]=sho[frame-2][X][R]-crx; sho[0][Z][R]=sho[frame-2][Z][R]; sho[0][Y][R]=sho[frame-2][Y][R];
elb[0][X][L]=elb[frame-2][X][L]-crx; elb[0][Z][L]=elb[frame-2][Z][L]; elb[0][Y][L]=elb[frame-2][Y][L]; elb[0][X][R]=elb[frame-2][X][R]-crx; elb[0][Z][R]=elb[frame-2][Z][R];
27 CROCK.C 9 / 23
elb[0][Y][R]=elb[frame-2][Y][R];
wri[0][X][L]=wri[frame-2][X][L]-crx; wri[0][Z][L]=wri[frame-2][Z][L]; wri[0][Y][L]=wri[frame-2][Y][L]; wri[0][X][R]=wri[frame-2][X][R]-crx; wri[0][Z][R]=wri[frame-2][Z][R]; wri[0][Y][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[frame-1][X][L]-crx; sho[1][Z][L]=sho[frame-1][Z][L]; sho[1][Y][L]=sho[frame-1][Y][L]; sho[1][X][R]=sho[frame-1][X][R]-crx; sho[1][Z][R]=sho[frame-1][Z][R]; sho[1][Y][R]=sho[frame-1][Y][R];
elb[1][X][L]=elb[frame-1][X][L]-crx; elb[1][Z][L]=elb[frame-1][Z][L]; elb[1][Y][L]=elb[frame-1][Y][L]; elb[1][X][R]=elb[frame-1][X][R]-crx; elb[1][Z][R]=elb[frame-1][Z][R]; elb[1][Y][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[frame-1][Y][L]; wri[1][X][R]=wri[frame-1][X][R]-crx; wri[1][Z][R]=wri[frame-1][Z][R]; wri[1][Y][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(" Centre of Gravity Calculation ** %5d \r", page); delay(10); }
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) { for( side = port; side <(star+1); side ++) {
/* 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], sho[grind][Z][side],elb[grind][X][side], elb[grind][Z][ side]); theta_larm[grind][side]=trutan( elb[grind][X][side], elb[grind][Z][side],wri[grind][X][side], wri[grind][Z][ side]); theta_trnk[grind][port]=trutan( knk[grind][X][port], knk[grind][Z][port],hic[grind][X][port], hic[grind][Z][ port]); theta_head[grind][port]=trutan( hed[grind][X][port], hed[grind][Z][port],knk[grind][X][port], knk[grind][Z][ port]);
} 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
for( side = port; side <(star+1); side ++) {
/* head */ avel_head[j-1][port]=(theta_head[j-1][port]-theta_head[j-2][port]) / tim_inc; avel_head[j][port]= (theta_head[j][port]- theta_head[j-1][port]) / tim_inc; aacc_head[j][port]= (avel_head[j-1][port]-avel_head[j-1][port]) / tim_inc; /* trnk */ avel_trnk[j-1][port]=(theta_trnk[j-1][port]-theta_trnk[j-2][port]) / tim_inc; avel_trnk[j][port]= (theta_trnk[j][port]- theta_trnk[j-1][port]) / tim_inc; aacc_trnk[j][port]= (avel_trnk[j][port]-avel_trnk[j-1][port]) / tim_inc; /* larm */ avel_larm[j-1][side]=(theta_larm[j-1][side]-theta_larm[j-2][side]) / tim_inc; avel_larm[j][side]= (theta_larm[j][side]- theta_larm[j-1][side]) / tim_inc; aacc_larm[j][side]= (avel_larm[j][side]-avel_larm[j-1][side]) / tim_inc; /* uarm */ avel_uarm[j-1][side]=(theta_uarm[j-1][side]-theta_uarm[j-2][side]) / tim_inc; avel_uarm[j][side]= (theta_uarm[j][side]- theta_uarm[j-1][side]) / tim_inc; aacc_uarm[j][side]= (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[frame-2][X][L]; acc_uarm[3][Z][L]=acc_uarm[frame-2][Z][L]; acc_uarm[3][X][R]=acc_uarm[frame-2][X][R]; acc_uarm[3][Z][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[frame-1][X][L]; acc_larm[2][Z][L]=acc_larm[frame-1][Z][L]; acc_larm[2][X][R]=acc_larm[frame-1][X][R]; acc_larm[2][Z][R]=acc_larm[frame-1][Z][R];
acc_uarm[2][X][L]=acc_uarm[frame-1][X][L]; acc_uarm[2][Z][L]=acc_uarm[frame-1][Z][L]; acc_uarm[2][X][R]=acc_uarm[frame-1][X][R]; acc_uarm[2][Z][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", cog_larm[wind][X][L], cog_larm[wind][Z][L], cog_larm[wind] [X][R], cog_larm[wind][Z][R]); fprintf( box_lp_out, " %8.1f %8.1f %8.1f %8.1f", cog_uarm[wind][X][L], cog_uarm[wind][Z][L], cog_uarm[wind] [X][R], cog_uarm[wind][Z][R]); fprintf( box_lp_out, " %8.1f %8.1f %8.1f %8.1f\n", cog_trnk[wind][X][L], cog_trnk[wind][Z][L], cog_head[wind] [X][L], cog_head[wind][Z][L]);
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);
for( wind = 0; wind < frame; wind ++) {
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);
for( wind = 2; wind < frame; wind ++) { 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);
for( wind = 2; wind < frame; wind ++) { fprintf( box_la_out, " %8.4f %8.4f %8.4f %8.4f", acc_larm[wind][X][L], acc_larm[wind][Z][L], acc_larm[wind] [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], acc_uarm[wind] [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], acc_head[wind] [X][L], acc_head[wind][Z][L]); } /*------*/ /* Routine to print out hip centre position data */ /*------*/ for( wind = 2; wind < frame; wind ++) { 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],
35 CROCK.C 17 / 23
vel_hic[wind][Z], acc_hic[wind][X], acc_hic[wind][Z]); }
printf(" \n"); printf(" \n"); printf(" \n"); printf(" Reaction Calculations\n"); printf(" ------\n"); printf(" 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 = 0.03 * length; lz_box_0 =-0.005 * length + 0.315 /* Average distance between COG & hip_c */; lx_box = lx_box_0; lz_box = lz_box_0;
Dx[0]=lx_box_0; Ax[0]=0.0; Vx[1]=-0.02; Dz[0]=lz_box_0; Az[0]=0.0; Vz[0]=vel_hic[1][Z]; Dm[1]=-2.0; Am[1]=0.0; Vm[1]=-16; Dd[1]=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]) / 1000; lx_larm_R =(elb[k][X][R]-cog_larm[k][X][R]) / 1000; lz_larm_L =(elb[k][Z][L]-cog_larm[k][Z][L]) / 1000; lz_larm_R =(elb[k][Z][L]-cog_larm[k][Z][L]) / 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]) / 1000; lx_uarm_R =(sho[k][X][R]-cog_uarm[k][X][R]) / 1000; lz_uarm_L =(sho[k][Z][L]-cog_uarm[k][Z][L]) / 1000; lz_uarm_R =(sho[k][Z][R]-cog_uarm[k][Z][R]) / 1000;
nx_uarm_L =(cog_uarm[k][X][L]-elb[k][X][L]) / 1000; nx_uarm_R =(cog_uarm[k][X][R]-elb[k][X][R]) / 1000; nz_uarm_L =(cog_uarm[k][Z][L]-elb[k][Z][L]) / 1000; nz_uarm_R =(cog_uarm[k][Z][R]-elb[k][Z][R]) / 1000;
/* calculate reactions at shoulders */ fx_uarm_L = fx_larm_L + mass_uarm * acc_uarm[k][X][L]; fx_uarm_R = fx_larm_R + mass_uarm * acc_uarm[k][X][R]; fz_uarm_L = fz_larm_L + mass_uarm *(acc_uarm[k][Z][L]+grav ); fz_uarm_R = fz_larm_R + mass_uarm *(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," Lower Arm"); fprintf(fname," Upper Arm"); fprintf(fname," Trunk"); fprintf(fname," Head \n"); fprintf(fname," Left Right"); fprintf(fname," Left Right");
41 CROCK.C 23 / 23
fprintf(fname," Left Right\n"); fprintf(fname,"XZXZ"); fprintf(fname,"XZXZ"); fprintf(fname,"XZXZ\n"); }
42 WALK1009.C 1 / 11
/* walk */
#include
#define ExtFuncCount 1 /* Must increase this count if new external functions are added */ int funcload _((void)); int funcunload _((void)); int dofun _((void)); double absd(double); int strods(char*, double*); char *exfun[] = {/*MSG0*/"walk"}; /* No "C:" -- to be called as an AutoLISP function, not as an AutoCAD command */ double absd(double);
/* 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; /* Normal result code (default) */ int stat;
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, /* Hip Coords */ kne_l, kne_r, /* Knee Coords */ foo_r, foo_l, /* Foot Coords */ ank_r, ank_l, /* Ankle Coords */ toe_r, toe_l, /* Toe Coords */ hel_r, hel_l, /* Heel Coords */ knk_m, hed_m, /* Neck and Head Coords */ sho_r, sho_l, /* Shoulder Coords */ elb_r, elb_l, /* Elbow Coords */ wri_r, wri_l, /* Wrist Coords */ hip_m, chi_m; /* 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"); /* Open the file. */
if (position == NULL) /* Return a "1" in the event */ { /* the file could not be found.*/ return(999); }
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]=jline[0]; hip_r[Y]=jline[1]; hip_r[Z]=jline[2]; hip_l[X]=jline[3]; hip_l[Y]=jline[4]; hip_l[Z]=jline[5];
kne_r[X]=jline[6]; kne_r[Y]=jline[7]; kne_r[Z]=jline[8]; kne_l[X]=jline[9]; kne_l[Y]=jline[10]; kne_l[Z]=jline[11];
ank_r[X]=jline[12]; ank_r[Y]=jline[13]; ank_r[Z]=jline[14]; ank_l[X]=jline[15]; ank_l[Y]=jline[16]; ank_l[Z]=jline[17];
48 WALK1009.C 7 / 11
foo_r[X]=jline[18]; foo_r[Y]=jline[19]; foo_r[Z]=jline[20]; foo_l[X]=jline[21]; foo_l[Y]=jline[22]; foo_l[Z]=jline[23];
toe_r[X]=jline[24]; toe_r[Y]=jline[25]; toe_r[Z]=jline[26]; toe_l[X]=jline[27]; toe_l[Y]=jline[28]; toe_l[Z]=jline[29];
hel_r[X]=jline[30]; hel_r[Y]=jline[31]; hel_r[Z]=jline[32]; hel_l[X]=jline[33]; hel_l[Y]=jline[34]; hel_l[Z]=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]=jline[42]; sho_r[Y]=jline[43]; sho_r[Z]=jline[44]; sho_l[X]=jline[45]; sho_l[Y]=jline[46]; sho_l[Z]=jline[47];
elb_r[X]=jline[48]; elb_r[Y]=jline[49]; elb_r[Z]=jline[50]; elb_l[X]=jline[51]; elb_l[Y]=jline[52];
49 WALK1009.C 8 / 11
elb_l[Z]=jline[53];
wri_r[X]=jline[54]; wri_r[Y]=jline[55]; wri_r[Z]=jline[56]; wri_l[X]=jline[57]; wri_l[Y]=jline[58]; wri_l[Z]=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); ads_command(RTSTR, line, RT3DPOINT, toe_l, RT3DPOINT, foo_l, RTSTR, nullstring, NULL); ads_command(RTSTR, line, RT3DPOINT, toe_l, RT3DPOINT, foo_l, RTSTR, nullstring, NULL); ads_command(RTSTR, line, RT3DPOINT, toe_l, RT3DPOINT, foo_l, RTSTR, nullstring, NULL);
/*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 red and blue and draw feet */ ads_command(RTSTR, layer, RTSTR, set, RTSTR, RED, RTSTR, nullstring, NULL); ads_command(RTSTR, line, RT3DPOINT, foo_l, RT3DPOINT, ank_l, RTSTR, nullstring, NULL); ads_command(RTSTR, line, RT3DPOINT, hel_l, RT3DPOINT, ank_l, RTSTR, nullstring, NULL); ads_command(RTSTR, layer, RTSTR, set, RTSTR, BLUE, RTSTR, nullstring, NULL); ads_command(RTSTR, line, RT3DPOINT, foo_r, RT3DPOINT, ank_r, RTSTR, nullstring, NULL); ads_command(RTSTR, line, RT3DPOINT, hel_r, RT3DPOINT, ank_r, RTSTR, nullstring, NULL);
/* Change layer to green and draw pelvis */ ads_command(RTSTR, layer, RTSTR, set, RTSTR, GREEN, RTSTR, nullstring, NULL); 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 Red and Blue and draw upper arms */ ads_command(RTSTR, layer, RTSTR, set, RTSTR, RED, RTSTR, nullstring, NULL); ads_command(RTSTR, line, RT3DPOINT, sho_l, RT3DPOINT, elb_l, RTSTR, nullstring, NULL); ads_command(RTSTR, layer, RTSTR, set, RTSTR, BLUE, RTSTR, nullstring, NULL); ads_command(RTSTR, line, RT3DPOINT, sho_r, RT3DPOINT, elb_r, RTSTR, nullstring, NULL);
/* Change Layer to Red and Blue and draw lower arms */ ads_command(RTSTR, layer, RTSTR, set, RTSTR, RED, RTSTR, nullstring, NULL); ads_command(RTSTR, line, RT3DPOINT, elb_l, RT3DPOINT, wri_l, RTSTR, nullstring, NULL); ads_command(RTSTR, layer, RTSTR, set, RTSTR, BLUE, RTSTR, nullstring, NULL); ads_command(RTSTR, line, RT3DPOINT, elb_r, RT3DPOINT, wri_r, RTSTR, nullstring, NULL);
/* Change Layer to green and draw Neck */ ads_command(RTSTR, layer, RTSTR, set, RTSTR, GREEN, RTSTR, nullstring, NULL); 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
54 THONG_1.C 2 / 48 float right_trim, /* load distribution on right foot */ front_trim, /* load distribution accross legs */ ft_rate, /* rate of change of front trim */ left_trim, /*load distribution left leg */ mass_left, /* load on left leg */ mass_right; /* load on right leg */ float pitchd, rolld; /* pitch and roll in degrees */ int SAG_ENABLE = 0, /* enable balance in the frontal plane */ LON_ENABLE = 0; /* enable longitudinal balance routine */ 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 bios_ticks; /* elapsed time in biosticks */ double lower_leg = 1057, /*lower leg length */ upper_leg = 1045; /* upper leg length */ double deg_rad = 0.0175, /*degrees to radians factor */ rad_deg = 57.273; /* radians to degree factor */ double phase = 0.0; /* gait phase */ double right_hip_ext, /* Right hip extension angle in (deg). */ left_hip_ext, /* Left hip extension angle in (deg). */ right_knee_ext, /* right knee extesnion angle in deg */ left_knee_ext, /* left knee extension angle in deg */ right_foot_ext, /* right foot extension angle in deg */ left_foot_ext; /* left foot extension angle in deg */ double beta_right, /* calculated knee angle */ beta_left, epsilon_left, /* intermediate anngles */ epsilon_right,
55 THONG_1.C 3 / 48
psi_left, psi_right; double right_hip_height, /* right hip height in mm */ left_hip_height, /* left hip height in mm */ abs_dist_right, /*absolute distane right */ abs_dist_left, /*absolute distance left */ right_hip_displ, /*x displacement of hip */ left_hip_displ; int bios_rate; /* bios_tick rate */ int logg = 0; int period = 180, /* Gait period */ ab_adj = 0; /* 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 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 /* Convert all data into the Communication package format byte 0: 'X' byte 1: 1 C3 C2 C1 Ps P13 P12 P11 byte 2: P2 byte 3: P3 byte 4: 1 S4 S3 S2 S1 Vs V13 V12 V11 byte 5: V2 byte 6: CRC P --- Position ( -500 - 500 ) C --- Command ( 0 - 7) S --- Status ( 0 - 15 ) V --- Velocity ( -300 - 300 ) CRC --- Checksum for byte 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 ) //chech the sign of position { sign = 1; input_data.position *= -1; } else { 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) // combine with Position sing *(cp+1)=*(cp+1)|0x08; else *(cp+1)=*(cp+1)&0xf7; /* 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 ) // byte 4 = Status | Vs | V1 {*(cp+4)= (*(cp+4)|0x4)&0xff;} else {*(cp+4)= *(cp+4)&0xfb;}; *(cp+5)=(temp2 | 0x80)&0xff; *(cp+6)=crc(cp+1,5); // calculate Checksum if (*(cp+6)==0xd || *(cp+6)==0xa) (*(cp+6))++; return ; } 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 )>>4); *(analog_out + 2)=0x30 |(analog_out1 & 0xf ); *(analog_out + 3)=0x30 |((analog_out2 & 0xf0 )>>4); *(analog_out + 4)=0x30 |(analog_out2 & 0xf ); *(analog_out + 5)=0x30 |((analog_out3 & 0xf0 )>>4); *(analog_out + 6)=0x30 |(analog_out3 & 0xf ); *(analog_out + 7)=0x30 |((analog_out4 & 0xf0 )>>4); *(analog_out + 8)=0x30 |(analog_out4 & 0xf ); *(analog_out + 9)=crc(analog_out+1,8); if (*(analog_out + 9)==0xd || *(analog_out + 9)==0xa ) (*(analog_out +9)) ++; } /* 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 7 / 48 // reading position with sign output_data->position =(*(cp+1)&0x07)* 100 +(*(cp+2)-0x30 )*10 +(*(cp+3)-0x30 ); if ( (*(cp+1)&0x8)!=0 ) output_data->position *=-1; // 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,<E_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 = analog_in1; anal_old_2 = analog_in2; anal_old_3 = analog_in3; anal_old_4 = analog_in4; analog_in1 = (*(analog_in+1)-0x30)*16 + (*(analog_in+2)-0x30); analog_in2 = (*(analog_in+3)-0x30)*16 + (*(analog_in+4)-0x30); analog_in3 = (*(analog_in+5)-0x30)*16 + (*(analog_in+6)-0x30); analog_in4 = (*(analog_in+7)-0x30)*16 + (*(analog_in+8)-0x30); } else { analog_in1 = anal_old_1; analog_in2 = anal_old_2; analog_in3 = anal_old_3; analog_in4 = anal_old_4; } 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 10 / 48 if (_fstrlen(LFI_IN)==0) 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 struct find_t ffblk; 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"); // if file exist, rename and read it memset (compass_data ,0,30); fp = fopen("ip06.old","r+"); if (fp == NULL) return 999; cm = fgets( compass_data,30,fp); fclose ( fp); 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; struct find_t ffblk; 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(); while ((a <300)&&((status =get_input()) == 0)) { 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, /* desired right foot balance point */ LEFT_POINT = 0.5, /* desired left foot balance point */ LAT_POINT = 0.5, /* desired distribution between feet */ FOOT_ZONE = 0.2, /* longitudinal foot balance zone */ LAT_ZONE = 0.05, /* sagital balance zone */ low_lim_left, /* left foot lower limit for long balance */ low_lim_right, /* left foot upper limit for long balance */ up_lim_left, /* right foot lower limit for long balance */ up_lim_right, /* right foot upper limit for long balance */ low_lat_lim_1, /* limits of lateral adjustment zone 1 */ up_lat_lim_1, /* limits of lateral adjustment zone 1 */ low_lat_lim_2, /* limits of lateral adjustment zone 2 */ up_lat_lim_2, /* limits of lateral adjustment zone 2 */ ft_new = 0.5, /* New Historiocal value for trim */ ft_old; /* 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, /* previous biostick value */ new_bios_ticks; /* current bios time */ int lat_bal_comp; /* lateral balance position compensation */ int screen_count = 0, body = 0; /* body geometry adjustment flag */ int R_LIM_IN, R_LIM_OUT, L_LIM_IN, L_LIM_OUT; /* 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 18 / 48 LTE_O.cmd = 3; RFO_O.cmd = 3; LFO_O.cmd = 3; RFI_O.cmd = 3; LFI_O.cmd = 3; RKE_O.cmd = 3; LKE_O.cmd = 3; RHE_O.cmd = 3; LHE_O.cmd = 3; RHA_O.cmd = 3; LHA_O.cmd = 3; RHR_O.cmd = 3; LHR_O.cmd = 3; RHE_O.position = 270; /* initial stance joint positions */ LHE_O.position = 285; RKE_O.position = 180; LKE_O.position = 160; RFO_O.position = 30 ; LFO_O.position = 30; RTE_O.position = 15; LTE_O.position = 15; RFI_O.position =-17; LFI_O.position =-29; RHA_O.position = 97; LHA_O.position = 82 ; RHR_O.position =-15; LHR_O.position = 0; kb_temp = 0; LON_COUNT_L = 0; /* flags to switch longitudinal control */ LON_COUNT_R = 2; RHA_CAL = RHA_O.position; /* set calibrated (post bal) hip ab posns */ LHA_CAL = LHA_O.position; 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; /* loop inc for sagital balance */ lat_bal_float = 0.0; /* initial compensation for sagital balance */ logg = 0; up_lat_lim_1 = LAT_POINT + LAT_ZONE; /* calculate balance limits */ 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; /* ______*/ /*| 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)) { new_bios_ticks = biostime(0, 0L); 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; new_bios_ticks = biostime(0, 0L); 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) /* ' key */ { LON_ENABLE = 0; LAT_ENABLE = 0; GYR_ENABLE = 0; SAG_ENABLE = 0; SWAY_ENABLE = 0; } if (kb_temp == 60) LAT_ENABLE = 4; /* < key */ if (kb_temp == 62) LON_ENABLE = 4; /* > key */ if (kb_temp == 63) SAG_ENABLE = 4; /* ? key */ if (kb_temp == 58) GYR_ENABLE = 4; /* : Key */ if (kb_temp == 124) /* | Key */ { SWAY_ENABLE = 4; SAG_ENABLE = 0; } 73 THONG_1.C 21 / 48 /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ /* Adjust body geometry */ /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ /* Adjust lateral hip position */ if (kb_temp == 57) /* 9 Key */ { RHA_CAL = RHA_CAL - 1; LHA_CAL = LHA_CAL + 1; } if (kb_temp == 55) /* 7 Key */ { RHA_CAL = RHA_CAL + 1; LHA_CAL = LHA_CAL - 1; } /* 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) RFO_O.position = RFO_O.position + 1; /* c Key */ if (kb_temp == 67) 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) LTE_O.position = LTE_O.position + 1; /* a Key */ if (kb_temp == 65) 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) RHR_O.position = RHR_O.position + 1; /* b Key */ if (kb_temp == 66) 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; up_lat_lim_1 = LAT_POINT + LAT_ZONE; /* calculate balance limits */ 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; } /* Toggle debug logging */ if (kb_temp == 61 ) /* = 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 = 1; analog_out2 = 10; analog_out3 = 100; analog_out4 = 255; To_HC11(); //send command to HC11 a = send_output(); 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); next(3,analog[1],analog_in2); next(3,analog[2],analog_in3); next(3,analog[3],analog_in4); 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 ( rol_m < roll_min ) roll_min = rol_m; if ( rol_m > 127 ) roll_min = 127; if (rol_m > roll_max) roll_max = rol_m; if (rol_m < 127) roll_max = 127; if ( trim_d < trim_min ) trim_min = trim_d; if ( trim_d > 50 ) trim_min = 50; if (trim_d > trim_max) trim_max = trim_d; if (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 = RFO_I.status & 1; /* Set foot contact limit */ R_LIM_OUT = RFO_I.status & 2; /* switch flags */ L_LIM_IN = LFO_I.status & 2; L_LIM_OUT = LFO_I.status & 1; /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ /* 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_2 ) lat_bal_float = lat_bal_float - 2 * lat_bal_inc; 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; /* send status */ LFI_O.status = 0; /* 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 > LAT_POINT - 2 * LAT_ZONE)) { RFI_O.status = 4; /* send status */ LFI_O.status = 4; /* to hc11 with enable bit */ } } /*:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::*/ 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 "); textcolor (DARKGRAY); for (j=0; j 84 THONG_1.C 32 / 48 textbackground(GREEN); cprintf("^"); textbackground(BLACK); textcolor (DARKGRAY); for (j=0; j cprintf("L_Foot "); textcolor (DARKGRAY); for (j=0; j cprintf("Front "); textcolor (DARKGRAY); for (j=0; j cprintf("Pitch "); textcolor (DARKGRAY); for (j=0; j cprintf("Roll "); textcolor (DARKGRAY); 85 THONG_1.C 33 / 48 for (j=0; j textcolor (DARKGRAY); cprintf(" Left Foot "); textbackground(GREEN); if (L_LIM_OUT == 0) textbackground(RED); cprintf(""); textbackground(BLACK); cprintf(""); textbackground(GREEN); if (L_LIM_IN == 0) textbackground(RED); cprintf(""); textbackground(BLACK); cprintf(""); textbackground(GREEN); if (R_LIM_IN == 0) textbackground(RED); cprintf(""); textbackground(BLACK); cprintf(""); textbackground(GREEN); if (R_LIM_OUT == 0) textbackground(RED); cprintf(""); textbackground(BLACK); 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 > pos:%3d CMD:%1d Status:%1d Vel: %3d \n\r", LFI_I.position,LFI_I.cmd,LFI_I.status,LFI_I.velocity); cprintf( "\n"); textcolor(LIGHTGREEN); 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); textcolor(LIGHTRED); 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"); textcolor(LIGHTGREEN); 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 > pos:%3d CMD:%1d Status:%1d Vel: %3d \n\r", RHR_I.position,RHR_I.cmd,RHR_I.status,RHR_I.velocity); cprintf( "\n"); textcolor(LIGHTRED); 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 > pos:%3d CMD:%1d Status:%1d Vel: %3d \n\r", 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 37 / 48 textcolor(CYAN); cprintf( "Nav Data: Pitch %3d Roll %3d Air %3d Head %5f\n\r", pp, rr, aa, comp); } 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); textcolor(LIGHTRED); 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); textcolor(LIGHTRED); 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); textcolor(LIGHTRED); 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 Head %5.2f\n\r", pit_m, pitchd, rol_m, rolld, comp); } /*##########################################################################*/ /* */ /* 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, // if delay is longer 40< and <60 c3 = 0, // if delay is longer 60< and <90 c4 = 0, // if delay is longer 90< and <120 c5=0 ; // if delay is longer 120< int ESC = 0; // ESC flag /* char test[7]; /*Test character string array */ time_t t_tmp1, t_tmp2; // 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 = 0; RTE_O.position=500; RTE_O.velocity =300; LTE_O.status = 0x02; LTE_O.cmd = 0; LTE_O.position=-500; LTE_O.velocity = 300; RFO_O.status = 0x03; RFO_O.cmd = 0; RFO_O.position=-123; RFO_O.velocity =-5; LFO_O.status = 0x04; LFO_O.cmd = 0; LFO_O.position=256; LFO_O.velocity = 5; RFI_O.status = 0x05; RFI_O.cmd = 0; RFI_O.position=-256; RFI_O.velocity =100; LFI_O.status = 0x06; LFI_O.cmd = 0; LFI_O.position=0; LFI_O.velocity =-100; RKE_O.status = 0x07; RKE_O.cmd = 0; 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; /* get the compass value, if it's > 370 */ /* send an error value */ horizon(); /* 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 = 7; /*H/h for LHA */ if (key_in == 0x68 ) LHA_O.cmd = 4; if (key_in == 0x74 ) LHA_O.cmd = 2; /* t for lhA cal */ if (key_in == 84 ) LHA_O.cmd = 6; /* t for lhA cal */ /* Right Hip Abduction */ if (key_in == 0x4e ) RHA_O.cmd = 7; /* N/n for RHA */ if (key_in == 0x6E ) RHA_O.cmd = 4; if (key_in == 0x79 ) RHA_O.cmd = 2; /* y for RHA cal */ if (key_in == 89 ) RHA_O.cmd = 6; /* y for RHA cal */ /* Left Hip Extension */ if (key_in == 0x4A ) LHE_O.cmd = 7; /* J/j FOR LHE */ if (key_in == 0x6A ) LHE_O.cmd = 4; if (key_in == 0x75 ) LHE_O.cmd = 2; /* u for lhE cal */ if (key_in == 85 ) LHE_O.cmd = 6; /* u for lhE cal */ /* Right Hip Extension */ if (key_in == 0x4D ) RHE_O.cmd = 7; /* M/m for RHE */ if (key_in == 0x6D ) RHE_O.cmd = 4; if (key_in == 0x69 ) RHE_O.cmd = 2; /* i for RHE cal */ if (key_in == 73 ) RHE_O.cmd = 6; /* i for RHE cal */ /* Left Hip Rotation */ if (key_in == 0x47 ) LHR_O.cmd = 7; /* G/g for LHR */ if (key_in == 0x67 ) LHR_O.cmd = 4; if (key_in == 0x71 ) LHR_O.cmd = 2; /* q for lhr cal */ if (key_in == 81 ) LHR_O.cmd = 6; /* q for lhr cal */ /* Right Hip Rotation */ if (key_in == 0x42 ) RHR_O.cmd = 7; /* B/b for RHR */ if (key_in == 0x62 ) RHR_O.cmd = 4; if (key_in == 0x77 ) RHR_O.cmd = 2; /* w for Rhr cal */ if (key_in == 87 ) RHR_O.cmd = 6; /* w for Rhr cal */ 95 THONG_1.C 43 / 48 /* Left Knee Extension */ if (key_in == 0x46 ) LKE_O.cmd = 7; /* F/f for LKE */ if (key_in == 0x66 ) LKE_O.cmd = 4; if (key_in == 0x65 ) LKE_O.cmd = 2; /* e for lhr cal */ if (key_in == 69 ) LKE_O.cmd = 6; /* e for lhr cal */ /* Right Knee Extension */ if (key_in == 0x56 ) RKE_O.cmd = 7; /* F/f for rKE */ if (key_in == 0x76 ) RKE_O.cmd = 4; if (key_in == 0x72 ) RKE_O.cmd = 2; /* e for rhr cal */ if (key_in == 82 ) RKE_O.cmd = 6; /* e for rhr cal */ /* Left Toe Extension */ if (key_in == 0x41 ) LTE_O.cmd = 7; /* A/a for lto */ if (key_in == 0x61 ) LTE_O.cmd = 4; /* Left Foot Rotation */ if (key_in == 0x53 ) LFI_O.cmd = 7; /* S/s for LFI */ if (key_in == 0x73 ) LFI_O.cmd = 4; /* Left Foot Extension */ if (key_in == 0x44 ) LFO_O.cmd = 7; /* D/d for LFO */ if (key_in == 0x64 ) LFO_O.cmd = 4; /* Right Toe Extension */ if (key_in == 0x5a ) RTE_O.cmd = 7; /* Z/z for rto */ if (key_in == 0x7a ) RTE_O.cmd = 4; /* Right Foot Rotation */ if (key_in == 0x58 ) RFI_O.cmd = 7; /* X/x for rFI */ if (key_in == 0x78 ) RFI_O.cmd = 4; /* Right Foot Extension */ if (key_in == 0x43 ) RFO_O.cmd = 7; /* C/c for RFO */ if (key_in == 0x63 ) RFO_O.cmd = 4; /*...... */ /* Left Foot Calibration */ if (key_in == 123) { 96 THONG_1.C 44 / 48 LTE_O.cmd = 6; /* Set all foot cylinders to stop */ LFO_O.cmd = 6; LFI_O.cmd = 6; } if (key_in == 91) /* Begin calibration */ { if (LTE_I.cmd != 2) { LTE_O.cmd = 2; /* send calibrate command until */ LFO_O.status = 0; /* toe HC11 sends back "2" to indicate */ LFI_O.status = 0; /* toe calibration is complete */ LTE_O.status = 0; } else if (LFI_I.cmd != 2) /* once toe calibratrion is complete */ { /* begin inside cylinder calibration */ LFI_O.cmd = 2; /* wait for a "2" to indicate completion */ } else if ((LFO_I.cmd ==2)&&((LTE_I.status&1)==0)) { LFO_O.status = 9; /* indicate calibration is complete */ LFI_O.status = 9; /* if outside cylinder hc11 returns "2" */ LTE_O.status = 9; /* 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 ==2)&&((RTE_I.status&1)==0)) { RFO_O.status = 9; RFI_O.status = 9; RTE_O.status = 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 = 1; analog_out2 = 10; analog_out3 = 100; analog_out4 = 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; while ((a <350)&&((status =get_input()) == 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); next(3,analog[0],analog_in1); next(3,analog[1],analog_in2); next(3,analog[2],analog_in3); next(3,analog[3],analog_in4); 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 #define ArraySize 100 FILE *ip_handle; FILE *op_handle; FILE *(log_handle[16]); FILE *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 2 / 19 char data_buf[400]; int Port_No_Array[ArraySize]; /*I have no idea why at this moment, just do what the book says*/ int Total_No_Port = 0; /* Total number of P747 Port*/ int Total_com = 0; /* Total number of normal com port;*/ int fatal_error =0; //fatal error flag - halts execution if true int system_time_var=0; //timing variable for delay routine delay(int n) void Init_MP(); // Declaration of multiport initialisation routine void Initialize(); // Declaration of system variable initialisation routine void Init_Screen(); // Declaration of routine to update screen void WriteTime(FILE *); // Declaration of routine to write time stamp to log files void System_Log(char *); // Declaration of routine to write text into log file void CloseAllFile(void); // Declaration of routine to close log files void CloseAllPort(void); // Declaration of routine to close ports int readdata(); // Declaration of routine to read output data and write to HC11 void Rece_and_Write(); // Declaration of routine to read data from hc11 and write void Port6_WR(); // Declaration of routine to read and write compas data void Port1_WR(); // Declaration of routine to read and write COM1 data void hang(int); // Declaration of routine to set a hang void check_com_line(char *); // 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 ms*/ { time_t time1; long tmp; tmp = n * system_time_var ; while (tmp) { tmp --; time(&time1); }; } // 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);}; // wait until end of second time(&tmp_time1); time(&tmp_time2); while ( tmp_time2 == tmp_time1) { system_time_var++; time(&tmp_time2); }; system_time_var = system_time_var / 1000; /*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 ++; WriteTime(sys_handle); 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 ++; WriteTime(sys_handle); 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) { WriteTime(sys_handle); fatal_error ++; fprintf(sys_handle,"Warning:%d Found ports\n",Total_No_Port); System_Log("System shut down"); }; WriteTime(sys_handle); 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) /*set all port to 9600,8 bits,None, 1 stop bit*/ { fprintf(sys_handle, "Port %d error %d",port,tmp1); WriteTime(sys_handle); 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); }; 107 MP2X.C 7 / 19 if ((tmp1 =sio_ioctl(6,B4800,BIT_8| P_NONE| STOP_1)) != 0) /*set port 6 to 4800,8 bits,None, 1 stop bit*/ { fprintf(sys_handle, "Port %d error %d",6,tmp1); WriteTime(sys_handle); fatal_error ++; System_Log("System Shut down,"); } sio_lctrl(6, 3); /* set DTR RTS on */ sio_flowctrl(6,0); /* set NO flow control */ sio_open(6); 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); WriteTime(sys_handle); fatal_error ++; System_Log("System Shut down,"); } sio_lctrl(port, 3); /* set DTR RTS on */ sio_flowctrl(port,0); /* set NO flow control */ sio_open(port); } WriteTime(sys_handle); 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 ) { WriteTime(sys_handle); fprintf(sys_handle, "Can not close port %d",port); } else { WriteTime(sys_handle); 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); WriteTime(sys_handle); 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 ) { // printf(" file not found "); 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]++;}; if ((cp=strstr(data_buf,"p10"))!= NULL){strncpy(output_buf[4],(cp +=3),8);op_count[4]++;}; if ((cp=strstr(data_buf,"p11"))!= NULL){strncpy(output_buf[5],(cp +=3),8);op_count[5]++;}; if ((cp=strstr(data_buf,"p12"))!= NULL){strncpy(output_buf[6],(cp +=3),8);op_count[6]++;}; if ((cp=strstr(data_buf,"p13"))!= NULL){strncpy(output_buf[7],(cp +=3),8);op_count[7]++;}; if ((cp=strstr(data_buf,"p14"))!= NULL){strncpy(output_buf[8],(cp +=3),8);op_count[8]++;}; if ((cp=strstr(data_buf,"p15"))!= NULL){strncpy(output_buf[9],(cp +=3),8);op_count[9]++;}; if ((cp=strstr(data_buf,"p16"))!= NULL){strncpy(output_buf[10],(cp +=3),8);op_count[10]++;}; if ((cp=strstr(data_buf,"p17"))!= NULL){strncpy(output_buf[11],(cp +=3),8);op_count[11]++;}; if ((cp=strstr(data_buf,"p18"))!= NULL){strncpy(output_buf[12],(cp +=3),8);op_count[12]++;}; if ((cp=strstr(data_buf,"p19"))!= NULL){strncpy(output_buf[13],(cp +=3),8);op_count[13]++;}; if ((cp=strstr(data_buf,"p20"))!= NULL){strncpy(output_buf[14],(cp +=3),8);op_count[14]++;}; if ((cp=strstr(data_buf,"p21"))!= NULL){strncpy(output_buf[15],(cp +=3),11);op_count[15]++;}; output_buf[1][7]= 0xd; output_buf[2][7]= 0xd; output_buf[3][7]= 0xd; output_buf[4][7]= 0xd; output_buf[5][7]= 0xd; output_buf[6][7]= 0xd; output_buf[7][7]= 0xd; output_buf[8][7]= 0xd; output_buf[9][7]= 0xd; output_buf[10][7]= 0xd; output_buf[11][7]= 0xd; output_buf[12][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(" communicating finish "); return 1; } 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) { WriteTime(sys_handle); 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)) { R_data = sio_getch(1); fprintf(log_handle[0],"%c",R_data); printf("%c",R_data); hang(100); Num_byte++; } fprintf( log_handle[0],"\n"); } memset(data_buf,0,50); Num_byte =(int)sio_iqueue(1); //if (Num_byte < 30) return; 114 MP2X.C 14 / 19 R_data = sio_getch(1); // while (R_data != 'P' && R_data >= 0) // R_data = sio_getch(1); if (R_data < 0) return; tmp = 0; do { data_buf[tmp]=R_data; tmp++; R_data = sio_getch(1); } 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); memset(data_buf,0,100); remove("ip08.dat"); rename("tmp","ip08.dat"); } else { open_retry[0]++; WriteTime(sys_handle); 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; memset(data_buf,0,50); Num_byte =(int)sio_iqueue(6); if (Num_byte < 30) return; R_data = sio_getch(6); 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++; R_data = sio_getch(6); } 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); memset(data_buf,0,100); //remove(ip_filename[0]); //rename("tmp",ip_filename[0]); } else { open_retry[0]++; WriteTime(sys_handle); 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); remove(op_filename[0]); if (R_data == 0 ) return; if ((error = sio_flush(6,2))!=0) { WriteTime(sys_handle); 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*/ { WriteTime(sys_handle); fprintf(sys_handle, "Port %d error %d",6,error); return; }; sio_lctrl(6, 3); /* set DTR RTS on */ sio_flowctrl(6,0); /* set NO flow control */ 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) { WriteTime(sys_handle); 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);}; remove(op_filename[0]); 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*/ { WriteTime(sys_handle); fprintf(sys_handle, "Port %d error %d",6,error); return; }; sio_lctrl(6, 3); /* set DTR RTS on */ sio_flowctrl(6,0); /* set NO flow control */ 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; }; // 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. if (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********************* \n"); }; k=0; hang(2); } 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 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; /* Main loop flag */ long int time[4], /* Historical time vector */ sex, /* Temporary seconds storage */ msex; /* temporary millisecond storage */ /* Communications variables */ int pos_diff_1, /* POS2 - POS1 */ pos_diff_2, /* POS3 - POS1 */ ROLLI, /* Inner roll switch status 1 = Off */ ROLLO; /* Outer roll switch status 1 = Off */ int i; /* temp counter */ int term = 0, /* velocity flag */ step[4], /* temp position vector */ chron[4], /* 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, /* calculated control value */ contr_o = 127, /* output integer control value */ pos_e = 0, /* position error */ pos_t = 0; /* target position */ int min_up = 165, /*minium value to go up and down */ min_dn = 90, ZEDS = 0, LAT_ENABLE = 0, FOOT_LIM = 3, roll_comp = 0; /*####################################################################*/ /*# Update Global Variables #*/ void update(void) { msex = msec; sex = sec; count1 = peek(0x1820); /* Read high count byte */ count2 = peek(0x1821); /* Read low count byte */ ROLLO = peek(0x1823)&0x02; /* Read outer roll switch */ ROLLI = peek(0x1823)&0x01; /* Read 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 3 / 12 time[1]=time[2]; /* Update time vector */ time[2]=time[3]; time[3]=sex + msex * 1000; pos_diff_1 = posn[3]-posn[2]; /* Calculate position diffs */ pos_diff_2 = posn[2]-posn[1]; 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]; chron[2]=chron[3]; 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]; chron[1]=chron[2]; chron[2]=chron[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 pos_e =(posn[3]-pos_t ); if ( pos_e <-1 ) contr = min_dn; if ( pos_e <-3 ) contr = min_dn - 2; if ( pos_e <-6 ) contr = min_dn - 4; if ( pos_e <-9 ) contr = min_dn - 8; if ( pos_e > 1 ) contr = min_up; if ( pos_e > 3 ) contr = min_up + 2; if ( pos_e > 6 ) contr = min_up + 4; if ( pos_e > 9 ) contr = min_up + 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; pos_e =(posn[3]-pos_t ); if ( pos_e <-1 ) contr = min_dn; if ( pos_e <-3 ) contr = min_dn - 2; if ( pos_e <-6 ) contr = min_dn - 4; if ( pos_e <-9 ) contr = min_dn - 8; if ( pos_e > 1 ) contr = min_up; if ( pos_e > 3 ) contr = min_up + 2; if ( pos_e > 6 ) contr = min_up + 4; if ( pos_e > 9 ) contr = min_up + 8; contr_o = contr; poke (0x1820, contr_o); CMD_OUT = CMD_IN; VEL_OUT = /* POS_IN */ LAT_ENABLE; STAT_OUT = STAT_IN; } 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]=0; time[1]=0; time[2]=0; time[3]=0; posn[0]=0; posn[1]=0; posn[2]=0; posn[3]=0; /* Initialise HC11 */ poke (0x1820, 0x7f); /* Set analogue outputto zero */ poke (0x1821, 0x01); /* Enable analogue output */ poke (0x1822, 0x01); /* 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 ) cycle(); if ( CMD_IN == zero ) reset_c(); if ( CMD_IN == cal ) calibrate(); if ( CMD_IN == stay ) stance(); if ( CMD_IN == hold ) place(); if ( CMD_IN == move ) speed(); if ( CMD_IN == crash) die(); if ( CMD_IN == stop ) cut(); CMD_OUT = CMD_IN; } /* 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 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 = peek(0x1810); /* Read high count byte */ count1_2 = peek(0x1811); /* Read low count byte */ count2_1 = peek(0x1820); /* Read high count byte */ count2_2 = peek(0x1821); /* Read low count byte */ 134 roll2.c 3 / 5 posn[1][1]=posn[1][2]; /* Update position vector */ posn[1][2]=posn[1][3]; posn[1][3]=count1_2 + count1_1 * 256; posn[2][1]=posn[2][2]; /* Update position vector */ posn[2][2]=posn[2][3]; posn[2][3]=count2_2 + count2_1 * 256; switches = peek(0x1823); sw1 = switches & 0x01; sw2 = switches & 0x02; sw3 = switches & 0x04; sw4 = switches & 0x08; sw5 = switches & 0x10; 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 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"); printf( " Program terminated.....\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); /* set NO flow control */ 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]=(D_POS / 1000 ); split[2]=(D_POS - split[3]*1000)/100; split[1]=(D_POS - split[3]*1000 - split[2]*100)/10; split[0]=(D_POS - split[3]*1000 - split[2]*100 - split[1]*10); split[4]=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]=split[3]+48; TX_pack[4]=split[2]+48; TX_pack[5]=split[1]+48; TX_pack[6]=split[0]+48; TX_pack[7]=split[4]+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 6 / 6 sio_timeout(18 * 5); /* set timeout value = 5 sec */ printf("\nFlush all ports input/output queue....\n"); for (port = 0; port < total_port; port++) { 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 int run; /* Main loop flag */ long int time[4], /* Historical time vector */ sex, /* Temporary seconds storage */ msex; /* temporary millisecond storage */ int POS1, /* Position / 100 */ POS2, /* Position / 10 */ POS3, /* Position / 1 */ STAT_IN, /* Input Status */ STAT_OUT, /* Out Status */ CHKSM, /* TX Checksum */ ROLLI, /* Inner roll switch status 1 = Off */ ROLLO; /* Outer roll switch status 1 = Off */ int pos_diff_1, /* POS2 - POS1 */ pos_diff_2; /* POS3 - POS1 */ /* 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]=0; time[1]=0; time[2]=0; time[3]=0; posn[0]=0; posn[1]=0; posn[2]=0; posn[3]=0; /* Initialise HC11 */ poke (0x1820, 0x7f); /* Set analogue outputto zero */ poke (0x1821, 0x01); /* Enable analogue output */ poke (0x1822, 0x01); /* Set encoder count to zero */ /**********************************************************************/ /* Start - Main Loop - while run > 0 */ /**********************************************************************/ while( run > 0) { msex = msec; sex = sec; count1 = peek(0x1820); /* Read high count byte */ count2 = peek(0x1821); /* Read low count byte */ ROLLO = peek(0x1823)&0x02; /* Read outer roll switch */ ROLLI = peek(0x1823)&0x01; /* Read inner roll switch */ posn[1]=posn[2]; /* Update position vector */ 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; pos_diff_1 = posn[3]-posn[2]; /* Calculate position diffs */ pos_diff_2 = posn[2]-posn[1]; STATUS = ROLLI + ROLLO; POS1 = posn[3]/100; POS2 = posn[3]-POS1 * 100; POS3 = posn[3]-POS1 - POS2 * 10; CHKSM = STATUS ^ POS1 ^ POS2 ^ POS3; /* Check to see if command is available and determine what command is */ if(packet) { INCHK = inbuffer[1]^inbuffer[2]^inbuffer[3]^inbuffer[4]; OUTPUT[0]=69; OUTPUT[1]=STATUS; OUTPUT[2]=POS1 + 48; OUTPUT[3]=POS2 + 48; OUTPUT[4]=POS3 + 48; OUTPUT[5]=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 1 / 4 #include char test[7]; /*Test character string array */ time_t t_tmp1, t_tmp2; /* Initialise test array */ test[0]=88; /* X */ test[1]=83; /* S */ test[2]=49; /* 1 */ test[3]=50; /* 2 */ test[4]=51; /* 3 */ test[5]=test[1]^test[2]^test[3]^test[4]; /* Checksum */ /* Initialise joint output strings */ for (i = 0; i < 6; i++) { RTE_OUT[i]=test[i]; LTE_OUT[i]=test[i]; RFO_OUT[i]=test[i]; LFO_OUT[i]=test[i]; RFI_OUT[i]=test[i]; LFI_OUT[i]=test[i]; 148 MULTI_RS.C 2 / 4 RKE_OUT[i]=test[i]; LKE_OUT[i]=test[i]; RHE_OUT[i]=test[i]; LHE_OUT[i]=test[i]; RHA_OUT[i]=test[i]; LHA_OUT[i]=test[i]; RHR_OUT[i]=test[i]; LHR_OUT[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( RTE_OUT[i]!=test[i])e_level = e_level + 1; if( LTE_OUT[i]!=test[i])e_level = e_level + 1; if( RFO_OUT[i]!=test[i])e_level = e_level + 1; if( LFO_OUT[i]!=test[i])e_level = e_level + 1; if( RFI_OUT[i]!=test[i])e_level = e_level + 1; if( LFI_OUT[i]!=test[i])e_level = e_level + 1; if( RKE_OUT[i]!=test[i])e_level = e_level + 1; if( LKE_OUT[i]!=test[i])e_level = e_level + 1; if( RHE_OUT[i]!=test[i])e_level = e_level + 1; if( LHE_OUT[i]!=test[i])e_level = e_level + 1; if( RHA_OUT[i]!=test[i])e_level = e_level + 1; if( LHA_OUT[i]!=test[i])e_level = e_level + 1; if( RHR_OUT[i]!=test[i])e_level = e_level + 1; if( LHR_OUT[i]!=test[i])e_level = e_level + 1; } /* printf("Rte is %s CRC is %d",RTE_IN, crc(RTE_IN+1,5) - RTE_IN[6]); printf("Lte is %s CRC is %d",LTE_IN, crc(LTE_IN+1,5) - LTE_IN[6]); printf("Rfo is %s CRC is %d",RFO_IN, crc(RFO_IN+1,5) - RFO_IN[6]); 150 MULTI_RS.C 4 / 4 printf("Lfo is %s CRC is %d",LFO_IN, crc(LFO_IN+1,5) - LFO_IN[6]); printf("Rfi is %s CRC is %d",RFI_IN, crc(RFI_IN+1,5) - RFI_IN[6]); printf("Lti is %s CRC is %d",LFI_IN, crc(LFI_IN+1,5) - LFI_IN[6]); printf("Rke is %s CRC is %d",RKE_IN, crc(RKE_IN+1,5) - RKE_IN[6]); printf("Lke is %s CRC is %d",LKE_IN, crc(LKE_IN+1,5) - 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 1 / 8 /* * HC11 Communications Package */ #include int GAS; /* port number for gas plasma display */ char tome[10]; /* result of itoa conversion */ int tyme; /* integer which is converted to character */ int messlen1, messlen2; /* message lengths of GAS outputs */ char *message1,*message2; /* gas outputs */ z = 0; message1 = "\rLoad S19 card "; /* header for gas display announcement */ message2 = "\r "; /* blank string for gas display */ messlen1 = 16; /* length of header */ messlen2 = 22; /* 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"); 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"); printf( " Program terminated.....\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"); 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); /* set NO flow control */ 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: strcpy(file_name,"rhr005.s19"); break; case 17: strcpy(file_name,"lhr005.s19"); break; case 18: strcpy(file_name,"rte005.s19"); break; case 19: strcpy(file_name,"lfo005.s19"); break; case 20: strcpy(file_name,"rfo005.s19"); break; case 21: strcpy(file_name,"analog.s19"); break; default: exit(0); }; printf("\n 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"); printf( " Program terminated.....\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 ( h == 1) printf( "\r-"); if ( h == 5) printf( "\r\\"); if ( h == 9) printf( "\r|"); if ( h == 13) printf( "\r/"); /*putchar(c);*/ /* Display the character actually sent to the screen */ q = fputc(c, output); /* Store the character to the output check file */ delay(1); for(jj=0; jj<100; jj++) { for (kk=0; kk<1000; kk++) { } } } while (c != EOF); i = sio_putch(p, EOF); c = 13; j = 0; t = 0; for (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( rawin[j+0]=='d') t = t+1; if( rawin[j+1]=='o') t = t+1; if( rawin[j+2]=='n') t = t+1; if( rawin[j+3]=='e') t = 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); for (port = 0; port < total_port; port++) { 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 HC11_DATA RTE_O,RTE_I,LTE_O,LTE_I; struct HC11_DATA RFO_O,RFO_I,LFO_O,LFO_I; struct HC11_DATA RFI_O,RFI_I,LFI_O,LFI_I; struct HC11_DATA RKE_O,RKE_I,LKE_O,LKE_I; struct HC11_DATA RHE_O,RHE_I,LHE_O,LHE_I; struct HC11_DATA RHA_O,RHA_I,LHA_O,LHA_I; struct HC11_DATA 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_in2; unsigned int analog_in3; unsigned int analog_in4; unsigned int analog_out1; unsigned int analog_out2; unsigned int analog_out3; unsigned int analog_out4; 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); /* store all data to a file and pass to mp*/ int crc(char *,int); /* 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 #ifdef WANT_SERVOS /* servo variables */ unsigned int servo_position[3]; /* MAX of 3 servos */ #endif #ifdef WANT_KEYBD /* keyboard variables */ static unsigned char keypatch[70]= { 0x55, 0x50, 0x4B, 0x46, 0x41, 0, 0, 0, 0x56, 0x51, 0x4C, 0x47, 0x42, 0, 0, 0, 0x57, 0x52, 0x4D, 0x48, 0x43, 0, 0, 0, 0x58, 0x53, 0x4E, 0x49, 0x44, 0, 0, 0, 162 F1INTTES.H 2 / 10 0x59, 0x54, 0x4F, 0x4A, 0x45, 0, 0, 0, 0x5A, 0x30, 0x31, 0x34, 0x37, 0, 0, 0, 0x20, 0x2E, 0x32, 0x35, 0x38, 0, 0, 0, 0x0A, 0x1A, 0x33, 0x36, 0x39, 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 POS_IN; /* Demand position from PC */ int POS_OUT; /* Current position in HC11 */ int VEL1; /* velocity /100 */ int VEL2; /* velocity - velocity / 100 */ int VEL_IN; /* Demand velocity from PC */ int VEL_OUT; /* Current velocity to PC */ int CMD_IN, /* Command input 0 - 5 from PC */ CMD_OUT, /* Command output 0 - 5 to PC */ STAT_IN, /* Input Status from PC */ STAT_OUT, /* Out Status to PC */ CHKSM, /* TX Checksum to PC */ INCHK; /* RX Checksum from PC */ char OUTPUT[7]; /* Output string */ int POS1; /* Position / 100 */ int POS2; /* Position / 10 */ int POS3; /* Position / 1 */ int sign; /* sign variable */ int comtemp; /*temp variable */ 163 F1INTTES.H 3 / 10 #endif #ifdef WANT_MOTORS /* Motor variables */ unsigned char motor_ctl; /* Motor control high nybble on/off, low nybble dir */ unsigned int speed[4]; /* Motor speed masks */ #endif #ifdef WANT_STEPPERS /* Stepper motor routines 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 4 / 10 unsigned char step_value; }; #ifndef SERVO1_NEUTRAL #define SERVO1_NEUTRAL 3050 /* Defines 1525 uS neutral position */ #endif #ifndef SERVO1_STEP #define SERVO1_STEP 17 /* Defines step size */ #endif #ifndef SERVO2_NEUTRAL #define SERVO2_NEUTRAL 3050 /* Defines 1525 uS neutral position */ #endif #ifndef SERVO2_STEP #define SERVO2_STEP 17 /* Defines step size */ #endif #ifndef SERVO3_NEUTRAL #define SERVO3_NEUTRAL 3050 /* Defines 1525 uS neutral position */ #endif #ifndef SERVO3_STEP #define SERVO3_STEP 17 /* Defines step size */ #endif const struct servo_param parms[3]={ { SERVO1_NEUTRAL, SERVO1_STEP }, { SERVO2_NEUTRAL, SERVO2_STEP }, { SERVO3_NEUTRAL, SERVO3_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 */ } if (servo_position[1]) { /* * 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"); asm(" std 0x18,x"); asm(" tsx"); /* Restore X */ } if (servo_position[2]) { 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 6 / 10 asm(" std 0x1a,x"); asm(" 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); /* actually do it */ return; } #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]^inbuffer[2]^inbuffer[3]^inbuffer[4]^inbuffer[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]-48 )*10 +(inbuffer[3]-48 ); if ((inbuffer[1]&8)!=0 ) POS_IN = POS_IN *-1; VEL_IN =(inbuffer[4]&3)*100 +(inbuffer[5]&0x7f); if (( inbuffer[4]&4)!=0 ) VEL_IN = VEL_IN *-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 = POS_OUT / 100; POS2 =(POS_OUT - POS1 * 100)/10 ; POS3 = POS_OUT - POS1*100 - POS2 * 10; if ( sign == -1 ) { POS1 = POS1 | 0x8; } OUTPUT[0]=88; if (CMD_IN == 101) OUTPUT[0]=101; OUTPUT[1]=POS1 |((CMD_OUT & 0x7)<<4 )|0x80 ; OUTPUT[2]=POS2+ 48; OUTPUT[3]=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