main.cpp

00001 //Includes
00002 #include "mbed.h"
00003 #include "7366_lib.h"
00004 #include "TLE5206_lib.h"
00005 
00006 // Caractéristiques de structure
00007 #define RAYON_ROUE 0.025 // [m]
00008 #define DIST1 0.15 // [m]
00009 #define DIST2 0.15 // [m]
00010 #define DIST3 0.0075 // [m]
00011 #define PI 3.14159265359
00012 #define RESOLUTION_ENCO 14336 // = 14*1024 = rapport_reduction * nbr de top par tour
00013 #define ETALONNAGE_LACET 0.88
00014 #define ETALONNAGE_XY 1.065
00015 // Liaison SPI avec les compteurs 
00016 #define SPI_SCLK PA_5 //A4
00017 #define SPI_MISO PA_6 //A5
00018 #define SPI_MOSI PA_7 //A6
00019 #define SPI_CS3 PA_0//A0
00020 #define SPI_CS2 PA_1
00021 #define SPI_CS1 PA_3
00022 
00023 // Vers le pont en H
00024 #define H1_IN1 PA_9  //D1 - PWM1/2
00025 #define H1_IN2 PA_10 //D0 - PWM1/3
00026 #define H2_IN1 PA_8  //D9 - PWM1/1
00027 #define H2_IN2 PB_7  //D4 - PWM17/1
00028 #define H3_IN1 PB_6  //D5 - PWM16/1
00029 #define H3_IN2 PA_4  //A3 - PWM3/2
00030 
00031 #define DECOUP_HACH 50 //us - 20 000 kHz pour les oreilles
00032 #define PERIODE_AFF 500 //ms
00033 #define PERIODE_ASSERV 25 //ms
00034 #define Te 25 //ms /!\ Te doit être égale à la période d'asservissement
00035 
00036 // Constantes Asservissement
00037 #define GAIN_POS 1.4
00038 #define GAIN_ANG 5
00039 #define Ti 5000000 //ms
00040 #define GAIN_POS_INT 0.1
00041 #define GAIN_ANG_INT 0.1
00042 #define ERREUR_POS 0
00043 #define ERREUR_ANG 0
00044 
00045 Serial pc(USBTX,USBRX);
00046 Timer timer;
00047 DigitalOut myled(LED3);
00048 
00049 SPI_7366 compt1(SPI_MOSI, SPI_MISO, SPI_SCLK,SPI_CS1);
00050 SPI_7366 compt2(SPI_MOSI, SPI_MISO, SPI_SCLK,SPI_CS2);
00051 SPI_7366 compt3(SPI_MOSI, SPI_MISO, SPI_SCLK,SPI_CS3);
00052 
00053 TLE5206 moteur1(H1_IN1,H1_IN2);
00054 TLE5206 moteur2(H2_IN1,H2_IN2);
00055 TLE5206 moteur3(H3_IN1,H3_IN2);
00056 
00057 struct Vect3{
00058     double x;
00059     double y;
00060     double z;
00061 };
00062 
00063 
00064 struct Vect2{
00065     float x;
00066     float y;
00067 };
00068 
00069 Vect3 initVect3(){
00070     Vect3 result;
00071     result.x = 0;
00072     result.y = 0;
00073     result.z = 0;
00074     return result;
00075 }
00076 
00077 Vect3 calculatePosition(){
00078     static Vect3 theta = initVect3();
00079     static Vect3 position = initVect3();
00080     Vect3 dTheta;
00081     double dPsi = 0;
00082     
00083     dTheta.x = 2*PI/RESOLUTION_ENCO*compt2.read_value() - theta.x;
00084     dTheta.y = 2*PI/RESOLUTION_ENCO*compt1.read_value() - theta.y;
00085     dTheta.z = 2*PI/RESOLUTION_ENCO*compt3.read_value() - theta.z;
00086     theta.x += dTheta.x;
00087     theta.y += dTheta.y;
00088     theta.z += dTheta.z;
00089     
00090     dPsi = ETALONNAGE_LACET*RAYON_ROUE * ( dTheta.x + dTheta.y + dTheta.z)/(DIST1 + DIST2 + DIST3);
00091 
00092     position.z += dPsi;// Psi actuel
00093     
00094     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;
00095     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;
00096     
00097     return position;
00098 }
00099 
00100 Vect3 calcErreur(Vect3 consigne, Vect3 position){
00101     Vect3 erreur;
00102     erreur.x = position.x - consigne.x;
00103     erreur.y = position.y - consigne.y;
00104     erreur.z = position.z - consigne.z;
00105     return erreur;
00106 }
00107 
00108 Vect3 calcCommandeXYZ(Vect3 erreur){
00109     /*
00110     static Vect3 commande;
00111     static Vect3 oldErreur;
00112     if (erreur.x > ERREUR_POS){
00113         commande.x = GAIN_POS*erreur.x;
00114         oldErreur.x = 0;
00115     } else {
00116         commande.x = commande.x + GAIN_POS*(Ti+Te)/Te*erreur.x - GAIN_POS*oldErreur.x;
00117         //commande.x = GAIN_POS*(erreur.x + GAIN_POS_INT*commande.x);
00118     }
00119     if (erreur.y > ERREUR_POS){
00120         commande.y = GAIN_POS*erreur.y;
00121     } else {
00122         commande.y = commande.y + GAIN_POS*(Ti+Te)/Te*erreur.y - GAIN_POS*oldErreur.y;
00123         //commande.y = GAIN_POS*(erreur.y + GAIN_POS_INT*commande.y);
00124     }
00125     if (erreur.z > ERREUR_ANG){
00126         commande.z = GAIN_ANG*erreur.z;
00127     } else {
00128         commande.z = commande.z + GAIN_ANG*(Ti+Te)/Te*erreur.z - GAIN_ANG*oldErreur.z;
00129         //commande.z = GAIN_ANG*(erreur.z + GAIN_ANG_INT*commande.z);
00130     }
00131     oldErreur = erreur;
00132     return commande;
00133     */
00134     static Vect3 integrateur = initVect3();
00135     Vect3 commande;
00136     if (abs(erreur.x) > ERREUR_POS){
00137         integrateur.x = 0;
00138     } else {
00139         integrateur.x = integrateur.x + Te*erreur.x;
00140     }
00141     commande.x = GAIN_POS*(erreur.x + integrateur.x/Ti);
00142     
00143     if (abs(erreur.y) > ERREUR_POS){
00144         integrateur.y = 0;
00145     } else {
00146         integrateur.y = integrateur.y + Te*erreur.y;
00147     }
00148     commande.y = GAIN_POS*(erreur.y + integrateur.y/Ti);
00149     
00150     if (abs(erreur.z) > ERREUR_ANG){
00151         integrateur.z = 0;
00152     } else {
00153         integrateur.z = integrateur.z + Te*erreur.z;
00154     }
00155     commande.z = GAIN_ANG*(erreur.z + integrateur.z/Ti);
00156     
00157     return commande;
00158     
00159 }
00160 
00161 Vect3 calcCommande123(Vect3 commandeXYZ, Vect3 position){
00162     Vect3 commande123;
00163     commande123.x = -commandeXYZ.x*cos(position.z+0*PI/3)+commandeXYZ.y*sin(position.z+0*PI/3)+commandeXYZ.z;
00164     commande123.y = -commandeXYZ.x*cos(position.z+2*PI/3)+commandeXYZ.y*sin(position.z+2*PI/3)+commandeXYZ.z;
00165     commande123.z = -commandeXYZ.x*cos(position.z+4*PI/3)+commandeXYZ.y*sin(position.z+4*PI/3)+commandeXYZ.z;
00166     return commande123;
00167 }
00168 
00169 void moveBot(Vect3 commande123){
00170     moteur1.write(commande123.y); 
00171     moteur2.write(commande123.x);
00172     moteur3.write(commande123.z);  
00173 }
00174 
00175 int main(){
00176     
00177     //setup
00178     compt1.setup();
00179     compt2.setup();
00180     compt3.setup();
00181     
00182     moteur1.setup(DECOUP_HACH);
00183     moteur2.setup(DECOUP_HACH);
00184     moteur3.setup(DECOUP_HACH);
00185     
00186     timer.start();
00187     pc.printf("SETUP effectue\n\r");
00188     
00189     //variables
00190     Vect3 position    = initVect3();
00191     Vect3 erreur      = initVect3();
00192     Vect3 commandeXYZ = initVect3();
00193     Vect3 commande123 = initVect3();
00194     Vect3 consigne    = initVect3();
00195     consigne.x = 0.50;
00196     consigne.y = 0;
00197     consigne.z = 0;
00198 
00199     uint32_t seuilAffichage = PERIODE_AFF;
00200     uint32_t seuilAsserv = PERIODE_ASSERV;
00201     
00202     // Loop
00203     while(1) {
00204         
00205         if (timer.read_ms() > seuilAffichage){
00206             seuilAffichage += PERIODE_AFF;
00207             pc.printf("lacet : %f\n\rpositionX : %f\n\rpositionY: %f\n\n\r",360/(2*PI)*position.z, position.x, position.y);
00208             pc.printf("erreur lacet : %f\n\rerreurX : %f\n\rereeurY: %f\n\n\r",erreur.z, erreur.x, erreur.y);
00209             pc.printf("commande lacet : %f\n\rcommandeX : %f\n\rcommandeY: %f\n\n\r",commande123.z, commande123.x, commande123.y);
00210             //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());
00211             //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());
00212             
00213             myled = !myled;
00214         }
00215         if (timer.read_ms() > seuilAsserv){
00216             seuilAsserv += PERIODE_ASSERV;
00217             position    = calculatePosition();
00218             erreur      = calcErreur(consigne, position);
00219             commandeXYZ = calcCommandeXYZ(erreur);
00220             commande123 = calcCommande123(commandeXYZ, position);
00221             moveBot(commande123);
00222         }        
00223     }
00224 }