So now that I have give E.R.I.C. the ability to find out where its feet are (see forward kinematics and how the algorithm is implemented) I need to get the thing moving its legs!

The first thing I want to address is the initial powering-up of E.R.I.C. Ideally I when powered up I want it to ascertain where each of its feet are and then move them to a set position (or pose) in a controlled manner.

It would be good to wrap this up into a nice little function and have it run of a timer - so that all the feet arrive at the end position at the same time with all 3-axis synchronized.

To do this I will look at Newton's laws of linear motion and translate them into a mathematical algorithm to describe the position of the feet at any moment in time.

The particular equation I'm interested in is s = ½(u + V)t

This means that at any moment in time (t) the position will be ½ the initial velocity plus the current velocity multiplied by the time since the motion started. If we assume that the initial and current velocity are identical then we can simply find the current position by multiplying the velocity by how long its been moving and add it to the initial position.

So the order of events to do this will be as follows:

1. Work out where the feet are now
2. Calculate how far they need to move in each direction to get to the desired end position
3. State how long I want it to take to get there
4. Divide the distance by the time to get the velocity
5. At set periods (t) update the new desired position (the set point) by multiplying the time and velocity and adding them to the initial position
6. Keep doing this until the time is up and hence the feet reach it final position

So I can use my program developed in my E.R.I.C. Implementation page as a starting point and add this 'pose' as a new function.

Function Concept

What would be really useful is that, instead of having a function for each pose, have a single function that can be sent a pose to execute.

The way I can do this is to store the positions for each servo in the 'pose' in memory and then point the function at the correct memory space to execute the pose. I can do this by setting up an array using the 'const' keyword. This means that the values held in the specified memory location are constant and cannot change. I then send the function a pointer to the start of the array.

Const / Pointer Test

I write a short program to test how this would work (i'm really not a software engineer so this is a bit new to me!)

The code is as follows...

#include "mbed.h"

Serial pc (USBTX,USBRX);

const int Pose1[] = {1,2,3,4,5,6};
const int Pose2[] = {10,20,30,40,50,60};
const int Pose3[] = {100,200,300,400,500,600};

void Poses(const int *servo_angles) {       // Pass a pointer to a memory location
    pc.printf("\n\n\rServo angles are...\n\r");
    for (int i=0; i<6; i++) {
        pc.printf("%d,",servo_angles[i]);    // Print the content of the memory space
    }
}

int main() {
    pc.printf("*** Pose Test ***");
    Poses(Pose1);           // Send it pose 1
    Poses(Pose2);           // Send it pose 2
    Poses(Pose3);           // Send it pose 3
}

When compiled and run the output is...

This shows that the method of sending poses to a function would work. Now to write the final function.

Function Development

So to develop the function I'll follow the order of events stated earlier. But before I do that I'll need to set up the arrays of data for the poses. I'll set these up with a global scope in the 'Globals' file.

/****************************************************
Poses - Target servo position data
Format: Right_X, Right_Y, Right_Z, Left_X, Left_Y, Left_Z
****************************************************/
const int Stand[] = {70, 0, -200, -70, 0, -200};   
const int Sit[] = {70, 0, -150, -70, 0, -150}; 
const int Walk[] = {100, 120, -150, -100, -120, -150};

These arrays also need to be declared in the header file for the program.

The program using this function is published here...

The function that has been developed here is the 'Set_Pose' function. A 'TIME' value is used to control how long it takes for the legs to reach the final position. This can be changed and in future could maybe by a value sent to the function along with the pose to be executed?

The program was tested and it worked, moving the legs smoothly between the different poses. I did notice that the code runs a lot quicker than the servos. This means that a wait() function must be used after the pose to allow the servos to reach the final position.

Further Work

While this method is perfect for doing basic moves - for example getting E.R.I.C. to sit down when it hears the appropriate command, it is far too crude for getting E.R.I.C. to walk. For this I need some kind of profile engine or additional function to move him forward 1 step or turn 90°. This will be what I work on next...