File content as of revision 5:a81d551cfffa:

/************************************
*  DB le 28/05/2015  MX28-Scan3D_DB05-IRQ
*	Mesure de luminosité avec adafruit GA1A12S202 : Ok
*   Codage Accelerometre
*   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 10		// MX-106R
#define ID_SERVO_HORIZONTALE 1	// 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

#define NBRE_ACK 256 // nombre d'acquisition pour la moyenne sur mesure luminosité
#define NBRE_ACQ_ACC 100 // nombre d'acquisition pour la moyenne sur mesure accceleration

DigitalOut led01(LED1);
DigitalOut led02(LED2);
DigitalOut led03(LED3);
DigitalOut led04(LED4);

DigitalOut dir(p17);
DigitalOut laser(p5);
DigitalOut panelLed2(p7);
DigitalOut panelLed3(p8);
AnalogIn mesLux(p20); // Entrée pour mesure de luminosité

int valeurLux;
short valeurAccX;
short valeurAccY;
short valeurAccZ;
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

I2C i2cLsm303D(p9, p10);   // Création objet et affectation SDA1, SCL1 pour l'accéléromètre LSM303D
const int addrLsm303D = 0x3C; // adresse du LSM303D avec SDO/SA0=0

//---------------------------------------------------------------------------------------------
// Set Motor gain

//------------------------------------------------------------------------------------------------
// 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;
}

void setMotorGainSpeed(){
	char min = 1;
	char max = 2;
	write(ID_SERVO_VERTICAL, 5, MX28_WRITE_DATA, MX28_MOVING_SPEED_L, min, max);
	write(ID_SERVO_HORIZONTALE, 5, MX28_WRITE_DATA, MX28_MOVING_SPEED_L, min, max);
		
	serialPc.printf("gains set\n");
}


// 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;
		}
	}
}
// Mesure de la luminosité avec moyenne sur NBRE_ACK
int mesureDeLux() {
    float sommeMesure=0;
    int valL;
    for(int i=0; i<NBRE_ACK;i++) {
        sommeMesure += mesLux;
    }
    valL = ((sommeMesure*3.3*1000)/NBRE_ACK); // mesure en mV
    return valL;
}
short mesureAcc(char sub) {
	char dataLu[2];
	short valAcc=0;
	int sommeValAcc=0;
    for(int i=0; i<NBRE_ACQ_ACC;i++) {
		dataLu[0]=sub; // OUT_X_L_A ou Y ou Z, registre acc X,Y,Z
	    i2cLsm303D.write(addrLsm303D, dataLu, 1);
	    i2cLsm303D.read(addrLsm303D, dataLu, 1);
	    valAcc = dataLu[0];
	    dataLu[0]=sub+1; // OUT_X_H_A registre acc X
	    i2cLsm303D.write(addrLsm303D, dataLu, 1);
	    i2cLsm303D.read(addrLsm303D, dataLu, 1);
	    valAcc |= dataLu[0]<<8;
	    sommeValAcc += valAcc;
	}
    valAcc = sommeValAcc/NBRE_ACQ_ACC;
    int val = int (valAcc*2000)/32768;
    valAcc = val; //prendre la partie entière
    return valAcc;
}

//---------------------------------------------------------------------------------------------
// 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(3200); // 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(1); //pour laisser le temps au moteur de se déplacer, 
            
            int incr = 0;
            
            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;
            
            /*while ((abs(valeurAngleVertCentiemeDegres - anglePositionVerticale * 100) > 100
                    || abs(valeurAngleHorizontaleCentiemeDegres - anglePositionHorizontale * 100) > 100)
                && incr < 60)
                {
                    wait(1);
		            int valeurLue = lirePositionServo(ID_SERVO_VERTICAL);
		            valeurAngleVertCentiemeDegres = (valeurLue * 100) * 360/4096;
		            wait(0.2);
		            valeurLue = lirePositionServo(ID_SERVO_HORIZONTALE);
		            valeurAngleHorizontaleCentiemeDegres = (valeurLue * 100) * 360/4096;
                    incr++;
                    
                    //pc.printf(" Goal = %d ; Current = %d", goalHoriz, currentPositionHoriz);
                }*/
                       
            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
            serialPc.printf("AccX = %+-0#5d AccY = %+-0#5d AccZ = %+-0#5d\n",valeurAccX,valeurAccY,valeurAccZ);
        }
		if ((bufferRec[0] == '7')&&(numeroOctetRecu == 2)) { // si c'est une demande mesure luminosité
             
             serialPc.printf("$OK_lumiere_%0#4d/\n",valeurLux);
        }
        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));

    // Config Accelerometre
            char dataLu[2];
            dataLu[0]=0x20; // Registre CTRL1
            dataLu[1]=0x67; // Accelero ON et data rate = 100Hz
            i2cLsm303D.write(addrLsm303D, dataLu, 2);
            dataLu[0]=0x26; // Registre CTRL7
            dataLu[1]=0x82; // Accelero ON et data rate = 100Hz
            i2cLsm303D.write(addrLsm303D, dataLu, 2);

	// Gain moteurs
	setMotorGainSpeed();

    while(1) {
        led01 = 0;
        wait(0.1);
        led01 = 1;
        wait(0.1);
        valeurLux = mesureDeLux(); //mesure cyclique de luminosité
        valeurAccX = mesureAcc(0x28);
        valeurAccY = mesureAcc(0x2A);
        valeurAccZ = mesureAcc(0x2C);
    }
}