Diff: main.cpp

Revision:

2:43daa01ba06d

Parent:

1:538b13cb6699

Child:

3:a3074e835b35

--- a/main.cpp	Mon May 18 21:07:26 2015 +0000
+++ b/main.cpp	Tue May 19 21:03:01 2015 +0000
@@ -1,13 +1,17 @@
 /************************************
-*  DB le 12/05/2015  MX28-Scan3D_DB03-IRQ
-*   Pilotage MX-28 MX-106 avec liaison série : Trame Scan3D
+*  DB le 19/05/2015  MX28-Scan3D_DB04-IRQ
+*   Programme final conforme au protocole de communication
+*   Reste à faire : - lecture accéléromètre 
+*                   - vérification de positions atteintes par servomoteurs
+*
+*   Pilotage MX-106 (SERVO_VERTICAL) et MX-28 (SERVO_HORIZONTALE)
+*   avec liaison série : Trame Scan3D
 *		&abcdefghij/ début $, fin /, longueur variable
-*		à partir de prog pour Trame VoiceTronics2015
 *   Réception Série par IRQ
 *   Piloter les positions des 2 servomoteurs V et H
 *   Débit = 57142bit/s
 *
-* Particularité Matrice Led Neopixels
+* Matrice Led Neopixels
 *  Une seule instance possible avec la classe NeoStrip : 
 *  Problème, pour nous il faut 3 voies : panelLed1, panelLed2 et panelLed3 !
 *  les 2 autres voies "faite à la main" avec les fonctions :
@@ -21,12 +25,12 @@
 #include "MX-28_DefConstantes.h"
 #include "NeoStrip.h"
 
-#define ID_SERVO_VERTICAL 2		// MX-106R
-#define ID_SERVO_HORIZONTALE 3	// MX-28R
+#define ID_SERVO_VERTICAL 3		// MX-106R
+#define ID_SERVO_HORIZONTALE 2	// MX-28R
 
 #define N1 64 // Nombre de pixels(Led) du panneau Led 1
-#define N2 64 // Nombre de pixels(Led) du panneau Led 2
-#define N3 64 // Nombre de pixels(Led) du panneau Led 3
+#define N2 128 // Nombre de pixels(Led) du panneau Led 2
+#define N3 128 // Nombre de pixels(Led) du panneau Led 3
 
 DigitalOut led01(LED1);
 DigitalOut led02(LED2);
@@ -93,6 +97,28 @@
 }
 
 //---------------------------------------------------------------------------------------------
+// Lire position d'un servomoteur MX-28 ou MX-106
+int lirePositionServo(char idServo) {
+            //Vider le buffer de réception uartMX28
+            while(uartMX28.readable()) {
+                uartMX28.getc();
+            }
+            write(idServo, 4, MX28_READ_DATA, MX28_PRESENT_POSITION_L,2);
+            char octetRecuMX[8];
+            char i=0;
+            int tempoReception=0;
+            while((tempoReception < 1000000)&&(i<8)) {
+                if(uartMX28.readable()) {
+                    octetRecuMX[i] = uartMX28.getc();
+                    i++;
+                }
+                tempoReception++;
+            }
+            int valeurLue = octetRecuMX[6] * 256 + octetRecuMX[5];
+            return valeurLue;
+}
+
+//---------------------------------------------------------------------------------------------
 // fonction écriture Bit code 1 et 0 pour matrice Led Neopixels
 void writeBitCode1(int numMatriceLed)
 {
@@ -163,44 +189,22 @@
             //Pour Debug
             //serialPc.printf("idServ = %d \tvaleurPosition = %d\n",ID_SERVO_VERTICAL, valeurPositionVerticale);
             //serialPc.printf("idServ = %d \tvaleurPosition = %d\n",ID_SERVO_HORIZONTALE, valeurPositionHorizontale);
+                         
+			//--Solution simple pour retourner la position mais sans tester "positions demandéess=atteintes"
+            // test positions atteintes à faire
+            wait(2); //pour laisser le temps au moteur de se déplacer, 
+            bufferRec[0] = '1'; //pour retourner la mesure de la position
+            numeroOctetRecu = 2; //pour retourner la mesure de la position
         }
         if ((bufferRec[0] == '1')&&(numeroOctetRecu == 2)) { // si demande de lecture de la position
             led03 = 1; // indic
-
-            //Vider le buffer de réception uartMX28
-            while(uartMX28.readable()) {
-                uartMX28.getc();
-            }
-            write(ID_SERVO_VERTICAL, 4, MX28_READ_DATA, MX28_PRESENT_POSITION_L,2);
-            char octetRecuMX[8];
-            char i=0;
-            int tempoReception=0;
-            while((tempoReception < 1000000)&&(i<8)) {
-                if(uartMX28.readable()) {
-                    octetRecuMX[i] = uartMX28.getc();
-                    i++;
-                }
-                tempoReception++;
-            }
-            int valeurLue = octetRecuMX[6] * 256 + octetRecuMX[5];
+			int valeurLue = lirePositionServo(ID_SERVO_VERTICAL);
             int valeurAngleVertCentiemeDegres = (valeurLue * 100) * 360/4096;
-            	//serialPc.printf("Valeur lue vertical%d\n",valeurLue); ///////////Pour test A modifier !!!!!
             wait(0.2);
-            
-            write(ID_SERVO_HORIZONTALE, 4, MX28_READ_DATA, MX28_PRESENT_POSITION_L,2);
-            i=0;
-            tempoReception=0;
-            while((tempoReception < 1000000)&&(i<8)) {
-                if(uartMX28.readable()) {
-                    octetRecuMX[i] = uartMX28.getc();
-                    i++;
-                }
-                tempoReception++;
-            }
-            valeurLue = octetRecuMX[6] * 256 + octetRecuMX[5];
+            valeurLue = lirePositionServo(ID_SERVO_HORIZONTALE);
             int valeurAngleHorizontaleCentiemeDegres = (valeurLue * 100) * 360/4096;
-            	//serialPc.printf("Valeur lue horisontal%d\n",valeurLue); ///////////Pour test A modifier !!!!!
-            serialPc.printf("OK_moteurs_%0#5d_%d",valeurAngleVertCentiemeDegres, valeurAngleHorizontaleCentiemeDegres);
+
+            serialPc.printf("$OK_moteurs_%0#5d_%0#5d/\n",valeurAngleVertCentiemeDegres, valeurAngleHorizontaleCentiemeDegres);
 
             led03 = 0; //
         }
@@ -209,12 +213,12 @@
                 //Laser on
                 led02 = 1; // indic commande Laser on
                 laser = 1;
-                serialPc.printf("OK_laser_1");
+                serialPc.printf("$OK_laser_1/\n");
             } else {
                 //Laser off
                 led02 = 0; // indic commande Laser off
                 laser = 0;
-                serialPc.printf("OK_laser_0");
+                serialPc.printf("$OK_laser_0/\n");
             }
         }
         if ((bufferRec[0] == '3')&&(numeroOctetRecu == 11)) { // si commande panneau led supérieur
@@ -237,6 +241,7 @@
             panelLed1.write();
 
             wait(0.2);
+            serialPc.printf("$OK_panneau_1_%c%c%c_%c%c%c_%c%c%c/\n",bufferRec[1],bufferRec[2],bufferRec[3],bufferRec[4],bufferRec[5],bufferRec[6],bufferRec[7],bufferRec[8],bufferRec[9]);
             led03 = 0; // indic commande panneau led supérieur
         }
         if ((bufferRec[0] == '4')&&(numeroOctetRecu == 11)) { // si commande panneau led latéral 1
@@ -255,6 +260,7 @@
 			int valeurBleu = (h*100+i*10+j);
 			writeMatriceLed(2,N2,valeurRouge, valeurVerte, valeurBleu);
             wait(0.2);
+            serialPc.printf("$OK_panneau_2_%c%c%c_%c%c%c_%c%c%c/\n",bufferRec[1],bufferRec[2],bufferRec[3],bufferRec[4],bufferRec[5],bufferRec[6],bufferRec[7],bufferRec[8],bufferRec[9]);
             led03 = 0; // indic commande panneau led
         }
         if ((bufferRec[0] == '5')&&(numeroOctetRecu == 11)) { // si commande panneau led latéral 2
@@ -273,10 +279,11 @@
 			int valeurBleu = (h*100+i*10+j);
 			writeMatriceLed(3,N3,valeurRouge, valeurVerte, valeurBleu);
             wait(0.2);
+            serialPc.printf("$OK_panneau_3_%c%c%c_%c%c%c_%c%c%c/\n",bufferRec[1],bufferRec[2],bufferRec[3],bufferRec[4],bufferRec[5],bufferRec[6],bufferRec[7],bufferRec[8],bufferRec[9]);
             led03 = 0; // indic commande panneau led
         }
         if ((bufferRec[0] == '6')&&(numeroOctetRecu == 2)) { // si c'est une demande mesure accéléromètre
-            //LECTURE ET ENVOIE des mesures d'accélérations
+            //LECTURE ET ENVOI des mesures d'accélérations
             // A Faire
         }