File content as of revision 0:8a7c1e92d067:

/**********************************************************************
 This function puts E.R.I.C. into a standard position
 regardless of E.R.I.C.'s initial positioning
 Notebook: http://mbed.org/users/ms523/notebook/eric-initialisation-/
***********************************************************************/
#include "FK.h"

void Set_Pose(const int *pose, float duration) {
    
    float StartPos[12];         // Array to hold current servo pos
    FK_Engine(StartPos);        // Get the initial foot positions...
    
    // Apply global offsets to allow for hip width
    StartPos[0] = StartPos[0] + X_OFFSET;       // Fore right leg
    StartPos[3] = StartPos[3] - X_OFFSET;       // Fore left leg
    StartPos[6] = StartPos[6] + X_OFFSET;       // Hind right leg
    StartPos[9] = StartPos[9] - X_OFFSET;       // Hind left leg
    
    // Create arrays to hold each servo's distance, velocity and set point
    int distance[12];
    float velocity[12];
    int set_point[12];

    // Calculate the distance to travel in each axis...
    // Equation: Distance = End Position - Start Position
    for(int i=0;i<12;i++){
        distance[i] = pose[i] - StartPos[i];        // Calculate the move distance
        velocity[i] = distance[i] / duration;       // Calculate the move velocity
    }

    // Create a timer to control the movement
    Timer Movement_timer;

    // Create structs to hold joint positions
    Joint_Angles Hind_left, Hind_right;                 // Variables for joint angles
    Joint_Angles Fore_left, Fore_right;                 // Variables for joint angles

    Movement_timer.start();                             // Start the movement timer
    float time = 0;                                     // Create a variable to read the time

    while (time < duration) {
        // Get the current time
        time = Movement_timer.read();
        if (time > duration)
            time = duration;

        // Calculate current set points
        for(int i=0;i<12;i++){
            set_point[i] = (int)(time * velocity[i]) + StartPos[i];
        }
   
        // Calculate the required joint angles...
        Fore_right = IK_Engine(set_point[0],set_point[1],set_point[2],FORE);
        Fore_left = IK_Engine(set_point[3],set_point[4],set_point[5],FORE);
        Hind_right = IK_Engine(set_point[6],set_point[7],set_point[8],HIND);
        Hind_left = IK_Engine(set_point[9],set_point[10],set_point[11],HIND);

        // Translate the joint angles into Servo Angles       
        Fore_right.lateral = Fore_right.lateral + 135;
        Fore_right.hip = 355 - Fore_right.hip;
        Fore_right.knee = 305 - Fore_right.knee;
        Fore_left.lateral = 165 - Fore_left.lateral;
        Fore_left.hip = Fore_left.hip - 55;
        Fore_left.knee = Fore_left.knee - 5;  
        Hind_right.lateral = 165 - Hind_right.lateral;
        Hind_right.hip = 340 - Hind_right.hip;
        Hind_right.knee = 320 - Hind_right.knee;
        Hind_left.lateral = Hind_left.lateral + 135;
        Hind_left.hip = Hind_left.hip - 40;
        Hind_left.knee = Hind_left.knee - 20;

        // And finally move the leg to the current set point
        Fore_Right_Hip_Lateral.SetGoal((int)Fore_right.lateral);
        Fore_Right_Hip.SetGoal((int)Fore_right.hip);
        Fore_Right_Knee.SetGoal((int)Fore_right.knee);
        Fore_Left_Hip_Lateral.SetGoal((int)Fore_left.lateral);
        Fore_Left_Hip.SetGoal((int)Fore_left.hip);
        Fore_Left_Knee.SetGoal((int)Fore_left.knee);
        Hind_Right_Hip_Lateral.SetGoal((int)Hind_right.lateral);       
        Hind_Right_Hip.SetGoal((int)Hind_right.hip);
        Hind_Right_Knee.SetGoal((int)Hind_right.knee);
        Hind_Left_Hip_Lateral.SetGoal((int)Hind_left.lateral);
        Hind_Left_Hip.SetGoal((int)Hind_left.hip);
        Hind_Left_Knee.SetGoal((int)Hind_left.knee);
    }
}