Stephane Swanepoel / Mbed 2 deprecated mbed_multiplex_matrix

Screen-Puppet

Dependencies:   Matrix MatrixMath PCA9547 PowerControl mbed

Fork of mbed_multiplex by Stephane Swanepoel

CalculMatrix.h@1:f0f34b17c4f0, 2015-09-04 (annotated)

Committer:
yenzo
Date:
Fri Sep 04 21:44:01 2015 +0000
Revision:
1:f0f34b17c4f0
With matrix

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yenzo 1:f0f34b17c4f0 1 #include "Matrix.h"
yenzo 1:f0f34b17c4f0 2 #include "MatrixMath.h"
yenzo 1:f0f34b17c4f0 3
yenzo 1:f0f34b17c4f0 4 /*--------------------------------------------------------*/
yenzo 1:f0f34b17c4f0 5 /*--------------------------------------------------------*/
yenzo 1:f0f34b17c4f0 6 /*----------------Definition des matrices-----------------*/
yenzo 1:f0f34b17c4f0 7 /*--------------------------------------------------------*/
yenzo 1:f0f34b17c4f0 8 /*--------------------------------------------------------*/
yenzo 1:f0f34b17c4f0 9
yenzo 1:f0f34b17c4f0 10
yenzo 1:f0f34b17c4f0 11 //Valeur des rotations
yenzo 1:f0f34b17c4f0 12 Matrix Bassin(4,1);
yenzo 1:f0f34b17c4f0 13
yenzo 1:f0f34b17c4f0 14 Matrix EpauleGauche(4,1);
yenzo 1:f0f34b17c4f0 15 Matrix CoudeGauche(4,1);
yenzo 1:f0f34b17c4f0 16
yenzo 1:f0f34b17c4f0 17 Matrix EpauleDroite(4,1);
yenzo 1:f0f34b17c4f0 18 Matrix CoudeDroit(4,1);
yenzo 1:f0f34b17c4f0 19
yenzo 1:f0f34b17c4f0 20 Matrix CuisseGauche(4,1);
yenzo 1:f0f34b17c4f0 21 Matrix MolletGauche(4,1);
yenzo 1:f0f34b17c4f0 22
yenzo 1:f0f34b17c4f0 23 Matrix CuisseDroite(4,1);
yenzo 1:f0f34b17c4f0 24 Matrix MolletDroit(4,1);
yenzo 1:f0f34b17c4f0 25
yenzo 1:f0f34b17c4f0 26
yenzo 1:f0f34b17c4f0 27 //Valeurs des points des articulations
yenzo 1:f0f34b17c4f0 28 Matrix Barycentre(4,1); //valeurs à 0 par défaut
yenzo 1:f0f34b17c4f0 29
yenzo 1:f0f34b17c4f0 30 Matrix PosMilieuHautCorps(4,1);
yenzo 1:f0f34b17c4f0 31 Matrix PosMilieuBasCorps(4,1);
yenzo 1:f0f34b17c4f0 32
yenzo 1:f0f34b17c4f0 33 Matrix PosEpauleGauche(4,1);
yenzo 1:f0f34b17c4f0 34 Matrix PosEpauleDroite(4,1);
yenzo 1:f0f34b17c4f0 35
yenzo 1:f0f34b17c4f0 36 Matrix PosCoudeGauche(4,1);
yenzo 1:f0f34b17c4f0 37 Matrix PosCoudeDroit(4,1);
yenzo 1:f0f34b17c4f0 38
yenzo 1:f0f34b17c4f0 39 Matrix PosMainGauche(4,1);
yenzo 1:f0f34b17c4f0 40 Matrix PosMainDroite(4,1);
yenzo 1:f0f34b17c4f0 41
yenzo 1:f0f34b17c4f0 42 Matrix PosCuisseGauche(4,1);
yenzo 1:f0f34b17c4f0 43 Matrix PosCuisseDroite(4,1);
yenzo 1:f0f34b17c4f0 44
yenzo 1:f0f34b17c4f0 45 Matrix PosGenouGauche(4,1);
yenzo 1:f0f34b17c4f0 46 Matrix PosGenouDroit(4,1);
yenzo 1:f0f34b17c4f0 47
yenzo 1:f0f34b17c4f0 48 Matrix PosPiedGauche(4,1);
yenzo 1:f0f34b17c4f0 49 Matrix PosPiedDroit(4,1);
yenzo 1:f0f34b17c4f0 50
yenzo 1:f0f34b17c4f0 51
yenzo 1:f0f34b17c4f0 52 //Valeurs des transformations à effectuer
yenzo 1:f0f34b17c4f0 53 Matrix Trans1_EpauleGauche(4,4);
yenzo 1:f0f34b17c4f0 54 Matrix Trans2_EpauleGauche(4,4);
yenzo 1:f0f34b17c4f0 55
yenzo 1:f0f34b17c4f0 56 Matrix Trans_CoudeGauche(4,4);
yenzo 1:f0f34b17c4f0 57
yenzo 1:f0f34b17c4f0 58 Matrix Trans1_EpauleDroite(4,4);
yenzo 1:f0f34b17c4f0 59 Matrix Trans2_EpauleDroite(4,4);
yenzo 1:f0f34b17c4f0 60
yenzo 1:f0f34b17c4f0 61 Matrix Trans_CoudeDroit(4,4);
yenzo 1:f0f34b17c4f0 62
yenzo 1:f0f34b17c4f0 63 Matrix Trans1_CuisseGauche(4,4);
yenzo 1:f0f34b17c4f0 64 Matrix Trans2_CuisseGauche(4,4);
yenzo 1:f0f34b17c4f0 65
yenzo 1:f0f34b17c4f0 66 Matrix Trans_GenouGauche(4,4);
yenzo 1:f0f34b17c4f0 67
yenzo 1:f0f34b17c4f0 68 Matrix Trans1_CuisseDroite(4,4);
yenzo 1:f0f34b17c4f0 69 Matrix Trans2_CuisseDroite(4,4);
yenzo 1:f0f34b17c4f0 70
yenzo 1:f0f34b17c4f0 71 Matrix Trans_GenouDroit(4,4);
yenzo 1:f0f34b17c4f0 72
yenzo 1:f0f34b17c4f0 73
yenzo 1:f0f34b17c4f0 74 /*--------------------------------------------------------*/
yenzo 1:f0f34b17c4f0 75 /*--------------------------------------------------------*/
yenzo 1:f0f34b17c4f0 76 /*----------------Definition des fonctions----------------*/
yenzo 1:f0f34b17c4f0 77 /*--------------------------------------------------------*/
yenzo 1:f0f34b17c4f0 78 /*--------------------------------------------------------*/
yenzo 1:f0f34b17c4f0 79
yenzo 1:f0f34b17c4f0 80
yenzo 1:f0f34b17c4f0 81 void INIT_MATRICE(int mesure[][3]){
yenzo 1:f0f34b17c4f0 82
yenzo 1:f0f34b17c4f0 83 //Definition des rotations au matrice des articulations
yenzo 1:f0f34b17c4f0 84 Bassin << mesure[0][0]
yenzo 1:f0f34b17c4f0 85 << mesure[0][1]
yenzo 1:f0f34b17c4f0 86 << mesure[0][2]
yenzo 1:f0f34b17c4f0 87 << 1;
yenzo 1:f0f34b17c4f0 88
yenzo 1:f0f34b17c4f0 89 EpauleGauche << mesure[1][0]
yenzo 1:f0f34b17c4f0 90 << mesure[1][1]
yenzo 1:f0f34b17c4f0 91 << mesure[1][2]
yenzo 1:f0f34b17c4f0 92 << 1;
yenzo 1:f0f34b17c4f0 93
yenzo 1:f0f34b17c4f0 94 CoudeGauche << mesure[2][0]
yenzo 1:f0f34b17c4f0 95 << mesure[2][1]
yenzo 1:f0f34b17c4f0 96 << mesure[2][2]
yenzo 1:f0f34b17c4f0 97 << 1;
yenzo 1:f0f34b17c4f0 98
yenzo 1:f0f34b17c4f0 99 EpauleDroite << mesure[3][0]
yenzo 1:f0f34b17c4f0 100 << mesure[3][1]
yenzo 1:f0f34b17c4f0 101 << mesure[3][2]
yenzo 1:f0f34b17c4f0 102 << 1;
yenzo 1:f0f34b17c4f0 103
yenzo 1:f0f34b17c4f0 104 CoudeDroit << mesure[4][0]
yenzo 1:f0f34b17c4f0 105 << mesure[4][1]
yenzo 1:f0f34b17c4f0 106 << mesure[4][2]
yenzo 1:f0f34b17c4f0 107 << 1;
yenzo 1:f0f34b17c4f0 108
yenzo 1:f0f34b17c4f0 109 CuisseGauche << mesure[5][0]
yenzo 1:f0f34b17c4f0 110 << mesure[5][1]
yenzo 1:f0f34b17c4f0 111 << mesure[5][2]
yenzo 1:f0f34b17c4f0 112 << 1;
yenzo 1:f0f34b17c4f0 113
yenzo 1:f0f34b17c4f0 114 MolletGauche << mesure[6][0]
yenzo 1:f0f34b17c4f0 115 << mesure[6][1]
yenzo 1:f0f34b17c4f0 116 << mesure[6][2]
yenzo 1:f0f34b17c4f0 117 << 1;
yenzo 1:f0f34b17c4f0 118
yenzo 1:f0f34b17c4f0 119 CuisseDroite << mesure[7][0]
yenzo 1:f0f34b17c4f0 120 << mesure[7][1]
yenzo 1:f0f34b17c4f0 121 << mesure[7][2]
yenzo 1:f0f34b17c4f0 122 << 1;
yenzo 1:f0f34b17c4f0 123
yenzo 1:f0f34b17c4f0 124 MolletDroit << mesure[8][0]
yenzo 1:f0f34b17c4f0 125 << mesure[8][1]
yenzo 1:f0f34b17c4f0 126 << mesure[8][2]
yenzo 1:f0f34b17c4f0 127 << 1;
yenzo 1:f0f34b17c4f0 128
yenzo 1:f0f34b17c4f0 129 //Definition des valeurs des points des articulations
yenzo 1:f0f34b17c4f0 130 Barycentre << 0 //à redéfinir
yenzo 1:f0f34b17c4f0 131 << 0
yenzo 1:f0f34b17c4f0 132 << 0
yenzo 1:f0f34b17c4f0 133 << 1;
yenzo 1:f0f34b17c4f0 134
yenzo 1:f0f34b17c4f0 135 PosMilieuHautCorps << Barycentre(1,1)
yenzo 1:f0f34b17c4f0 136 << Barycentre(2,1) + 166
yenzo 1:f0f34b17c4f0 137 << Barycentre(3,1)
yenzo 1:f0f34b17c4f0 138 << 1;
yenzo 1:f0f34b17c4f0 139
yenzo 1:f0f34b17c4f0 140 PosMilieuBasCorps << Barycentre(1,1)
yenzo 1:f0f34b17c4f0 141 << Barycentre(2,1) - 110
yenzo 1:f0f34b17c4f0 142 << Barycentre(3,1)
yenzo 1:f0f34b17c4f0 143 << 1;
yenzo 1:f0f34b17c4f0 144
yenzo 1:f0f34b17c4f0 145 PosEpauleGauche << Barycentre(1,1) - 84
yenzo 1:f0f34b17c4f0 146 << Barycentre(2,1) + 166
yenzo 1:f0f34b17c4f0 147 << Barycentre(3,1)
yenzo 1:f0f34b17c4f0 148 << 1;
yenzo 1:f0f34b17c4f0 149
yenzo 1:f0f34b17c4f0 150 PosEpauleDroite << Barycentre(1,1) + 84
yenzo 1:f0f34b17c4f0 151 << Barycentre(2,1) + 166
yenzo 1:f0f34b17c4f0 152 << Barycentre(3,1)
yenzo 1:f0f34b17c4f0 153 << 1;
yenzo 1:f0f34b17c4f0 154
yenzo 1:f0f34b17c4f0 155 PosCuisseGauche << Barycentre(1,1) - 61.5
yenzo 1:f0f34b17c4f0 156 << Barycentre(2,1) - 110
yenzo 1:f0f34b17c4f0 157 << Barycentre(3,1)
yenzo 1:f0f34b17c4f0 158 << 1;
yenzo 1:f0f34b17c4f0 159
yenzo 1:f0f34b17c4f0 160 PosCuisseDroite << Barycentre(1,1) + 61.5
yenzo 1:f0f34b17c4f0 161 << Barycentre(2,1) - 110
yenzo 1:f0f34b17c4f0 162 << Barycentre(3,1)
yenzo 1:f0f34b17c4f0 163 << 1;
yenzo 1:f0f34b17c4f0 164
yenzo 1:f0f34b17c4f0 165 //Définition des valeurs des transformations
yenzo 1:f0f34b17c4f0 166
yenzo 1:f0f34b17c4f0 167 Trans1_EpauleGauche << 1 << 0 << 0 << -84
yenzo 1:f0f34b17c4f0 168 << 0 << 1 << 0 << 166
yenzo 1:f0f34b17c4f0 169 << 0 << 0 << 1 << 0
yenzo 1:f0f34b17c4f0 170 << 0 << 0 << 0 << 1;
yenzo 1:f0f34b17c4f0 171
yenzo 1:f0f34b17c4f0 172 Trans2_EpauleGauche << 1 << 0 << 0 << 0
yenzo 1:f0f34b17c4f0 173 << 0 << 1 << 0 << -118
yenzo 1:f0f34b17c4f0 174 << 0 << 0 << 1 << 0
yenzo 1:f0f34b17c4f0 175 << 0 << 0 << 0 << 1;
yenzo 1:f0f34b17c4f0 176
yenzo 1:f0f34b17c4f0 177 Trans_CoudeGauche << 1 << 0 << 0 << 0
yenzo 1:f0f34b17c4f0 178 << 0 << 1 << 0 << -117
yenzo 1:f0f34b17c4f0 179 << 0 << 0 << 1 << 0
yenzo 1:f0f34b17c4f0 180 << 0 << 0 << 0 << 1;
yenzo 1:f0f34b17c4f0 181
yenzo 1:f0f34b17c4f0 182 Trans1_EpauleDroite << 1 << 0 << 0 << 84
yenzo 1:f0f34b17c4f0 183 << 0 << 1 << 0 << 166
yenzo 1:f0f34b17c4f0 184 << 0 << 0 << 1 << 0
yenzo 1:f0f34b17c4f0 185 << 0 << 0 << 0 << 1;
yenzo 1:f0f34b17c4f0 186
yenzo 1:f0f34b17c4f0 187 Trans2_EpauleDroite << 1 << 0 << 0 << 0
yenzo 1:f0f34b17c4f0 188 << 0 << 1 << 0 << -118
yenzo 1:f0f34b17c4f0 189 << 0 << 0 << 1 << 0
yenzo 1:f0f34b17c4f0 190 << 0 << 0 << 0 << 1;
yenzo 1:f0f34b17c4f0 191
yenzo 1:f0f34b17c4f0 192 Trans_CoudeDroit << 1 << 0 << 0 << 0
yenzo 1:f0f34b17c4f0 193 << 0 << 1 << 0 << -117
yenzo 1:f0f34b17c4f0 194 << 0 << 0 << 1 << 0
yenzo 1:f0f34b17c4f0 195 << 0 << 0 << 0 << 1;
yenzo 1:f0f34b17c4f0 196
yenzo 1:f0f34b17c4f0 197 Trans1_CuisseGauche << 1 << 0 << 0 << -61.5
yenzo 1:f0f34b17c4f0 198 << 0 << 1 << 0 << -110
yenzo 1:f0f34b17c4f0 199 << 0 << 0 << 1 << 0
yenzo 1:f0f34b17c4f0 200 << 0 << 0 << 0 << 1;
yenzo 1:f0f34b17c4f0 201
yenzo 1:f0f34b17c4f0 202 Trans2_CuisseGauche << 1 << 0 << 0 << 0
yenzo 1:f0f34b17c4f0 203 << 0 << 1 << 0 << -150
yenzo 1:f0f34b17c4f0 204 << 0 << 0 << 1 << 0
yenzo 1:f0f34b17c4f0 205 << 0 << 0 << 0 << 1;
yenzo 1:f0f34b17c4f0 206
yenzo 1:f0f34b17c4f0 207 Trans_GenouGauche << 1 << 0 << 0 << 0
yenzo 1:f0f34b17c4f0 208 << 0 << 1 << 0 << -210
yenzo 1:f0f34b17c4f0 209 << 0 << 0 << 1 << 0
yenzo 1:f0f34b17c4f0 210 << 0 << 0 << 0 << 1;
yenzo 1:f0f34b17c4f0 211
yenzo 1:f0f34b17c4f0 212 Trans1_CuisseDroite << 1 << 0 << 0 << 61.5
yenzo 1:f0f34b17c4f0 213 << 0 << 1 << 0 << -110
yenzo 1:f0f34b17c4f0 214 << 0 << 0 << 1 << 0
yenzo 1:f0f34b17c4f0 215 << 0 << 0 << 0 << 1;
yenzo 1:f0f34b17c4f0 216
yenzo 1:f0f34b17c4f0 217 Trans2_CuisseDroite << 1 << 0 << 0 << 0
yenzo 1:f0f34b17c4f0 218 << 0 << 1 << 0 << -150
yenzo 1:f0f34b17c4f0 219 << 0 << 0 << 1 << 0
yenzo 1:f0f34b17c4f0 220 << 0 << 0 << 0 << 1;
yenzo 1:f0f34b17c4f0 221
yenzo 1:f0f34b17c4f0 222 Trans_GenouDroit << 1 << 0 << 0 << 0
yenzo 1:f0f34b17c4f0 223 << 0 << 1 << 0 << -210
yenzo 1:f0f34b17c4f0 224 << 0 << 0 << 1 << 0
yenzo 1:f0f34b17c4f0 225 << 0 << 0 << 0 << 1;
yenzo 1:f0f34b17c4f0 226 }
yenzo 1:f0f34b17c4f0 227
yenzo 1:f0f34b17c4f0 228 void AngleCalculate(float angle[]){
yenzo 1:f0f34b17c4f0 229 PosMilieuHautCorps = MatrixMath::RotX((Bassin.getNumber(1,1)*PI)/180.0)*MatrixMath::RotZ((Bassin.getNumber(1,1)*PI)/180.0)*PosMilieuHautCorps;
yenzo 1:f0f34b17c4f0 230 PosMilieuBasCorps = MatrixMath::RotX((Bassin.getNumber(1,1)*PI)/180.0)*MatrixMath::RotZ((Bassin.getNumber(1,1)*PI)/180.0)*PosMilieuBasCorps;
yenzo 1:f0f34b17c4f0 231
yenzo 1:f0f34b17c4f0 232 PosEpauleGauche = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*PosEpauleGauche;
yenzo 1:f0f34b17c4f0 233 PosEpauleDroite = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*PosEpauleDroite;
yenzo 1:f0f34b17c4f0 234
yenzo 1:f0f34b17c4f0 235 Matrix RotXYZG = MatrixMath::RotX((EpauleGauche.getNumber(1,1)*PI)/180.0)*MatrixMath::RotY((EpauleGauche.getNumber(2,1)*PI)/180.0)*MatrixMath::RotZ((EpauleGauche.getNumber(3,1)*PI)/180.0);
yenzo 1:f0f34b17c4f0 236 PosCoudeGauche = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_EpauleGauche*(RotXYZG*(Trans2_EpauleGauche*Barycentre)));
yenzo 1:f0f34b17c4f0 237 Matrix RotXYZD = MatrixMath::RotX((EpauleDroite.getNumber(1,1)*PI)/180.0)*MatrixMath::RotY((EpauleDroite.getNumber(2,1)*PI)/180.0)*MatrixMath::RotZ((EpauleDroite.getNumber(3,1)*PI)/180.0);
yenzo 1:f0f34b17c4f0 238 PosCoudeDroit = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_EpauleDroite*(RotXYZG*(Trans2_EpauleDroite*Barycentre)));
yenzo 1:f0f34b17c4f0 239
yenzo 1:f0f34b17c4f0 240 PosMainGauche = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_EpauleGauche*(RotXYZG*(Trans2_EpauleGauche*(MatrixMath::RotX((EpauleGauche.getNumber(1,1)*PI)/180.0)*(MatrixMath::RotX((CoudeGauche.getNumber(1,1)*PI)/180.0)*(Trans_CoudeGauche*Barycentre))))));
yenzo 1:f0f34b17c4f0 241 PosMainDroite = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_EpauleGauche*(RotXYZG*(Trans2_EpauleDroite*(MatrixMath::RotX((EpauleDroite.getNumber(1,1)*PI)/180.0)*(MatrixMath::RotX((CoudeDroit.getNumber(1,1)*PI)/180.0)*(Trans_CoudeDroit*Barycentre))))));
yenzo 1:f0f34b17c4f0 242
yenzo 1:f0f34b17c4f0 243 PosCuisseGauche = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*PosCuisseGauche;
yenzo 1:f0f34b17c4f0 244 PosCuisseDroite = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*PosCuisseDroite;
yenzo 1:f0f34b17c4f0 245
yenzo 1:f0f34b17c4f0 246 Matrix RotXZG = MatrixMath::RotX((CuisseGauche.getNumber(1,1)*PI)/180.0)*MatrixMath::RotY((CuisseGauche.getNumber(2,1)*PI)/180.0);
yenzo 1:f0f34b17c4f0 247 PosGenouGauche = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_CuisseGauche*(RotXZG*(Trans2_CuisseGauche*Barycentre)));
yenzo 1:f0f34b17c4f0 248 Matrix RotXZD = MatrixMath::RotX((CuisseDroite.getNumber(1,1)*PI)/180.0)*MatrixMath::RotY((CuisseDroite.getNumber(2,1)*PI)/180.0);
yenzo 1:f0f34b17c4f0 249 PosGenouDroit = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_CuisseDroite*(RotXZD*(Trans2_CuisseDroite*Barycentre)));
yenzo 1:f0f34b17c4f0 250
yenzo 1:f0f34b17c4f0 251 PosPiedGauche = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_CuisseGauche*(RotXZG*(Trans2_CuisseGauche*(MatrixMath::RotX((CuisseGauche.getNumber(1,1)*PI)/180.0)*(MatrixMath::RotX((MolletGauche.getNumber(1,1)*PI)/180.0)*(Trans_GenouGauche*Barycentre))))));
yenzo 1:f0f34b17c4f0 252 PosPiedDroit = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_CuisseDroite*(RotXZD*(Trans2_CuisseDroite*(MatrixMath::RotX((CuisseDroite.getNumber(1,1)*PI)/180.0)*(MatrixMath::RotX((MolletDroit.getNumber(1,1)*PI)/180.0)*(Trans_GenouDroit*Barycentre))))));
yenzo 1:f0f34b17c4f0 253
yenzo 1:f0f34b17c4f0 254
yenzo 1:f0f34b17c4f0 255
yenzo 1:f0f34b17c4f0 256 }