Diff: main.cpp

Revision:

1:b9cade20e07e

Parent:

0:879aa9d0247b

Child:

4:0cab1f60b700

--- a/main.cpp	Tue Feb 12 17:39:05 2013 +0000
+++ b/main.cpp	Wed May 27 14:21:44 2015 +0000
@@ -1,10 +1,144 @@
+/************************************
+*  DB le 25/03/2015
+*   Pilotage AX-12A avec liaison série : Trame VoiceTronics2015
+*   Piloter les positions
+*   Débit = 1Mbit/s
+*************************************/
+
 #include "mbed.h"
- 
-Serial pc(USBTX, USBRX); // tx, rx
+#include "MX-28_DefConstantes.h"
+
+#define NBRE_ACK 256 // nombre d'acquisition pour la moyenne mesure pression 
  
+DigitalOut led01(LED1);
+DigitalOut led02(LED2);
+DigitalOut led03(LED3);
+DigitalOut led04(LED4); 
+DigitalOut LedRouge(p23); 
+
+DigitalOut dir(p8);
+AnalogOut  servoVanne(p18);
+AnalogIn mesPression(p20);
+
+unsigned char numeroOctetRecu=0;
+char octetRecu; //premier octet reçu
+char bufferRec[5]; // buffer de réception serialPc
+int valeurPressionPascal;
+
+//Serial serialPc(p28, p27); // tx, rx (c'est l'uart2 du LPC1768)compatible LabVIEW
+Serial serialPc(USBTX, USBRX); // tx, rx, 9600baud par défaut, pas compatible LabVIEW
+Serial uart3(p9, p10);  // tx, rx pour servomoteurs numériques
+
+
+// Envoi de la trame de pilotage a un servomoteur AX-12A
+void write (char id, char longueurTrame, char instruction, char param1 = NULL, char param2 = NULL, char param3 = NULL, char param4 = NULL)
+{
+    char Cks;
+    
+    Cks = ~( id + longueurTrame + instruction + param1 + param2 + param3 + param4); //calcul du checkSum
+    //serialPc.printf("Cks : %d\n", Cks);
+    dir = 1;    
+    uart3.putc(0xFF);
+    uart3.putc(0xFF);
+    uart3.putc(id);
+    uart3.putc(longueurTrame);
+    uart3.putc(instruction);
+    if (longueurTrame >= 3) { uart3.putc(param1); }
+    if (longueurTrame >= 4) { uart3.putc(param2); }
+    if (longueurTrame >= 5) { uart3.putc(param3); }
+    if (longueurTrame >= 6) { uart3.putc(param4); }
+    uart3.putc(Cks);
+    
+    wait_us(MX28_WAIT_AFTER_WRITE);  
+    dir = 0;
+} 
+
+// Set goal position of engine
+void setPosition(char id, int goal)
+{
+    char goal_h = goal >> 8;
+    char goal_l = goal;   
+    //serialPc.printf("Goal set : %d %d %d\n", goal, goal_h, goal_l);
+    write(id, 5, MX28_WRITE_DATA, MX28_GOAL_POSITION_L, goal_l, goal_h);
+}
+
+// Mesure de la pression avec moyenne sur NBRE_ACK
+int mesureDePression() {
+    float sommePression=0;
+    int valP;
+    for(int i=0; i<NBRE_ACK;i++) {
+        sommePression += mesPression;
+    }
+    valP = ((sommePression*3.3*1.7979)/0.000805/NBRE_ACK);
+    return valP;
+}
+// Reception d'un octet
+void receptionPc() {
+    // fonction appelée par interruption si réception sur serialPc
+    led04 =1;
+    octetRecu = serialPc.getc();
+    if(octetRecu == '$') {
+        numeroOctetRecu = 0;
+        memset(&bufferRec[0], 0, sizeof(bufferRec));
+    }
+    else {
+        bufferRec[numeroOctetRecu-1] = octetRecu;
+    }
+    if(numeroOctetRecu == 5) {
+        if (bufferRec[0] == '0') // si c'est une commande de position AX12
+        {            
+            int idServo = bufferRec[1] - 0x30;
+            int a1 = bufferRec[2] - 0x30;
+            int a2 = bufferRec[3] - 0x30;
+            int a3 = bufferRec[4] - 0x30;
+            int anglePosition = (a1 * 100) + (a2 * 10) + a3;
+            int valeurPosition = anglePosition * 1023 / 300;
+            setPosition(idServo, valeurPosition);
+                //Pour Debug
+                //serialPc.printf("idServ = %d \tvaleurPosition = %d\n",idServo, valeurPosition);
+        }
+        if (bufferRec[0] == '1') // si c'est une commande pour servovanne
+        {            
+            int a1 = bufferRec[2] - 0x30;
+            int a2 = bufferRec[3] - 0x30;
+            int a3 = bufferRec[4] - 0x30;
+            float commandeVanne = ((a1 * 100) + (a2 * 10) + a3)*3/3.3/100; // à étalonner !!!
+            servoVanne = commandeVanne;
+                //Pour Debug
+                //serialPc.printf("Commande servovanne %d %%\n",int (commandeVanne*100*3.3/3));
+        }
+        if (bufferRec[0] == '2') // si c'est une demande de pression
+        {            
+                    //Pour Debug, valeur fixe retournée
+                    //serialPc.printf("$203625"); // pour test envoie de la mesure=3625Pa
+            serialPc.printf("$2%0#5d",valeurPressionPascal); //voir cours Garreta page 85
+        }        
+
+        numeroOctetRecu = 0;
+    }
+    else { 
+        numeroOctetRecu++;
+    }
+    led04 =0;
+    }
+     
 int main() {
-    pc.printf("Hello World!\n");
+
+    uart3.baud(1000000);
+    serialPc.baud(115200);
+    serialPc.attach(&receptionPc); // defini la fonction interruption
+    wait(1);
+        //serialPc.printf("Entrer une commande\n");
+    led01 = 1;
+    LedRouge=1;
+        // Clear buffer
+        memset(&bufferRec[0], 0, sizeof(bufferRec));
+
     while(1) {
-        pc.putc(pc.getc() + 1);
+        led01 = 0;       
+        wait(0.1);
+        led01 = 1;       
+        wait(0.1);
+        valeurPressionPascal = mesureDePression(); //mesure cyclique de pression
     }
 }
\ No newline at end of file