Martin Smith / Mbed 2 deprecated ERIC_Walking_4_Legs

E.R.I.C.'s first attempt to walk

Dependencies:   mbed AX12_Hardware

Set_Pose.cpp@0:8a7c1e92d067, 2011-10-16 (annotated)

Committer:
ms523
Date:
Sun Oct 16 16:01:08 2011 +0000
Revision:
0:8a7c1e92d067

        

        
Who changed what in which revision?

        

            
                
UserRevisionLine numberNew contents of line

            
            


ms523

0:8a7c1e92d067


     1

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





ms523

0:8a7c1e92d067


     2

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





ms523

0:8a7c1e92d067


     3

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





ms523

0:8a7c1e92d067


     4

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





ms523

0:8a7c1e92d067


     5

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





ms523

0:8a7c1e92d067


     6

#include "FK.h"





ms523

0:8a7c1e92d067


     7







ms523

0:8a7c1e92d067


     8

void Set_Pose(const int *pose, float duration) {





ms523

0:8a7c1e92d067


     9

    





ms523

0:8a7c1e92d067


    10

    float StartPos[12];         // Array to hold current servo pos





ms523

0:8a7c1e92d067


    11

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





ms523

0:8a7c1e92d067


    12

    





ms523

0:8a7c1e92d067


    13

    // Apply global offsets to allow for hip width





ms523

0:8a7c1e92d067


    14

    StartPos[0] = StartPos[0] + X_OFFSET;       // Fore right leg





ms523

0:8a7c1e92d067


    15

    StartPos[3] = StartPos[3] - X_OFFSET;       // Fore left leg





ms523

0:8a7c1e92d067


    16

    StartPos[6] = StartPos[6] + X_OFFSET;       // Hind right leg





ms523

0:8a7c1e92d067


    17

    StartPos[9] = StartPos[9] - X_OFFSET;       // Hind left leg





ms523

0:8a7c1e92d067


    18

    





ms523

0:8a7c1e92d067


    19

    // Create arrays to hold each servo's distance, velocity and set point





ms523

0:8a7c1e92d067


    20

    int distance[12];





ms523

0:8a7c1e92d067


    21

    float velocity[12];





ms523

0:8a7c1e92d067


    22

    int set_point[12];





ms523

0:8a7c1e92d067


    23







ms523

0:8a7c1e92d067


    24

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





ms523

0:8a7c1e92d067


    25

    // Equation: Distance = End Position - Start Position





ms523

0:8a7c1e92d067


    26

    for(int i=0;i<12;i++){





ms523

0:8a7c1e92d067


    27

        distance[i] = pose[i] - StartPos[i];        // Calculate the move distance





ms523

0:8a7c1e92d067


    28

        velocity[i] = distance[i] / duration;       // Calculate the move velocity





ms523

0:8a7c1e92d067


    29

    }





ms523

0:8a7c1e92d067


    30







ms523

0:8a7c1e92d067


    31

    // Create a timer to control the movement





ms523

0:8a7c1e92d067


    32

    Timer Movement_timer;





ms523

0:8a7c1e92d067


    33







ms523

0:8a7c1e92d067


    34

    // Create structs to hold joint positions





ms523

0:8a7c1e92d067


    35

    Joint_Angles Hind_left, Hind_right;                 // Variables for joint angles





ms523

0:8a7c1e92d067


    36

    Joint_Angles Fore_left, Fore_right;                 // Variables for joint angles





ms523

0:8a7c1e92d067


    37







ms523

0:8a7c1e92d067


    38

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





ms523

0:8a7c1e92d067


    39

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





ms523

0:8a7c1e92d067


    40







ms523

0:8a7c1e92d067


    41

    while (time < duration) {





ms523

0:8a7c1e92d067


    42

        // Get the current time





ms523

0:8a7c1e92d067


    43

        time = Movement_timer.read();





ms523

0:8a7c1e92d067


    44

        if (time > duration)





ms523

0:8a7c1e92d067


    45

            time = duration;





ms523

0:8a7c1e92d067


    46







ms523

0:8a7c1e92d067


    47

        // Calculate current set points





ms523

0:8a7c1e92d067


    48

        for(int i=0;i<12;i++){





ms523

0:8a7c1e92d067


    49

            set_point[i] = (int)(time * velocity[i]) + StartPos[i];





ms523

0:8a7c1e92d067


    50

        }





ms523

0:8a7c1e92d067


    51

   





ms523

0:8a7c1e92d067


    52

        // Calculate the required joint angles...





ms523

0:8a7c1e92d067


    53

        Fore_right = IK_Engine(set_point[0],set_point[1],set_point[2],FORE);





ms523

0:8a7c1e92d067


    54

        Fore_left = IK_Engine(set_point[3],set_point[4],set_point[5],FORE);





ms523

0:8a7c1e92d067


    55

        Hind_right = IK_Engine(set_point[6],set_point[7],set_point[8],HIND);





ms523

0:8a7c1e92d067


    56

        Hind_left = IK_Engine(set_point[9],set_point[10],set_point[11],HIND);





ms523

0:8a7c1e92d067


    57







ms523

0:8a7c1e92d067


    58

        // Translate the joint angles into Servo Angles       





ms523

0:8a7c1e92d067


    59

        Fore_right.lateral = Fore_right.lateral + 135;





ms523

0:8a7c1e92d067


    60

        Fore_right.hip = 355 - Fore_right.hip;





ms523

0:8a7c1e92d067


    61

        Fore_right.knee = 305 - Fore_right.knee;





ms523

0:8a7c1e92d067


    62

        Fore_left.lateral = 165 - Fore_left.lateral;





ms523

0:8a7c1e92d067


    63

        Fore_left.hip = Fore_left.hip - 55;





ms523

0:8a7c1e92d067


    64

        Fore_left.knee = Fore_left.knee - 5;  





ms523

0:8a7c1e92d067


    65

        Hind_right.lateral = 165 - Hind_right.lateral;





ms523

0:8a7c1e92d067


    66

        Hind_right.hip = 340 - Hind_right.hip;





ms523

0:8a7c1e92d067


    67

        Hind_right.knee = 320 - Hind_right.knee;





ms523

0:8a7c1e92d067


    68

        Hind_left.lateral = Hind_left.lateral + 135;





ms523

0:8a7c1e92d067


    69

        Hind_left.hip = Hind_left.hip - 40;





ms523

0:8a7c1e92d067


    70

        Hind_left.knee = Hind_left.knee - 20;





ms523

0:8a7c1e92d067


    71







ms523

0:8a7c1e92d067


    72

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





ms523

0:8a7c1e92d067


    73

        Fore_Right_Hip_Lateral.SetGoal((int)Fore_right.lateral);





ms523

0:8a7c1e92d067


    74

        Fore_Right_Hip.SetGoal((int)Fore_right.hip);





ms523

0:8a7c1e92d067


    75

        Fore_Right_Knee.SetGoal((int)Fore_right.knee);





ms523

0:8a7c1e92d067


    76

        Fore_Left_Hip_Lateral.SetGoal((int)Fore_left.lateral);





ms523

0:8a7c1e92d067


    77

        Fore_Left_Hip.SetGoal((int)Fore_left.hip);





ms523

0:8a7c1e92d067


    78

        Fore_Left_Knee.SetGoal((int)Fore_left.knee);





ms523

0:8a7c1e92d067


    79

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





ms523

0:8a7c1e92d067


    80

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





ms523

0:8a7c1e92d067


    81

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





ms523

0:8a7c1e92d067


    82

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





ms523

0:8a7c1e92d067


    83

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





ms523

0:8a7c1e92d067


    84

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





ms523

0:8a7c1e92d067


    85

    }





ms523

0:8a7c1e92d067


    86

}