October 29, 2012 at 18:23We collect and make the budget hexapod run
A lot of things have to be done before we get to this picture:
Omitting the stories about exactly how I came up with the idea of building a hexapod (these were tons of videos on YouTube), I will go straight to the process of selecting parts. It was January 2012. I knew right away what I want from my robot and what not. I wanted:
- each leg should have 3 degrees of freedom - 3dof (3 dimensions of freedom). Because the simpler option 2dof - does not give such an insect feel, and 4dof - is unnecessary, 3dof and so allows you to freely move the tip of the leg in 3D space;
- 6 legs; again, this is no longer 4 (then the robot clumsily jumps), but also not 8, like spiders, and already excessively;
- small;
- cheap;
- minimum boards and connections;
The post is big.
The first of course was to choose a motherboard for the crumbs. I managed to read a lot of good and bad by that time about Arduino. But it was he who looked at him as the main option. There was no time to solder the controllers ourselves, and taking more advanced boards with ARM cpu, for example, is expensive, and understanding how to program them, how to work with PWM outputs, etc. And arduina: the IDE started, the code hit, upload clicked - and hello, it already blinks to you. Beauty! ;)
First, I started looking at arduino mega and clones, because the number of PWM outputs that could be used to steer the servos was plentiful. Let me remind you that for 3dof hexapods you need 3 * 6 = 18 servos, and separate channels for controlling them. But then I found a real Jazz among the arduino mega, this is a Dagu board called Red Back Spider Controller. Here she is on ebay.
She offers all her outputs in the form of ready-made 3 pins (ground, power, signal), and power interconnection. The power supply of the controller itself is stabilized, but it goes to the dviglov connectors as it is (UPD: not as it is, but also stabilized 5 volts. And apparently it is isolated from the power supply of the controller, since 18 servos do not interfere with the controller’s operation). This allows you to simply apply 7-30 volts of sufficient power to the power terminal (the power supply from the eee pc 901 to 12V and 3A - it turned out to be enough to buzz with all 18 servos) and not fool your head with separate power supply of logic and dviglov. Also, this will make it possible in the future to easily plant all this monster on a pack of 7.4-volt Li-Po batteries. And with all this, from a software point of view, this is the usual Arduino mega, compatible with software and libs, and with iron (except for shields, installed directly on the original mega - they will not roll). True, the price is even higher than even the original mega, but all the other advantages outweighed this.
Further servos. On ebay, there are many different ones for micro servo. I took the most powerful of the smallest and cheapest, weighing 9 grams, plastic gearboxes. If you take lots where they are sent in batches - it turns out cheaper. I took 3 packs of 6, it seems, and less than $ 2 came out. Looking ahead, I’ll say that I regret that I did not spend more and did not take servos with metal gears and ball bearings. These plastic ones turned out to have quite noticeable backlash, and a characteristic crunch with excessive force when the gears slip. Because of backlash, kinematics are pretty hard to fine-tune for sure (yes, it turned out to be the hardest).
That's all that I ordered, with the delivery it turned out about $ 100. Batteries and transmitters / receivers for control and radio control - left for later. Because I have a radio-controlled machine and it’s not interesting, but what really interested me was legs! The video of smoothly running hexapods on YouTube was bewitching , I watched it, reviewed it, and each time tears rolled down my cheeks, and I grunted “I want!” Grunted. I don’t want to order such a finished thing, but I want to do something myself!
While I was waiting for an order, I read how enlightened people revive their creations. Of course, inverse kinematics immediately surfaced ( translation) If we say simply and immediately about the articulated “limbs”, then direct kinematics is when the hinge angles are applied to the input, and at the output we have a model of the limb in space, and the coordinates of the extreme point of the limb. The reverse kinematics - obviously works the other way around - the coordinates of the extreme point of the limb arrive at the entrance, where we need to reach, and at the output we get the angles that we need to turn the hinges to do this. Servos just get the input angular position that they need to turn into (one signal wire encoded PWM / PWM ).
I started to write. I started with what I read: think over the implementation of IR according to the method described there. But the feeling quickly came that for my case it was overly complicated. Moreover, both cumbersome to implement and computationally very complicated - the calculation is iterative. And I have 6 legs, for each of which I need to count IR, and only 16 MHz is not the smartest AVR architecture. But only 3 degrees of freedom. And it’s easy to guess that one can reach an arbitrary point in the “reach area” only in one way. The decision is already ripe in the head.
But then came February and the premises - one from China, the other from the UK. The first thing, of course, I just played around with the Arduino board - I blinked the LED and dipped into the speaker connected there. Then he began to implement the actual IR, already in hardware. For what I built a prototype legs from improvised materials (rather soft plastic that is easy to cut with scissors, screws and nozzles - all of the servo sets). This leg of the terminator was fixed directly to the Arduino board. You can consider how the joints are made in a budget.
I admired this matter, and dreamed that if I, on the basis of this robot, solder a terminator in the future who will declare war on humanity, then John Connor and Schwarzenegger will return to me here in the past, and they will take this prototype and melt it in Orodruin. But no one returned, took nothing, and I calmly continued.
It turned out that IR does not need to be afraid at all, in my case it all came down to banal trigonometry geometry. To make it easier to access joints, I turned to Wikipedia and read about insects. They have special names for limb elements:
Russian also has its own and very interesting names for this, but “basin”, “trochanter”, “drumstick”, etc., being in the code, would not let me fall asleep. Therefore, I left the names of Coxa, Femur, Tibia to 3 limbs and corresponding serves. From the prototype legs above it can be seen that I do not even have a separate part for coxa. These are just two servos fastened with elastic bands. Femur - implemented a strip of plastic, to which levers of servos are attached on both sides. Thus, the last remaining servomotor is the beginning of tibia, to extend which another piece of plastic is bolted to it.
I started the editor, without hesitation, created the file Leg.h, and in it the class Leg. Well, a bunch of auxiliary turbidity.) Let there be a point A in space (ax, ay, az), which you need to reach. Then the top view looks like this:
In the figure, I immediately showed a way to calculate the first angle - this is the angle of rotation of the servo controlling Coxa, which rotates the entire limb in the horizontal plane. In the diagram, the variables used in the code (by no means all) are immediately indicated in red. Not very mathematical, but convenient. It can be seen that the angle of interest to us is elementary. First, primaryCoxaAngle - is just the angle (0; A) to the X axis (which is equivalent to the angle of point A in polar coordinates). But the diagram shows that at the same time the leg itself is not laid out at this angle. The reason is that the axis of rotation of coxa is not on the “leg line” - I do not know how to say it correctly. It is not in the plane in which the other 2 joints rotate and the tip of the leg is located, here. This can be easily compensated by counting additionalCoxaAngle (how to count it - I don’t even bother to stop,
In total, we have the first piece of code, these are the internals of the reach (Point & dest) method:
Here dest is the point where you want to go, _cStart is the coordinates of the attachment point (and the center of rotation) of coxa, in hDist we consider the distance from _cStart to dest in the horizontal plane. DONT_MOVE is just a flag meaning that coxa does not need to be rotated anywhere but left in its current position (because dest - somewhere right on the axis of rotation of coxa - rarely, but it happens). Here cAngle is already the angle by which the servo will need to deviate from its initial angle (which is in the middle of its operating range). It can be seen that _cStartAngle is also used - this is the angle in the space by which the servo is rotated by devolt during installation. I’ll talk about _thirdQuarterFix later if I don’t forget.
Then everything becomes even simpler. We just need to look at the “leg line” plane mentioned above:
At the same time, the task suddenly comes down to finding the intersection point of 2 circles. One is at the point where our femur "grows" from, the second is the point where we need to reach (with already local 2d coordinates). The radii of the circles are the lengths of femur and tibia, respectively. If the circles intersect, then in one of the 2 points you can position the joint. We always choose the upper one so that the “knees” of the monster are curved up and not down. If they do not intersect, then we will not reach the target point. A little more code, the transition to the plane is elementary, only a couple of pitfalls are still taken into account and documented in the comments, so that I won’t puzzle later, parsing the code. For simplicity, in this local coordinate “leg plane” I chose the point where the femur grows from the origin:
Now localDestX and localDestY are the coordinates of the target point. All that remains is to find the intersection point of the circles with the centers in (0,0) and (localDestX, localDestY), and the radii _fLength and _tLength (respectively, the length of femur and the length of tibia). The schoolchild will also cope with this, but here I made quite a few mistakes, therefore, to check myself and in general so that anyone could check what kind of dumb formulas were, I left links to sources where this elementary geometric problem was clearly and clearly developed:
That’s all, there’s just a little bit left - according to the received coordinates, calculate the actual angles for the femur and tibia servos:
Again elementary - angular coordinates and all. I hope the naming of variables should already be clear, for example, _fStartAngle is the femur start angle, the angle at which femur is defaulted. And the last line of the reach () method (he said let's go, and waved his hand):
The move method already gives commands directly to the servers. In fact, in it then I had to add all kinds of things to protect against bad angles (which the servo cannot turn, but will try), as well as for other legs, which are vividly located and / or directed in other directions. But for now, we are working with only one paw.
These pieces are already the final code, which is far from perfect, and for sure it can be significantly improved. But it works! Having never gone beyond the school course of geometry-trigonometry, we have implemented a fully functional inverse kinematics for 3dof legs! Yes, and we get the solution right away, in one iteration. For this to work, the leg had to be carefully measured and the class configured with the data. including angular, which are the most difficult to measure on the finished product. Maybe if you design in auto-cad and make beautiful renderings - it would be easier with measuring angles, but I had neither the time nor the desire to deal with this pathos.
February had just begun, and the footage video was ready. To check the IR, I forced my foot to describe all kinds of figures in space (for this it was necessary to call reach successively, bypassing the points on the rectangle, or circle, the code is boring and dull, so I don’t bring it (and after finishing the experiments with circling primitives, I sawed it out at all) ):
Then it was necessary to finish playing with this craft, you couldn’t jump on one leg (although such a robot would be really interesting). But I need a hexapod. I went to the nearest flea market to look for plexiglass. I found 2 excellent pieces - one 3 mm thick (just for the body, I thought), the other 2 mm and blue (excellent limbs, in tone with the servos). After a couple of weeks, I cut out an evening to make something out of this. He made sketches on paper. tried on - everything seems to be ok, then it’s up to the hacksaw.
And here it is, a monster overseas, six-legged. When I tested one leg, I fed this business with some kind of left feeder from the external screw. Enough. But eating 6 legs from him was already scary. Therefore, I hung my hands for a while, thinking that I still needed to get a suitable nursery. But it turned out to be much simpler, I already mentioned above - the feeder from eee pc 901 came up. Well and excellent.
Debugging the work of 6 legs turned out to be even more difficult than writing an engine of one leg. Half of the legs were mirrored relative to the other. In addition, all are directed in different directions. In general, I configured and configured everything for a very long time, and this did not really inspire me, because there was no convenient debugging tool, the maximum I could count on was the output of the log in Serial. And that one worked fine from the main * .ino file, and from the connected Leg.h - the desired object was no longer seen. He twisted crutches for the log (facepalm). Over time, refactor. And then spring came, the bike season was open at full strength, and I threw my six-legged pet into the closet. So it went all summer and the warm part of autumn.
But it began to rain, it became cold, and the hexapod was extracted. His legs were debugged, including the very _thirdQuarterFix was introduced for the polarAngle calculation function. The problem was that 2 legs (left middle and left hind) moved so that most of the time they were in the third quarter:
And I had a naive polarAngle - it returned the angles from -pi to pi, relative to the X axis. And, if sometimes one of these 2 legs needed to be turned into the 2nd quarter, then the polarAngle value jumped from -pi to pi, which actually negatively affected the further calculation. Fixed with a crutch - for these 2 legs polarAngle is considered "different." I am ashamed, ashamed of me for the code, but the whole project is a proof of concept, the sole purpose of which is just to understand whether I can assemble a realistic moving hexapod or not. Therefore, the code should work, and right now. And then refactoring - refactoring.
Having dealt with the 3rd quarter, he began to pedal step patterns. To do this, I introduced the default point into the Leg class, i.e. in which the foot is when the robot is at attention and level. This point can be tuned, the main thing is that all the legs are on the same z coordinate (so that the legs are actually physically on the same plane, Leg has an even lower level tuneRestAngles ()). And within the same Z coordinate, they can be moved almost arbitrarily. Almost - because the range of motion is not infinite, and so as not to go beyond this range with a step - default I tried to place the position of the legs somewhere closer to the center of this range.
I don’t bring the code here in the text, it is too elementary, and in the end I will give links to the full version of all sorts - at the same time I will learn to use github.
The step sequence chose a simple one - 3 feet on the ground, 3 - rearranged in the air. Thus, the coordinates of the legs relative to their default position - can be divided into 2 groups. For these two groups, I did a step in the loop (see the walk () function in Buggy.ino). And in the end, each leg calculated its own individual coordinate based on its default coordinate.
And he went! But so far only forward. He put rubber bands on his legs so that he would not slip on linoleum so much. And rushed to shoot it on video to show friends.
To a-pod, of course, far. But I haven’t finished yet.) I was tired of the evening - and added the ability to move in any direction (but without turning the case.)). Plus, for smoothing between movements, I added a function (smoothTo ()) that gently moves legs (lifting up, again in 2 groups, one of which is always at the bottom, the creature stands on it while the other rises and moves) to a new position. This is necessary so that the creature does not jerk sharply with its feet, changing the direction of movement (this feature is so lacking for many game characters of past years). And he briskly ran in any direction - sideways, diagonally:
Both grand sorts files can be viewed here . I give a link to a specific revision, at the time of writing this text. Since in the future, everything can become completely different.
The results that can be distinguished so far are:
- riveting the hexapod itself is a doable job;
- write him the kinematics himself from scratch - also quite possible for anyone (the developer);
- the budget can be minimal, the only thing you really need to spend money on is servos; and so, if there is a soldering iron, then you can do with any microcontroller; the more convenient, the more expensive, however;
- it’s better not to save on servers, but the cheapest ones work too;
- I have not experienced such a pleasure from programming since I was 9 when I first saw the zx spectrum on a computer mug and learned how to write the first programs for it; it’s so cool when your code doesn’t just work somewhere and shows something to someone, but runs right in front of you and scares the cat.
Ahead are more advanced algorithms for smooth movement of the body, well, wireless control and batteries, of course.
Omitting the stories about exactly how I came up with the idea of building a hexapod (these were tons of videos on YouTube), I will go straight to the process of selecting parts. It was January 2012. I knew right away what I want from my robot and what not. I wanted:
- each leg should have 3 degrees of freedom - 3dof (3 dimensions of freedom). Because the simpler option 2dof - does not give such an insect feel, and 4dof - is unnecessary, 3dof and so allows you to freely move the tip of the leg in 3D space;
- 6 legs; again, this is no longer 4 (then the robot clumsily jumps), but also not 8, like spiders, and already excessively;
- small;
- cheap;
- minimum boards and connections;
The post is big.
The first of course was to choose a motherboard for the crumbs. I managed to read a lot of good and bad by that time about Arduino. But it was he who looked at him as the main option. There was no time to solder the controllers ourselves, and taking more advanced boards with ARM cpu, for example, is expensive, and understanding how to program them, how to work with PWM outputs, etc. And arduina: the IDE started, the code hit, upload clicked - and hello, it already blinks to you. Beauty! ;)
First, I started looking at arduino mega and clones, because the number of PWM outputs that could be used to steer the servos was plentiful. Let me remind you that for 3dof hexapods you need 3 * 6 = 18 servos, and separate channels for controlling them. But then I found a real Jazz among the arduino mega, this is a Dagu board called Red Back Spider Controller. Here she is on ebay.
She offers all her outputs in the form of ready-made 3 pins (ground, power, signal), and power interconnection. The power supply of the controller itself is stabilized, but it goes to the dviglov connectors as it is (UPD: not as it is, but also stabilized 5 volts. And apparently it is isolated from the power supply of the controller, since 18 servos do not interfere with the controller’s operation). This allows you to simply apply 7-30 volts of sufficient power to the power terminal (the power supply from the eee pc 901 to 12V and 3A - it turned out to be enough to buzz with all 18 servos) and not fool your head with separate power supply of logic and dviglov. Also, this will make it possible in the future to easily plant all this monster on a pack of 7.4-volt Li-Po batteries. And with all this, from a software point of view, this is the usual Arduino mega, compatible with software and libs, and with iron (except for shields, installed directly on the original mega - they will not roll). True, the price is even higher than even the original mega, but all the other advantages outweighed this.
Further servos. On ebay, there are many different ones for micro servo. I took the most powerful of the smallest and cheapest, weighing 9 grams, plastic gearboxes. If you take lots where they are sent in batches - it turns out cheaper. I took 3 packs of 6, it seems, and less than $ 2 came out. Looking ahead, I’ll say that I regret that I did not spend more and did not take servos with metal gears and ball bearings. These plastic ones turned out to have quite noticeable backlash, and a characteristic crunch with excessive force when the gears slip. Because of backlash, kinematics are pretty hard to fine-tune for sure (yes, it turned out to be the hardest).
That's all that I ordered, with the delivery it turned out about $ 100. Batteries and transmitters / receivers for control and radio control - left for later. Because I have a radio-controlled machine and it’s not interesting, but what really interested me was legs! The video of smoothly running hexapods on YouTube was bewitching , I watched it, reviewed it, and each time tears rolled down my cheeks, and I grunted “I want!” Grunted. I don’t want to order such a finished thing, but I want to do something myself!
While I was waiting for an order, I read how enlightened people revive their creations. Of course, inverse kinematics immediately surfaced ( translation) If we say simply and immediately about the articulated “limbs”, then direct kinematics is when the hinge angles are applied to the input, and at the output we have a model of the limb in space, and the coordinates of the extreme point of the limb. The reverse kinematics - obviously works the other way around - the coordinates of the extreme point of the limb arrive at the entrance, where we need to reach, and at the output we get the angles that we need to turn the hinges to do this. Servos just get the input angular position that they need to turn into (one signal wire encoded PWM / PWM ).
I started to write. I started with what I read: think over the implementation of IR according to the method described there. But the feeling quickly came that for my case it was overly complicated. Moreover, both cumbersome to implement and computationally very complicated - the calculation is iterative. And I have 6 legs, for each of which I need to count IR, and only 16 MHz is not the smartest AVR architecture. But only 3 degrees of freedom. And it’s easy to guess that one can reach an arbitrary point in the “reach area” only in one way. The decision is already ripe in the head.
But then came February and the premises - one from China, the other from the UK. The first thing, of course, I just played around with the Arduino board - I blinked the LED and dipped into the speaker connected there. Then he began to implement the actual IR, already in hardware. For what I built a prototype legs from improvised materials (rather soft plastic that is easy to cut with scissors, screws and nozzles - all of the servo sets). This leg of the terminator was fixed directly to the Arduino board. You can consider how the joints are made in a budget.
I admired this matter, and dreamed that if I, on the basis of this robot, solder a terminator in the future who will declare war on humanity, then John Connor and Schwarzenegger will return to me here in the past, and they will take this prototype and melt it in Orodruin. But no one returned, took nothing, and I calmly continued.
It turned out that IR does not need to be afraid at all, in my case it all came down to banal trigonometry geometry. To make it easier to access joints, I turned to Wikipedia and read about insects. They have special names for limb elements:
Russian also has its own and very interesting names for this, but “basin”, “trochanter”, “drumstick”, etc., being in the code, would not let me fall asleep. Therefore, I left the names of Coxa, Femur, Tibia to 3 limbs and corresponding serves. From the prototype legs above it can be seen that I do not even have a separate part for coxa. These are just two servos fastened with elastic bands. Femur - implemented a strip of plastic, to which levers of servos are attached on both sides. Thus, the last remaining servomotor is the beginning of tibia, to extend which another piece of plastic is bolted to it.
I started the editor, without hesitation, created the file Leg.h, and in it the class Leg. Well, a bunch of auxiliary turbidity.) Let there be a point A in space (ax, ay, az), which you need to reach. Then the top view looks like this:
In the figure, I immediately showed a way to calculate the first angle - this is the angle of rotation of the servo controlling Coxa, which rotates the entire limb in the horizontal plane. In the diagram, the variables used in the code (by no means all) are immediately indicated in red. Not very mathematical, but convenient. It can be seen that the angle of interest to us is elementary. First, primaryCoxaAngle - is just the angle (0; A) to the X axis (which is equivalent to the angle of point A in polar coordinates). But the diagram shows that at the same time the leg itself is not laid out at this angle. The reason is that the axis of rotation of coxa is not on the “leg line” - I do not know how to say it correctly. It is not in the plane in which the other 2 joints rotate and the tip of the leg is located, here. This can be easily compensated by counting additionalCoxaAngle (how to count it - I don’t even bother to stop,
In total, we have the first piece of code, these are the internals of the reach (Point & dest) method:
float hDist = sqrt( sqr(dest.x - _cStart.x) + sqr(dest.y - _cStart.y) );
float additionalCoxaAngle = hDist == 0.0 ? DONT_MOVE
: asin( _cFemurOffset / hDist );
float primaryCoxaAngle = polarAngle(dest.x - _cStart.x, dest.y - _cStart.y, _thirdQuarterFix);
float cAngle = hDist == 0.0 ? DONT_MOVE
: primaryCoxaAngle - additionalCoxaAngle - _cStartAngle;
Here dest is the point where you want to go, _cStart is the coordinates of the attachment point (and the center of rotation) of coxa, in hDist we consider the distance from _cStart to dest in the horizontal plane. DONT_MOVE is just a flag meaning that coxa does not need to be rotated anywhere but left in its current position (because dest - somewhere right on the axis of rotation of coxa - rarely, but it happens). Here cAngle is already the angle by which the servo will need to deviate from its initial angle (which is in the middle of its operating range). It can be seen that _cStartAngle is also used - this is the angle in the space by which the servo is rotated by devolt during installation. I’ll talk about _thirdQuarterFix later if I don’t forget.
Then everything becomes even simpler. We just need to look at the “leg line” plane mentioned above:
At the same time, the task suddenly comes down to finding the intersection point of 2 circles. One is at the point where our femur "grows" from, the second is the point where we need to reach (with already local 2d coordinates). The radii of the circles are the lengths of femur and tibia, respectively. If the circles intersect, then in one of the 2 points you can position the joint. We always choose the upper one so that the “knees” of the monster are curved up and not down. If they do not intersect, then we will not reach the target point. A little more code, the transition to the plane is elementary, only a couple of pitfalls are still taken into account and documented in the comments, so that I won’t puzzle later, parsing the code. For simplicity, in this local coordinate “leg plane” I chose the point where the femur grows from the origin:
// Moving to local Coxa-Femur-target coordinate system
// Note the case when hDist <= _cFemurOffset. This is for the blind zone.
// We never can't reach the point that is nearer to the _cStart then
// femur offset (_fStartFarOffset)
float localDestX = hDist <= _cFemurOffset
? - _fStartFarOffset
: sqrt(sqr(hDist) - sqr(_cFemurOffset)) - _fStartFarOffset;
float localDestY = dest.z - _fStartZOffset;
// Check reachability
float localDistSqr = sqr(localDestX) + sqr(localDestY);
if (localDistSqr > sqr(_fLength + _tLenght))
{
log("Can't reach!");
return false;
}
Now localDestX and localDestY are the coordinates of the target point. All that remains is to find the intersection point of the circles with the centers in (0,0) and (localDestX, localDestY), and the radii _fLength and _tLength (respectively, the length of femur and the length of tibia). The schoolchild will also cope with this, but here I made quite a few mistakes, therefore, to check myself and in general so that anyone could check what kind of dumb formulas were, I left links to sources where this elementary geometric problem was clearly and clearly developed:
// Find joint as circle intersect ( equations from http://e-maxx.ru/algo/circles_intersection & http://e-maxx.ru/algo/circle_line_intersection )
float A = -2 * localDestX;
float B = -2 * localDestY;
float C = sqr(localDestX) + sqr(localDestY) + sqr(_fLength) - sqr(_tLenght);
float X0 = -A * C / (sqr(A) + sqr(B));
float Y0 = -B * C / (sqr(A) + sqr(B));
float D = sqrt( sqr(_fLength) - (sqr(C) / (sqr(A) + sqr(B))) );
float mult = sqrt ( sqr(D) / (sqr(A) + sqr(B)));
float ax, ay, bx, by;
ax = X0 + B * mult;
bx = X0 - B * mult;
ay = Y0 - A * mult;
by = Y0 + A * mult;
// Select solution on top as joint
float jointLocalX = (ax > bx) ? ax : bx;
float jointLocalY = (ax > bx) ? ay : by;
That’s all, there’s just a little bit left - according to the received coordinates, calculate the actual angles for the femur and tibia servos:
float primaryFemurAngle = polarAngle(jointLocalX, jointLocalY, false);
float fAngle = primaryFemurAngle - _fStartAngle;
float primaryTibiaAngle = polarAngle(localDestX - jointLocalX, localDestY - jointLocalY, false);
float tAngle = (primaryTibiaAngle - fAngle) - _tStartAngle;
Again elementary - angular coordinates and all. I hope the naming of variables should already be clear, for example, _fStartAngle is the femur start angle, the angle at which femur is defaulted. And the last line of the reach () method (he said let's go, and waved his hand):
move(cAngle, fAngle, tAngle);
The move method already gives commands directly to the servers. In fact, in it then I had to add all kinds of things to protect against bad angles (which the servo cannot turn, but will try), as well as for other legs, which are vividly located and / or directed in other directions. But for now, we are working with only one paw.
These pieces are already the final code, which is far from perfect, and for sure it can be significantly improved. But it works! Having never gone beyond the school course of geometry-trigonometry, we have implemented a fully functional inverse kinematics for 3dof legs! Yes, and we get the solution right away, in one iteration. For this to work, the leg had to be carefully measured and the class configured with the data. including angular, which are the most difficult to measure on the finished product. Maybe if you design in auto-cad and make beautiful renderings - it would be easier with measuring angles, but I had neither the time nor the desire to deal with this pathos.
February had just begun, and the footage video was ready. To check the IR, I forced my foot to describe all kinds of figures in space (for this it was necessary to call reach successively, bypassing the points on the rectangle, or circle, the code is boring and dull, so I don’t bring it (and after finishing the experiments with circling primitives, I sawed it out at all) ):
Then it was necessary to finish playing with this craft, you couldn’t jump on one leg (although such a robot would be really interesting). But I need a hexapod. I went to the nearest flea market to look for plexiglass. I found 2 excellent pieces - one 3 mm thick (just for the body, I thought), the other 2 mm and blue (excellent limbs, in tone with the servos). After a couple of weeks, I cut out an evening to make something out of this. He made sketches on paper. tried on - everything seems to be ok, then it’s up to the hacksaw.
And here it is, a monster overseas, six-legged. When I tested one leg, I fed this business with some kind of left feeder from the external screw. Enough. But eating 6 legs from him was already scary. Therefore, I hung my hands for a while, thinking that I still needed to get a suitable nursery. But it turned out to be much simpler, I already mentioned above - the feeder from eee pc 901 came up. Well and excellent.
Debugging the work of 6 legs turned out to be even more difficult than writing an engine of one leg. Half of the legs were mirrored relative to the other. In addition, all are directed in different directions. In general, I configured and configured everything for a very long time, and this did not really inspire me, because there was no convenient debugging tool, the maximum I could count on was the output of the log in Serial. And that one worked fine from the main * .ino file, and from the connected Leg.h - the desired object was no longer seen. He twisted crutches for the log (facepalm). Over time, refactor. And then spring came, the bike season was open at full strength, and I threw my six-legged pet into the closet. So it went all summer and the warm part of autumn.
But it began to rain, it became cold, and the hexapod was extracted. His legs were debugged, including the very _thirdQuarterFix was introduced for the polarAngle calculation function. The problem was that 2 legs (left middle and left hind) moved so that most of the time they were in the third quarter:
And I had a naive polarAngle - it returned the angles from -pi to pi, relative to the X axis. And, if sometimes one of these 2 legs needed to be turned into the 2nd quarter, then the polarAngle value jumped from -pi to pi, which actually negatively affected the further calculation. Fixed with a crutch - for these 2 legs polarAngle is considered "different." I am ashamed, ashamed of me for the code, but the whole project is a proof of concept, the sole purpose of which is just to understand whether I can assemble a realistic moving hexapod or not. Therefore, the code should work, and right now. And then refactoring - refactoring.
Having dealt with the 3rd quarter, he began to pedal step patterns. To do this, I introduced the default point into the Leg class, i.e. in which the foot is when the robot is at attention and level. This point can be tuned, the main thing is that all the legs are on the same z coordinate (so that the legs are actually physically on the same plane, Leg has an even lower level tuneRestAngles ()). And within the same Z coordinate, they can be moved almost arbitrarily. Almost - because the range of motion is not infinite, and so as not to go beyond this range with a step - default I tried to place the position of the legs somewhere closer to the center of this range.
I don’t bring the code here in the text, it is too elementary, and in the end I will give links to the full version of all sorts - at the same time I will learn to use github.
The step sequence chose a simple one - 3 feet on the ground, 3 - rearranged in the air. Thus, the coordinates of the legs relative to their default position - can be divided into 2 groups. For these two groups, I did a step in the loop (see the walk () function in Buggy.ino). And in the end, each leg calculated its own individual coordinate based on its default coordinate.
And he went! But so far only forward. He put rubber bands on his legs so that he would not slip on linoleum so much. And rushed to shoot it on video to show friends.
To a-pod, of course, far. But I haven’t finished yet.) I was tired of the evening - and added the ability to move in any direction (but without turning the case.)). Plus, for smoothing between movements, I added a function (smoothTo ()) that gently moves legs (lifting up, again in 2 groups, one of which is always at the bottom, the creature stands on it while the other rises and moves) to a new position. This is necessary so that the creature does not jerk sharply with its feet, changing the direction of movement (this feature is so lacking for many game characters of past years). And he briskly ran in any direction - sideways, diagonally:
Both grand sorts files can be viewed here . I give a link to a specific revision, at the time of writing this text. Since in the future, everything can become completely different.
The results that can be distinguished so far are:
- riveting the hexapod itself is a doable job;
- write him the kinematics himself from scratch - also quite possible for anyone (the developer);
- the budget can be minimal, the only thing you really need to spend money on is servos; and so, if there is a soldering iron, then you can do with any microcontroller; the more convenient, the more expensive, however;
- it’s better not to save on servers, but the cheapest ones work too;
- I have not experienced such a pleasure from programming since I was 9 when I first saw the zx spectrum on a computer mug and learned how to write the first programs for it; it’s so cool when your code doesn’t just work somewhere and shows something to someone, but runs right in front of you and scares the cat.
Ahead are more advanced algorithms for smooth movement of the body, well, wireless control and batteries, of course.