File content as of revision 2:43daa01ba06d:

/************************************
*  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
*   Réception Série par IRQ
*   Piloter les positions des 2 servomoteurs V et H
*   Débit = 57142bit/s
*
* 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 3		// MX-106R
#define ID_SERVO_HORIZONTALE 2	// MX-28R

#define N1 64 // Nombre de pixels(Led) du panneau Led 1
#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);
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);
}

//---------------------------------------------------------------------------------------------
// 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)
{
	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);
                         
			//--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
			int valeurLue = lirePositionServo(ID_SERVO_VERTICAL);
            int valeurAngleVertCentiemeDegres = (valeurLue * 100) * 360/4096;
            wait(0.2);
            valeurLue = lirePositionServo(ID_SERVO_HORIZONTALE);
            int valeurAngleHorizontaleCentiemeDegres = (valeurLue * 100) * 360/4096;

            serialPc.printf("$OK_moteurs_%0#5d_%0#5d/\n",valeurAngleVertCentiemeDegres, valeurAngleHorizontaleCentiemeDegres);

            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;
                serialPc.printf("$OK_laser_1/\n");
            } else {
                //Laser off
                led02 = 0; // indic commande Laser off
                laser = 0;
                serialPc.printf("$OK_laser_0/\n");
            }
        }
        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);
            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
            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);
            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
            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);
            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 ENVOI 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);
    }
}