The 3DOF leg.
Notice that I haven’t mentioned some of the scaling
(the 127s) used by the program to work with Atom Basic
functions, nor the range limiting equations that use the
min and max functions to insure our calculated angles
are within the range of motion of the respective servos.
These limits and the lengths of the femur and tibia are
all specified as constants in the start of the program.
Change them if you change the leg component dimensions.
We’ll do these calcs for both legs. Imagine all the calcs
that need to be done for a hexapod or an octapod!
Move Those Legs!
Now that we’ve got our three servo angles per leg,
we’ll scale them into pulse ranges that our servos can
understand. We’ll also use the min and max functions
again to insure that the calculated values won’t send a
servo into the stops. (You wouldn’t like that!) Some
min/max constant examples:
HipH_PulseMax - HipH_PulseMin
HipV_AngleMax - HipV_AngleMin
You’ll also see three more variables: HipH_Pulse,
HipV_Pulse, and Knee_Pulse. You guessed it! These are
the pulse values we’ll send to the servos later. For the
calculation of each of these three variables, we’ll use
Another angle of the
46 SERVO 10.2009
constants for the allowed range of the servos, such as
HipV_PulseMin and HipV_PulseMax, for example.
[ “#”,FRHH,FRHH2,”P”,DEC HipH_Pulse(2)
With these values in hand, we’ll send them to the
SSC- 32 with the special Atom Basic command serout.
We send out a string of characters and values that the
SSC- 32 interprets for us and continuously positions our
servos. Believe me, the SSC- 32 saves a LOT of work
(calculations) getting our servos to their positions! We send
the servo (character constants defined as RRHV,RRHV2
and others), a ‘P’ for a position command, and a decimal
value which is the position of the servo we just calculated.
We do this for all six of our servos. Whew!
Before we can do those IK calculations and move the
legs, we need to back up a bit and figure out which leg
to lift and when. In the Tripod subroutines, we basically
alternate between the two legs — lifting one of them,
moving the lifted leg forward, and the “grounded” leg back
to take a step. The “Tripod” and “Steps” variables are used
to walk us through dividing up the gait into little steps to
make the moves smooth, and alternating which leg is up.
YPos(Index) = -Tibia_Length + Height
XPos(Index) = XPos(Index) + (XPos2(Index)
- (XPos(Index) - (HipV_HipH +
ZPos(Index) = ZPos(Index) + (ZPos2(Index)
We’ve now updated the X, Y, and Z positions for the
feet by a little bit each time through the loop.
This entire gait action is controlled by a big loop; it
proceeds when we have a reason to move the legs
(MovesDelay) and have finished the last move of the legs.
We can’t very well issue new move commands to the legs
while the SSC- 32 is still moving them from the previous
commands, now can we?
Did I Miss Anything?
There are some little subroutines that set up the trig
functions that I mentioned. I won’t go over them here.
We have some initialization routines for the leg’s starting
positions (H3Init and InitPos), but that’s about it. There
are also three LEDs that can optionally be wired in to allow
monitoring of the PS2 or COM activity (yellow), and the
two legs (red and green).
A program very similar to the Atom Basic program can
be written and compiled by MBasic Pro from Basic Micro.