Martin Smith / Mbed 2 deprecated ERIC_First_Steps

Set_Pose.cpp@0:0965dacb3caf, 2011-08-21 (annotated)

Committer:
ms523
Date:
Sun Aug 21 08:25:00 2011 +0000
Revision:
0:0965dacb3caf

        

        
Who changed what in which revision?

        

            
                
UserRevisionLine numberNew contents of line

            
            


ms523

0:0965dacb3caf


     1

/**********************************************************************





ms523

0:0965dacb3caf


     2

 This function puts E.R.I.C. into a standard position





ms523

0:0965dacb3caf


     3

 regardless of E.R.I.C.'s initial positioning





ms523

0:0965dacb3caf


     4

 Notebook: http://mbed.org/users/ms523/notebook/eric-initialisation-/





ms523

0:0965dacb3caf


     5

***********************************************************************/





ms523

0:0965dacb3caf


     6

#include "ERIC.h"





ms523

0:0965dacb3caf


     7







ms523

0:0965dacb3caf


     8

#define TIME 0.5000       // Time to complete the movement





ms523

0:0965dacb3caf


     9







ms523

0:0965dacb3caf


    10

void Set_Pose(const int *pose) {





ms523

0:0965dacb3caf


    11







ms523

0:0965dacb3caf


    12

    Positions StartPos = FK_Engine();     // Get the initial foot positions...





ms523

0:0965dacb3caf


    13

    // Apply global offsets to allow for hip width





ms523

0:0965dacb3caf


    14

    StartPos.X_right = StartPos.X_right + X_OFFSET;





ms523

0:0965dacb3caf


    15

    StartPos.X_left = StartPos.X_left - X_OFFSET;





ms523

0:0965dacb3caf


    16







ms523

0:0965dacb3caf


    17

    // Create variables to hold the set points





ms523

0:0965dacb3caf


    18

    int X_right_distance, Y_right_distance, Z_right_distance;





ms523

0:0965dacb3caf


    19

    int X_left_distance, Y_left_distance, Z_left_distance;





ms523

0:0965dacb3caf


    20







ms523

0:0965dacb3caf


    21

    // Calculate the distance to travel in each axis...





ms523

0:0965dacb3caf


    22

    // Equation: Distance = End Position - Start Position





ms523

0:0965dacb3caf


    23

    X_right_distance = pose[0] - StartPos.X_right;





ms523

0:0965dacb3caf


    24

    Y_right_distance = pose[1] - StartPos.Y_right;





ms523

0:0965dacb3caf


    25

    Z_right_distance = pose[2] - StartPos.Z_right;





ms523

0:0965dacb3caf


    26

    X_left_distance = pose[3] - StartPos.X_left;





ms523

0:0965dacb3caf


    27

    Y_left_distance = pose[4] - StartPos.Y_left;





ms523

0:0965dacb3caf


    28

    Z_left_distance = pose[5] - StartPos.Z_left;





ms523

0:0965dacb3caf


    29







ms523

0:0965dacb3caf


    30

    // Calculate Velocity for each axis...





ms523

0:0965dacb3caf


    31

    // Equation: Velcoity = Distance / Time





ms523

0:0965dacb3caf


    32

    float X_right_velocity = X_right_distance / TIME;





ms523

0:0965dacb3caf


    33

    float Y_right_velocity = Y_right_distance / TIME;





ms523

0:0965dacb3caf


    34

    float Z_right_velocity = Z_right_distance / TIME;





ms523

0:0965dacb3caf


    35

    float X_left_velocity = X_left_distance / TIME;





ms523

0:0965dacb3caf


    36

    float Y_left_velocity = Y_left_distance / TIME;





ms523

0:0965dacb3caf


    37

    float Z_left_velocity = Z_left_distance / TIME;





ms523

0:0965dacb3caf


    38







ms523

0:0965dacb3caf


    39

    // Create a timer to control the movement





ms523

0:0965dacb3caf


    40

    Timer Movement_timer;





ms523

0:0965dacb3caf


    41







ms523

0:0965dacb3caf


    42

    /***********************************************************





ms523

0:0965dacb3caf


    43

    Sart moving the legs to the 'Stand' pose





ms523

0:0965dacb3caf


    44

    ***********************************************************/





ms523

0:0965dacb3caf


    45

    // Create variables to hold the joint angles & current set points





ms523

0:0965dacb3caf


    46

    Joint_Angles Hind_left, Hind_right;                 // Variables for joint angles





ms523

0:0965dacb3caf


    47

    float Hind_right_X, Hind_right_Y, Hind_right_Z;     // Variables for the right leg





ms523

0:0965dacb3caf


    48

    float Hind_left_X, Hind_left_Y, Hind_left_Z;        // Variables for the left leg





ms523

0:0965dacb3caf


    49







ms523

0:0965dacb3caf


    50

    Movement_timer.start();                             // Start the movement timer





ms523

0:0965dacb3caf


    51

    float time = 0;                                     // Create a variable to read the time





ms523

0:0965dacb3caf


    52







ms523

0:0965dacb3caf


    53

    while (time < TIME) {





ms523

0:0965dacb3caf


    54

        // Get the current time





ms523

0:0965dacb3caf


    55

        time = Movement_timer.read();





ms523

0:0965dacb3caf


    56

        if (time > TIME)





ms523

0:0965dacb3caf


    57

            time = TIME;





ms523

0:0965dacb3caf


    58







ms523

0:0965dacb3caf


    59

        // Calculate current set points





ms523

0:0965dacb3caf


    60

        Hind_right_X = (time * X_right_velocity) + StartPos.X_right;





ms523

0:0965dacb3caf


    61

        Hind_right_Y = (time * Y_right_velocity) + StartPos.Y_right;





ms523

0:0965dacb3caf


    62

        Hind_right_Z = (time * Z_right_velocity) + StartPos.Z_right;





ms523

0:0965dacb3caf


    63

        Hind_left_X = (time * X_left_velocity) + StartPos.X_left;





ms523

0:0965dacb3caf


    64

        Hind_left_Y = (time * Y_left_velocity) + StartPos.Y_left;





ms523

0:0965dacb3caf


    65

        Hind_left_Z = (time * Z_left_velocity) + StartPos.Z_left;





ms523

0:0965dacb3caf


    66







ms523

0:0965dacb3caf


    67

        // Calculate the required joint angles...





ms523

0:0965dacb3caf


    68

        Hind_right = IK_Engine(Hind_right_X,Hind_right_Y,Hind_right_Z);





ms523

0:0965dacb3caf


    69

        Hind_left = IK_Engine(Hind_left_X,Hind_left_Y,Hind_left_Z);





ms523

0:0965dacb3caf


    70







ms523

0:0965dacb3caf


    71

        // Translate the joint angles into Servo Angles





ms523

0:0965dacb3caf


    72

        Hind_right.lateral = 165 - Hind_right.lateral;





ms523

0:0965dacb3caf


    73

        Hind_right.hip = 340 - Hind_right.hip;





ms523

0:0965dacb3caf


    74

        Hind_right.knee = 320 - Hind_right.knee;





ms523

0:0965dacb3caf


    75

        Hind_left.lateral = Hind_left.lateral + 135;





ms523

0:0965dacb3caf


    76

        Hind_left.hip = Hind_left.hip - 40;





ms523

0:0965dacb3caf


    77

        Hind_left.knee = Hind_left.knee - 20;





ms523

0:0965dacb3caf


    78







ms523

0:0965dacb3caf


    79

        // And finally move the leg to the current set point





ms523

0:0965dacb3caf


    80

        Right_Hip_Lateral.SetGoal((int)Hind_right.lateral);





ms523

0:0965dacb3caf


    81

        Right_Hip.SetGoal((int)Hind_right.hip);





ms523

0:0965dacb3caf


    82

        Right_Knee.SetGoal((int)Hind_right.knee);





ms523

0:0965dacb3caf


    83

        Left_Hip_Lateral.SetGoal((int)Hind_left.lateral);





ms523

0:0965dacb3caf


    84

        Left_Hip.SetGoal((int)Hind_left.hip);





ms523

0:0965dacb3caf


    85

        Left_Knee.SetGoal((int)Hind_left.knee);





ms523

0:0965dacb3caf


    86

    }





ms523

0:0965dacb3caf


    87

}