Diff: main.cpp

Revision:

0:129e8567ddbd

Child:

1:538b13cb6699

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun May 17 17:29:44 2015 +0000
@@ -0,0 +1,294 @@
+/************************************
+*  DB le 12/05/2015  MX28-Scan3D_DB03-IRQ
+*   Pilotage MX-28 MX-106 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
+*  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 :
+*  - void writeBitCode1(numMatriceLed) et void writeBitCode0(numMatriceLed)
+*  - void writeMatriceLed(int numMatriceLed, int N, int valRouge, int valVerte, int valBleu)
+*  
+* Cible : Carte mbed_S3DHQ_2015
+*************************************/
+
+#include "mbed.h"
+#include "MX-28_DefConstantes.h"
+#include "NeoStrip.h"
+
+#define ID_SERVO_VERTICAL 2		// MX-106R
+#define ID_SERVO_HORIZONTALE 3	// 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
+
+DigitalOut led01(LED1);
+DigitalOut led02(LED2);
+DigitalOut led03(LED3);
+DigitalOut led04(LED4);
+
+DigitalOut dir(p17);
+DigitalOut laser(p5);
+DigitalOut panelLed2(p7);
+DigitalOut panelLed3(p8);
+
+unsigned char numeroOctetRecu=0;
+char octetRecu; //premier octet reçu
+char bufferRec[12]; // buffer de réception serialPc
+
+Serial serialPc(USBTX, USBRX); // tx, rx, écarté car driver incompatible avec LabVIEW
+//Serial serialPc(p28, p27); // tx, rx (c'est l'uart2 du LPC1768)
+//Serial uartMX28(p13, p14);  // tx, rx pour MX28 (uart1 du LPC1768)
+Serial uartMX28(p28, p27);  // tx, rx pour MX28 (uart1 du LPC1768)
+
+NeoStrip panelLed1(p6, N1); // creation de l'objet panelLed1 cde par p6, N1 Led
+//NeoStrip panelLed2(p7, N2); // creation de l'objet panelLed2 cde par p7, N2 Led
+//NeoStrip panelLed3(p8, N3); // creation de l'objet panelLed3 cde par p8, N3 Led
+
+//------------------------------------------------------------------------------------------------
+// Envoi de la trame de pilotage a un servomoteur MX-28
+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;
+    uartMX28.putc(0xFF);
+    uartMX28.putc(0xFF);
+    uartMX28.putc(id);
+    uartMX28.putc(longueurTrame);
+    uartMX28.putc(instruction);
+    if (longueurTrame >= 3) {
+        uartMX28.putc(param1);
+    }
+    if (longueurTrame >= 4) {
+        uartMX28.putc(param2);
+    }
+    if (longueurTrame >= 5) {
+        uartMX28.putc(param3);
+    }
+    if (longueurTrame >= 6) {
+        uartMX28.putc(param4);
+    }
+    uartMX28.putc(Cks);
+
+    wait_us(MX28_WAIT_AFTER_WRITE); // Attendre l'envoie complet de la trame.
+    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);
+}
+
+//---------------------------------------------------------------------------------------------
+// fonction écriture Bit code 1 et 0 pour matrice Led Neopixels
+void writeBitCode1(int numMatriceLed)
+{
+	if(numMatriceLed==2){panelLed2=1;}
+	else{panelLed3=1;}
+	for(int i=0;i<37;i++) {int x = i*i*i; x=x*x;} // pour T1H=700ns
+	if(numMatriceLed==2){panelLed2=0;}
+	else{panelLed3=0;}
+	for(int i=0;i<16;i++) {int x = i*i*i; x=x*x;} // pour T1L= 450ns + 150ns pour exe fonction
+}
+void writeBitCode0(int numMatriceLed)
+{	
+	if(numMatriceLed==2){panelLed2=1;}
+	else{panelLed3=1;}
+	for(int i=0;i<15;i++) {int x = i*i*i; x=x*x;} // pour T0H=350ns
+	if(numMatriceLed==2){panelLed2=0;}
+	else{panelLed3=0;}
+	for(int i=0;i<32;i++) {int x = i*i*i; x=x*x;} // pour T0L=650ns+150ns pour exe fonction
+}
+//---------------------------------------------------------------------------------------------
+// fonction écriture matrice Led Neopixels
+void writeMatriceLed(int numMatriceLed, int N, int valRouge, int valVerte, int valBleu)
+{
+	int valeurCouleurVRB = valVerte*65536+valRouge*256+valBleu;
+	for(int i=0;i<N;i++) {
+	int mask_1sur24Bits = 0x800000;
+		for(int j=0;j<24;j++) {
+			if(valeurCouleurVRB & mask_1sur24Bits) {
+				writeBitCode1(numMatriceLed);
+			}
+			else {
+				writeBitCode0(numMatriceLed);
+			}
+			mask_1sur24Bits = mask_1sur24Bits >> 1;
+		}
+	}
+}
+
+
+//---------------------------------------------------------------------------------------------
+// fonction appelée par interruption si réception sur serialPc
+void receptionPc()
+{
+    led04 =1;
+    octetRecu = serialPc.getc();
+    if(octetRecu == '$') {
+        numeroOctetRecu = 0;
+        memset(&bufferRec[0], 0, sizeof(bufferRec));
+    } else {
+        bufferRec[numeroOctetRecu-1] = octetRecu;
+///Debug        serialPc.printf("%c",octetRecu);	////////
+    }
+    if(octetRecu == '/') {
+        if ((bufferRec[0] == '0')&&(numeroOctetRecu == 8)) { // si c'est une commande de position MX28+MX106
+            int b = bufferRec[1] - 0x30;
+            int c = bufferRec[2] - 0x30;
+            int d = bufferRec[3] - 0x30;
+            int e = bufferRec[4] - 0x30;
+            int f = bufferRec[5] - 0x30;
+            int g = bufferRec[6] - 0x30;
+            int anglePositionVerticale = (b * 100) + (c * 10) + d;
+            int valeurPositionVerticale = anglePositionVerticale * 4095 / 360;
+            setPosition(ID_SERVO_VERTICAL, valeurPositionVerticale);
+            int anglePositionHorizontale = (e * 100) + (f * 10) + g;
+            int valeurPositionHorizontale = anglePositionHorizontale * 4095 / 360;
+            wait_us(2000); // Attendre fin de la trame de réponse du Servo Vertical
+            //setPosition(ID_SERVO_HORIZONTALE, valeurPositionHorizontale);
+            //Pour Debug
+            serialPc.printf("idServ = %d \tvaleurPosition = %d\n",ID_SERVO_VERTICAL, valeurPositionVerticale);
+            serialPc.printf("idServ = %d \tvaleurPosition = %d\n",ID_SERVO_HORIZONTALE, valeurPositionHorizontale);
+        }
+        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++;
+            }
+            /*for(int i=0;i<8;i++) {
+            	octetRecuMX[i] = uartMX28.getc();
+            }
+            for(int i=0;i<8;i++) {
+            	serialPc.putc(octetRecuMX[i]);
+            }*/
+            int valeurLue = octetRecuMX[6] * 256 + octetRecuMX[5];
+            serialPc.printf("Valeur lue %d\n",valeurLue); ///////////Pour test A modifier !!!!!
+            wait(0.2);
+            led03 = 0; //
+        }
+        if ((bufferRec[0] == '2')&&(numeroOctetRecu == 3)) { // si c'est une commande Laser
+            if (bufferRec[1] == '1') {
+                //Laser on
+                led02 = 1; // indic commande Laser on
+                laser = 1;
+            } else {
+                //Laser off
+                led02 = 0; // indic commande Laser off
+                laser = 0;
+            }
+        }
+        if ((bufferRec[0] == '3')&&(numeroOctetRecu == 11)) { // si commande panneau led supérieur
+            led03 = 1; // indic commande panneau led supérieur
+            // A terminer
+            int b = bufferRec[1] - 0x30;
+            int c = bufferRec[2] - 0x30;
+            int d = bufferRec[3] - 0x30;
+            int e = bufferRec[4] - 0x30;
+            int f = bufferRec[5] - 0x30;
+            int g = bufferRec[6] - 0x30;
+            int h = bufferRec[7] - 0x30;
+            int i = bufferRec[8] - 0x30;
+            int j = bufferRec[9] - 0x30;
+			int colorLed1 = (b*100+c*10+d)*65536+(e*100+f*10+g)*256+(h*100+i*10+j);
+            panelLed1.clear();
+            for(int n=0;n<N1;n++) {
+				panelLed1.setPixel(n,colorLed1);
+			}
+            panelLed1.write();
+
+            wait(0.2);
+            led03 = 0; // indic commande panneau led supérieur
+        }
+        if ((bufferRec[0] == '4')&&(numeroOctetRecu == 11)) { // si commande panneau led latéral 1
+            led03 = 1; // indic commande panneau led
+            int b = bufferRec[1] - 0x30;
+            int c = bufferRec[2] - 0x30;
+            int d = bufferRec[3] - 0x30;
+            int e = bufferRec[4] - 0x30;
+            int f = bufferRec[5] - 0x30;
+            int g = bufferRec[6] - 0x30;
+            int h = bufferRec[7] - 0x30;
+            int i = bufferRec[8] - 0x30;
+            int j = bufferRec[9] - 0x30;
+			int valeurRouge = (b*100+c*10+d);
+			int valeurVerte = (e*100+f*10+g);
+			int valeurBleu = (h*100+i*10+j);
+			writeMatriceLed(2,N2,valeurRouge, valeurVerte, valeurBleu);
+            wait(0.2);
+            led03 = 0; // indic commande panneau led
+        }
+        if ((bufferRec[0] == '5')&&(numeroOctetRecu == 11)) { // si commande panneau led latéral 2
+            led03 = 1; // indic commande panneau led
+            int b = bufferRec[1] - 0x30;
+            int c = bufferRec[2] - 0x30;
+            int d = bufferRec[3] - 0x30;
+            int e = bufferRec[4] - 0x30;
+            int f = bufferRec[5] - 0x30;
+            int g = bufferRec[6] - 0x30;
+            int h = bufferRec[7] - 0x30;
+            int i = bufferRec[8] - 0x30;
+            int j = bufferRec[9] - 0x30;
+			int valeurRouge = (b*100+c*10+d);
+			int valeurVerte = (e*100+f*10+g);
+			int valeurBleu = (h*100+i*10+j);
+			writeMatriceLed(3,N3,valeurRouge, valeurVerte, valeurBleu);
+            wait(0.2);
+            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
+            // A Faire
+        }
+
+        numeroOctetRecu = 0;
+    } else {
+        numeroOctetRecu++;
+    }
+    led04 =0;
+}
+
+int main()
+{
+    uartMX28.baud(57142); // débit standard pour les MX-28 et -106
+    serialPc.baud(115200);
+    serialPc.attach(&receptionPc); // defini la fonction interruption
+    panelLed1.setBrightness(1);	// par défaut à 50%
+    //serialPc.printf("Entrer une commande\n");
+    led01 = 1;
+    // Clear buffer
+    memset(&bufferRec[0], 0, sizeof(bufferRec));
+
+    while(1) {
+        led01 = 0;
+        wait(0.1);
+        led01 = 1;
+        wait(0.1);
+    }
+}