main.cpp

00001 /************************************
00002 *  DB le 25/03/2015 modif le 27/05/2015
00003 *   Pilotage AX-12A avec liaison série : Trame VoiceTronics2015
00004 *   Piloter les positions
00005 *   Débit = 1Mbit/s
00006 * commande 0/10volt Ok
00007 * mesure pression avec filtrage par moyenne
00008 *************************************/
00009 
00010 #include "mbed.h"
00011 #include "MX-28_DefConstantes.h"
00012 
00013 #define NBRE_ACK 256 // nombre d'acquisition pour la moyenne sur mesure pression 
00014  
00015 DigitalOut led01(LED1);
00016 DigitalOut led02(LED2);
00017 DigitalOut led03(LED3);
00018 DigitalOut led04(LED4); 
00019 DigitalOut LedRouge(p23); 
00020 
00021 DigitalOut dir(p8);
00022 AnalogOut  servoVanne(p18);
00023 AnalogIn mesPression(p20);
00024 
00025 unsigned char numeroOctetRecu=0;
00026 char octetRecu; //premier octet reçu
00027 char bufferRec[5]; // buffer de réception serialPc
00028 int valeurPressionPascal;
00029 
00030 //Serial serialPc(p28, p27); // tx, rx (c'est l'uart2 du LPC1768)compatible LabVIEW
00031 Serial serialPc(USBTX, USBRX); // tx, rx, 9600baud par défaut, pas compatible LabVIEW
00032 Serial uart3(p9, p10);  // tx, rx pour servomoteurs numériques
00033 
00034 
00035 // Envoi de la trame de pilotage a un servomoteur AX-12A
00036 void write (char id, char longueurTrame, char instruction, char param1 = NULL, char param2 = NULL, char param3 = NULL, char param4 = NULL)
00037 {
00038     char Cks;
00039     
00040     Cks = ~( id + longueurTrame + instruction + param1 + param2 + param3 + param4); //calcul du checkSum
00041     //serialPc.printf("Cks : %d\n", Cks);
00042     dir = 1;    
00043     uart3.putc(0xFF);
00044     uart3.putc(0xFF);
00045     uart3.putc(id);
00046     uart3.putc(longueurTrame);
00047     uart3.putc(instruction);
00048     if (longueurTrame >= 3) { uart3.putc(param1); }
00049     if (longueurTrame >= 4) { uart3.putc(param2); }
00050     if (longueurTrame >= 5) { uart3.putc(param3); }
00051     if (longueurTrame >= 6) { uart3.putc(param4); }
00052     uart3.putc(Cks);
00053     
00054     wait_us(MX28_WAIT_AFTER_WRITE);  
00055     dir = 0;
00056 } 
00057 
00058 // Set goal position of engine
00059 void setPosition(char id, int goal)
00060 {
00061     char goal_h = goal >> 8;
00062     char goal_l = goal;   
00063     //serialPc.printf("Goal set : %d %d %d\n", goal, goal_h, goal_l);
00064     write(id, 5, MX28_WRITE_DATA, MX28_GOAL_POSITION_L, goal_l, goal_h);
00065 }
00066 
00067 // Mesure de la pression avec moyenne sur NBRE_ACK
00068 int mesureDePression() {
00069     float sommePression=0;
00070     int valP;
00071     for(int i=0; i<NBRE_ACK;i++) {
00072         sommePression += mesPression;
00073     }
00074     valP = ((sommePression*3.3*1.7979)/0.000805/NBRE_ACK);
00075     return valP;
00076 }
00077 // Reception d'un octet
00078 void receptionPc() {
00079     // fonction appelée par interruption si réception sur serialPc
00080     led04 =1;
00081     octetRecu = serialPc.getc();
00082     if(octetRecu == '$') {
00083         numeroOctetRecu = 0;
00084         memset(&bufferRec[0], 0, sizeof(bufferRec));
00085     }
00086     else {
00087         bufferRec[numeroOctetRecu-1] = octetRecu;
00088     }
00089     if(numeroOctetRecu == 5) {
00090         if (bufferRec[0] == '0') // si c'est une commande de position AX12
00091         {            
00092             int idServo = bufferRec[1] - 0x30;
00093             int a1 = bufferRec[2] - 0x30;
00094             int a2 = bufferRec[3] - 0x30;
00095             int a3 = bufferRec[4] - 0x30;
00096             int anglePosition = (a1 * 100) + (a2 * 10) + a3;
00097             int valeurPosition = anglePosition * 1023 / 300;
00098             setPosition(idServo, valeurPosition);
00099                 //Pour Debug
00100                 //serialPc.printf("idServ = %d \tvaleurPosition = %d\n",idServo, valeurPosition);
00101         }
00102         if (bufferRec[0] == '1') // si c'est une commande pour servovanne
00103         {            
00104             int a1 = bufferRec[2] - 0x30;
00105             int a2 = bufferRec[3] - 0x30;
00106             int a3 = bufferRec[4] - 0x30;
00107             float commandeVanne = ((a1 * 100) + (a2 * 10) + a3)*3/3.3/100; // à étalonner !!!
00108             servoVanne = commandeVanne;
00109                 //Pour Debug
00110                 //serialPc.printf("Commande servovanne %d %%\n",int (commandeVanne*100*3.3/3));
00111         }
00112         if (bufferRec[0] == '2') // si c'est une demande de pression
00113         {            
00114                     //Pour Debug, valeur fixe retournée
00115                     //serialPc.printf("$203625"); // pour test envoie de la mesure=3625Pa
00116             serialPc.printf("$2%0#5d",valeurPressionPascal); //voir cours Garreta page 85
00117         }        
00118 
00119         numeroOctetRecu = 0;
00120     }
00121     else { 
00122         numeroOctetRecu++;
00123     }
00124     led04 =0;
00125     }
00126      
00127 int main() {
00128 
00129     uart3.baud(1000000);
00130     serialPc.baud(115200);
00131     serialPc.attach(&receptionPc); // defini la fonction interruption
00132     wait(1);
00133         //serialPc.printf("Entrer une commande\n");
00134     led01 = 1;
00135     LedRouge=1;
00136         // Clear buffer
00137         memset(&bufferRec[0], 0, sizeof(bufferRec));
00138 
00139     while(1) {
00140         led01 = 0;       
00141         wait(0.1);
00142         led01 = 1;       
00143         wait(0.1);
00144         valeurPressionPascal = mesureDePression(); //mesure cyclique de pression
00145     }
00146 }