Diff: main.cpp

Revision:

3:737ac9c24ca5

Parent:

2:486bb9b6bd78

Child:

4:80f612396136

Child:

6:85ade96c99b1

--- a/main.cpp	Tue Feb 12 10:52:27 2019 +0000
+++ b/main.cpp	Wed Feb 13 18:12:28 2019 +0000
@@ -3,6 +3,14 @@
 #include "7366_lib.h"
 #include "TLE5206_lib.h"
 
+// Caractéristiques de structure
+#define RAYON_ROUE 0.005 // [m]
+#define DIST1 0.015 // [m]
+#define DIST2 0.015 // [m]
+#define DIST3 0.015 // [m]
+#define PI 3.14159265359
+#define RESOLUTION_ENCO 14336*4 // = 14*1024*4 = rapport_reduction * nbr de top par tour
+
 // Liaison SPI avec les compteurs 
 #define SPI_SCLK PA_5 //A4
 #define SPI_MISO PA_6 //A5
@@ -23,6 +31,10 @@
 #define PERIODE_AFF 500 //ms
 #define PERIODE_ASSERV 50 //ms
 
+// Constantes Asservissement
+#define GAIN_POS 0.0001
+#define GAIN_ANG 0.0001
+
 Serial pc(USBTX,USBRX);
 Timer timer;
 DigitalOut myled(LED3);
@@ -35,8 +47,82 @@
 TLE5206 moteur2(H2_IN1,H2_IN2);
 TLE5206 moteur3(H3_IN1,H3_IN2);
 
-int main() {
+struct Vect3{
+    double x;
+    double y;
+    double z;
+};
+
+
+struct Vect2{
+    float x;
+    float y;
+};
+
+Vect3 initVect3(){
+    Vect3 result;
+    result.x = 0;
+    result.y = 0;
+    result.z = 0;
+    return result;
+}
+
+Vect3 calculatePosition(){
+    
+    static Vect3 theta = initVect3();
+    static Vect3 position = initVect3();
+    Vect3 dTheta;
+    double dPsi = 0;
+    
+    dTheta.x = 2*PI/RESOLUTION_ENCO*compt2.read_value() - theta.x;
+    dTheta.y = 2*PI/RESOLUTION_ENCO*compt1.read_value() - theta.y;
+    dTheta.z = 2*PI/RESOLUTION_ENCO*compt3.read_value() - theta.z;
+    theta.x += dTheta.x;
+    theta.y += dTheta.y;
+    theta.z += dTheta.z;
+    
+    dPsi = RAYON_ROUE * ( dTheta.x + dTheta.y + dTheta.z)/(DIST1 + DIST2 + DIST3);
 
+    position.z += dPsi;// Psi actuel
+    
+    position.x += ((-RAYON_ROUE * dTheta.x + DIST1 * dPsi) *  cos(position.z) + (-RAYON_ROUE * dTheta.y + DIST1 * dPsi) *  cos(position.z + 2*PI/3) + (-RAYON_ROUE * dTheta.z + DIST1 * dPsi)*cos(position.z + 4*PI/3))*2/3;
+    position.y +=-((-RAYON_ROUE * dTheta.x + DIST1 * dPsi) *  sin(position.z) + (-RAYON_ROUE * dTheta.y + DIST1 * dPsi) *  sin(position.z + 2*PI/3) + (-RAYON_ROUE * dTheta.z + DIST1 * dPsi)*sin(position.z  + 4*PI/3))*2/3;
+    
+    return position;
+}
+
+Vect3 calcErreur(Vect3 consigne, Vect3 position){
+    Vect3 erreur;
+    erreur.x = position.x - consigne.x;
+    erreur.x = position.y - consigne.y;
+    erreur.x = position.z - consigne.z;
+    return erreur;
+}
+
+Vect3 calcCommandeXYZ(Vect3 erreur){
+    Vect3 commande;   
+    commande.x = GAIN_POS*erreur.x;
+    commande.y = GAIN_POS*erreur.y;
+    commande.z = GAIN_ANG*erreur.z;
+    return commande;
+}
+
+Vect3 calcCommande123(Vect3 commandeXYZ, Vect3 position){
+    Vect3 commande123;
+    commande123.x = -commandeXYZ.x*cos(position.z+0*PI/3)+commandeXYZ.y*sin(position.z+0*PI/3)+commandeXYZ.z;
+    commande123.y = -commandeXYZ.x*cos(position.z+2*PI/3)+commandeXYZ.y*sin(position.z+2*PI/3)+commandeXYZ.z;
+    commande123.z = -commandeXYZ.x*cos(position.z+4*PI/3)+commandeXYZ.y*sin(position.z+4*PI/3)+commandeXYZ.z;
+    return commande123;
+}
+
+void moveBot(Vect3 commande123){
+    moteur1.write(commande123.x); 
+    moteur2.write(commande123.y);
+    moteur3.write(commande123.z);  
+}
+
+int main(){
+    
     //setup
     compt1.setup();
     compt2.setup();
@@ -51,12 +137,17 @@
     moteur3.write(0.7);
     
     timer.start();
+    pc.printf("SETUP effectue");
     
     //variables
-    int32_t position = 0;
-    int32_t consigne = (1<<15);
-    int32_t erreur = 0;
-    float gain = 0.0001;
+    Vect3 position    = initVect3();
+    Vect3 erreur      = initVect3();
+    Vect3 commandeXYZ = initVect3();
+    Vect3 commande123 = initVect3();
+    Vect3 consigne    = initVect3();
+    consigne.x = 0.10;
+    consigne.y = 0.05;
+    consigne.z = PI/2;
     uint32_t seuilAffichage = PERIODE_AFF;
     uint32_t seuilAsserv = PERIODE_ASSERV;
     
@@ -64,15 +155,20 @@
     while(1) {
         
         if (timer.read_ms() > seuilAffichage){
-            position = compt2.read_value();
             seuilAffichage += PERIODE_AFF;
-            pc.printf("valeur lue :  %d \n\r", position);
+            pc.printf("lacet : %f\n\rpositionX : %f\n\rpositionY: %f\n\n\r",position.z, position.x, position.y);
+            //pc.printf("compt3 : %f\n\rcompt1 : %f\n\rcompt2: %f\n\n\r",2*PI/RESOLUTION_ENCO*compt3.read_value(),2*PI/RESOLUTION_ENCO*compt1.read_value(), 2*PI/RESOLUTION_ENCO*compt2.read_value());
+            //pc.printf("compt3 : %f\n\rcompt1 : %f\n\rcompt2: %f\n\n\r",2*PI/RESOLUTION_ENCO*compt3.read_value(),2*PI/RESOLUTION_ENCO*compt1.read_value(), 2*PI/RESOLUTION_ENCO*compt2.read_value());
             myled = !myled;
         }
         if (timer.read_ms() > seuilAsserv){
             seuilAsserv += PERIODE_ASSERV;
-            erreur = compt2.read_value() - consigne;
-            moteur2.write(gain * erreur);
-       }
+            position    = calculatePosition();
+            erreur      = calcErreur(consigne, position);
+            commandeXYZ = calcCommandeXYZ(erreur);
+            commande123 = calcCommande123(commandeXYZ, position);
+            moveBot(commande123);
+        }
+        
     }
 }
\ No newline at end of file