Stephane Swanepoel
Screen-Puppet
Dependencies: Matrix MatrixMath PCA9547 PowerControl mbed
Fork of
mbed_multiplex
by 
Stephane Swanepoel
Revision 1:f0f34b17c4f0, committed 2015-09-04
- Comitter:
- yenzo
- Date:
- Fri Sep 04 21:44:01 2015 +0000
- Parent:
- 0:80f939ca1f14
- Commit message:
- With matrix
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/CalculMatrix.h Fri Sep 04 21:44:01 2015 +0000
@@ -0,0 +1,256 @@
+#include "Matrix.h"
+#include "MatrixMath.h"
+
+/*--------------------------------------------------------*/
+/*--------------------------------------------------------*/
+/*----------------Definition des matrices-----------------*/
+/*--------------------------------------------------------*/
+/*--------------------------------------------------------*/
+
+
+//Valeur des rotations
+Matrix Bassin(4,1);
+
+Matrix EpauleGauche(4,1);
+Matrix CoudeGauche(4,1);
+
+Matrix EpauleDroite(4,1);
+Matrix CoudeDroit(4,1);
+
+Matrix CuisseGauche(4,1);
+Matrix MolletGauche(4,1);
+
+Matrix CuisseDroite(4,1);
+Matrix MolletDroit(4,1);
+
+
+//Valeurs des points des articulations
+Matrix Barycentre(4,1); //valeurs à 0 par défaut
+
+Matrix PosMilieuHautCorps(4,1);
+Matrix PosMilieuBasCorps(4,1);
+
+Matrix PosEpauleGauche(4,1);
+Matrix PosEpauleDroite(4,1);
+
+Matrix PosCoudeGauche(4,1);
+Matrix PosCoudeDroit(4,1);
+
+Matrix PosMainGauche(4,1);
+Matrix PosMainDroite(4,1);
+
+Matrix PosCuisseGauche(4,1);
+Matrix PosCuisseDroite(4,1);
+
+Matrix PosGenouGauche(4,1);
+Matrix PosGenouDroit(4,1);
+
+Matrix PosPiedGauche(4,1);
+Matrix PosPiedDroit(4,1);
+
+
+//Valeurs des transformations à effectuer
+Matrix Trans1_EpauleGauche(4,4);
+Matrix Trans2_EpauleGauche(4,4);
+
+Matrix Trans_CoudeGauche(4,4);
+
+Matrix Trans1_EpauleDroite(4,4);
+Matrix Trans2_EpauleDroite(4,4);
+
+Matrix Trans_CoudeDroit(4,4);
+
+Matrix Trans1_CuisseGauche(4,4);
+Matrix Trans2_CuisseGauche(4,4);
+
+Matrix Trans_GenouGauche(4,4);
+
+Matrix Trans1_CuisseDroite(4,4);
+Matrix Trans2_CuisseDroite(4,4);
+
+Matrix Trans_GenouDroit(4,4);
+
+
+/*--------------------------------------------------------*/
+/*--------------------------------------------------------*/
+/*----------------Definition des fonctions----------------*/
+/*--------------------------------------------------------*/
+/*--------------------------------------------------------*/
+
+
+void INIT_MATRICE(int mesure[][3]){
+
+ //Definition des rotations au matrice des articulations
+ Bassin << mesure[0][0]
+ << mesure[0][1]
+ << mesure[0][2]
+ << 1;
+
+ EpauleGauche << mesure[1][0]
+ << mesure[1][1]
+ << mesure[1][2]
+ << 1;
+
+ CoudeGauche << mesure[2][0]
+ << mesure[2][1]
+ << mesure[2][2]
+ << 1;
+
+ EpauleDroite << mesure[3][0]
+ << mesure[3][1]
+ << mesure[3][2]
+ << 1;
+
+ CoudeDroit << mesure[4][0]
+ << mesure[4][1]
+ << mesure[4][2]
+ << 1;
+
+ CuisseGauche << mesure[5][0]
+ << mesure[5][1]
+ << mesure[5][2]
+ << 1;
+
+ MolletGauche << mesure[6][0]
+ << mesure[6][1]
+ << mesure[6][2]
+ << 1;
+
+ CuisseDroite << mesure[7][0]
+ << mesure[7][1]
+ << mesure[7][2]
+ << 1;
+
+ MolletDroit << mesure[8][0]
+ << mesure[8][1]
+ << mesure[8][2]
+ << 1;
+
+ //Definition des valeurs des points des articulations
+ Barycentre << 0 //à redéfinir
+ << 0
+ << 0
+ << 1;
+
+ PosMilieuHautCorps << Barycentre(1,1)
+ << Barycentre(2,1) + 166
+ << Barycentre(3,1)
+ << 1;
+
+ PosMilieuBasCorps << Barycentre(1,1)
+ << Barycentre(2,1) - 110
+ << Barycentre(3,1)
+ << 1;
+
+ PosEpauleGauche << Barycentre(1,1) - 84
+ << Barycentre(2,1) + 166
+ << Barycentre(3,1)
+ << 1;
+
+ PosEpauleDroite << Barycentre(1,1) + 84
+ << Barycentre(2,1) + 166
+ << Barycentre(3,1)
+ << 1;
+
+ PosCuisseGauche << Barycentre(1,1) - 61.5
+ << Barycentre(2,1) - 110
+ << Barycentre(3,1)
+ << 1;
+
+ PosCuisseDroite << Barycentre(1,1) + 61.5
+ << Barycentre(2,1) - 110
+ << Barycentre(3,1)
+ << 1;
+
+ //Définition des valeurs des transformations
+
+ Trans1_EpauleGauche << 1 << 0 << 0 << -84
+ << 0 << 1 << 0 << 166
+ << 0 << 0 << 1 << 0
+ << 0 << 0 << 0 << 1;
+
+ Trans2_EpauleGauche << 1 << 0 << 0 << 0
+ << 0 << 1 << 0 << -118
+ << 0 << 0 << 1 << 0
+ << 0 << 0 << 0 << 1;
+
+ Trans_CoudeGauche << 1 << 0 << 0 << 0
+ << 0 << 1 << 0 << -117
+ << 0 << 0 << 1 << 0
+ << 0 << 0 << 0 << 1;
+
+ Trans1_EpauleDroite << 1 << 0 << 0 << 84
+ << 0 << 1 << 0 << 166
+ << 0 << 0 << 1 << 0
+ << 0 << 0 << 0 << 1;
+
+ Trans2_EpauleDroite << 1 << 0 << 0 << 0
+ << 0 << 1 << 0 << -118
+ << 0 << 0 << 1 << 0
+ << 0 << 0 << 0 << 1;
+
+ Trans_CoudeDroit << 1 << 0 << 0 << 0
+ << 0 << 1 << 0 << -117
+ << 0 << 0 << 1 << 0
+ << 0 << 0 << 0 << 1;
+
+ Trans1_CuisseGauche << 1 << 0 << 0 << -61.5
+ << 0 << 1 << 0 << -110
+ << 0 << 0 << 1 << 0
+ << 0 << 0 << 0 << 1;
+
+ Trans2_CuisseGauche << 1 << 0 << 0 << 0
+ << 0 << 1 << 0 << -150
+ << 0 << 0 << 1 << 0
+ << 0 << 0 << 0 << 1;
+
+ Trans_GenouGauche << 1 << 0 << 0 << 0
+ << 0 << 1 << 0 << -210
+ << 0 << 0 << 1 << 0
+ << 0 << 0 << 0 << 1;
+
+ Trans1_CuisseDroite << 1 << 0 << 0 << 61.5
+ << 0 << 1 << 0 << -110
+ << 0 << 0 << 1 << 0
+ << 0 << 0 << 0 << 1;
+
+ Trans2_CuisseDroite << 1 << 0 << 0 << 0
+ << 0 << 1 << 0 << -150
+ << 0 << 0 << 1 << 0
+ << 0 << 0 << 0 << 1;
+
+ Trans_GenouDroit << 1 << 0 << 0 << 0
+ << 0 << 1 << 0 << -210
+ << 0 << 0 << 1 << 0
+ << 0 << 0 << 0 << 1;
+}
+
+void AngleCalculate(float angle[]){
+ PosMilieuHautCorps = MatrixMath::RotX((Bassin.getNumber(1,1)*PI)/180.0)*MatrixMath::RotZ((Bassin.getNumber(1,1)*PI)/180.0)*PosMilieuHautCorps;
+ PosMilieuBasCorps = MatrixMath::RotX((Bassin.getNumber(1,1)*PI)/180.0)*MatrixMath::RotZ((Bassin.getNumber(1,1)*PI)/180.0)*PosMilieuBasCorps;
+
+ PosEpauleGauche = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*PosEpauleGauche;
+ PosEpauleDroite = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*PosEpauleDroite;
+
+ 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);
+ PosCoudeGauche = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_EpauleGauche*(RotXYZG*(Trans2_EpauleGauche*Barycentre)));
+ 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);
+ PosCoudeDroit = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_EpauleDroite*(RotXYZG*(Trans2_EpauleDroite*Barycentre)));
+
+ 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))))));
+ 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))))));
+
+ PosCuisseGauche = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*PosCuisseGauche;
+ PosCuisseDroite = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*PosCuisseDroite;
+
+ Matrix RotXZG = MatrixMath::RotX((CuisseGauche.getNumber(1,1)*PI)/180.0)*MatrixMath::RotY((CuisseGauche.getNumber(2,1)*PI)/180.0);
+ PosGenouGauche = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_CuisseGauche*(RotXZG*(Trans2_CuisseGauche*Barycentre)));
+ Matrix RotXZD = MatrixMath::RotX((CuisseDroite.getNumber(1,1)*PI)/180.0)*MatrixMath::RotY((CuisseDroite.getNumber(2,1)*PI)/180.0);
+ PosGenouDroit = MatrixMath::RotY((Bassin.getNumber(2,1)*PI)/180.0)*(Trans1_CuisseDroite*(RotXZD*(Trans2_CuisseDroite*Barycentre)));
+
+ 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))))));
+ 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))))));
+
+
+
+}
\ No newline at end of file
--- a/MPU9250.h Fri Sep 04 21:37:38 2015 +0000 +++ b/MPU9250.h Fri Sep 04 21:44:01 2015 +0000 @@ -1,4 +1,5 @@ #include "DefineMPU.h" +#include "CalculMatrix.h" #include "mbed.h" #include "math.h"
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Matrix.lib Fri Sep 04 21:44:01 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/Yo_Robot/code/Matrix/#a4014ab0a8cf
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MatrixMath.lib Fri Sep 04 21:44:01 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/Yo_Robot/code/MatrixMath/#93948a9bbde2