Diff: main.cpp

Revision:

0:e91488ba4701

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Mar 25 12:41:24 2019 +0000
@@ -0,0 +1,216 @@
+//Includes
+#include "mbed.h"
+#include "7366_lib.h"
+#include "TLE5206_lib.h"
+
+// Caractéristiques de structure
+#define RAYON_ROUE 0.025 // [m]
+#define DIST1 0.15 // [m]
+#define DIST2 0.15 // [m]
+#define DIST3 0.0075 // [m]
+#define PI 3.14159265359
+#define RESOLUTION_ENCO 14336 // = 14*1024 = rapport_reduction * nbr de top par tour
+#define ETALONNAGE_LACET 0.88
+#define ETALONNAGE_XY 1.065
+// Liaison SPI avec les compteurs 
+#define SPI_SCLK PA_5 //A4
+#define SPI_MISO PA_6 //A5
+#define SPI_MOSI PA_7 //A6
+#define SPI_CS3 PA_0//A0
+#define SPI_CS2 PA_1
+#define SPI_CS1 PA_3
+
+// Vers le pont en H
+#define H1_IN1 PA_9  //D1 - PWM1/2
+#define H1_IN2 PA_10 //D0 - PWM1/3
+#define H2_IN1 PA_8  //D9 - PWM1/1
+#define H2_IN2 PB_7  //D4 - PWM17/1
+#define H3_IN1 PB_6  //D5 - PWM16/1
+#define H3_IN2 PA_4  //A3 - PWM3/2
+
+#define DECOUP_HACH 50 //us - 20 000 kHz pour les oreilles
+#define PERIODE_AFF 500 //ms
+#define PERIODE_ASSERV 25 //ms
+
+// Constantes Asservissement
+#define GAIN_POS 1.4
+#define GAIN_ANG 5
+#define GAIN_POS_INT 0 // 1/Te
+#define GAIN_ANG_INT 0 // 1/Te
+#define ERREUR_POS 0 // transition P-PI
+#define ERREUR_ANG 0
+
+Serial pc(USBTX,USBRX);
+Timer timer;
+DigitalOut myled(LED3);
+
+SPI_7366 compt1(SPI_MOSI, SPI_MISO, SPI_SCLK,SPI_CS1);
+SPI_7366 compt2(SPI_MOSI, SPI_MISO, SPI_SCLK,SPI_CS2);
+SPI_7366 compt3(SPI_MOSI, SPI_MISO, SPI_SCLK,SPI_CS3);
+
+TLE5206 moteur1(H1_IN1,H1_IN2);
+TLE5206 moteur2(H2_IN1,H2_IN2);
+TLE5206 moteur3(H3_IN1,H3_IN2);
+
+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 = ETALONNAGE_LACET*RAYON_ROUE * ( dTheta.x + dTheta.y + dTheta.z)/(DIST1 + DIST2 + DIST3);
+
+    position.z += dPsi;// Psi actuel
+    
+    position.x += ETALONNAGE_XY*((-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 +=-ETALONNAGE_XY*((-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.y = position.y - consigne.y;
+    erreur.z = position.z - consigne.z;
+    return erreur;
+}
+
+Vect3 calcCommandeXYZ(Vect3 erreur){
+    
+    static Vect3 integrateur = initVect3();
+    Vect3 commande;
+    if (abs(erreur.x) > ERREUR_POS){
+        integrateur.x = 0;
+    } else {
+        integrateur.x = integrateur.x + PERIODE_ASSERV*erreur.x;
+    }
+    commande.x = GAIN_POS*(erreur.x + GAIN_POS_INT*integrateur.x);
+    
+    if (abs(erreur.y) > ERREUR_POS){
+        integrateur.y = 0;
+    } else {
+        integrateur.y = integrateur.y + PERIODE_ASSERV*erreur.y;
+    }
+    commande.y = GAIN_POS*(erreur.y + GAIN_POS_INT*integrateur.y);
+    
+    if (abs(erreur.z) > ERREUR_ANG){
+        integrateur.z = 0;
+    } else {
+        integrateur.z = integrateur.z + PERIODE_ASSERV*erreur.z;
+    }
+    commande.z = GAIN_ANG*(erreur.z + GAIN_ANG_INT*integrateur.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.y); 
+    moteur2.write(commande123.x);
+    moteur3.write(commande123.z);  
+}
+int main(){
+    
+    //setup
+    compt1.setup();
+    compt2.setup();
+    compt3.setup();
+    
+    moteur1.setup(DECOUP_HACH);
+    moteur2.setup(DECOUP_HACH);
+    moteur3.setup(DECOUP_HACH);
+    
+    timer.start();
+    pc.printf("SETUP effectue\n\r");
+    
+    //variables
+    Vect3 position    = initVect3();
+    Vect3 erreur      = initVect3();
+    Vect3 commandeXYZ = initVect3();
+    Vect3 commande123 = initVect3();
+    Vect3 consigne    = initVect3();
+    consigne.x = 0.50;
+    consigne.y = 0;
+    consigne.z = 0;
+
+    uint32_t seuilAffichage = PERIODE_AFF;
+    uint32_t seuilAsserv = PERIODE_ASSERV;
+    uint8_t instruction = 0;
+    int flagAsserv = -1;
+    // Loop
+    while(1) {
+        
+
+        if (pc.readable()){
+            pc.read(instruction, 1);
+            if (instruction == 0 ){
+                consigne.x = 0.50;
+                consigne.y = 0;
+                consigne.z = 0;
+                flagAsserv = 1;
+            }
+            else if (instruction == 1){
+                consigne.x = 0;
+                consigne.y = 0.50;
+                consigne.z = 0;
+                flagAsserv = 1;
+            }
+            else if(instruction ==2){
+                consigne.x = 0;
+                consigne.y = 0;
+                consigne.z = 1.6;
+                flagAsserv = 1;
+            }
+        }
+
+        if (timer.read_ms() > seuilAffichage){
+            seuilAffichage += PERIODE_AFF;
+            pc.printf("%f;%f;%f",position.x, position.y, position.z);
+            myled = !myled;
+        }
+        if (timer.read_ms() > seuilAsserv and flagAsserv == 1){
+            seuilAsserv += PERIODE_ASSERV;
+            position    = calculatePosition();
+            erreur      = calcErreur(consigne, position);
+            commandeXYZ = calcCommandeXYZ(erreur);
+            commande123 = calcCommande123(commandeXYZ, position);
+            moveBot(commande123);
+        }        
+    }
+}
\ No newline at end of file