Martin Smith / Mbed 2 deprecated ERIC_Walking_4_Legs

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

Dependencies:   mbed AX12_Hardware

FK_Engine.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

#include "FK.h"





ms523

0:8a7c1e92d067


     2







ms523

0:8a7c1e92d067


     3

void FK_Engine(float *pos){





ms523

0:8a7c1e92d067


     4







ms523

0:8a7c1e92d067


     5

// Read the ax-12+ Servos and translate into joint angles α, θ and φ for each leg





ms523

0:8a7c1e92d067


     6

    float fore_right_alpha = Fore_Right_Hip_Lateral.GetPosition() - 135;





ms523

0:8a7c1e92d067


     7

    float fore_right_theta = 355 - Fore_Right_Hip.GetPosition();





ms523

0:8a7c1e92d067


     8

    float fore_right_phi = 305 - Fore_Right_Knee.GetPosition();





ms523

0:8a7c1e92d067


     9

    float fore_left_alpha = 165 - Fore_Left_Hip_Lateral.GetPosition();





ms523

0:8a7c1e92d067


    10

    float fore_left_theta = Fore_Left_Hip.GetPosition() + 55;





ms523

0:8a7c1e92d067


    11

    float fore_left_phi = Fore_Left_Knee.GetPosition() + 5;   





ms523

0:8a7c1e92d067


    12

    float hind_right_alpha = 165 - Hind_Right_Hip_Lateral.GetPosition();





ms523

0:8a7c1e92d067


    13

    float hind_right_theta = 340 - Hind_Right_Hip.GetPosition();





ms523

0:8a7c1e92d067


    14

    float hind_right_phi = 320 - Hind_Right_Knee.GetPosition();





ms523

0:8a7c1e92d067


    15

    float hind_left_alpha = Hind_Left_Hip_Lateral.GetPosition() - 135;





ms523

0:8a7c1e92d067


    16

    float hind_left_theta = Hind_Left_Hip.GetPosition() + 40;





ms523

0:8a7c1e92d067


    17

    float hind_left_phi = Hind_Left_Knee.GetPosition() + 20;      





ms523

0:8a7c1e92d067


    18

    float temp;                             // used to store maths





ms523

0:8a7c1e92d067


    19

    





ms523

0:8a7c1e92d067


    20

// Convert into radians for maths





ms523

0:8a7c1e92d067


    21

    hind_right_alpha = hind_right_alpha * PI / 180;





ms523

0:8a7c1e92d067


    22

    hind_right_theta = hind_right_theta  * PI / 180;





ms523

0:8a7c1e92d067


    23

    hind_right_phi = hind_right_phi * PI / 180;





ms523

0:8a7c1e92d067


    24

    hind_left_alpha = hind_left_alpha * PI / 180;





ms523

0:8a7c1e92d067


    25

    hind_left_theta = hind_left_theta  * PI / 180;





ms523

0:8a7c1e92d067


    26

    hind_left_phi = hind_left_phi * PI / 180;





ms523

0:8a7c1e92d067


    27

    fore_right_alpha = fore_right_alpha * PI / 180;





ms523

0:8a7c1e92d067


    28

    fore_right_theta = fore_right_theta  * PI / 180;





ms523

0:8a7c1e92d067


    29

    fore_right_phi = fore_right_phi * PI / 180;





ms523

0:8a7c1e92d067


    30

    fore_left_alpha = fore_left_alpha * PI / 180;





ms523

0:8a7c1e92d067


    31

    fore_left_theta = fore_left_theta  * PI / 180;





ms523

0:8a7c1e92d067


    32

    fore_left_phi = fore_left_phi * PI / 180;





ms523

0:8a7c1e92d067


    33







ms523

0:8a7c1e92d067


    34

// Next calculate point 1 for all legs





ms523

0:8a7c1e92d067


    35

    // Start with right fore leg





ms523

0:8a7c1e92d067


    36

    float x1_fore_right = sin(fore_right_alpha);





ms523

0:8a7c1e92d067


    37

    x1_fore_right = x1_fore_right * B;





ms523

0:8a7c1e92d067


    38

    float y1_fore_right = 0;





ms523

0:8a7c1e92d067


    39

    float z1_fore_right = cos(fore_right_alpha);





ms523

0:8a7c1e92d067


    40

    z1_fore_right = z1_fore_right * B;





ms523

0:8a7c1e92d067


    41

    // Then left fore leg





ms523

0:8a7c1e92d067


    42

    float x1_fore_left = sin(fore_left_alpha + PI);  //Add PI to invert to get the sign correct





ms523

0:8a7c1e92d067


    43

    x1_fore_left = x1_fore_left * B;            





ms523

0:8a7c1e92d067


    44

    float y1_fore_left = 0;





ms523

0:8a7c1e92d067


    45

    float z1_fore_left = cos(fore_left_alpha);





ms523

0:8a7c1e92d067


    46

    z1_fore_left = z1_fore_left * B;





ms523

0:8a7c1e92d067


    47

    // Then hind right leg





ms523

0:8a7c1e92d067


    48

    float x1_hind_right = sin(hind_right_alpha);





ms523

0:8a7c1e92d067


    49

    x1_hind_right = x1_hind_right * B;





ms523

0:8a7c1e92d067


    50

    float y1_hind_right = 0;





ms523

0:8a7c1e92d067


    51

    float z1_hind_right = cos(hind_right_alpha);





ms523

0:8a7c1e92d067


    52

    z1_hind_right = z1_hind_right * B;





ms523

0:8a7c1e92d067


    53

    // Finally hind left leg





ms523

0:8a7c1e92d067


    54

    float x1_hind_left = sin(hind_left_alpha + PI);  //Add PI to invert to get the sign correct





ms523

0:8a7c1e92d067


    55

    x1_hind_left = x1_hind_left * B;            





ms523

0:8a7c1e92d067


    56

    float y1_hind_left = 0;





ms523

0:8a7c1e92d067


    57

    float z1_hind_left = cos(hind_left_alpha);





ms523

0:8a7c1e92d067


    58

    z1_hind_left = z1_hind_left * B;





ms523

0:8a7c1e92d067


    59







ms523

0:8a7c1e92d067


    60

// Now calculate point 2 





ms523

0:8a7c1e92d067


    61

    // Aagain start with the fore right leg..





ms523

0:8a7c1e92d067


    62

    float x2_fore_right = cos(fore_right_theta + PI);





ms523

0:8a7c1e92d067


    63

    x2_fore_right = x2_fore_right * ULNA;





ms523

0:8a7c1e92d067


    64

    temp = cos(fore_right_alpha);





ms523

0:8a7c1e92d067


    65

    x2_fore_right = x2_fore_right * temp;





ms523

0:8a7c1e92d067


    66

    x2_fore_right = x1_fore_right + x2_fore_right;





ms523

0:8a7c1e92d067


    67

    float y2_fore_right = sin(fore_right_theta);





ms523

0:8a7c1e92d067


    68

    y2_fore_right = y2_fore_right * ULNA;





ms523

0:8a7c1e92d067


    69

    y2_fore_right = y2_fore_right + y1_fore_right;





ms523

0:8a7c1e92d067


    70

    float z2_fore_right = cos(fore_right_theta);





ms523

0:8a7c1e92d067


    71

    z2_fore_right = z2_fore_right * ULNA;





ms523

0:8a7c1e92d067


    72

    temp = sin(fore_right_alpha);





ms523

0:8a7c1e92d067


    73

    z2_fore_right = z2_fore_right * temp;





ms523

0:8a7c1e92d067


    74

    z2_fore_right = z1_fore_right + z2_fore_right;





ms523

0:8a7c1e92d067


    75

    // Then the fore left leg





ms523

0:8a7c1e92d067


    76

    float x2_fore_left = cos(fore_left_theta + PI);  





ms523

0:8a7c1e92d067


    77

    x2_fore_left = x2_fore_left * ULNA;





ms523

0:8a7c1e92d067


    78

    temp = cos(fore_left_alpha + PI);       //Add PI to invert to get the sign correct





ms523

0:8a7c1e92d067


    79

    x2_fore_left = x2_fore_left * temp;





ms523

0:8a7c1e92d067


    80

    x2_fore_left = x1_fore_left + x2_fore_left;





ms523

0:8a7c1e92d067


    81

    float y2_fore_left = sin(fore_left_theta);





ms523

0:8a7c1e92d067


    82

    y2_fore_left = y2_fore_left * ULNA;





ms523

0:8a7c1e92d067


    83

    y2_fore_left = y2_fore_left + y1_fore_left;





ms523

0:8a7c1e92d067


    84

    float z2_fore_left = cos(fore_left_theta);





ms523

0:8a7c1e92d067


    85

    z2_fore_left = z2_fore_left * ULNA;





ms523

0:8a7c1e92d067


    86

    temp = sin(fore_left_alpha);





ms523

0:8a7c1e92d067


    87

    z2_fore_left = z2_fore_left * temp;





ms523

0:8a7c1e92d067


    88

    z2_fore_left = z1_fore_left + z2_fore_left;





ms523

0:8a7c1e92d067


    89

    // Then the hind right leg





ms523

0:8a7c1e92d067


    90

    float x2_hind_right = cos(hind_right_theta + PI);





ms523

0:8a7c1e92d067


    91

    x2_hind_right = x2_hind_right * FEMUR;





ms523

0:8a7c1e92d067


    92

    temp = cos(hind_right_alpha);





ms523

0:8a7c1e92d067


    93

    x2_hind_right = x2_hind_right * temp;





ms523

0:8a7c1e92d067


    94

    x2_hind_right = x1_hind_right + x2_hind_right;





ms523

0:8a7c1e92d067


    95

    float y2_hind_right = sin(hind_right_theta);





ms523

0:8a7c1e92d067


    96

    y2_hind_right = y2_hind_right * FEMUR;





ms523

0:8a7c1e92d067


    97

    y2_hind_right = y2_hind_right + y1_hind_right;





ms523

0:8a7c1e92d067


    98

    float z2_hind_right = cos(hind_right_theta);





ms523

0:8a7c1e92d067


    99

    z2_hind_right = z2_hind_right * FEMUR;





ms523

0:8a7c1e92d067


   100

    temp = sin(hind_right_alpha);





ms523

0:8a7c1e92d067


   101

    z2_hind_right = z2_hind_right * temp;





ms523

0:8a7c1e92d067


   102

    z2_hind_right = z1_hind_right + z2_hind_right;





ms523

0:8a7c1e92d067


   103

    // Finally the hind left leg...





ms523

0:8a7c1e92d067


   104

    float x2_hind_left = cos(hind_left_theta + PI);  





ms523

0:8a7c1e92d067


   105

    x2_hind_left = x2_hind_left * FEMUR;





ms523

0:8a7c1e92d067


   106

    temp = cos(hind_left_alpha + PI);       //Add PI to invert to get the sign correct





ms523

0:8a7c1e92d067


   107

    x2_hind_left = x2_hind_left * temp;





ms523

0:8a7c1e92d067


   108

    x2_hind_left = x1_hind_left + x2_hind_left;





ms523

0:8a7c1e92d067


   109

    float y2_hind_left = sin(hind_left_theta);





ms523

0:8a7c1e92d067


   110

    y2_hind_left = y2_hind_left * FEMUR;





ms523

0:8a7c1e92d067


   111

    y2_hind_left = y2_hind_left + y1_hind_left;





ms523

0:8a7c1e92d067


   112

    float z2_hind_left = cos(hind_left_theta);





ms523

0:8a7c1e92d067


   113

    z2_hind_left = z2_hind_left * FEMUR;





ms523

0:8a7c1e92d067


   114

    temp = sin(hind_left_alpha);





ms523

0:8a7c1e92d067


   115

    z2_hind_left = z2_hind_left * temp;





ms523

0:8a7c1e92d067


   116

    z2_hind_left = z1_hind_left + z2_hind_left;





ms523

0:8a7c1e92d067


   117







ms523

0:8a7c1e92d067


   118

// Finally calculate point 3 





ms523

0:8a7c1e92d067


   119

    // Again start with the fore right leg





ms523

0:8a7c1e92d067


   120

    float x3_fore_right = cos(fore_right_theta - fore_right_phi);





ms523

0:8a7c1e92d067


   121

    x3_fore_right = x3_fore_right * RADIUS;





ms523

0:8a7c1e92d067


   122

    temp = cos(fore_right_alpha);





ms523

0:8a7c1e92d067


   123

    x3_fore_right = x3_fore_right * temp;





ms523

0:8a7c1e92d067


   124

    x3_fore_right = x2_fore_right + x3_fore_right;





ms523

0:8a7c1e92d067


   125

    float y3_fore_right = sin(fore_right_theta + PI - fore_right_phi);





ms523

0:8a7c1e92d067


   126

    y3_fore_right = y3_fore_right * RADIUS;





ms523

0:8a7c1e92d067


   127

    y3_fore_right = y2_fore_right + y3_fore_right;





ms523

0:8a7c1e92d067


   128

    float z3_fore_right = cos(fore_right_theta + PI - fore_right_phi);





ms523

0:8a7c1e92d067


   129

    z3_fore_right = z3_fore_right * RADIUS;





ms523

0:8a7c1e92d067


   130

    temp = sin(fore_right_alpha);





ms523

0:8a7c1e92d067


   131

    z3_fore_right = z3_fore_right * temp;





ms523

0:8a7c1e92d067


   132

    z3_fore_right = z2_fore_right + z3_fore_right;





ms523

0:8a7c1e92d067


   133

    // Then the fore left leg





ms523

0:8a7c1e92d067


   134

    float x3_fore_left = cos(fore_left_theta - fore_left_phi);





ms523

0:8a7c1e92d067


   135

    x3_fore_left = x3_fore_left * RADIUS;





ms523

0:8a7c1e92d067


   136

    temp = cos(fore_left_alpha + PI);           //Add PI to invert to get the sign correct





ms523

0:8a7c1e92d067


   137

    x3_fore_left = x3_fore_left * temp;





ms523

0:8a7c1e92d067


   138

    x3_fore_left = x2_fore_left + x3_fore_left;





ms523

0:8a7c1e92d067


   139

    float y3_fore_left = sin(fore_left_theta + PI - fore_left_phi);





ms523

0:8a7c1e92d067


   140

    y3_fore_left = y3_fore_left * RADIUS;





ms523

0:8a7c1e92d067


   141

    y3_fore_left = y2_fore_left + y3_fore_left;





ms523

0:8a7c1e92d067


   142

    float z3_fore_left = cos(fore_left_theta + PI - fore_left_phi);





ms523

0:8a7c1e92d067


   143

    z3_fore_left = z3_fore_left * RADIUS;





ms523

0:8a7c1e92d067


   144

    temp = sin(fore_left_alpha);





ms523

0:8a7c1e92d067


   145

    z3_fore_left = z3_fore_left * temp;





ms523

0:8a7c1e92d067


   146

    z3_fore_left = z2_fore_left + z3_fore_left;    





ms523

0:8a7c1e92d067


   147

    // Then the hind right leg..





ms523

0:8a7c1e92d067


   148

    float x3_hind_right = cos(hind_right_theta - hind_right_phi);





ms523

0:8a7c1e92d067


   149

    x3_hind_right = x3_hind_right * TIBIA;





ms523

0:8a7c1e92d067


   150

    temp = cos(hind_right_alpha);





ms523

0:8a7c1e92d067


   151

    x3_hind_right = x3_hind_right * temp;





ms523

0:8a7c1e92d067


   152

    x3_hind_right = x2_fore_right + x3_hind_right;





ms523

0:8a7c1e92d067


   153

    float y3_hind_right = sin(hind_right_theta + PI - hind_right_phi);





ms523

0:8a7c1e92d067


   154

    y3_hind_right = y3_hind_right * TIBIA;





ms523

0:8a7c1e92d067


   155

    y3_hind_right = y2_hind_right + y3_hind_right;





ms523

0:8a7c1e92d067


   156

    float z3_hind_right = cos(hind_right_theta + PI - hind_right_phi);





ms523

0:8a7c1e92d067


   157

    z3_hind_right = z3_hind_right * TIBIA;





ms523

0:8a7c1e92d067


   158

    temp = sin(hind_right_alpha);





ms523

0:8a7c1e92d067


   159

    z3_hind_right = z3_hind_right * temp;





ms523

0:8a7c1e92d067


   160

    z3_hind_right = z2_hind_right + z3_hind_right;





ms523

0:8a7c1e92d067


   161

    // Finally the hind left leg





ms523

0:8a7c1e92d067


   162

    float x3_hind_left = cos(hind_left_theta - hind_left_phi);





ms523

0:8a7c1e92d067


   163

    x3_hind_left = x3_hind_left * TIBIA;





ms523

0:8a7c1e92d067


   164

    temp = cos(hind_left_alpha + PI);           //Add PI to invert to get the sign correct





ms523

0:8a7c1e92d067


   165

    x3_hind_left = x3_hind_left * temp;





ms523

0:8a7c1e92d067


   166

    x3_hind_left = x2_hind_left + x3_hind_left;





ms523

0:8a7c1e92d067


   167

    float y3_hind_left = sin(hind_left_theta + PI - hind_left_phi);





ms523

0:8a7c1e92d067


   168

    y3_hind_left = y3_hind_left * TIBIA;





ms523

0:8a7c1e92d067


   169

    y3_hind_left = y2_hind_left + y3_hind_left;





ms523

0:8a7c1e92d067


   170

    float z3_hind_left = cos(hind_left_theta + PI - hind_left_phi);





ms523

0:8a7c1e92d067


   171

    z3_hind_left = z3_hind_left * TIBIA;





ms523

0:8a7c1e92d067


   172

    temp = sin(hind_left_alpha);





ms523

0:8a7c1e92d067


   173

    z3_hind_left = z3_hind_left * temp;





ms523

0:8a7c1e92d067


   174

    z3_hind_left = z2_hind_left + z3_hind_left;  





ms523

0:8a7c1e92d067


   175

    





ms523

0:8a7c1e92d067


   176

    





ms523

0:8a7c1e92d067


   177

    // Now updtae the array...   





ms523

0:8a7c1e92d067


   178

    pos[0] = x3_fore_right;





ms523

0:8a7c1e92d067


   179

    pos[1] = y3_fore_right;





ms523

0:8a7c1e92d067


   180

    pos[2] = z3_fore_right;





ms523

0:8a7c1e92d067


   181

    pos[3] = x3_fore_left;





ms523

0:8a7c1e92d067


   182

    pos[4] = y3_fore_left;





ms523

0:8a7c1e92d067


   183

    pos[5] = z3_fore_left;





ms523

0:8a7c1e92d067


   184

    pos[6] = x3_hind_right;





ms523

0:8a7c1e92d067


   185

    pos[7] = y3_hind_right;





ms523

0:8a7c1e92d067


   186

    pos[8] = z3_hind_right;





ms523

0:8a7c1e92d067


   187

    pos[9] = x3_hind_left;





ms523

0:8a7c1e92d067


   188

    pos[10] = y3_hind_left;





ms523

0:8a7c1e92d067


   189

    pos[11] = z3_hind_left;





ms523

0:8a7c1e92d067


   190

}