Stephane Swanepoel / Mbed 2 deprecated mbed_multiplex_matrix

Screen-Puppet

Dependencies:   Matrix MatrixMath PCA9547 PowerControl mbed

Fork of mbed_multiplex by Stephane Swanepoel

Files at this revision

API Documentation at this revision

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

CalculMatrix.h Show annotated file Show diff for this revision Revisions of this file
MPU9250.h Show annotated file Show diff for this revision Revisions of this file
Matrix.lib Show annotated file Show diff for this revision Revisions of this file
MatrixMath.lib Show annotated file Show diff for this revision Revisions of this file 
diff -r 80f939ca1f14 -r f0f34b17c4f0 CalculMatrix.h
--- /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

diff -r 80f939ca1f14 -r f0f34b17c4f0 MPU9250.h
--- 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"
 

diff -r 80f939ca1f14 -r f0f34b17c4f0 Matrix.lib
--- /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

diff -r 80f939ca1f14 -r f0f34b17c4f0 MatrixMath.lib
--- /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