Jul 4, 2014 - Ammar Akhlaq. 101519-024 ... Ahmad who continuously enlightened our minds and lit our paths throughout the course of the project.
FINAL YEAR PROJECT REPORT
Wirelessly Controlled Navigation of 18 DOF Six-Leg Hexapod Robot using a PS2 Controller
A PROJECT REPORT Submitted by
Ammar Akhlaq
101519-024
Muhammad Bilal Arif Chaudhary
101519-032
Azzad Uddin
101519-183
in partial fulfillment of the requirements for the award of degree of
BACHELOR OF SCIENCE IN ELECTRICAL ENGINEERING SCHOOL OF ENGINEERING
UNIVERSITY OF MANAGEMENT AND TECHNOLOGY JULY, 2014
STATEMENT OF SUBMISSON
A report submitted to the Department of Electrical Engineering In partial fulfillment of the requirements for the Degree of Bachelor in Science in Electrical Engineering By Ammar Akhlaq Azzad Uddin Muhammad Bilal Arif Chaudhary
University of Management and Technology, Lahore July 4, 2014
Project Advisor ______________________________
Project Co-Advisor ______________________________
Page ii of x
Dedication This project, the result of 2 years of our hard work and life long struggle is dedicated to our beloved parents. Without their support, guidance, and prayers we would never have accomplished such an enormous task.
Page iii of x
Acknowledgments We are thankful to ALMIGHTY ALLAH who granted us with the wisdom, the strength, and the courage to complete this project. We would like to thank our project advisor Mr. Jameel Ahmad who continuously enlightened our minds and lit our paths throughout the course of the project. We consider ourselves extremely fortunate to have him as our advisor and be able to gain from his banks of knowledge. We would also like to thank our project co-advisor, Mr Asim Butt for his support.
Page iv of x
Abstract Robots, the product of human desire for a perfect servant, serve us in almost all areas of our life. We use robots not just for the industrial purposes but also to achieve task in hazardous environments where placing human beings would risk their lives. Our robot is made to serve that very purpose of saving lives in life threatening situations; it can reach places where the size of the human beings limit their potential. It finds application in places struck with natural disaster like an Earthquake, or landslide to complete search and rescue missions, human inflicted situations like searching and disabling a bomb, or to provide surveillance for the military. The design of the robot is inspired from the biological insects, their ability to navigate in uneven places was a great motivation for the design of our robot. 2 legged robots require a large array of sensors to keep them balanced but the increase in the number of legs keeps our robot stable without the need of several sensors. The hexapod is capable of providing 18 DOF freedom, 3 DOF per leg due to the 3 servo motors on each leg. The robot’s every motion is controlled through a PS2 controller. The robot is capable of providing wired as well as wireless control. The wired control is limited to the range of the controller’s wire while the wireless control is possible within a range of 15m. This hexapod can also be used as a prototype to implement a similar robot on a large scale to replace vehicles like a tank, to carry payload from one place to another, assist a smaller hexapod in the search and rescue missions.
Page v of x
Contents STATEMENT OF SUBMISSON ........................................................................................................ ii Dedication ............................................................................................................................................. iii Acknowledgments ................................................................................................................................ iv Abstract.................................................................................................................................................. v List of Figures ......................................................................................................................................... ix Chapter 1
Introduction ................................................................................................................... 1
1.1
Problem Statement................................................................................................................ 1
1.2
Objectives............................................................................................................................... 1
Chapter 2
Theoretical Background and Review of Literature ................................................... 2
2.1
History of Robots .................................................................................................................. 2
2.2
The Design of Robots ............................................................................................................ 2
2.2.1
Dynamics of Robotic Locomotion ................................................................................ 3
2.2.2
Legged Robotics ............................................................................................................ 3
Practical Uses ........................................................................................................................ 4
2.3
Chapter 3
Details of Theoretical Design ....................................................................................... 6
3.1
Initial Approach and Setbacks............................................................................................. 6
3.2
Learning Pointers.................................................................................................................. 7
3.3
Mathematical Structure ....................................................................................................... 8
3.4
Realization of the Structure .............................................................................................. 12
3.4.1
Material Issue .............................................................................................................. 12
3.4.2
Search for the Right Person ....................................................................................... 12
3.4.3
Making the Design on paper ...................................................................................... 13
Chapter 4
Practical Implementation ........................................................................................... 18
4.1
The Wooden Design ............................................................................................................ 19
4.2
Casting ................................................................................................................................. 19
4.3
Lathe Worker’s Tasks ........................................................................................................ 19
4.4
Assembling the Robot ......................................................................................................... 20
4.5
Advantages of Design .......................................................................................................... 21
Chapter 5
Motion Kinematics ...................................................................................................... 22
5.1
Our Approach towards the Motion of the Hexapod ........................................................ 22
5.2
Deriving the Inverse kinematic Equations........................................................................ 22
5.3
Algorithm development: ..................................................................................................... 31
5.3.1
Mathematical translation of mechanical structure: ................................................. 33
5.3.2
Human-Machine Interface: ........................................................................................ 38
5.3.3
Walking Mode ............................................................................................................. 38
Page vi of x
5.3.4
Translation mode: ....................................................................................................... 40
5.3.5
Rotation mode ............................................................................................................. 40
5.3.6
Single leg mode:........................................................................................................... 41
5.3.7
Calculations for angles................................................................................................ 42
Chapter 6
Remote Control ........................................................................................................... 45
6.1
Need for Control ................................................................................................................. 45
6.2
Control Method Selection................................................................................................... 45
6.3
PS2 Controller .................................................................................................................... 46
6.3.1
Pins of the Controller .................................................................................................. 46
6.3.2
Connecting the 2 controllers ...................................................................................... 48
6.3.3
Communication ........................................................................................................... 53
6.3.4
Byte wise communication ........................................................................................... 54
6.3.5
Understanding the PS2 Controller’s Response ........................................................ 55
6.3.6
Using the Configuration Mode................................................................................... 56
6.3.7
PS2 Wireless Controller ............................................................................................. 58
6.3.8
PS2 communication Algorithm .................................................................................. 60
Chapter 7
Testing and Evaluation ............................................................................................... 62
Chapter 8
Results and Error Analysis ........................................................................................ 63
Chapter 9
Component Selection and Bill of Material ................................................................ 64
9.1
Component Order Guide.................................................................................................... 64
9.2
Bill of Material .................................................................................................................... 64
Chapter 10
Difficulties Faced ......................................................................................................... 65
Chapter 11
Manufacturability, Usability and Sustainability ...................................................... 66
Chapter 12
Future Development ................................................................................................... 67
Chapter 13
Societal Issues .............................................................................................................. 68
13.1
Ethical .................................................................................................................................. 68
13.2
Social .................................................................................................................................... 68
13.3
Economical........................................................................................................................... 68
13.4
Health and Safety ................................................................................................................ 68
13.5
Environmental Impact........................................................................................................ 68
Chapter 14
Conclusions and Recommendations .......................................................................... 69
References ............................................................................................................................................ 70 Appendices ........................................................................................................................................... 71 Appendix A ...................................................................................................................................... 71 Appendix B ...................................................................................................................................... 74 Appendix C ...................................................................................................................................... 79 Page vii of x
Appendix D .................................................................................................................................... 105
Page viii of x
List of Figures Figure 1 Cardboard Design...................................................................................................................... 6 Figure 2 Smoothing the Plate's Surface .................................................................................................. 6 Figure 3 Making Holes for Servos ........................................................................................................... 6 Figure 4 Servo Brackets ........................................................................................................................... 7 Figure 5 The Model Ready to be Joined.................................................................................................. 7 Figure 6 Assembled Model ..................................................................................................................... 7 Figure 7 Leg joined with Attached Servos............................................................................................... 8 Figure 8 Testing the Servos ..................................................................................................................... 8 Figure 9 5 Joints in Arthropods ............................................................................................................... 9 Figure 10 Coxa joints highlighted as dot on the circle ............................................................................ 9 Figure 11 Distance between 2 adjacent Coxa joints 'A' and between 2 opposite Coxa jonits '2A' ...... 10 Figure 12 Meeting of Tibia joints due to distance between adjacent Coxa joints................................ 10 Figure 13 Difference of 30o between the frame of reference of the leg and that of the body of a hexagon................................................................................................................................................. 11 Figure 14 Difference of 60o between the frame of reference of the leg and that of a square shaped body ...................................................................................................................................................... 11 Figure 15 Difference of 900 between the frame of reference of the leg and that of a triangle shaped body ...................................................................................................................................................... 11 Figure 16 Shape formed by placing dots at 130mm apart (not to scale) ............................................. 13 Figure 17 Circle formed by connecting the dots at radius of 130mm (not to scale) ............................ 13 Figure 18 Hexagon formed by connecting the Coxa joints ................................................................... 14 Figure 19 Connecting the opposite Coxa joints .................................................................................... 14 Figure 20 Creating the Coxa joint with the width of Servo motors ...................................................... 14 Figure 21 Erasing the outer circle and the hexagon ............................................................................. 15 Figure 22 The inner circle outlining the body of the hexapod .............................................................. 15 Figure 23 Final design of the central body ............................................................................................ 15 Figure 24 The ellipse with edge distance equal to length of Tibia ....................................................... 16 Figure 25 The ellipse with its major axis ............................................................................................... 16 Figure 26 The ellipse intersected by a circle at top and an ellipse at the bottom ................................ 17 Figure 27 Final design of the leg ........................................................................................................... 17 Figure 28 The central body on chart paper........................................................................................... 18 Figure 29 The final design of leg on chart paper .................................................................................. 18 Figure 30 Final design on cardboard ..................................................................................................... 18 Figure 31 Wooden design for casting process ...................................................................................... 19 Figure 32 Lathe worker working in the Workshop Lab ......................................................................... 20 Figure 33 Side View of the leg with angle of rotation of Coxa Joint ..................................................... 23 Figure 34 Top view of the leg with angle of rotation of Femer and Tibia joint .................................... 23 Figure 35 Side view of the leg for the calculation of angle along Z axis ............................................... 24 Figure 36 Triangular representation of Figure 35 ................................................................................. 25 Figure 37 Geometric representation of side view of leg for calculation of angles ............................... 25 Figure 38 Triangle representing law of cosines .................................................................................... 26 Figure 39 Geometric representation of body for calculation of body kinematics................................ 28 Figure 40 Geometric representation for driving equation to perform rotation along its own axis ..... 29 Figure 41 Triangular representation of Figure 40 ................................................................................. 30 Figure 42 Broad overview of hexapod’s functions ............................................................................... 32 Figure 43 Distance between 2 adjacent Coxa joints 'A' and between 2 opposite Coxa jonits '2A' ...... 33
Page ix of x
Figure 44 No virtual hexapod is formed ............................................................................................... 34 Figure 45 Virtual hexapod with side length 130mm ............................................................................. 34 Figure 46 Representation of virtual leg ................................................................................................ 36 Figure 47 Graphical representation of hexapod ................................................................................... 36 Figure 48 Mode selection through PS2 controller ................................................................................ 38 Figure 49 Metachronical gait “wave gait” and its time diagram .......................................................... 39 Figure 50 Ripple gait and it time diagram ............................................................................................. 39 Figure 51 Algorithm’s walking gait module .......................................................................................... 39 Figure 52 Algorithm's translation module ............................................................................................ 40 Figure 53 Algorithm’s rotation module ................................................................................................ 41 Figure 54 Algorithm’s single leg control module .................................................................................. 41 Figure 55 Wireless 5-channel RF controller .......................................................................................... 45 Figure 56 Pins inside a male connector of the PS2 controller with pin names .................................... 46 Figure 57 Connections between the PS2 controller and PIC microcontroller ...................................... 47 Figure 58 Circuit diagram of the PS2 controller to PIC microcontroller interface ................................ 48 Figure 59 PS2 wired controller .............................................................................................................. 49 Figure 60 Male connector of PS2 controller to connect the female header of the wires .................... 49 Figure 61 Wire with femal and male header ........................................................................................ 49 Figure 62 PS2 controller connected to the circuit using female to male headers ................................ 50 Figure 63 Male connector of PS2 controller connect with female headers ......................................... 50 Figure 64 PS2 to USB converter ............................................................................................................ 51 Figure 65 PS2 controller extension cable.............................................................................................. 51 Figure 66 Female connector cut from the PS2 convertor..................................................................... 51 Figure 67 PS2 connector to board connector ....................................................................................... 51 Figure 68 PS2 controller connected to the breadboard using the connector ...................................... 52 Figure 69 Close look at the circuit with the connector ......................................................................... 52 Figure 70 PS2 Controller communication tree...................................................................................... 61
Page x of x
Chapter 1 Introduction
1.1
Problem Statement
The aim of the project is to design a 6-legged robot (hexapod) that is capable of 18 degree of freedom and control its motion using a PS2 controller. The choice of 6 legs is based on the disadvantages of using a robot with less legs; a two legged robot’s center of gravity changes continuously, the movements of a three legged robot would be less stable, a four legged robot cannot turn about its axis, a five legged robot would be asymmetric. A 6 legged robot can not only turn all these disadvantages into advantages but it can also be configured to move with less legs if the condition requires it to do so. The PS2 controller was chosen to control the motion to avoid a non-professional look and use a controller that is a result of long term research instead of making a controller just to suit our minimum requirements; the PS2 controller provides us with the freedom to use different combination/sequence of buttons for different functions. A wheeled robot will face disadvantages when moving on uneven surfaces; it will struggle with non-stable movements on the uneven surface, it won’t be able to get back on its wheels if overturned, its tires will slip on wet surfaces. A small two legged robot is also at a serious disadvantage when it has to walk on an uneven surface; it’s continuously changing center of gravity will need to be reinforced by some sensors to keep it standing on its two legs. Whereas, a six-legged robot has its biggest advantage when faced with motion on such rough terrain. The 6-legged robot will be able to move on the rough terrain even if one or more of its legs gets damaged. On a rough terrain the robot can walk like a natural predator.
1.2
Objectives
Designing a completely functioning robot of this level in our university was considered a humungous task and we were asked by many teachers to give it up and work on just a small portion of the robot but our goal was clear and we weren’t going to be obstructed by anyone. We knew that at our level it would be more than enough to present a functioning robot and adding extra features such as sensors, camera, and autonomous motion can be kept for later additions. So we kept ourselves restricted to the designing, walking algorithm and controlling of the hexapod.
1 of 111
Chapter 2 Theoretical Background and Review of Literature
2.1
History of Robots
Robots are a direct product of human desire for perfect servants that do not complain, do not get tired and do the assigned tasks whenever required to and with a higher efficiency too. The term “Robot” was first used by Karel Capek in his play “R.U.R” (Rossuum’s Universal Robots) in 1921, the word comes from the Czech word “robota” meaning “compulsory labor”. But the human desire to automate tasks is older than Christ; around 200 B.C. Greek inventor Ctesibus designed water clocks that had movable figures on them. The water clocks were built to replace the hour glasses that had to be turned over after all the sands had fallen. Other notable works in the field of robotics include Issac Asimov’s book “Runaround”, which was a book about robots and was written in 1942, it contained the “Three Laws of Robotics”: A robot may not injure a human being or, through inaction, allow a human being to come to harm. A robot must obey the orders given to it by human beings, except where such orders would conflict with the first law. A robot must protect its own existence as long as such protection does not conflict with the first or second law. [1] In 1865, John Brainerd created the Steam Man which used to wheeled carts. His design was improved by Frank Reade Jr. in 1885, who built the Electric Man. [2] In 1948, W. Grey Walter created Elmer and Elsie. The turtle like robots could find their charging stations when their battery ran low. [2] In 1968, Mcgee and Frank created the first computer controlled walking machine at the University of South Carolina. [2] In 1969, Victor Scheinman created the first electrically powered, computer controlled robot arm. [2] These were only some of the stepping stones leading to today’s robot like the Honda’s ASIMO, Aldebaran Robotics’ Nao, Hitachi's EMIEW2, Boston Dynamics’ BigDog and many more.
2.2
The Design of Robots
When we get sick we go to the doctor, when we need woodcraft we go to the wood worker, and likewise for any other problem we face, we go to the specialist. When we need to design robots for a specific task we look at nature to see how nature tackles that problem; how some animals can survive in shivering cold weather? How birds can achieve the feat of flight? How small creatures can walk on even the most uneven surfaces? The design of robots made by human beings have been always inspired by nature and why wouldn’t they be? We see so much perfection and harmony in nature’s ways that we see it as the greatest inspiration.
2 of 111
2.2.1 Dynamics of Robotic Locomotion Robots are not always required to be stationary and do manipulatory work, sometimes we need them to do work that involves moving from one place to another. This is where mobile robots come in. Mobile robots include track robots, wheeled robots and legged robots. The track robots have the best traction among the 3; it would not slip even on highly slippery surfaces, it can easily move on rough terrain, it has the ability to lift very heavy loads since the weight would be spread over the entire surface. But all this comes at a price; the continuous link to the ground hinders the robot’s maneuverability; it is very hard to turn, it is very slow while moving on any surface due to the unwantedly high friction. The life of rubber tracks is very small as compared to wheels or legs. The worst of all the tracks are highly prone to getting dislodged. [3] The wheeled robots can provide high speeds at a very low production cost and still remain very light. It has very few parts that can get impaired. The only disadvantage of wheeled robots is their inability to move on rough terrain and pass obstacles. [3] Legged robots, the inspiration of nature, can have the advantages of both track and legged robots and even eliminate some of their disadvantages; their legs can be made with nonslippery material, they have much higher maneuverability than track or wheeled robots, in comparison, they have very little contact to the ground at any time so they face very little friction, they are much faster than track robots and their speeds are halfway between track and legged robots, they do not have the issue of small life that track and wheels face, no risk of getting dislodged tires either, they inherently have the ability to walk on rough terrain, hence abolishing the greatest inability faced by wheeled and track robots. 2.2.2 Legged Robotics Currently the wheeled robots are so much in real-world use that it might appear that it is the best and only suitable robot to work on and with. But, the biggest prerequisite with wheeled robots are paved surfaces otherwise the robots motion might not even be possible. This requirement of the wheeled robots make more than 50% of the Earth surface unreachable to them. Some specially designed vehicles and robots (track robots) can move on bumpy surfaces but to achieve this feat they consume a lot of energy. Legged robots, on the other hand, are capable of providing much superior mobility even on nature’s most cruel terrains. Since each leg moves independent of the others the robots as a whole faces much lower friction. Moreover, the legged robots can be programmed to modify their gait according to the specific terrain in front of them. The robot’s legs will touch the ground only at selected points according to the terrain in front of it. Hence the legged robot is the robot of choice on irregular surfaces where the can also move in a required direction without a change in the direction the body is facing. Legged robots come in variety of sizes with 1 or more legs, with more legs contributing to a more stable robot. Each leg also increases the requirement of degree of freedom; as moving a leg requires a minimum of 2 degrees of freedom, one for lifting and one for pivoting. Increase in the degrees of freedom also enables the robot to get up if it falls besides enabling the possibility of different gaits. With legged robots it is also possible to vary their height, which can help while walking on irregular surfaces by introducing a damping effect and can also help in crawling into very tight places.
3 of 111
With wheeled and track robots there is a great risk in leaving them unattended in rash conditions as if their tracks/wheels get damaged it could mean complete immobilization but in case of legged robots, they have redundant legs to help them continue their motion even when a few of the legs get damaged. With 1 legged robot, the biggest challenge is stability. Even in stationary position it is impossible to keep the robot statically stable because the support point is down to 1. The 1 legged robot can only be made dynamically stable by either continuously changing its centre of gravity or by providing some other counteractive force. With 2 legged robots, the challenge is again maintaining their balance since they are also capable of providing dynamic stability. The centre of gravity of the 2 legged robots continuously changes and there is no algorithm to keep them standing on their 2 feet. Another disadvantage with the 2 legged robots is the strength requirement of the 2 legs as they have to support the weight of the entire body. Only a robot with three or more legs is capable of offering static stability but when its centre of mass is inside a triangle formed between any 3 of its legs. And with more legs it is also possible to distribute the weight of the body equally on all legs. A 3-legged would appear as a clear alternative but they become inoperable if any of their legs get damaged. The solution to this is obviously in increasing the number of legs which also increases the DOF. Increasing the number of legs to 4 would help but this hinders the robots ability of motion by making its ability to turn very limited; the robot will not be able to turn about its axis. The most popular legged robot configuration is the 6-legged robot. This configuration is preferred because the control requirement to keep the robot stable is virtually eliminated. But there is also a disadvantage with the coordination complexity, controlling 6 legs, usually each with 3 DOF, is a hectic task and it is preferably decentralized. Even the creature of nature that are the inspiration for such robots employ the concept of decentralized leg control [4]. Distributing the control system makes the design, development and testing of each subsystem systematically easier, while also simplifying the understandability. Other advantages of the hexapod include its ability to continue its ability to move even if 1 or more of its legs get damaged, it is capable of continuing its motion with a minimum of three legs.
2.3
Practical Uses
The hexapod is highly effective at any place where a wheeled robot could be used and it is also much more stable at any place where a legged robot is required, because of its extra legs providing the extra stability. Its extra legs will also enable to carry more payload. Some practical examples of the use of hexapod: Crabster CR200 is one of the best example of the practical application of hexapod robots. It is capable of underwater movement for deep sea exploration, where no human beings has ever been. The robots made by Zenta, especially MorpHex, shows the motions a hexapod robot is capable of.
4 of 111
It could also become a part of a bomb disposal squad by equipping its legs with wire cutters and placing a video camera on it, this would keep the human members of the squad much safer.
These reasons fuelled our choice of making the hexapod. In the subsequent chapters the designing, control algorithm and controlling of the hexapod is discussed. The next chapter covers the mechanical design.
5 of 111
Chapter 3 Details of Theoretical Design
3.1
Initial Approach and Setbacks
After the decision of designing the hexapod the next step towards the realization of our idea was creating a model of the hardware structure which we would be aiming to create. The hardware was also important because of the absence of any simulation software that we could use to test our algorithm. The discussion on the algorithm derivation is the topic of the next chapter.
Figure 1 Cardboard Design
The first model (Figure 1) was based on the rough sketch we had in our minds; it was obviously based on our prior knowledge of Mathematics and Physics but just the mental calculation as it was meant to be only a prototype. We first made a cardboard model to see if it would work. By the looks of it, it appeared that it would be capable of providing us with the required stability. It was completely designed by us and since we were new to this we had no idea from where we could get the model converted into a better material, so without any guidance we decided to go to the trunk makers (called patti in Urdu) and get the model constructed. They agreed to give us designed plates according to our requirements but the drilling, assembling and final
Figure 2 Smoothing the Plate's Surface
Figure 3 Making Holes for Servos
6 of 111
finishing became our headache as they had no previous experience of it. We had to do the smoothing (Figure 2), drilling (Figure 3, Figure 4), and assembling ourselves:
Figure 4 Servo Brackets
Due to our inexperience, we had a setback while buying the appropriate nuts, bolts, and drill bits required to join the structure. We went to Brandreth Road and bought the nuts, bolts, and drill bits at an extremely high price and they are still unused in our component boxes. And this became the only expenditure that we regret. We could get much better components from Township market which we realized later. To quote a shopkeeper from Brandreth road, “At this place we can sale mud by making (calling) it gold”.
3.2
Learning Pointers
After completing the structure assembly we started looking for our mistakes in the model we made (Figure 6). The first mistake we realized was that the material was not suitable for the final design because it was not at all rigid, the vibrations the material showed during the drilling procedure was the first clue to the material’s non-viability. The material was way too thin, 2mm stainless steel (chadar). We went for a light material because we were inexperienced in the capability of our servo motors.
Figure 5 The Model Ready to be Joined
Figure 6 Assembled Model
7 of 111
But it was not a complete waste, it helped us with the work we aimed to achieve with it. As mentioned earlier, there is no simulation software that we could use to test the initial or the complete final algorithm. And the prototype proved to be invaluable in the testing of our servo motors (Figure 8).
Figure 8 Testing the Servos
Figure 7 Leg joined with Attached Servos
The vibrations that first appeared during the drilling procedure more than doubled when we were testing the servos. The material had no tensile strength, it could not get proper grip with the ground (thin material), and the vibrations were because the material was way too flexible, the legs and the central structure were not in accord; the legs were too big and the centre was too small, being stainless steel the material was not very drill friendly; we needed sharp bits and high speed drill but at that time such drill was not available to us. Now at least the high speed drill is available in our workshop lab due to our continuous complaints. After the completion of the model at least we were not a laughing stock in front of the lathe workers to whom we had something to show as to what we want them to make. And it made our jobs a lot easier when we went to Gujranwala dying and tools, PITAC Lahore and other lathe workers.
3.3
Mathematical Structure
Looking at the model we made, we saw that the legs could collide if they came too close to each other. And since we had no idea how the algorithm, based on forward and inverse kinematics (discussed in the next chapter), would work nor was it completely ready at that time, we decided to increase the size of the centre and reduce the size of the legs, to consequently increase the distance between legs. This somewhat helped solve the collision issue for us. The new design was made out of aluminium and was packing considerable weight. The new design was less of hit and trial and based more on the experience we had after facing the issues with the previous structure. The legs of an Arthropod contains primarily of 5 joints (Figure 9); the Coxa joint, Trochanter joint, Femur joint, Tibia joint, and the Tarsus joint. But for keeping our work easier and the code less complex we decided to make our robot with 3 joints; the Coxa joint, the Femur joint (we combined Trochanter and Femur joint), and the Tibia joint (combining Tibia and Tarsus
8 of 111
joint). This made each leg of our hexapod capable of 3 DOF. For a total of 18 DOF (3 DOF x 6 legs).
Figure 9 5 Joints in Arthropods
The geometry of our robot helped a lot in its designing; by joining the Coxa joint of each leg we can make a complete circle (Figure 10).
Figure 10 Coxa joints highlighted as dot on the circle
We kept the lengths of each joint of our robot simple from mathematical point of view: Tibia = 110 mm Femur = 57 mm Coxa = 30 mm So approximately the following mathematical relationship exists, Femur = Tibia/2 Coxa = Femur/2 = Tibia/4
9 of 111
The geometry of our robot helped because just by knowing the distance between 2 Coxa joints we can design the entire model. A
2A
Figure 11 Distance between 2 adjacent Coxa joints 'A' and between 2 opposite Coxa jonits '2A'
Just by knowing A in the Figure 11 we can keep the diameter = 2A and the whole hexagon can be completed accurately. In the new design that we made based on previous experience of the prototype, we tried to compensate when 2 legs come in close proximity of each other, i.e. In 2 adjacent legs, 1 leg moves clockwise and the other moves anti-clockwise ( Figure 12). Legs meeting at Tibia after moving 60°
Anticlockwise Motion
Clockwise Motion
30°
Coxa Joint 2
Coxa Joint 1
Figure 12 Meeting of Tibia joints due to distance between adjacent Coxa joints
10 of 111
To avoid their collision we used the property of hexagon in which there is a difference of 30 o between the frame of reference of each leg (with its Coxa as reference) and the frame of reference of the body (centre), in case of a hexagon (Figure 13).
YB
Θ=30
``
Figure 13 Difference of 30o between the frame of reference of the leg and that of the body of a hexagon
YB
Θ=90
Θ=60
XL
The same thing exists for other geometrical structures. There is a difference of 60o between the frame of reference of the body and the frame of reference of each leg in a square (Figure 14). And there is a difference of 90o between the frame of reference of the body and the frame of each leg in case of a triangle (Figure 15).
YL YB
XB
Figure 14 Difference of 60o between the frame of reference of the leg and that of a square shaped body
Figure 15 Difference of 900 between the frame of reference of the leg and that of a triangle shaped body
11 of 111
We utilized this property of hexagon in our new design. To test this property of the hexagon in our new design, we fixed all the legs of our robot perpendicular to the body, then after turning off the power to the motors we tested the maximum turn we could give to each leg, the servos of Coxa joint had enough space to at least reach their full turning ability. But, that didn’t mean that the legs would not collide, as previously mentioned we had no way to test the algorithm as it wasn’t ready by then, the only thing that we could do was to provide maximum compensation our motors could allow, keeping in mind their ability to lift the weight of the new material (aluminium) + the electronics + the battery.
3.4
Realization of the Structure
3.4.1 Material Issue After the experience with the previous material we knew we could not use such a weak material but at the same time we could not overload our servo motors with the weight of a heavy material. By the time we needed the new design our minds were quite matured with the experiences about our field and we knew how we could get the work done properly. Our material of choice became aluminium because of its high tensile strength + light weight, combined with the non-availability of any other viable alternative in Lahore. The next issue was to search for the right person, in that regard we were guided by our Project coordinator. 3.4.2 Search for the Right Person To get the structure made we were advised by Mr. Khan Nazir after our FP1 presentation to go to PITAC to get some professional help. We were of the opinion that we could make the design on AUTOCAD and give it to them to get it made by CNC (Complete Numerical Control) and milling machines. But when we shared our idea with them they told us that the cost of making our entire robot could reach Rs. 30000 because the operating cost of the CNC machine for a single product is quite high, it is only preferred by companies when they have to do large scale production. The Engineer at PITAC advised us that since we are students, it would be preferable for us and our pockets if we get a casting of our design instead of using the CNC machine. It would give us a few advantages; the first would be that the cutting cost which would have been done through wire cut would be saved. Secondly, for using the CNC machine we would have had to provide a complete aluminium sheet to the machine which the machine then cuts according to the requirements, but the cost of a complete aluminium sheet is quite high in the market. The Engineer told us that there are people in Bilal Gang who can help us with the casting, in which we would have the freedom to choose the diameter and length of the cast through metal cut. When we went to the person at Bilal Gang he told us that casting requires a wooden design, even after we showed him our model, since it requires exact dimensions. He also referred us to a person near our university in Township market (Nursery stop), he added that the person is quite talented and he could solve our problem quite easily. We again explained the lathe worker at Township everything that we had told previously to the Engineer at PITAC and the lathe worker at Bilal Gang. Amazingly, he understood our aim quite well and guided us through the procedure that would be done to get us the final design we require, the procedure was the same that we were previously told by the Engineer at PITAC. This lathe worker also required of us the wooden design to make the cast based on the dimensions of the wooden frame.
12 of 111
3.4.3 Making the Design on paper Based on our calculations we made a design on chart paper, got it cut into cardboard and got a wooden design cut according to the cardboard design and then gave it to the lathe worker for making an aluminium design out of it. The steps involved in making the design on paper are outlined here (in the form of instructions): The first step is to decide the distance between 2 Coxa joints, based on our calculations we kept the distance at 130mm. Then we place 2 dots at 130mm apart from each other. The distance of each point from the center is also equal to 130mm. Then we continue placing dots at 130mm apart from the adjacent dot and 130mm apart from the center as well. This gives us a hexagonal shape (Figure 16):
Figure 16 Shape formed by placing dots at 130mm apart (not to scale)
Connecting the dots gives us a circle of radius 130mm (Figure 17):
Figure 17 Circle formed by connecting the dots at radius of 130mm (not to scale)
13 of 111
Joining the adjacent dots at the smallest distance gives us the shape of the hexagon (Figure 18):
Figure 18 Hexagon formed by connecting the Coxa joints
Figure 19 Connecting the opposite Coxa joints
Connecting the opposite Coxa joints at the middle would give the following shape (Figure 19): The reason for doing this is to establish the width of the Coxa joint and leave a floating slab like portion apart from the body for connecting a servo at the Coxa joint. After that draw 2 more lines at each Coxa joint to establish the width of the Coxa joint (Figure 20):
Figure 20 Creating the Coxa joint with the width of Servo motors
14 of 111
Draw an inner circle with the sides of the hexagon as a tangent line, this circle is the main body and separates the slab like portion of the Coxa joint (Figure 22):
Figure 22 The inner circle outlining the body of the hexapod
Erase the outer circle and the hexagon leaving the point of intersection of the 2 untouched as it will form the center point for the Servo motor (Figure 21):
Figure 21 Erasing the outer circle and the hexagon
With the outer circle and the hexagon erased the design of the central body is complete (Figure 23):
Figure 23 Final design of the central body
15 of 111
With the design of the central body complete we are left with the design of the Tibia joint. The steps involved in making the Tibia joint on paper are outlined below (in the form of instructions): The first step is to draw a vertical ellipse with the distance from top to bottom (the length of the major axis) equal to the length of the Tibia joint, for us the length was 30mm (Figure 24):
Figure 24 The ellipse with edge distance equal to length of Tibia
Draw the major axis of the ellipse (Figure 25):
Figure 25 The ellipse with its major axis
Draw a circle at the top and an ellipse at the bottom, the sizes are completely according to your requirement just be sure to leave enough space between the 2 for the servo motor (Figure 26):
16 of 111
Figure 26 The ellipse intersected by a circle at top and an ellipse at the bottom
Erase the outer edge of the intersecting circle and the ellipse along with the outer edge of the original ellipse that falls inside the 2 circles (Figure 27):
Figure 27 Final design of the leg
17 of 111
Chapter 4 Practical Implementation We made all these designs on a chart paper (Figure 28, Figure 29) and then on cardboard (Figure 30), before getting them cut out in wood (Figure 31):
Figure 28 The central body on chart paper
Figure 30 Final design on cardboard
18 of 111
Figure 29 The final design of leg on chart paper
4.1
The Wooden Design
After making the designs on the cardboard, we took them to a carpenter in township market along with our lathe worker. He explained to the carpenter to make wooden pattern of the designs of the central body and legs. The carpenter took the designs and drew the exact design on lasani board, whose thickness is 1cm. Then he cut the board to get the exact shapes on wooden bandsaw (ara machine). Then he smoothened the cut wood on surface planer machine (randa machine).
Figure 31 Wooden design for casting process
4.2
Casting
Once completed, the lathe worker took the finished wooden product (central body and leg) and on the next day took them to cast maker (dhalai wala), who took the wooden designs and used them to make a cavity on a sand bath. The sand bath was prepared with the Pakistani desi formula, i.e. mixing used Mobil oil with the sand to glue the sand and also prevent the sand from sticking to molten aluminium. The caster then poured the molten aluminium and other material called bartan into the cavity and when the aluminium cooled down the cast was obtained. This whole process took 3 days.
4.3
Lathe Worker’s Tasks
Once the cast was obtained the job of the lathe worker started. First he had to reduce the thickness of the casted objects because they weighed more than what the servos were capable of bearing, the total weight of all the parts was a little above 1 kg. The lathe worker reduced the thickness of the central plate from 10mm to 4mm and that of the leg from 10mm to 3mm. This reduction in thickness reduced the weight to around 500 g, which was within the bearable range of the servo motors. All this was done on lathe machine by metal cut. We also learned how the machine worked and was operated. The biggest problem that we faced during this phase was the laziness of our
19 of 111
lathe worker accompanied with Wapda’s blessings on electricity. The lathe worker made countless excuses, some days he would not be at the shop, some days he would be out of city due to someone’s death, sometimes he would be busy doing someone else’s work, most used excuse of his was that he couldn’t complete our work because of lack of electricity. Despite his excuses the good thing about our lathe worker was that there was a great neatness in his work, the finishing was very clean because of his steady hand. So despite all his excuses we enjoyed working with him. We visited him every day to check on his progress with our work. Mostly he’d show us the same thing for 2-3 days, and sometimes he would show half complete work on some part. The biggest issue according to him was that whenever he would sit down to do our work the light would go off. So because of the fast approaching date of project submission we decided to bring him to the workshop lab of our university to complete the remaining work on our robot. We requested sir Khan for the permission to bring him to the university and thankfully he allowed us. Sir Khan also instructed A.D. Sir of workshop lab to cooperate with us in this matter. A.D. sir was very helpful to us in this matter, he stayed with us in the lab beyond his university specified timings, and he even changed his schedule to suit our needs. Without his cooperation it would never have been possible to complete the hardware within the due time. The lathe worker came to the university for 3 consecutive days to work, he worked continuously for 6-8 hours in the workshop lab (Figure 32). We were there with him working on assembling the parts as soon as he completed his part, this also helped us in saving some of our time.
Figure 32 Lathe worker working in the Workshop Lab
4.4
Assembling the Robot
After learning from our previous experiences at Brandreth road we decided to consult with our lathe worker for the nuts and bolts, he referred us to the shops where we could find the required parts. The only time we had to go to Brandreth road was to get the aluminium sheets for the servo brackets which was not possible to cast as they required bending and a cast of such material was not feasible, moreover, the stiffness at this part was not necessary, the central body and the legs were the only parts we required to be rigid.
20 of 111
When we assembled the robot completely and put all the electronics in it the robot was were slow in its movements, we realized that we were almost pushing the motors to their limits and this was not something we could continue for long as it risked damaging the servos. So we decided to get a hole at the center of the structure as that much aluminium was not necessary and it also enabled us to move our electronics to a lower level, not visible at the top. We hanged a small plastic plate attached to the edges of the hole, this plastic held the electronics at a lower level and allowed us to hide the electronics and try to enhance the beauty of the upper surface. This was a much stable structure; there were no vibrations in it, it was much better looking to the eye as it was quite stiff and had bigger area; it looked that something is in front of us and we are actually working on something (we were criticized on the small size of the previous structure). We were finally confident that this structure would work, it reaffirmed our faith in our abilities.
4.5
Advantages of Design
The biggest advantage of our design is its portability; the disassembling of our hardware is extremely easy, we just have to open six screws to separate the legs and we would get a design similar to figure 6, which is easily transportable. Another advantage is to use the design for a different robot by only changing the base structure, one with different number of legs, the legs are strong enough to hold even if they are used on a quad-bot. We could replace one of the legs and place some other equipment in its place, such as a grabber or a wire cutter, and the robot will still be in a working condition with only 5 legs. The design can also be used to mimic the looks of other six legged creatures by just changing the base structure and hence making it look more like the creature we wish to mimic.
21 of 111
Chapter 5 Motion Kinematics The motion of a robot is governed by forward and inverse kinematics. The forward and inverse kinematics relate 4 things, the joint angles of each of the 3 joints, and the position of the endeffector (or tool point). In forward kinematics we know the angles of the three joints and using that we try to find the position of the end-effector. In inverse kinematics, as the name suggests, we do the exact opposite, we know the position where we want the end effector to be and then we find the joint angles to reach that position.
5.1
Our Approach towards the Motion of the Hexapod
Just like every other student in our country working on robotics, our first instinct was to use hard coded angle and move each motor of each leg at a specific angle to move the robot to different positions. But, later thanks to the proper guidance of our project advisor, Mr. Jameel Ahmad, we moved to the proper tracks and used the method that is practically supposed to be used for such a complex system. Once we got proper command over the kinematics even we realized how important its implementation is and also unlocked new horizons for us, it enabled us to code motions that we weren’t even imagining in the beginning. Just based on the definition we knew that we would not need the forward kinematics for our robot, we would only need the inverse kinematic equations as we would only instruct the hexapod to move to a specific coordinate using the PS2 controller (discussed later), specifically its analog. And knowing the required position we would then have to calculate the required angles for each motor of each leg to reach those coordinates. For using inverse kinematics for our hexapod we needed to establish a centre which would serve as a reference for the entire robot. The PS2 controller would be used to change the coordinates of the reference point and the other parts would follow. Obviously the reference was the central body, the PS2 controller would be used to change its coordinates and then the remaining body, i.e. all the motors of all the legs would move accordingly to move the centre to the required position.
5.2
Deriving the Inverse kinematic Equations
Hexapod have many advantages over the wheel Locomotives but it also possess more complexities in its design; orientation, gait pattern, manufacturing, assembling, navigation, stability and control. All the six legs must be in perfect coordination to produce versatile, effective and elegant movements. Mathematical modelling plays very important role in the stability, control and navigation of robots. Mathematical modelling is the process of capturing the Physics of machine using mathematical tools and describing the complete mechanical structure and system of robot in form of mathematical equations. Then these equations are used to control and navigate the behaviour and motion of robotic structure. Without proper modelling of the system the controlling will not be an easy task. Mathematical modelling provides us the path, following which will make the robot versatile, delicate and athletic. The action and gesticulations of robots can be made much smooth with the assistance of modelled mathematical calculations. Mathematical modelling will present the whole design summary of the robot dynamics in the form of parametric equations. It also enables us to choose more
22 of 111
feasible, established and settled design as the performance of the hexapod largely depends on the mechanical design selected. As described above, the reference of our design is the central body and all the angles that are calculated are to move the centre to a required position. To calculate the angles, we transform the frame of reference of the body to that of each leg and calculate the orientation of the leg that would be required for the movement of the central body to the required position. When we transform the frame of reference of the body to that of the leg, we get the coordinates to which the leg would be required to move, so according to the requirement of inverse kinematic we have the coordinates to which the end-effector has to move, now we can find the required angles of each servo using inverse kinematics. Outlined below are the steps we took to derive the inverse kinematic equations for our hexapod:
z-axis
x-axis
θ1
Figure 33 Side View of the leg with angle of rotation of Coxa Joint
y-axis
x-axis
θ2
θ3
Figure 34 Top view of the leg with angle of rotation of Femer and Tibia joint
23 of 111
The first step is visualizing the top and the side views (show in Figure 33, Figure 34) to know what angles the leg is capable of rotating at and in which plane. That is done by seeing the 3 dimensional motions in 2 dimensions. To get the 2D description of the 3D system we divided the three axis coordinate system into two sets of coordinate systems. XY axis coordinate system with X axis as horizontal part of rectangular coordinate system and Y axis as vertical. The Z axis is perpendicular to both X and Y axis and towards the users. While the other coordinate system comprises of Z axis and Y axis, here Z axis is vertical and Y axis is the horizontal component of rectangular coordinate system. X axis is perpendicular to the plane containing Y and Z plane and towards the user. After the transformation from 3D to 2D the 2nd step is to drive the Inverse kinematics equations. For the derivation of equation there are two types of approaches one is by using the iterative modelling method and the other is geometric method which is used for developing of inverse kinematics equations for system x-axis
P (X,Y,Z)
θ1 = 90°
XL
θ1 = 0°
θ1 = -90°
Figure 35 Side view of the leg for the calculation of angle along Z axis
with three or less than three DOF which is called analytical method. We used the latter approach for the development of our mathematical equations. As shown in Figure 36 the XY coordinate system can be used for the derivation of the angle associated with the Coxa joint of the leg. By applying trigonometry on Figure 36 we can find the angle between two lines of the right angle triangle, provided we know the lengths of X and Y. As we have the knowledge about X and Y coordinates of legs we can use them to find the angle as: 𝑌
tan θ1 = 𝑋
=>
θ1 = tan-1 (𝑌⁄𝑋)
It can be seen in Figure 36 that the length of leg is hypotenuse in this case which can be calculated by using the Pythagoras theorem: Length of leg = √𝑋 2 + 𝑌 2 The coordinates X and Y can also be calculated if we have information about length of leg and the angle it has moved to. According to the principles of rectangular coordinate system, the following relations hold:
24 of 111
X = length of leg cosθ1 Y = length of leg sinθ1 y-axis
(X,Y)
Y
θ1
x-axis X
Figure 36 Triangular representation of Figure 35
Now we can use the YZ coordinate system to find the other 2 angles. Angle associated with Femur and Tibia joints can be calculated form this plane of reference.
z
3 L1
2
Z offset
Length of leg
(y,z)
y
Figure 37 Geometric representation of side view of leg for calculation of angles
First we find the Femur joint angle using Figure 37. In this geometric representation of the side view of leg, L1 correspond to length of Coxa, L2 to Femur and L3 is for Tibia length. Z offset represent the vertical height from the ground.𝜃2 is the angle accompanying with Femur and 𝜃3
25 of 111
is angle associated with Tibia joint. To get equation for Femur joint angle which is 𝜃2 we have to divide the angle between Y axis and Tibia with the help of angle bisector represented by dotted line in Figure 37. Name the bisecting line as ABS and the two angle as ∅3 and ∅5 respectively. As it can be seen clearly that due to angle bisector ABS two right angle triangles has been formed. We can find 𝜃2 using following relationship: 𝜃2 = ∅1 + ∅4 −
𝜋 2
Finding ∅1 is little bit tricky. As trigonometry tell us that any angle can be found if we now the length of three side of the triangle by using Cosine rules which can be describe as:
A
c b
C
a
B
Figure 38 Triangle representing law of cosines
If the three angle between each side of triangle are A, B, and C and length of each side of triangle are given by a, b, and c. Each side in term of other two side and angle between them can be given by following equation: 𝑎2 = 𝑏 2 + 𝑐 2 − 2𝑏𝑐 cos 𝐴 𝑏 2 = 𝑎2 + 𝑐 2 − 2𝑎𝑐 cos 𝐵 𝑐 2 = 𝑏 2 + 𝑎2 − 2𝑏𝑎 cos 𝐶 It can be noted that any one unknown can be found if other three parameters are known to us. Manipulating above equation we can write equation for finding the angle as: 𝐴 = cos −1
𝑏 2 + 𝑐 2 − 𝑎2 2𝑏𝑐
𝐵 = cos −1
𝑎2 + 𝑐 2 − 𝑏2 2𝑎𝑐
−1
𝑏 2 + 𝑎2 − 𝑐 2 2𝑏𝑎
𝐶 = cos
With the help of law of Cosine we can drive equation for ∅1 as ∅1 = cos −1 (
ABS 2 + 𝐿22 − 𝐿23 ) 2ABS𝐿2
26 of 111
Here ABS can be give as: 𝐴𝐵𝑆 = √(𝐿𝑒𝑛𝑔𝑡ℎ 𝑜𝑓 𝑙𝑒𝑔 − 𝐿1 )2 + 𝑍 2 ABS is equal to the hypotenuse of lower right angle triangle in figure for geometric representation of leg. After finding the ∅1 , we can now work on finding ∅4 which is angle between base of right angle represented by Z offset and perpendicular which is represented by the length of leg - Coxa length. As base and perpendicular are known tangent inverse can be used as:
∅4 = tan−1(
𝐿𝑒𝑛𝑔𝑡ℎ 𝑜𝑓 𝑙𝑒𝑔 − 𝐿1 ) 𝑍 𝑜𝑓𝑓𝑠𝑒𝑡
Now we can drive expression for 𝜃2 as: 𝐴𝐵𝑆 2 + 𝐿22 − 𝐿23 𝐿𝑒𝑛𝑔𝑡ℎ 𝑜𝑓 𝑙𝑒𝑔 − 𝐿1 𝜃2 = cos −1 ( ) + tan−1( ) − 90 2𝐴𝐵𝑆𝐿2 𝑍 𝑜𝑓𝑓𝑠𝑒𝑡 𝜃3 Can be easily calculated by using Law of Cosine: −𝐴𝐵𝑆 2 + 𝐿22 + 𝐿23 𝜃3 = 90 − cos−1 ( ) 2𝐿3 𝐿2 All the angles for joints are represented in the form of parametric equations which can be used in the development of algorithm for motion and control of the hexapod. But these equations are not enough. The main target is to move the centre of gravity of hexapod from one position to the other. For this purpose we have to develop the equations for calculation which should be able to give new values of X, Y, Z, after performing all kinds of body kinematics. For the derivation of equation consider the Figure 39:
27 of 111
X2
P(X2,Y2 )
P(X1,Y1 )
Y1
X1
Y2
Off set in Y O
Off set in X
Figure 39 Geometric representation of body for calculation of body kinematics
By using Figure 39 we will find out the position to which body of the robot will move. We have already developed the parametric equations for the leg. Inverse Kinematics equation for legs can be used to move the centre of gravity of the hexapod, for doing so, we have to find out how much the centre of gravity varies its position when legs of hexapod change their position. After finding the relationship between them we can also move the body by varying the position of legs. Let’s consider that P1 in the Figure 39 in the position of leg with respect to the Coxa joint (centre of the leg) while P2 is the point with respect to the centre of gravity of robot. As the equation for legs are known therefore the point P1 is consider as identified. From Figure 39 it can be concluded that the difference between P1 and P2 is equal to the distance from centre of gravity of body to the Coxa joint so therefore it can be write that: 𝑋2 = 𝑋1 + Offset in X 𝑌2 = 𝑌1 + Offset in Y Then distance between the centre of gravity and end point of the leg can be given as: 𝐷𝑖𝑠𝑡𝑎𝑛𝑐𝑒 𝑑 = √𝑋2 2 + 𝑌2 2 And the angle between centre of gravity of hexapod and last point of leg can be given by:
28 of 111
𝜃 = tan−1
𝑌2 𝑋2
With the help of above equation now be can move the centre of gravity of hexapod. For making the robot move about its own axis following calculation can provide us with the required equations: P(X1,Y1)
Position to which we want to rotate
Y1
P(X0,Y0)
Current Position
Y0
β α X0
Q(0,0) X1
Figure 40 Geometric representation for driving equation to perform rotation along its own axis
In the Figure 40, following are known: (X0, Y0) current position of robot. 𝛼= Angle of current position. 𝛽=Angle to which we want to rotate the robot. R= radius of body Q= Point of centre of gravity Now we have to find out the new position P(X1, Y1) after the rotation to 𝛽. tan 𝛼 =
𝑌0 𝑋0
𝛼 = tan−1
𝑌0 𝑋0
29 of 111
Similarly, tan(𝛼 + 𝛽) =
𝑌1 𝑋1
Y1=R*Sin(α+β)
α+β X1=R*Cos(α+β) Figure 41 Triangular representation of Figure 40
Now both the coordinates of new position can be easily be obtained from the Figure 41. 𝑋1 = 𝑅 cos(𝛼 + 𝛽) 𝑌2 = 𝑅 sin(𝛼 + 𝛽) Inverse kinematics equation of leg requires the X, Y which are the distance we want to move in X and Y coordinates respectively which is basically difference between both the points P(X0, Y0) and P(X1, Y1). 𝑋 = 𝑋1 − 𝑋0 𝑌 = 𝑌1 − 𝑌0 As discuss earlier that the frame of reference of body and legs are apart by 30 degree, therefore, we cannot simple write the X Y Z coordinates of body directly to the inverse kinematics equation for the legs. We must transform the body frame of reference into frame of reference of leg by using: cos 𝜃 𝑋` [ 𝑌 ` ] = [ sin 𝜃 0 𝑍`
− sin 𝜃 cos 𝜃 0
0 𝑋 ] ∗ [ 0 𝑌] 𝑍 1
Using the rotation matrix along the Z axis both the frame of reference are mapped together and coordinates of base of hexapod can be now given to the parametric equations of leg. For the three dimensional rotation we can use the rotation matrixes given below: 1 𝑅(𝛼) = [0 0
0 cos 𝛼 sin 𝛼
30 of 111
0 − sin 𝛼 ] cos 𝛼
𝑐𝑜𝑠 𝛽 𝑅(𝛽) = [ 0 − 𝑠𝑖𝑛 𝛽
cos 𝛾 𝑅(𝛾) = [ sin 𝛾 0
0 𝑠𝑖𝑛 𝛽 1 1 ] 1 cos 𝛽
− sin 𝛾 cos 𝛾 0
0 0] 1
Here 𝛼= Rotation along X axis. 𝛽= Rotation along Y axis. 𝛾= Rotation along Z axis. 3D rotation matrix can be derived by multiplying each matrix in desired order: 𝑅 = 𝑅(𝛼) ∗ 𝑅(𝛽) ∗ 𝑅( 𝛾)
𝑐𝑜𝑠 𝛽 cos 𝛾 𝑅 = [sin 𝛼 𝑠𝑖𝑛 𝛽 cos 𝛾 + cos 𝛼 sin 𝛾 sin 𝛼 sin 𝛾 − cos 𝛼 𝑠𝑖𝑛 𝛽 cos 𝛾
− 𝑐𝑜𝑠 𝛽 sin 𝛾 sin 𝛾 sin 𝛼 𝑠𝑖𝑛 𝛽 + cos 𝛾 cos 𝛼 cos 𝛼 𝑠𝑖𝑛 𝛽 sin 𝛾 + sin 𝛼 cos 𝛾
𝑠𝑖𝑛 𝛽 − sin 𝛼 𝑐𝑜𝑠 𝛽 ] cos 𝛼 𝑐𝑜𝑠 𝛽
𝑋 𝑋` [𝑌 ` ] = 𝑅 [𝑌 ] 𝑍 𝑍` By using above equation we can perform 3D rotation.
5.3
Algorithm development:
Development of algorithm is the most crucial and challenging task in the development process of hexapod. Without an algorithm for controlling the proper motions of the hexapod is an impossible task to achieve. There are many undesirable series of actions which are beyond the control during locomotion. Example of one such problem are the unwanted jerks during the walking of hexapod which gives very serious issues for dynamic stability of the robot. Algorithm manage all the activities of robot in logical and systematic order for the controlling and navigation under the guidance of the user. With the help of proper algorithm one can create moves for locomotion which are beyond the imagination. Our hexapod works on the algorithm base on the kinematics equations, mainly on the inverse kinematics. User most of the time provides the X, Y, Z coordinates through the PS2 controller analog joysticks, the input informs the microcontroller with the amount of variation it has to bring in the coordinates of the center, and on the basis of the input provided the algorithm in the microcontroller calculates the angles to move the servos and consequently the robot. There is no defined angle in the whole algorithm, every single angle is calculated each time the user provides an input. We are mainly focusing on moving the centre of gravity from one point to other. Let say the hexapod has to move from point B to point A, if hexapod require six steps to complete the task then in every step algorithm calculates eighteen angles, therefore, total angles
31 of 111
calculated from moving robot from point B to point A will be 108 (18 angles x 6 steps). Algorithm implemented on the hexapod contains whole mathematical model of mechanical structure of hexapod. Very minor difference in the parameters of length given in algorithm and actual length can make the motions of the hexapod very unstable. Whole algorithm implemented can be divided into following sections: Mathematical translation of mechanical structure. Human-Machine interface. Walking mode. Translation mode. Rotation mode. Single leg mode. Calculations of Angle Turn on the hexapod
Initialize all variables with default values
Hexapod is configured in walking mode as default
Start the translation sequence
If translation mode
Wait for user command
If rotation mode
If walking
Start the single leg operation
Start the walking sequence
NO
Is mode switched? YES
Perform the calculation for angles
Figure 42 Broad overview of hexapod’s functions
32 of 111
Start the Rotation sequence
5.3.1 Mathematical translation of mechanical structure: Initial step in writing algorithm is to create the mathematical version of robot. Physical measurements are used for this purpose. All the measure values should be very accurate and precise. We know that for hexapod mathematical translation we have to develop virtual hexapod based on numbers which should be complete and accurate copy of the physical hexapod. The movements generated by virtual hexapod through numbers and calculation are then used to run the physical version of our walking machine. Basically we can say that the physical hexapod mimics the action of the virtual hexapod. To develop virtual hexapod we must first have to draw a hexagon. Which can be done by using the some equation we used while designing our mechanical structure.
A
2A
Figure 43 Distance between 2 adjacent Coxa joints 'A' and between 2 opposite Coxa jonits '2A'
If A is distance of one side of the hexagon then distance between all the six side of hexagon will also be A and the diameter has distance equal to 2A.The angle between each side is sixty degree. On basis of these fact we can develop our virtual hexapod by defining the length A. Side_Length_of_Hexagon is equal to A in the Algorithm. We can give any value to variable Side_Length_of_Hexagon. Case 1:
Side_Length_of_Hexagon=0
We can see in Figure 44 that no hexagon has virtually formed. The figure given below is totally created by mathematical equations just like our virtual hexapod. It gives us simulated visual form of virtual hexapod which help us to predict the behaviour and see how the virtual hexapod is working. Case 2:
Side_Length_of_Hexagon=130
When we assigned 130 which is actual distance between two consecutive Coxa joints to the variable a mathematical hexagon is formed with (0, 0) as its origin and the six corner has (65,112.6), (-65,112.6), (-130, 0), (-65,-112.6), (65, -112.6), (130, 0) as its coordinates.
33 of 111
Figure 44 No virtual hexapod is formed
Figure 45 Virtual hexapod with side length 130mm
To calculate the coordinates of six corners of virtual hexagon we have use the relations given below which automatically calculate each X and Y coordinate as shown below.
34 of 111
Coxa Body Center Offset
Equation
Current Numeric values
Body Coxa Offset X
Side_Length_of_Hexagon/2
65
Body Coxa Offset Y
Body Coxa Offset
√(Side_Length_of_Hexagon)2 + (Body Coxa Off set X)2
Side_Length_of_Hexagon
Leg X component of Number corners
112.6
130
Current Numeric values of X
Y component of corners
Current Numeric values of Y
1
Body Coxa Off set X
65
Body Coxa Off set Y
112.6
2
Body Coxa Offset
130
0
0
3
Body Coxa Offset X
65
-Body Coxa Off set Y
-112.6
4
-Body Coxa Offset X
-65
-Body Coxa Off set Y
65
5
-Body Coxa Offset
-130
0
0
6
-Body Coxa Off set X
-65
Body Coxa Off set Y
112.6
Next step in line after completing the virtual hexagon is to attach virtual legs so it can become complete hexapod. Dimension Coxa Length
35
Femur Length
65
Tibia Length
145
35 of 111
Figure 46 Representation of virtual leg
Blue line is length of Coxa, red is length of Femur, and green is length of Tibia. We now have complete virtual hexapod as shown in Figure 46.
Figure 47 Graphical representation of hexapod
Relationship to calculate the initial position of tip of each leg with respect to Coxa can be given as: Feet Position X (wrt Coxa)
Value
Feet Position Y (wrt Coxa)
val ue
Feet Position Z (wrt Coxa)
value
50
(Coxa Length + Femur Length) ∗ sin 60
86
Tibia Length
145
0
Tibia Length
145
-86
Tibia Length
145
1
(Coxa Length + Femur Length) ∗ cos 60
2
(Coxa Length+ Femur Length)
100
0
3
(Coxa Length+ Femur Length)*cos60
50
−(Coxa Length + Femur Length) ∗ sin 60
36 of 111
4
−(Coxa Length + Femur Length) ∗ cos 60
-50
−(Coxa Length + Femur Length) ∗ sin 60
5
−(Coxa Length + Femur Length)
-100
6
−(Coxa Length + Femur Length) ∗ cos 60
-50
-86
Tibia Length
145
0
0
Tibia Length
145
(Coxa Length + Femur Length) ∗ sin 60
86
Tibia Length
145
After defining the initial positions of legs the virtual hexapod is complete which is totally base on numeric values.
37 of 111
5.3.2 Human-Machine Interface: The user has complete control of our robot therefore he must communicate with the hexapod to tell it what to do. To accomplish this task we have use PS2 wireless controller which has 20
Connect the PS2 controller to Hexapod
Turn on anolog by pressing the button
Which button is pressed?
Left or right arrow is pressed
Left increases the speed and right decreases the speed
Up or down arrow is pressed
Δ is pressed
O is pressed
L1 is pressed
Up Increases and Increases the Switch to Single Switch to down decreases vertical length to leg mode Translation mode max the vertical length
L2 is pressed
Switch to Rotation mode
R1 is pressed
R2 is pressed
Double the travel Double the length vertical lift length
O is pressed
L1 is Pressed
L2 is pressed
R1 is pressed
R2 is pressed
Switch back to walking mode
Switch back to walking mode
Switch back to walking mode
Switch back to walking mode
Switch back to walking mode
Figure 48 Mode selection through PS2 controller
channels for inputs. Two analog channels allowed us to give X Y Z coordinates easily. The PS2 controller was perfect match for us to provide HMI (Human-Machine Interface). Details about the operation and function of PS2 controller are the topic of the next chapter. 5.3.3 Walking Mode In this mode the hexapod will walk by using two type of biological inspired walking styles. Metachronical gait (“wave gait”) and Ripple gait. Metachronical gait “wave gait”: In this type of gait the robot lifts one leg at a time and puts down another leg at the same time while four legs on the ground push the body forward.
38 of 111
Ripple Gait “Two Wave Gait”: In this gait two legs goes in air at any time. Both the legs are 180 degree apart. The time diagram is shown below.
RF LM RL
LF RM LR
Figure 49 Metachronical gait “wave gait” and its time diagram
RF RM RL
LF
LM LR
Figure 50 Ripple gait and it time diagram Select the leg sequence
Go back to main NO
Check for event on controller analog buttons
Select is pressed?
NO
YES Count=Count+1
Yes
If Count is
Go to gait generation function
2 Switch to ripple gate
Figure 51 Algorithm’s walking gait module
39 of 111
1
Switch to wave gate
5.3.4 Translation mode: In translation mode the legs of the hexapod remain stationary and its body moves to different position within the range the maximum distance travelled by body will be equal to the initial default length of the leg. The Coxa travels to the position of Tibia joint and leg gets completely folded. Start the Translation mode
Check for the event on PS2 controller
If left analog is moved up or down
Translation towards and away from the user
If left analog is moved Left or Right
Translate left or right
L2 is pressed
If right analog is moved left or right
If right analog is moved up or down
Change the height with change in analog value
Switch back to Walking mode
Rotate left or right
Figure 52 Algorithm's translation module
5.3.5 Rotation mode In this mode the hexapod perform its actions using three dimensional rotation matrix given in body kinematics section. It has the capability to Roll, pitch and yaw in X, Y and Z axis respectively.
40 of 111
Start the Rotation mode
Check for the event on PS2 controller
If left analog is moved up or down
Perform Yaw about Z axis
Switch back to Walking mode
L2 is pressed
If left analog is Moved left or right
If right analog is moved up or down
Perform Roll about X axis
Change the height with change in analog value
If right analog is moved left or right
Perform Pitch about Y axis
Figure 53 Algorithm’s rotation module
5.3.6 Single leg mode: Single leg selected
Wait for the event on Controller
O is pressed
Switch to walking mode
Leg select =Leg select+1
If leg select is equal to
Left front leg is selected
Left Middle leg is selected
Left Rear leg is selected
Right front leg is selected
Right Middle leg is selected
Right Rear leg is selected
Figure 54 Algorithm’s single leg control module
In this mode we can control the single legs one by one. When one leg is selected rest of 5 legs and body remains completely stationary. We can control all eight DOF therefore this function has its own importance.
41 of 111
5.3.7 Calculations for angles The last step and most important step of algorithm is calculation for the angles by using the inverse kinematic equations. There are total 12 inputs which can change the X, Y, Z coordinates of hexapod. This portion of algorithm takes care of all of them and generates 18 output angles on the bases of the mathematical operations. So our hexapod is therefore a Multiple Input Multiple Output (MIMO) system. All the mode changes the values of X, Y, Z coordinates which are accumulated here and combined result will be produced which runs the virtual hexapod in the algorithm and the real hexapod uses the angles calculated by the algorithm to moves itself physically. Angles calculated by the algorithm is converted in to servo counts which changes the angles of servo motors on each joint of hexapod. The combine effect of these changes in motors angle will produce the walking and other functionality of the hexapod. All the inputs are mentioned in the table given below. Inputs
Function
Position X
Change the X-coordinates for translation of body
Position Y
Change the Y-coordinates for translation of body
Position Z
Change the Z-coordinates for translation of body
Rotation X
Take the value for Roll
Rotation Y
Take the value for Pitch
Rotation Z
Take the value for Yaw
Body offset Z
Change the vertical distance between ground and centre of gravity of hexapod.
Travel length
Change the minimum travel length of the hexapod. Both X and Y by same amount
Leg Lift Height
Change the lift height of leg during walking.
Travel Length X
Distance travel in direction of X coordinate during walking.
Travel Length Y
Distance travel in direction of Y coordinate during walking.
Travel Rotation Z
Distance travel by the leg during the Rotation about its own axis.
Now on the basis of these inputs we have to calculate the total change in X, Y, and Z coordinates. These total values of X, Y, Z will then be given to the Inverse kinematic equations. The coordinates of body must be transformed into the coordinate system. Both the coordinates systems are in different frame of reference. Therefore we must transform one to the other. While the total values for walking are directly given to the inverse kinematics equations of legs without any transformation in coordinate systems. Finials values for body kinematics are given as:
42 of 111
Equations for calculating final X, Y, Z. Total X = Position X+ Feet Position X+ Body Coxa Offset X+ Gait Position X
Total Y = Position Y+ Feet Position Y+ Body Coxa Offset Y+ Gait Position Y
Total Z = Feet Position Z + Gait Position Z The finial values for X, Y, and Z calculated from above equations are then used to calculate new values of Body kinematics X, Y, and Z by using the rotation matrix. The angle for rotation matrix can be given by: Angles for Rotation Matrix 𝛂 = Rotation X 𝛃 = Rotation Y 𝜸 = Rotation Z + Travel Rotation Z
Current position X = Total X Y = Total Y Z = Total Z
𝑐𝑜𝑠 𝛽 cos 𝛾 𝑅 = [sin 𝛼 𝑠𝑖𝑛 𝛽 cos 𝛾 + cos 𝛼 sin 𝛾 sin 𝛼 sin 𝛾 − cos 𝛼 𝑠𝑖𝑛 𝛽 cos 𝛾
− 𝑐𝑜𝑠 𝛽 sin 𝛾 sin 𝛾 sin 𝛼 𝑠𝑖𝑛 𝛽 + cos 𝛾 cos 𝛼 cos 𝛼 𝑠𝑖𝑛 𝛽 sin 𝛾 + sin 𝛼 cos 𝛾
𝑋 𝑋` [𝑌 ` ] = 𝑅 [𝑌 ] 𝑍 𝑍`
Coordinates after performing 3D rotation Body kinematic X = X` Body kinematic Y = Y` Body kinematic Z = Z`
43 of 111
𝑠𝑖𝑛 𝛽 − sin 𝛼 𝑐𝑜𝑠 𝛽 ] cos 𝛼 𝑐𝑜𝑠 𝛽
After acquiring the result from matrix the algorithm then determines the new position of body of hexapod where it has to move and equation for new position calculation are given in table below:
New Position Equations New Position X = Body kinematic X + Feet Position X + Gait Position X + Position X
New Position Y = Body kinematic Y + Feet Position Y + Gait Position Y
+ Position Y
New Position Z = Body kinematic Z + Feet Position Z + Gait Position Z + Position Z
Once the New position values are calculated by the above equations, the algorithm transforms them by using the transformation matrix. Coordinates before transformation X = New Position X Y = New Position Y Z = New Position Z cos 𝜃 𝑋` [ 𝑌 ` ] = [ sin 𝜃 0 𝑍`
− sin 𝜃 cos 𝜃 0
0 𝑋 0] ∗ [𝑌 ] 𝑍 1
Coordinate after the transform Transformed X = X` Transformed Y = Y` Transformed Z = Z`
At the end transformed X, Y, Z are given to the Inverse kinematics equation for the calculation of all three angles.
44 of 111
Chapter 6 Remote Control
6.1
Need for Control
Without a proper control mechanism the robot is incapable of doing anything. The control can either be internal (autonomous robot), or external (remote control). Since the aim of our project is not to make an autonomous robot we need a way to send instructions to the robot so that the robot knows when it has to move forward, backward, left, or right, when it has to turn, when it has to duck, etc. And, the external commands need to be sent to the robot by some means wired or wireless. But, there are some constraints that need to be carefully considered before using any means of control.
6.2
Control Method Selection
The first option is obviously to control the robot using some sort of wired communication. But, that doesn’t just mean we’ll have to continuously stay with the robot but also means that our control over the robot will be limited to a few buttons that we can hold while controlling the robot. We can use wireless Transmitter and Receiver but that requires connecting the transmitter to some buttons or other means of inputting the information, meaning we’ll have to build our controller from ground up. But, just for reference the frequencies most transmitter receiver use for robots are 27 MHz, 72 MHz, 75 MHz, and 2.4 GHz. The 27 MHz frequency can be used for remote controlled cars, robots and aircraft and is used usually in cheap remote controlled toys. The 72 MHz frequency is only for use with RC aircrafts, it has quite long range but it is illegal to use it for ground robots. The 75 MHz frequency is for use of land robots. The most widely used 2.4 GHz is also available and is the one that has the minimum interference to range ratio. It is the frequency on which most wireless remotes work. After the frequency selection comes the no. of channels that we need. 1 channel means forward and backward motion, 2 channels translate to forward and backward, plus left and right motion. We can use a wireless RF controller such as this one:
Figure 55 Wireless 5-channel RF controller
45 of 111
But this a 5 channel controller that limits the number of inputs/instructions that can be given to the robot using the controller. The maximum available wireless RF controller still might not had been enough to meet our channel requirements. So we decided to go with the PS2 controller instead. It provides us with numerous advantages; it is product of years of research made by Sony, it has the aesthetic and ergonomic looks, the biggest advantage we had was of 20 channels, 2 analog sticks with 2 directions each and 16 buttons, providing more than enough channels for our use. And the analog could be used to instruct small change in the coordinates.
6.3
PS2 Controller
We were once again open to the option of using a wired controller as the standard PS2 controller is capable of providing us with 20 channels on a single package and could be interfaced with a microcontroller to work with our project. The PS2 controller is also available as a wireless controller with a proprietary 2.4 GHz wireless module which handles all the communication and allows to use all the functions of the controller that are possible with the wired controller within a range of 15m. The steps required to interface the PS2 controller with PIC microcontroller are outlined below (in the form of instructions): 6.3.1 Pins of the Controller The first step in interfacing the controller with any microcontroller is knowing the pins that are available to us at the male connector of the PS2 controller. Out of the 9 pins available only 6 are necessary for basic interfacing. These minimum pins include data, command, ground, 3.3V5V power, attention, and clock. We could also power the vibration motor by providing it 9V.
Figure 56 Pins inside a male connector of the PS2 controller with pin names
The pins need to be connected to the PIC microcontroller by following the configuration in Figure 57 below. The 9V (connected with a dotted line) is optional and is only needed if we plan to use the vibration feature. We did not use that feature in our project since there was no need or use of the vibration feature. We could have made the controller vibrate when our hexapod hit an obstacle but that would’ve needed a sensor and the sensors were part of our future modifications, so no vibrations for the moment.
46 of 111
Figure 57 Connections between the PS2 controller and PIC microcontroller
The Acknowledge pin and the 2nd pin were left unused. The Clock of the PS2 controller is connected to a pin of the PIC microcontroller, via a 220Ω resistor, at which the PIC sends the clock of the PS2 controller, meaning the PIC needs to set the pin as output. The Attention pin of the PS2 controller is connected to a pin of the PIC microcontroller, via a 220Ω resistor, which is set as output. The Power pin of PS2 controller needs to be connected to a voltage source of 3.3V-5V, there is no need of a high current output source; the controller can work even at 200mA. That is the lowest we went, it might work with lower current source but we didn’t test that. We tested several wired and wireless controllers, both original Sony and copy controllers, all the controllers worked fine even at 3.3V, so the 5V is suggested but not a necessity. The Ground pin of the PS2 controller is connected to the negative terminal of the 5V source. The Motor pin of the PS2 controller could be connected to a 9V source, again the 9V is not a necessity, a 4V source would also do the job but the speed of the motor will be slow. The source for the motor should be capable of providing at least 500mA. The Command pin of the PS2 controller is connected to a pin of the PIC microcontroller, via a 220Ω resistor, which is set as output. The Data pin of the PS2 controller is connected to a pin of the PIC microcontroller directly, but the pin is set as input. The Data pin is also connected to the 5V source via a 220Ω resistor, this resistor acts as a pull up resistance, since the PS2 controller is only capable of driving the pin low but is incapable of providing any voltage on the line. The Acknowledge pin can be used to confirm that the data has been received by the PS2 controller, for this purpose the line is kept normally high using a pull-up resistor of 10KΩ. The PS2 controller sends a zero on this pin 12µs after the last received byte. The Clock is set high when the PIC microcontroller reads the data from the PS2 controller and it is set to 0 when the data is sent from the PIC microcontroller to the PS2 controller.
47 of 111
The Attention pin acts as a slave select line and it is commonly kept high; when we hold it low we instruct the PS2 Controller to listen to the commands from the PIC microcontroller. After the command is done sending to the to the PS2 controller the Attention pin is set to high again. The Command pin is the main controlling pin of the PS2 controller, the PIC microcontroller sends the commands to the PS2 controller, such as initialize, polling, entering and exiting configure mode, controlling the vibration motors, through this pin. The Data pin is the pin through which the PS2 controller sends the data to the PIC microcontroller. This pin is kept normally high using a pull-up resistance since the controller can only bring the value of this pin to 0.
10KΩ
6.3.2 Connecting the 2 controllers
Output Bit 1 Output Bit 2
Oscillator
Output Bit 3 Output Bit 4
PIC16F887
Output Bit 0
220Ω
220Ω
Clock Attention Power Ground Command Data
220Ω
This is the circuit diagram that is to be implemented on the breadboard or the Vero board. Figure 58 Circuit diagram of the PS2 controller to PIC microcontroller interface
48 of 111
To connect the PS2 controller to the PIC microcontroller on a single board we have 3 different methods and 2 of them require buying an accessory from the gaming store and virtually destroying it, unfortunately they are also safe and better methods. Firstly, we can cut the wires at the end of a PS2 controller and solder it on a Vero board with the PIC microcontroller.
Figure 59 PS2 wired controller
But there are many issues with this approach; firstly it’ll be very hard to know which wire is data, which one is clock, etc. To differentiate them it would be required to open the PS2 controller to see the wires on the PCB board inside the controller. Secondly, the PS2 controller would be permanently bound to the project and reduce its portability while making it much more bulky then it has to be. Lastly, the controller would be completely wasted and no longer could be connected to the PS2 to play games. The second approach would be to connect the male header inside the PS2 connection jack with jumper wires with one female side and the other either open (for Vero board) or fitted with a male joint (for bread board).
Figure 60 Male connector of PS2 controller to connect the female header of the wires Figure 61 Wire with femal and male header
49 of 111
Figure 63 Male connector of PS2 controller connect with female headers
The female joint would of the jumper wire would go inside the male jack of the PS2 controller and the male joint of the jumper wire would go to an appropriate slot in the breadboard. The problem with this method is that the time required to connect the female jumper joints is too much and having to do it again and again is a tedious task. Moreover, it is not guaranteed that all the wires are properly connected inside the bread board hence making the debugging a headache.
Figure 62 PS2 controller connected to the circuit using female to male headers
50 of 111
The third option is to use one of these connectors:
Figure 65 PS2 controller extension cable
Figure 64 PS2 to USB converter
The main part is the female jack at one end of the above connectors to which the male connector of the PS2 controller can be connected. Cutting the cable of the connector at the indicated point would give wires which could be soldered to the Vero board for giving a permanent join. The wires can be tested by using a multimeter in connectivity mode to know which wire is which.
Figure 66 Female connector cut from the PS2 convertor
51 of 111
Figure 67 PS2 connector to board connector
Another option is to connect the wires end to another female header (pictured right) and solder a corresponding male header on the Vero board. This could arguably be the best of the 3 solutions since the connection can easily be tested using a multimeter, there would be no issue of loose connections, the portability of the robot will not be hindered since the PS2 controller would be plug n play, above all the controller would not be wasted and could still be used with the PS2 to play games.
Figure 68 PS2 controller connected to the breadboard using the connector
This is the method we used besides the trial done using the second method. We had a damaged PS2 to PS3 converter (pictured left) lying around and we repurposed it by cutting the wire as
Figure 69 Close look at the circuit with the connector
52 of 111
highlighted and then fixing them inside a female header. The breadboard we used while we were developing the code and the Vero board we made for later use, had a corresponding male header to connect with the female header we placed on the wires. And hence we made our own PS2 to Board connector. 6.3.3 Communication The PS2 controller uses SPI communication and it is a full-duplex communication; the data is sent and received simultaneously. An example of a short poll: Sample Value
Byte #
Pin
1st Byte
Command 0x01
This byte is always 0x01 and is used to indicate to the controller the start of the communication process.
Data
Ignored
2nd Byte
3rd Byte
4th Byte
Command 0x42
This byte indicates the function that we need the controller to perform. For example 0x42 is used for short poll, 0x43 is to enter or exit configuration mode, etc.
Data
0x41
The PS2 controller informs of the mode it is currently in, digital, analog, or configuration. 0x41 means digital. 0x73 means analog.
Command 0x00
No further command needs to be sent, only the response to the previous commands need to be received from the PS2 controller by the PIC microcontroller.
Data
Always 0x5A, this has no meaning.
0xFF
This and the following bytes are the controller’s response to the command 0x42. The controller tells the state of the buttons and the analog sticks. This is the first byte of the sequence and contains information about the buttons. (Byte 0)
Command 0x00 Data
6th Byte
0x5a
Command 0x00
Data
5th Byte
0xFF
Explanations
0xFF
This is the second byte of the sequence and also contains information about the buttons.
Command 0x00
Data
0x7F
This is the third byte of the sequence and it gives the value of the x-axis of the right analog. When the right analog is on the extreme right it gives the value ‘0’, when it is on the extreme left the byte gives the value ‘255’. At the centre its value is ‘127’
53 of 111
7th Byte
Command 0x00
Data
8th Byte
Command 0x00
Data
9th Byte
0x7F
This is the fourth byte of the sequence and it gives the value of the y-axis of the right analog. When the right analog is at the extreme up position it gives the value ‘0’, when it is on the extreme down position the byte gives the value ‘255’. At the centre its value is ‘127’
0x7F
This is the fifth byte of the sequence and it gives the value of the x-axis of the left analog. When the right analog is on the extreme right it gives the value ‘0’, when it is on the extreme left the byte gives the value ‘255’. At the centre its value is ‘127’
Command 0x00
Data
0x7F
This is the sixth and the last byte of the sequence and it gives the value of the y-axis of the right analog. When the right analog is at the extreme up position it gives the value ‘0’, when it is on the extreme down position the byte gives the value ‘255’. At the centre its value is ‘127’
6.3.4 Byte wise communication Just as shown in the table, the instructions are sent at the Command pin of the PS2 controller by the PIC microcontroller and the PS2 controller sends the response at the Data pin. At the start of any communication, the PS2 controller is sent 0x01 to initialize itself. In response to which the controller send the information about its current state, whether it’s in Digital mode, or Analog mode, or Configuration mode. The 2nd byte sent to the controller could be either to tell the controller to either enter configuration mode, or exit it, or just perform a short poll. The aim of entering the configuration mode could be to limit the number of buttons whose state is returned in result to the polling, switch the controller between analog and digital mode, get more status info to recognize which type of controller is connected (the connected controller could be a wired original Controller or a copy, or a wireless original Sony controller or a copy), or set a vibration motors to vibrate as a result of a specific byte sequence in the short poll. The vibration motors can be set to respond to a specific byte, when the first 2 byte in the PS2 controller’s response to the 0x42 command are generated they are compared by the PS2 controller to the pre-set vibration byte, if the 2 match the motors vibrate. We did not use the configuration since we currently didn’t needed any of the features available in the configuration mode. We didn’t want to limit the number of buttons as it was one of the motivating force behind our choice of the PS2 controller. Mode switching could have saved us from having to turn on analog mode each time we turn on the controller but the issue with this method is that the user can get confused whether the analog button is pressed or not; if he didn’t press it and it was turned on using this method. We did experiment with the auto on feature but decided not to use it later. We had no issue whether our PS2 controller was a copy or original controller, moreover, we already knew which controller we were holding so it was pretty much
54 of 111
pointless to try to find the controller id. Lastly, as previously stated the vibration motors were not of much use to us in our robot’s current state (without any sensors) so we didn’t use the vibration feature either. In the 2nd byte we only have to send 0x42 to the PS2 controller to start performing the short poll since we are only interested in the state of the buttons and analog sticks. The 3rd byte of the sequence then becomes a virtual waste, since we don’t send the controller any new information after the polling command until all the 6 bytes have been received and the data sent by the PS2 controller in this byte has no meaning. From the 4th byte onwards the PS2 controller starts sending its response to the 0x42 command and tells the state of the buttons in the first 2 bytes, and the status of the analog sticks in the final 4 bytes. The both analog sticks have 2 byte to represent their condition, one for x-axis and one for y-axis. Hence the total information about the PS2 controller is completely transferred in just 6 bytes. Bit #\ Bit 7 Byte #
Bit 6
Bit 5
Bit 4
Bit 3
Bit 2
Bit 1
Bit 0
Byte 0
←
↓
→
↑
Start
R3
L3
Select
Byte 1
□
X
O
Δ
R1
L1
R2
L2
Byte 2
Right Analog – X-Axis
Byte 3
Right Analog – Y-Axis
Byte 4
Left Analog – X-Axis
Byte 5
Left Analog – Y-Axis
6.3.5 Understanding the PS2 Controller’s Response Since the Data pin is kept normally high, using the pull-up resistance, the PS2 controller brings the value of a bit to 0 to represent that the button is being pressed. The byte 0 and byte 1 represent the state of the 16 buttons. The state of the analog sticks is represented in the bytes 2-4, where the state of each stick is completely defined using 2 bytes. Each analog sticks has 2 resistors (potentiometer) inside it, when we move the stick up-down, or left-right the resistance of the potentiometer increases or decreases. At the left side or the extreme down side the resistance is maximum. At the extreme right or up position the resistance is minimum. Examples: If the first response byte (byte 0) is 0xFF (255) = 0B11111111. All the bits are unchanged and this represents that none of the buttons was pressed. If the byte 0 is 0x76 (118) = 0b01110110. 3 bits have changed to zero and this means that the left arrow, the start button, and the select button were pressed.
55 of 111
Byte 1 is similar to the byte 0 only the corresponding bits are different. If the byte 2 or byte 4 has a value 0xFF (255) = 0b11111111, it means that the corresponding joystick is being held at the left side. If the byte 2 or byte 4 has a value 0x00 (0) = 0b00000000 it means that the corresponding joystick is being held at the right side. If the byte 2 or byte 4 has a value 0x7F (127) = 0b01111111 it means that the corresponding joystick is at the centre with respect to the x-axis. If the byte 3 or byte 5 has a value 0xFF (255) = 0b11111111, it means that the corresponding joystick is being held at the down position. If the byte 3 or byte 5 has a value 0x00 (0) = 0b00000000, it means that the corresponding joystick is being held at the up position. If the byte 3 or byte 5 has a value 0x7F (127) = 0b01111111 it means that the corresponding joystick is at the centre with respect to the y-axis. 6.3.6 Using the Configuration Mode Before entering the configuration mode the PS2 controller must be initialized and refreshed, for this the controller must be sent the 0x42 (short poll) command 3 times. After refreshing the controller we can use the 0x43 command to enter the configuration mode, example: Entering the configuration mode Sample Value
Byte #
Pin
1st Byte
Command 0x01
This byte is always 0x01 and is used to indicate to the controller the start of the communication process.
Data
Ignored
2nd Byte
3rd Byte
4th Byte
0xFF
Explanations
Command 0x43
This byte indicates the function that we need the controller to perform. For example 0x42 is used for short poll, 0x43 is to enter or exit configuration mode, etc.
Data
The PS2 controller informs of the mode it is currently in, digital, analog, or configuration. 0x41 means digital. 0x73 means analog.
0x41
Command 0x00
This byte needs to be blank.
Data
Always 0x5A, this has no meaning.
0x5a
Command 0x01
This byte is used to indicate whether we wish to enter the configuration mode or exit it. 0x01 is to enter the configuration mode. 0x00 is to exit the configuration mode. If the controller is not in configuration mode, a 0x00
56 of 111
here would make this sequence behave like 0x42 (short poll) but without vibration feature. Data 5th Byte
0xFF
This data is not used. The focus is entering or exiting the configuration mode not reading data.
Command 0x00 Data
0xFF
Unused
Switching between analog and digital mode Once in configuration mode we can use the 0x44 command to enter digital or analog mode and also lock the mode. Example: Sample Value
Byte #
Pin
1st Byte
Command 0x01
This byte is always 0x01 and is used to indicate to the controller the start of the communication process.
Data
Ignored
2nd Byte
3rd Byte
4th Byte
5th Byte
0xFF
Explanations
Command 0x44
This byte indicates the function that we need the controller to perform. 0x44 is only effective when the controller is already in configuration mode.
Data
Unused
0xF3
Command 0x00
This byte needs to be blank.
Data
Always 0x5A, this has no meaning.
0x5a
Command 0x01
This byte is used to switch between digital and analog mode. 0x01 is to enter analog mode. 0x00 is to enter digital mode.
Data
Unused
0x00
Command 0x03
This byte can be used to lock the controller in the current state, meaning the user will not be able to switch between analog and digital mode using the button on the controller. 0x03 is to lock the controller. Any other command will not lock the controller.
Data
Unread
0xFF
57 of 111
6.3.7 PS2 Wireless Controller At first we had no plans of using the wireless controller, we were ok with the wired controller, moreover, it was our actual plan, but at the end of our FP1 presentation we were forced by our project coordinator Mr. Khan Nazir to introduce a wireless controller which was initially part of our future modifications. When we were forced to do this we had no choice but to do what we were told to do. We bought an original Sony wireless controller and tried to interface it with our circuit that we made for the wired controller. To our luck, the controller actually worked with the same code and the same circuit. But the only issue was that the outputs were a little off; some buttons were returning wrong code (codes of some other button), and a few buttons were not returning any output at all. It took us a while to realize that the wireless controller was sending the data a little early in the bytes and a few buttons were also missing. We couldn’t use select, R3 or L3. And the data was coming 2 bits earlier by skipping L3 and R3, and there was no select button. This was the sequence of the wireless controller in contrast to the wired controller: Byte #
Wired Data
Wireless Data
1
Select
-
-
Start
-
↑
Start
→
↑
↓
→
←
↓
L2
←
R2
L2
L1
R2
R1
L1
Δ
R1
O
Δ
X
O
□
X
Right axis)
analog
(x-
□
Right axis)
analog
(x-
(x- Right axis)
analog
(x-
2
3
Right axis)
analog
58 of 111
4
5
Right axis)
analog
(x- Right axis)
analog
(x-
Right axis)
analog
(x- Right axis)
analog
(x-
Right axis)
analog
(x- Right axis)
analog
(x-
Right axis)
analog
(x- Right axis)
analog
(x-
Right axis)
analog
(x- Right axis)
analog
(x-
Right axis)
analog
(x- Right axis)
analog
(y-
Right axis)
analog
(x- Right axis)
analog
(y-
Right axis)
analog
(y- Right axis)
analog
(y-
Right axis)
analog
(y- Right axis)
analog
(y-
Right axis)
analog
(y- Right axis)
analog
(y-
Right axis)
analog
(y- Right axis)
analog
(y-
Right axis)
analog
(y- Right axis)
analog
(y-
Right axis)
analog
(y- Right axis)
analog
(y-
Right axis)
analog
(y- Left analog (x-axis)
Right axis)
analog
(y- Left analog (x-axis)
Left analog (x-axis)
Left analog (x-axis)
Left analog (x-axis)
Left analog (x-axis)
Left analog (x-axis)
Left analog (x-axis)
Left analog (x-axis)
Left analog (x-axis)
Left analog (x-axis)
Left analog (x-axis)
59 of 111
6
Left analog (x-axis)
Left analog (x-axis)
Left analog (x-axis)
Left analog (y-axis)
Left analog (x-axis)
Left analog (y-axis)
Left analog (y-axis)
Left analog (y-axis)
Left analog (y-axis)
Left analog (y-axis)
Left analog (y-axis)
Left analog (y-axis)
Left analog (y-axis)
Left analog (y-axis)
Left analog (y-axis)
Left analog (y-axis)
Left analog (y-axis)
Left analog (y-axis)
Left analog (y-axis)
-
Left analog (y-axis)
-
So all that we needed to do was adjust the code to expect the buttons a little earlier and we were good to go. The wireless controller improved the available portability. It reduced the wires and also removed the limitation of having to move with the robot because of the wire. The wireless controller enabled us to control the robot in a range of 30ft indoor, and 56ft in open areas. So we got a considerable improvement over the controls of our robot and the price was not considerably that high either, so it was a win-win for us. This would not have been possible if we weren’t forced by Mr. Khan, Thank you sir!
6.3.8 PS2 communication Algorithm The first step in communicating with the PS2 controller is slave-selection, for this we pull the Attention pin low. Then send 0x01 command to the PS2 controller to initialize the controller. Then send 0x42 (polling) command to the controller and simultaneously receive the PS2 controller’s status bit (digital or analog mode). Then read the next byte and don’t save the byte as it is meaningless. Nothing is sent to the PS2 controller for this communication cycle. Then receive 6 bytes of polling response from the PS2 controller. The order of the bytes is the same as previously discussed. To end the communication cycle pull the Attention pin high. The data is read bit by bit, LSB to MSB, and stored to form a complete byte. The process is as under: Call the send/receive function and provide it with the byte to be sent. Send the command bit to the PS2 controller on the Command pin. Pull the Clock pin low. Read the data bit from the Data pin. Pull the Clock pin high. Loop the function eight times to send/receive the whole byte.
60 of 111
Pull the Attention pin low
Send 0x01 to the PS2 controller
Send polling command to the PS2 controller and receive status byte
Don t send anything for one cycle
Start receiving polling response from the controller
Byte 0 Left Buttons
Byte 1 Right Buttons
Byte 2 Right Analog x-axis
Byte 3 Right Analog y-axis
Byte 4 Left Analog x-axis
Byte 5 Left Analog y-axis
Pull Attention Pin High
Communication Cycle Complete Figure 70 PS2 Controller communication tree
61 of 111
Chapter 7 Testing and Evaluation We tested the hexapod in different kinds of environments that it might face and we were gradually able to see the mistakes we made during the designing process. Since we are not mechanical engineers nor we had anyone to consult with in this regard we made the structure mostly on theoretical basis, we had purchased our servo motors before we even began working on the structure, so our structure was constrained by the capability of our servo motors. The first prototype we made was so light that the servo motor’s motion caused vibrations in it. The first design with aluminium had so much aluminium in it that the servos were pushed to their limits while moving the hexapod. So learning from our mistakes the final design we made had a hole at its center and was considerably small in diameter. The hole at the center not only reduced the extensive weight but also allowed us to place our electronics at a better suited place; hanged from the center of the central plate. Another issue we faced was that when we tested the hexapod on floor with less friction it started slipping and we even damaged 2 of our servos in the process when they were pushed too hard to keep the hexapod standing. It was never our objective to make the hexapod walk on shining and beautiful palace floors, we designed it with uneven high friction surfaces in mind, and it performs quite satisfactorily on high friction terrain. Nevertheless, we tried to provide compensation for the lack of friction by attaching rubber at the end-effector.
62 of 111
Chapter 8 Results and Error Analysis The final design of our hexapod performs quite well on all kinds of surfaces, even smooth ones. It is smooth in every motion it performs thanks to the high current supplying capability of our batteries. It is completely mobile; no wires limiting its motion and operational range. We can control its motion from a range of 10m in closed spaces and 15m-20m in open areas with less radio disturbance. Our hexapod perfectly mimicked the virtual hexapod’s motion that we simulated in the excel file, it was moving exactly according to our expectations following the inverse kinematics algorithm we implemented on it. But nothing in this world can be perfect, especially something made by human beings, and hence the hexapod is still open to a lot of improvements; we still have a constraint of 15m which is not enough for several practical applications, there are still improvements to be made in the mechanical design of the hexapod, we need to make the hexapod autonomous for situations when it moves out of the controller’s range or when the user only needs to assign the task and the task can be completed without further human interference, we need to place sensors to aid the autonomous motion, we need to use a better microcontroller that can provide faster processing for a faster response when a lot of constraints need to be covered for every movement; example moving on inclined surfaces, we need to adjust the motion for walking according to the terrain; again inclined surfaces. So in short this is obviously not a final product, there are still improvements to be made, calling this a proof of concept won’t be wrong; it works just as the way we intended it to, now we need to demand more.
63 of 111
Chapter 9 Component Selection and Bill of Material
9.1
Component Order Guide
Speaking with the heart of a student, in Lahore there aren’t many resources for finding suitable parts for robotics. The only shop is Hobby Lobby, and having a monopoly it charges quite highly. The 2nd option is online shops but again selection of a proper shop is necessary. Rarely, some of the parts might also be available at the shops at Hall road but that’s mostly luck. For example we wanted to but the PICKIT for programming the PIC microcontroller on freeing ourselves from removing it again and again. We actually found a shop at Hall road where it was available but when we made the final decision and went for buying it, the shop was out of stock. We had to turn to online shop roboticspk.com and we had to pay a large amount of money for a part that wasn’t as expensive at some other online stores but they were sold out. Our servo motors weren’t bought from Pakistan, we got them when one of our group member was in America. The motors available at hobby lobby are quite highly priced so we’d advise that the students do their homework and make a list of the required components well in advance and try to order the product through ebay.com or find a friend to get them from outside Pakistan as the prices here are simply too discouraging. 9.2
Bill of Material
Equipment
Estimated Cost
18 servo motors
Rs. 35,000
PICKit 3 programmer
Rs. 10,000
PS2 Controller and connector cable
Rs. 2,500
Aluminium Frame (prototype)
Rs. 6,000
Nuts and Bolts (specially designed)
Rs. 10,000
Microcontroller and ICs
Rs. 5,000
Battery
Rs. 5,000
Final Structure
Rs. 12,000
Miscellaneous (connecting wires, drill bits, nail etc.)
Rs. 5,000
Total
Rs. 90,500
64 of 111
Chapter 10
Difficulties Faced
The issue we faced during the course of our entire project was lack of a source of proper guidance. No teacher in our university had any practical experience with robotics. The only proper and most valuable guidance in our project came from our project advisor Jameel Ahmad, and that was to direct us to work on proper kinematics algorithm to drive and control the motions of our Hexapod. Without anyone to guide us, we only learned from making the mistakes ourselves, buying parts and then later realizing that we can’t use them (the nuts and bolts from Brandreth), or their use dying very shortly (the initial prototype). Then we also had to suffer from the lack of equipment in our university, we couldn’t get the right sized screw drivers, L key and wrench are nowhere to be seen. Even a drill machine with adjustable speed is not available in the university. We required a high powered drill machine while working with the structure but it wasn’t available until almost the end of our project. We had to bring these equipment from our homes or buy new ones where necessary. We also had to face the issue of early closing of all the labs till the mid of 2013, we had to occasionally work in the old Masjid hall and become a laughing stock to complete our work on the deadlines we had set for ourselves. It was mainly because of our repeated complaints that the lab timings of Projects lab was extended. We also discovered how much the students of Lahore have to suffer because absence of partnerships between industry and university when we wanted to use CNC machine for our final design. We are at a disadvantage even in regards of material availability. In comparison, students in Karachi and Islamabad have easy access to cheap CNC machines and better materials, we discovered this during our participation in COMPPAC 2014 where our competitor had far better designs to present.
65 of 111
Chapter 11
Manufacturability, Usability and Sustainability
Since we now have the design of our hexapod on wood we can use it to make as many casts as we like, the only deterring factor are the servo motors which would also be the price setting component. It can be used as a child’s play toy, for bio-mimicking, for search and rescue mission, for bomb disposal tasks, or even by military personals. Since the material we are using for the design is aluminium, the sustainability and durability of the design is quite high.
66 of 111
Chapter 12
Future Development
Following are the features that we plan on implementing as a part of our future additions into the project: Improvement in the structural material Addition of sensors like ultrasonic sensor, and force sensors etc. Image processing Using a more powerful microcontroller Using something better to serve as the boots to provide better grip Implement more walking algorithms Make the robot fully autonomous
67 of 111
Chapter 13
13.1
Societal Issues
Ethical
If used as a child’s play toy, it could alter the perception of kids towards spiders and other similar insects; they may stop considering them as scary and dirty. 13.2
Social
If we can successfully make the hexapod fully autonomous, it could even go to the extent of replacing dogs as the pets because it’ll be able to do more function and be far less demanding. 13.3
Economical
The only remaining economical issue is related with the price of servo motors, which aren’t going to go any cheaper anytime soon, so that issue can only be resolved in case of mass production. 13.4
Health and Safety
If operated in the normal environment there aren’t any health or safety concerns as long as the fuses or any other components aren’t damaged. 13.5
Environmental Impact
The hexapod doesn’t affect the environment so directly that it may become a cause of concern; it only affects the environment through indirect method like consumption of electricity or one time usage of aluminium.
68 of 111
Chapter 14
Conclusions and Recommendations
By the grace of Allah Almighty, we have accomplished our objectives and completed our final year project with success and achievement. But we are not going to stop here, this was just a stepping stone to get us to a world with boundaries that only we set for ourselves. We plan on continue working on this project and also expanding our work to design other, better robots. May Allah help us and guide us in our task. Ameen!
69 of 111
References [1] I. Asimov, “I, Robot,” 1942, pp. 20-33. [2] “RobotShop Distributon Inc.,” 2008. [Online]. Available: www.robotshop.com/media/files/PDF/timeline.pdf. [Accessed 21 May 2014]. [3] D. Calin, “Into Robotics,” [Online]. Available: http://www.intorobotics.com/wheels-vscontinuous-tracks-advantages-disadvantages/. [Accessed 22 May 2014]. [4] D. M. W. Hardt, M. Stelzer and P. D. O. V. Stryk, “Modellierung und Simulation der Dynamik des Laufens bei Roboter, Tier und Mensch,” vol. 2, pp. 55-63, 2002.
70 of 111
Appendices
Appendix A We started our circuitry with the use of mux to send the angles to discrete angles to the servos:
5
7 4 L S 9 5 9
74LS191
PIC18F452
SDO Update pulse
S D O
74LS191
Update pulse SDK
To other legs
74LS191
This method despite being successful, wasn’t usable for us, we didn’t want the hexapod to have jerky movements caused by the input of discrete angles. For example if the inputs are 15, 30, 45, 60, 75, 90, and 105, the servos would suddenly jump from 30 to 45 instead of going there gradually in small steps which would eventually become the cause of ugly and jumpy motion of the hexapod. Our way out was using extra microcontrollers in master-slave configuration; the master would only control the communication of instruction between the slaves of each leg and the PS2 controller and implement the Inverse kinematics algorithm, the slaves of each leg would be responsible for the motion of 3 servos of that leg to comply with the instructions of the master, and the slave of the PS2 controller would be responsible of getting the user’s instructions to the master microcontroller.
71 of 111
SLAVE CONTROLLER
SDI SDO SCK
SDO SDi SCK
SLAVE CONTROLLER
SS5
MASTER CONTROLLER
SDO SDI SCK
SDO
SCK SDI
SLAVE CONTROLLER
SS2
SCK
SDO SDI SCK
Update command
SLAVE CONTROLLER
SLAVE CONTROLLER
72 of 111
SLAVE CONTROLLER
This method too worked like we wanted it too but the motion of the hexapod was yet jerky, due to the delay of the clock cycles of every microcontroller and the delay caused by the slave microcontroller in updating the angles to the servos. Our final approach was the usage of faster and higher memory AVR microcontroller as the brain of the hexapod that handled the communication with each leg and the PS2 controller directly. The speed of the microcontroller enabled us to have a smooth motion of the hexapod, while the large memory of the microcontroller enabled us to code more motions like translation and rotation besides simple walking.
73 of 111
Appendix B This is the working code of the PS2 controller to PIC microcontroller interfacing in mikroBasic: program MyProject symbol psxCLK = PORTC.6 symbol psxCMD = PORTC.7 symbol psxDAT = PORTC.4 symbol psxATT = PORTC.5 dim psxOut as byte dim psxIn as byte dim psxID as byte dim i as byte dim psxStatus as byte dim psxLeftB as byte dim psxRightB as byte dim psxRJoyX as byte dim psxRJoyY as byte dim psxLJoyX as byte dim psxLJoyY as byte sub procedure psxTxRx(dim byref byteOut, byteIn as byte) byteIn=0 'Reset Receive byte for i = 0 to 7 'Read data bits LSB to MSB psxCMD=testbit(byteOut,i) 'Prepare first bit to send psxCLK=0 'Generate clock pulse delay_us(20) 'Used to regulate communication speed if psxDAT = 0 then Setbit(byteIn,i) 'Low Data line indicates set bit end if psxCLK=1 'End clock pulse Delay_us(20) 'Regulate speed next i end sub main: OSCCON=%01111110 'Set INTRC to 8Mhz ANSEL=0 'Set I/O to digital TRISA=0 'PortA all outputs PORTA=0 'PortA all off TRISC=%00010000 'PortC all output except SDI Pin (R1) PORTC=%01100000 'PortC psxCLK and psxAtt are high, all else low PortA.0=1 'Flashes LED on RA0 for debugging purposes
74 of 111
Delay_ms(500) PortA.0=0 Delay_ms(500) PortA.0=1 Delay_ms(200) PortA.0=0 Delay_ms(200)
do PORTA=0 psxATT=0 'psxAtt low, Activates Controller Delay_us(50) psxOut=0x01 'Start Signal psxTxRx(psxOut,psxIn) psxOut=0x42 'Request Status psxTxRx(psxOut,psxIn) psxID=psxIn 'Simultaneously receive controller ID psxOut=0 'Clear psxOut psxTxRx(psxOut,psxIn) 'Get psxStatus psxStatus=psxIn psxTxRx(psxOut,psxIn) 'Get Left side buttons psxLeftB=psxIn psxTxRx(psxOut,psxIn) 'Get Right side buttons psxRightB=psxIn psxTxRx(psxOut,psxIn) 'Get Right joystick X-axis byte psxRJoyX=psxIn psxTxRx(psxOut,psxIn) 'Get Right joystick Y-axis byte psxRJoyY=psxIn psxTxRx(psxOut,psxIn) 'Get Left joystick X-axis byte psxLJoyX=psxIn psxTxRx(psxOut,psxIn) 'Get Left joystick Y-axis byte psxLJoyY=psxIn Delay_us(50) psxATT=1 'Release controller by setting psxATT high
75 of 111
''''' Start Assigning outputs if psxLeftB.7 = 1 then ' < Button PORTA= 1 end if if psxLeftB.6 = 1 then ' V Button PORTA= 2 end if if psxLeftB.5 = 1 then ' > Button PORTA= 3 end if if psxLeftB.4 = 1 then ' ^ Button PORTA= 4 end if if psxRightB.7 = 1 then ' Square Button PORTA= 5 end if if psxRightB.6 = 1 then ' X Button PORTA= 6 end if if psxRightB.5 = 1 then ' O Button PORTA= 7 end if if psxRightB.4 = 1 then ' Triangle Button PORTA= 8 end if if psxRightB.3 = 1 then ' R1 Button PORTA= 9 end if if psxRightB.2 = 1 then ' L1 Button PORTA= 10 end if if psxRightB.1 = 1 then ' R2 Button PORTA= 11 end if if psxRightB.0 = 1 then ' L2 Button
76 of 111
PORTA= 12 end if if psxRJoyX >= 192 then ' Right Analog Right PORTA= 13 end if if psxRJoyX = 192 then ' Right Analog Up PORTA= 16 end if if psxLJoyX >= 192 then ' Left Analog Right PORTA= 17 end if if psxLJoyX = 192 then ' Left Analog Up PORTA= 20 end if Delay_ms(10) loop until false 'Loop Forever end.
77 of 111
Key
Value
Code
Left Arrow Down Arrow Right Arrow Up Arrow □ X O Δ R1 L1 R2 L2 Ranalog Right Ranalog Left Ranalog Down Ranalog Up Lanalog Right Lanalog Left Lanalog Down Lanalog Up
1 2 3 4 5 6 7 8 9 10 11 12 13
00001 00010 00011 00100 00101 00110 00111 01000 01001 01010 01011 01100 01101
14 15
01110 01111
16 17
10000 10001
18 19
10010 10011
20
10100
78 of 111
Appendix C This is the inverse kinematics algorithm implemented on the PIC microcontrollers:
(MASTER) unsigned char Theta1,Theta2,Theta3,Count2=0,Count3,datain,intiCom,ready,Thetax=0,Thetay,Thetaz,x,y,z1,reset; unsigned char Count,dataout; int i; unsigned int one,two,three,four,five,six; unsigned int Stepleg1,Stepleg1a,Stepleg1b; unsigned int Stepleg2,Stepleg2a,Stepleg2b; unsigned int Stepleg3,Stepleg3a,Stepleg3b; unsigned int Stepleg4,Stepleg4a,Stepleg4b; unsigned int Stepleg5,Stepleg5a,Stepleg5b; unsigned int Stepleg6,Stepleg6a,Stepleg6b; signed int GaitPos1X,GaitPos1Z,GaitPos1Y,GaitRot1Z; signed int GaitPos2X,GaitPos2Z,GaitPos2Y,GaitRot2Z; signed int GaitPos3X,GaitPos3Z,GaitPos3Y,GaitRot3Z; signed int GaitPos4X,GaitPos4Z,GaitPos4Y,GaitRot4Z; signed int GaitPos5X,GaitPos5Z,GaitPos5Y,GaitRot5Z; signed int GaitPos6X,GaitPos6Z,GaitPos6Y,GaitRot6Z; signed int LegLiftHeight =0; signed int TravelLengthX =0; signed int TravelLengthY =0; signed int TravelRotationZ=0; int Gait_Step=1; void Write (signed int mydata); void Write2(signed int mydata); void Write3(signed int mydata); void Write4(signed int mydata); void Write5(signed int mydata); void Write6(signed int mydata); void sent_data1(unsigned char data1 ,unsigned char data2 , unsigned char data3); void sent_data2(unsigned char data1 ,unsigned char data2 , unsigned char data3); void sent_data3(unsigned char data1 ,unsigned char data2 , unsigned char data3); void sent_data4(unsigned char data1 ,unsigned char data2 , unsigned char data3); void sent_data5(unsigned char data1 ,unsigned char data2 , unsigned char data3); void sent_data6(unsigned char data1 ,unsigned char data2 , unsigned char data3); void sysint(); void Map_input(); void Gait_Condition(); void LegOne(); void LegTwo(); void LegThree(); void LegFour(); void LegFive(); void LegSix(); void main(){ sysint();
79 of 111
while(1) { Map_input(); } } void LegOne() { if(Stepleg1==1) { GaitPos1X=0; GaitPos1Y=0; } else { if(Stepleg1a==1||Stepleg1b==1) { GaitPos1X=TravelLengthX/2; GaitPos1Y=TravelLengthY/2; } else { GaitPos1X= (TravelLengthX/2)-one*(TravelLengthX/4); GaitPos1Y= (TravelLengthY/2)-one*(TravelLengthY/4); } } GaitPos1Z=LegLiftHeight; } void LegTwo() { if(Stepleg2==1) { GaitPos2X=0; GaitPos2Y=0; } else { if(Stepleg2a==1||Stepleg2b==1) { GaitPos2X = TravelLengthX/2; GaitPos2Y = TravelLengthY/2; } else { GaitPos2X= (TravelLengthX2)-two*(TravelLengthX/4); GaitPos2Y= (TravelLengthY2)-two*(TravelLengthY/4); } } GaitPos2Z=LegLiftHeight; } void LegThree() {
80 of 111
if(Stepleg3==1) { GaitPos3X=0; GaitPos3Y=0; } else { if(Stepleg3a==1||Stepleg3b==1) { GaitPos3X = TravelLengthX/2; GaitPos3Y = TravelLengthY/2; } else { GaitPos3X= (TravelLengthX/2)-three*(TravelLengthX/4); GaitPos3Y= (TravelLengthY/2)-three*(TravelLengthY/4); } } GaitPos3Z=LegLiftHeight; } void LegFour() { if(Stepleg4==1) { GaitPos4X=0; GaitPos4Y=0; } else { if(Stepleg4a==1||Stepleg4b==1) { GaitPos4X = TravelLengthX/2; GaitPos4Y = TravelLengthY/2; } else { GaitPos4X= (TravelLengthX/2)-four*(TravelLengthX/4); GaitPos4Y= (TravelLengthY/2)-four*(TravelLengthY/4); } } GaitPos4Z=LegLiftHeight; } void LegFive() { if(Stepleg5==1) { GaitPos5X=0; GaitPos5Y=0; } else {
81 of 111
if(Stepleg5a==1||Stepleg5b==1) { GaitPos5X = TravelLengthX/2; GaitPos5Y = TravelLengthY/2; } else { GaitPos5X= (TravelLengthX/2)-five*(TravelLengthX/4); GaitPos5Y= (TravelLengthY/2)-five*(TravelLengthY/4); } } GaitPos5Z=LegLiftHeight; } void LegSix() { if(Stepleg6==1) { GaitPos6X=0; GaitPos6Y=0; } else { if(Stepleg6a==1||Stepleg6b==1) { GaitPos6X = TravelLengthX/2; GaitPos6Y = TravelLengthY/2; } else { GaitPos6X= (TravelLengthX/2)-six*(TravelLengthX/4); GaitPos6Y= (TravelLengthY/2)-six*(TravelLengthY/4); } } GaitPos6Z=LegLiftHeight; } void sysint() { TRISA=0xFF; // configure Port A as input ADCON1=0x06;// disable ADC and make all the pins of Port A as digital input. TRISD=0x00; // configure Port D as output PORTD=0x00; // Clear all pin of Port D TRISB=0x00; // configure Port B as output PORTB=0x00; // Clear all pin of Port B TRISE=0x07; // configure Port B as output TRISC.B2=0; // Output for Slave select TRISC.B3=0; // SCK output TRISC.B4=1; // SDI input TRISC.B5=0; // SDO output PORTC.B2=1; // SS select high no slave is select //Set SPI1 module to master mode, clock = Fosc/4, data sampled at the middle of interval, //clock idle state low and data transmitted at low to high edge:
82 of 111
SPI1_Init_Advanced(_SPI_MASTER_OSC_DIV4, _SPI_DATA_SAMPLE_MIDDLE, _SPI_CLK_IDLE_LOW, _SPI_LOW_2_HIGH); // Setup the spi } void Gait_Condition() { if(Gait_Step==1) { one =0; two =4; three =3; four =2; five =1; six =0; Stepleg1 =1; Stepleg1a =0; Stepleg1b =0; Stepleg2 =0; Stepleg2a =0; Stepleg2b =0; Stepleg3 =0; Stepleg3a =0; Stepleg3b =0; Stepleg4 =0; Stepleg4a =0; Stepleg4b =0; Stepleg5 =0; Stepleg5a =0; Stepleg5b =0; Stepleg6 =0; Stepleg6a =0; Stepleg6b =1; }else { if(Gait_Step==2) { one =0; two =0; three =4; four =3; five =2; six =1; Stepleg1 =0; Stepleg1a =1; Stepleg1b =0; Stepleg2 =1; Stepleg2a =0; Stepleg2b =0; Stepleg3 =0; Stepleg3a =0; Stepleg3b =0; Stepleg4 =0;
83 of 111
Stepleg4a =0; Stepleg4b =0; Stepleg5 =0; Stepleg5a =0; Stepleg5b =0; Stepleg6 =0; Stepleg6a =0; Stepleg6b =0; } else { if(Gait_Step==3) { one =1; two =0; three =0; four =4; five =3; six =2; Stepleg1 =0; Stepleg1a =0; Stepleg1b =0; Stepleg2 =0; Stepleg2a =1; Stepleg2b =0; Stepleg3 =1; Stepleg3a =0; Stepleg3b =0; Stepleg4 =0; Stepleg4a =0; Stepleg4b =0; Stepleg5 =0; Stepleg5a =0; Stepleg5b =0; Stepleg6 =0; Stepleg6a =0; Stepleg6b =0; } else { if(Gait_Step==4) { one =2; two =1; three =0; four =0; five =4; six =3; Stepleg1 =0; Stepleg1a =0; Stepleg1b =0;
84 of 111
Stepleg2 =0; Stepleg2a =0; Stepleg2b =0; Stepleg3 =0; Stepleg3a =1; Stepleg3b =0; Stepleg4 =1; Stepleg4a =0; Stepleg4b =0; Stepleg5 =0; Stepleg5a =0; Stepleg5b =0; Stepleg6 =0; Stepleg6a =0; Stepleg6b =0; } else { if(Gait_Step==5) { one =3; two =2; three =1; four =0; five =0; six =4; Stepleg1 =0; Stepleg1a =0; Stepleg1b =0; Stepleg2 =0; Stepleg2a =0; Stepleg2b =0; Stepleg3 =0; Stepleg3a =0; Stepleg3b =0; Stepleg4 =0; Stepleg4a =1; Stepleg4b =0; Stepleg5 =1; Stepleg5a =0; Stepleg5b =0; Stepleg6 =0; Stepleg6a =0; Stepleg6b =0; } else { if(Gait_Step==6) { one =4; two =3;
85 of 111
three =2; four =1; five =0; six =0; Stepleg1 =0; Stepleg1a =0; Stepleg1b =0; Stepleg2 =0; Stepleg2a =0; Stepleg2b =0; Stepleg3 =0; Stepleg3a =0; Stepleg3b =0; Stepleg4 =0; Stepleg4a =0; Stepleg4b =0; Stepleg5 =0; Stepleg5a =1; Stepleg5b =0; Stepleg6 =1; Stepleg6a =0; Stepleg6b =0; } } } } } } } void Map_input() { switch(PORTA) { case 0x01: TravelLengthX =40; TravelLengthY =0; LegLiftHeight =-20; TravelRotationZ=20; break; case 0x02: TravelLengthX =40; TravelLengthY =40; LegLiftHeight =-20; TravelRotationZ=20; break; case 0x03: TravelLengthX =0; TravelLengthY =40; LegLiftHeight =-20; TravelRotationZ=20;
86 of 111
break; case 0x04: TravelLengthX =-40; TravelLengthY =40; LegLiftHeight =-20; TravelRotationZ=20; break; case 0x05: TravelLengthX =-40; TravelLengthY =0; LegLiftHeight =-20; TravelRotationZ=20; break; case 0x06: TravelLengthX =-40; TravelLengthY =-40; LegLiftHeight =-20; TravelRotationZ=20; break; case 0x07: TravelLengthX =0; TravelLengthY =-40; LegLiftHeight =-20; TravelRotationZ=20; break; case 0x08: TravelLengthX =40; TravelLengthY =-40; LegLiftHeight =-20; TravelRotationZ=20; break; } Gait_Condition(); LegOne(); LegTwo(); LegThree(); LegFour(); LegFive(); LegSix(); sent_data1(GaitPos1X,GaitPos1Y,GaitPos1Z); sent_data2(GaitPos2X,GaitPos2Y,GaitPos2Z); sent_data3(GaitPos3X,GaitPos3Y,GaitPos3Z); sent_data4(GaitPos4X,GaitPos4Y,GaitPos4Z); sent_data5(GaitPos5X,GaitPos5Y,GaitPos5Z); sent_data6(GaitPos2X,GaitPos2Y,GaitPos2Z); Gait_Step++; if(Gait_Step>=7) { Gait_Step=1; } delay_ms(500);
// send data through Spi1 // send data through Spi2 // send data through Spi3 // send data through Spi4 // send data through Spi5 // send data through Spi6
87 of 111
} void Write(signed int mydata) { if(mydataRequired_delay3))// Theta 2 is greater then theta 1 and 3 { if(Required_delay1>Required_delay3)// theta 1 is greater then 3.Theta 3 is smallest { high3=Required_delay3; high1=(long int)(Required_delay1-Required_delay3); high2=(long int)(Required_delay2-Required_delay1); PORTE.B0=1; PORTE.B1=1; PORTE.B2=1; mydelay_10us(high3); PORTE.B2=0; mydelay_10us(high1); PORTE.B0=0; mydelay_10us(high2); PORTE.B1=0; }else { // theta 3 is greater then theta 1 .Theta 1 is smallest. high1=Required_delay1; high3=(long int)(Required_delay3-Required_delay1); high2=(long int)(Required_delay2-Required_delay3); PORTE.B0=1; PORTE.B1=1; PORTE.B2=1;
102 of 111
mydelay_10us(high1); PORTE.B0=0; mydelay_10us(high3); PORTE.B2=0; mydelay_10us(high2); PORTE.B1=0; } } else { if((Required_delay3>Required_delay1)&&(Required_delay3>Required_delay2)) // theta 3 is greater the theta 1 and theta 2. { if((Required_delay1>Required_delay2))// theta 1 is greater then theta 2. { high2=Required_delay2; high1=(long int)(Required_delay1-Required_delay2); high3=(long int)(Required_delay3-Required_delay1); PORTE.B0=1; PORTE.B1=1; PORTE.B2=1; mydelay_10us(high2); PORTE.B1=0; mydelay_10us(high1); PORTE.B0=0; mydelay_10us(high3); PORTE.B2=0; }else // theta 2 is greater then 1. { high1=Required_delay1; high2=(long int)(Required_delay2-Required_delay1); high3=(long int)(Required_delay3-Required_delay2); PORTE.B0=1; PORTE.B1=1; PORTE.B2=1; mydelay_10us(high1); PORTE.B0=0; mydelay_10us(high2); PORTE.B1=0; mydelay_10us(high3); PORTE.B2=0; } } } } } } } } }
103 of 111
} } } TMR1IF_bit =0; } } //Timer0 //Prescaler 1:1; TMR0 Preload = 252; Actual Interrupt Time : 10 us void mydelay_10us(long int a) { long int i; T0CON = 0xC8; for(i=1;i
![CSIS 0801 Final Year Project Final Report - HKU [PDF]](https://m.moam.info/img/260x300/csis-0801-final-year-project-final-report-hku-pdf_647bb703098a9e10768b45d7.jpg)
