Max Bismuth / Mbed 2 deprecated MX28-Scan3D_DB03-IRQ

Scan Mines

Dependencies:   NeoStrip mbed

Fork of MX28-Scan3D_DB03-IRQ by Denis Brousse

main.cpp@5:a81d551cfffa, 2015-06-03 (annotated)

Committer:
wumzi
Date:
Wed Jun 03 15:32:35 2015 +0000
Revision:
5:a81d551cfffa
Parent:
4:ca5bdb807fa1 
Modif pour tenter de setter vitesse

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dbrousse 0:129e8567ddbd 1 /************************************
dbrousse 3:a3074e835b35 2 * DB le 28/05/2015 MX28-Scan3D_DB05-IRQ
dbrousse 3:a3074e835b35 3 * Mesure de luminosité avec adafruit GA1A12S202 : Ok
dbrousse 3:a3074e835b35 4 * Codage Accelerometre
dbrousse 2:43daa01ba06d 5 * Programme final conforme au protocole de communication
dbrousse 2:43daa01ba06d 6 * Reste à faire : - lecture accéléromètre
dbrousse 2:43daa01ba06d 7 * - vérification de positions atteintes par servomoteurs
dbrousse 2:43daa01ba06d 8 *
dbrousse 2:43daa01ba06d 9 * Pilotage MX-106 (SERVO_VERTICAL) et MX-28 (SERVO_HORIZONTALE)
dbrousse 2:43daa01ba06d 10 * avec liaison série : Trame Scan3D
dbrousse 0:129e8567ddbd 11 * &abcdefghij/ début $, fin /, longueur variable
dbrousse 0:129e8567ddbd 12 * Réception Série par IRQ
dbrousse 0:129e8567ddbd 13 * Piloter les positions des 2 servomoteurs V et H
dbrousse 0:129e8567ddbd 14 * Débit = 57142bit/s
dbrousse 0:129e8567ddbd 15 *
dbrousse 2:43daa01ba06d 16 * Matrice Led Neopixels
dbrousse 0:129e8567ddbd 17 * Une seule instance possible avec la classe NeoStrip :
dbrousse 0:129e8567ddbd 18 * Problème, pour nous il faut 3 voies : panelLed1, panelLed2 et panelLed3 !
dbrousse 0:129e8567ddbd 19 * les 2 autres voies "faite à la main" avec les fonctions :
dbrousse 0:129e8567ddbd 20 * - void writeBitCode1(numMatriceLed) et void writeBitCode0(numMatriceLed)
dbrousse 0:129e8567ddbd 21 * - void writeMatriceLed(int numMatriceLed, int N, int valRouge, int valVerte, int valBleu)
dbrousse 0:129e8567ddbd 22 *
dbrousse 0:129e8567ddbd 23 * Cible : Carte mbed_S3DHQ_2015
dbrousse 0:129e8567ddbd 24 *************************************/
dbrousse 0:129e8567ddbd 25
dbrousse 0:129e8567ddbd 26 #include "mbed.h"
dbrousse 0:129e8567ddbd 27 #include "MX-28_DefConstantes.h"
dbrousse 0:129e8567ddbd 28 #include "NeoStrip.h"
dbrousse 0:129e8567ddbd 29
wumzi 5:a81d551cfffa 30 #define ID_SERVO_VERTICAL 10 // MX-106R
wumzi 5:a81d551cfffa 31 #define ID_SERVO_HORIZONTALE 1 // MX-28R
dbrousse 0:129e8567ddbd 32
dbrousse 0:129e8567ddbd 33 #define N1 64 // Nombre de pixels(Led) du panneau Led 1
dbrousse 2:43daa01ba06d 34 #define N2 128 // Nombre de pixels(Led) du panneau Led 2
dbrousse 2:43daa01ba06d 35 #define N3 128 // Nombre de pixels(Led) du panneau Led 3
dbrousse 0:129e8567ddbd 36
dbrousse 3:a3074e835b35 37 #define NBRE_ACK 256 // nombre d'acquisition pour la moyenne sur mesure luminosité
dbrousse 3:a3074e835b35 38 #define NBRE_ACQ_ACC 100 // nombre d'acquisition pour la moyenne sur mesure accceleration
dbrousse 3:a3074e835b35 39
dbrousse 0:129e8567ddbd 40 DigitalOut led01(LED1);
dbrousse 0:129e8567ddbd 41 DigitalOut led02(LED2);
dbrousse 0:129e8567ddbd 42 DigitalOut led03(LED3);
dbrousse 0:129e8567ddbd 43 DigitalOut led04(LED4);
dbrousse 0:129e8567ddbd 44
dbrousse 0:129e8567ddbd 45 DigitalOut dir(p17);
dbrousse 0:129e8567ddbd 46 DigitalOut laser(p5);
dbrousse 0:129e8567ddbd 47 DigitalOut panelLed2(p7);
dbrousse 0:129e8567ddbd 48 DigitalOut panelLed3(p8);
dbrousse 3:a3074e835b35 49 AnalogIn mesLux(p20); // Entrée pour mesure de luminosité
dbrousse 0:129e8567ddbd 50
dbrousse 3:a3074e835b35 51 int valeurLux;
dbrousse 3:a3074e835b35 52 short valeurAccX;
dbrousse 3:a3074e835b35 53 short valeurAccY;
dbrousse 3:a3074e835b35 54 short valeurAccZ;
dbrousse 0:129e8567ddbd 55 unsigned char numeroOctetRecu=0;
dbrousse 0:129e8567ddbd 56 char octetRecu; //premier octet reçu
dbrousse 0:129e8567ddbd 57 char bufferRec[12]; // buffer de réception serialPc
dbrousse 0:129e8567ddbd 58
dbrousse 0:129e8567ddbd 59 Serial serialPc(USBTX, USBRX); // tx, rx, écarté car driver incompatible avec LabVIEW
dbrousse 0:129e8567ddbd 60 //Serial serialPc(p28, p27); // tx, rx (c'est l'uart2 du LPC1768)
dbrousse 1:538b13cb6699 61 Serial uartMX28(p13, p14); // tx, rx pour MX28 (uart1 du LPC1768)
dbrousse 1:538b13cb6699 62 //Serial uartMX28(p28, p27); // tx, rx pour MX28 (uart1 du LPC1768)
dbrousse 0:129e8567ddbd 63
dbrousse 0:129e8567ddbd 64 NeoStrip panelLed1(p6, N1); // creation de l'objet panelLed1 cde par p6, N1 Led
dbrousse 0:129e8567ddbd 65 //NeoStrip panelLed2(p7, N2); // creation de l'objet panelLed2 cde par p7, N2 Led
dbrousse 0:129e8567ddbd 66 //NeoStrip panelLed3(p8, N3); // creation de l'objet panelLed3 cde par p8, N3 Led
dbrousse 0:129e8567ddbd 67
dbrousse 3:a3074e835b35 68 I2C i2cLsm303D(p9, p10); // Création objet et affectation SDA1, SCL1 pour l'accéléromètre LSM303D
dbrousse 3:a3074e835b35 69 const int addrLsm303D = 0x3C; // adresse du LSM303D avec SDO/SA0=0
dbrousse 3:a3074e835b35 70
wumzi 5:a81d551cfffa 71 //---------------------------------------------------------------------------------------------
wumzi 5:a81d551cfffa 72 // Set Motor gain
wumzi 5:a81d551cfffa 73
dbrousse 0:129e8567ddbd 74 //------------------------------------------------------------------------------------------------
dbrousse 0:129e8567ddbd 75 // Envoi de la trame de pilotage a un servomoteur MX-28
dbrousse 0:129e8567ddbd 76 void write (char id, char longueurTrame, char instruction, char param1 = NULL, char param2 = NULL, char param3 = NULL, char param4 = NULL)
dbrousse 0:129e8567ddbd 77 {
dbrousse 0:129e8567ddbd 78 char Cks;
dbrousse 0:129e8567ddbd 79
dbrousse 0:129e8567ddbd 80 Cks = ~( id + longueurTrame + instruction + param1 + param2 + param3 + param4); //calcul du checkSum
dbrousse 0:129e8567ddbd 81 //serialPc.printf("Cks : %d\n", Cks);
dbrousse 0:129e8567ddbd 82 dir = 1;
dbrousse 0:129e8567ddbd 83 uartMX28.putc(0xFF);
dbrousse 0:129e8567ddbd 84 uartMX28.putc(0xFF);
dbrousse 0:129e8567ddbd 85 uartMX28.putc(id);
dbrousse 0:129e8567ddbd 86 uartMX28.putc(longueurTrame);
dbrousse 0:129e8567ddbd 87 uartMX28.putc(instruction);
dbrousse 0:129e8567ddbd 88 if (longueurTrame >= 3) {
dbrousse 0:129e8567ddbd 89 uartMX28.putc(param1);
dbrousse 0:129e8567ddbd 90 }
dbrousse 0:129e8567ddbd 91 if (longueurTrame >= 4) {
dbrousse 0:129e8567ddbd 92 uartMX28.putc(param2);
dbrousse 0:129e8567ddbd 93 }
dbrousse 0:129e8567ddbd 94 if (longueurTrame >= 5) {
dbrousse 0:129e8567ddbd 95 uartMX28.putc(param3);
dbrousse 0:129e8567ddbd 96 }
dbrousse 0:129e8567ddbd 97 if (longueurTrame >= 6) {
dbrousse 0:129e8567ddbd 98 uartMX28.putc(param4);
dbrousse 0:129e8567ddbd 99 }
dbrousse 0:129e8567ddbd 100 uartMX28.putc(Cks);
dbrousse 0:129e8567ddbd 101
dbrousse 0:129e8567ddbd 102 wait_us(MX28_WAIT_AFTER_WRITE); // Attendre l'envoie complet de la trame.
dbrousse 0:129e8567ddbd 103 dir = 0;
dbrousse 0:129e8567ddbd 104 }
dbrousse 0:129e8567ddbd 105
wumzi 5:a81d551cfffa 106 void setMotorGainSpeed(){
wumzi 5:a81d551cfffa 107 char min = 1;
wumzi 5:a81d551cfffa 108 char max = 2;
wumzi 5:a81d551cfffa 109 write(ID_SERVO_VERTICAL, 5, MX28_WRITE_DATA, MX28_MOVING_SPEED_L, min, max);
wumzi 5:a81d551cfffa 110 write(ID_SERVO_HORIZONTALE, 5, MX28_WRITE_DATA, MX28_MOVING_SPEED_L, min, max);
wumzi 5:a81d551cfffa 111
wumzi 5:a81d551cfffa 112 serialPc.printf("gains set\n");
wumzi 5:a81d551cfffa 113 }
wumzi 5:a81d551cfffa 114
wumzi 5:a81d551cfffa 115
dbrousse 0:129e8567ddbd 116 // Set goal position of engine
dbrousse 0:129e8567ddbd 117 void setPosition(char id, int goal)
dbrousse 0:129e8567ddbd 118 {
dbrousse 0:129e8567ddbd 119 char goal_h = goal >> 8;
dbrousse 0:129e8567ddbd 120 char goal_l = goal;
dbrousse 0:129e8567ddbd 121 //serialPc.printf("Goal set : %d %d %d\n", goal, goal_h, goal_l);
dbrousse 0:129e8567ddbd 122 write(id, 5, MX28_WRITE_DATA, MX28_GOAL_POSITION_L, goal_l, goal_h);
dbrousse 0:129e8567ddbd 123 }
dbrousse 0:129e8567ddbd 124
dbrousse 0:129e8567ddbd 125 //---------------------------------------------------------------------------------------------
dbrousse 2:43daa01ba06d 126 // Lire position d'un servomoteur MX-28 ou MX-106
dbrousse 2:43daa01ba06d 127 int lirePositionServo(char idServo) {
dbrousse 2:43daa01ba06d 128 //Vider le buffer de réception uartMX28
dbrousse 2:43daa01ba06d 129 while(uartMX28.readable()) {
dbrousse 2:43daa01ba06d 130 uartMX28.getc();
dbrousse 2:43daa01ba06d 131 }
dbrousse 2:43daa01ba06d 132 write(idServo, 4, MX28_READ_DATA, MX28_PRESENT_POSITION_L,2);
dbrousse 2:43daa01ba06d 133 char octetRecuMX[8];
dbrousse 2:43daa01ba06d 134 char i=0;
dbrousse 2:43daa01ba06d 135 int tempoReception=0;
dbrousse 2:43daa01ba06d 136 while((tempoReception < 1000000)&&(i<8)) {
dbrousse 2:43daa01ba06d 137 if(uartMX28.readable()) {
dbrousse 2:43daa01ba06d 138 octetRecuMX[i] = uartMX28.getc();
dbrousse 2:43daa01ba06d 139 i++;
dbrousse 2:43daa01ba06d 140 }
dbrousse 2:43daa01ba06d 141 tempoReception++;
dbrousse 2:43daa01ba06d 142 }
dbrousse 2:43daa01ba06d 143 int valeurLue = octetRecuMX[6] * 256 + octetRecuMX[5];
dbrousse 2:43daa01ba06d 144 return valeurLue;
dbrousse 2:43daa01ba06d 145 }
dbrousse 2:43daa01ba06d 146
dbrousse 2:43daa01ba06d 147 //---------------------------------------------------------------------------------------------
dbrousse 0:129e8567ddbd 148 // fonction écriture Bit code 1 et 0 pour matrice Led Neopixels
dbrousse 0:129e8567ddbd 149 void writeBitCode1(int numMatriceLed)
dbrousse 0:129e8567ddbd 150 {
dbrousse 0:129e8567ddbd 151 if(numMatriceLed==2){panelLed2=1;}
dbrousse 0:129e8567ddbd 152 else{panelLed3=1;}
dbrousse 0:129e8567ddbd 153 for(int i=0;i<37;i++) {int x = i*i*i; x=x*x;} // pour T1H=700ns
dbrousse 0:129e8567ddbd 154 if(numMatriceLed==2){panelLed2=0;}
dbrousse 0:129e8567ddbd 155 else{panelLed3=0;}
dbrousse 0:129e8567ddbd 156 for(int i=0;i<16;i++) {int x = i*i*i; x=x*x;} // pour T1L= 450ns + 150ns pour exe fonction
dbrousse 0:129e8567ddbd 157 }
dbrousse 0:129e8567ddbd 158 void writeBitCode0(int numMatriceLed)
dbrousse 0:129e8567ddbd 159 {
dbrousse 0:129e8567ddbd 160 if(numMatriceLed==2){panelLed2=1;}
dbrousse 0:129e8567ddbd 161 else{panelLed3=1;}
dbrousse 0:129e8567ddbd 162 for(int i=0;i<15;i++) {int x = i*i*i; x=x*x;} // pour T0H=350ns
dbrousse 0:129e8567ddbd 163 if(numMatriceLed==2){panelLed2=0;}
dbrousse 0:129e8567ddbd 164 else{panelLed3=0;}
dbrousse 0:129e8567ddbd 165 for(int i=0;i<32;i++) {int x = i*i*i; x=x*x;} // pour T0L=650ns+150ns pour exe fonction
dbrousse 0:129e8567ddbd 166 }
dbrousse 0:129e8567ddbd 167 //---------------------------------------------------------------------------------------------
dbrousse 0:129e8567ddbd 168 // fonction écriture matrice Led Neopixels
dbrousse 0:129e8567ddbd 169 void writeMatriceLed(int numMatriceLed, int N, int valRouge, int valVerte, int valBleu)
dbrousse 0:129e8567ddbd 170 {
dbrousse 0:129e8567ddbd 171 int valeurCouleurVRB = valVerte*65536+valRouge*256+valBleu;
dbrousse 0:129e8567ddbd 172 for(int i=0;i<N;i++) {
dbrousse 0:129e8567ddbd 173 int mask_1sur24Bits = 0x800000;
dbrousse 0:129e8567ddbd 174 for(int j=0;j<24;j++) {
dbrousse 0:129e8567ddbd 175 if(valeurCouleurVRB & mask_1sur24Bits) {
dbrousse 0:129e8567ddbd 176 writeBitCode1(numMatriceLed);
dbrousse 0:129e8567ddbd 177 }
dbrousse 0:129e8567ddbd 178 else {
dbrousse 0:129e8567ddbd 179 writeBitCode0(numMatriceLed);
dbrousse 0:129e8567ddbd 180 }
dbrousse 0:129e8567ddbd 181 mask_1sur24Bits = mask_1sur24Bits >> 1;
dbrousse 0:129e8567ddbd 182 }
dbrousse 0:129e8567ddbd 183 }
dbrousse 0:129e8567ddbd 184 }
dbrousse 3:a3074e835b35 185 // Mesure de la luminosité avec moyenne sur NBRE_ACK
dbrousse 3:a3074e835b35 186 int mesureDeLux() {
dbrousse 3:a3074e835b35 187 float sommeMesure=0;
dbrousse 3:a3074e835b35 188 int valL;
dbrousse 3:a3074e835b35 189 for(int i=0; i<NBRE_ACK;i++) {
dbrousse 3:a3074e835b35 190 sommeMesure += mesLux;
dbrousse 3:a3074e835b35 191 }
dbrousse 3:a3074e835b35 192 valL = ((sommeMesure*3.3*1000)/NBRE_ACK); // mesure en mV
dbrousse 3:a3074e835b35 193 return valL;
dbrousse 3:a3074e835b35 194 }
dbrousse 3:a3074e835b35 195 short mesureAcc(char sub) {
dbrousse 3:a3074e835b35 196 char dataLu[2];
dbrousse 3:a3074e835b35 197 short valAcc=0;
dbrousse 3:a3074e835b35 198 int sommeValAcc=0;
dbrousse 3:a3074e835b35 199 for(int i=0; i<NBRE_ACQ_ACC;i++) {
dbrousse 3:a3074e835b35 200 dataLu[0]=sub; // OUT_X_L_A ou Y ou Z, registre acc X,Y,Z
dbrousse 3:a3074e835b35 201 i2cLsm303D.write(addrLsm303D, dataLu, 1);
dbrousse 3:a3074e835b35 202 i2cLsm303D.read(addrLsm303D, dataLu, 1);
dbrousse 3:a3074e835b35 203 valAcc = dataLu[0];
dbrousse 3:a3074e835b35 204 dataLu[0]=sub+1; // OUT_X_H_A registre acc X
dbrousse 3:a3074e835b35 205 i2cLsm303D.write(addrLsm303D, dataLu, 1);
dbrousse 3:a3074e835b35 206 i2cLsm303D.read(addrLsm303D, dataLu, 1);
dbrousse 3:a3074e835b35 207 valAcc |= dataLu[0]<<8;
dbrousse 3:a3074e835b35 208 sommeValAcc += valAcc;
dbrousse 3:a3074e835b35 209 }
dbrousse 3:a3074e835b35 210 valAcc = sommeValAcc/NBRE_ACQ_ACC;
dbrousse 3:a3074e835b35 211 int val = int (valAcc*2000)/32768;
dbrousse 3:a3074e835b35 212 valAcc = val; //prendre la partie entière
dbrousse 3:a3074e835b35 213 return valAcc;
dbrousse 3:a3074e835b35 214 }
dbrousse 0:129e8567ddbd 215
dbrousse 0:129e8567ddbd 216 //---------------------------------------------------------------------------------------------
dbrousse 0:129e8567ddbd 217 // fonction appelée par interruption si réception sur serialPc
dbrousse 0:129e8567ddbd 218 void receptionPc()
dbrousse 0:129e8567ddbd 219 {
dbrousse 0:129e8567ddbd 220 led04 =1;
dbrousse 0:129e8567ddbd 221 octetRecu = serialPc.getc();
dbrousse 0:129e8567ddbd 222 if(octetRecu == '$') {
dbrousse 0:129e8567ddbd 223 numeroOctetRecu = 0;
dbrousse 0:129e8567ddbd 224 memset(&bufferRec[0], 0, sizeof(bufferRec));
dbrousse 0:129e8567ddbd 225 } else {
dbrousse 0:129e8567ddbd 226 bufferRec[numeroOctetRecu-1] = octetRecu;
dbrousse 0:129e8567ddbd 227 ///Debug serialPc.printf("%c",octetRecu); ////////
dbrousse 0:129e8567ddbd 228 }
dbrousse 0:129e8567ddbd 229 if(octetRecu == '/') {
dbrousse 0:129e8567ddbd 230 if ((bufferRec[0] == '0')&&(numeroOctetRecu == 8)) { // si c'est une commande de position MX28+MX106
dbrousse 0:129e8567ddbd 231 int b = bufferRec[1] - 0x30;
dbrousse 0:129e8567ddbd 232 int c = bufferRec[2] - 0x30;
dbrousse 0:129e8567ddbd 233 int d = bufferRec[3] - 0x30;
dbrousse 0:129e8567ddbd 234 int e = bufferRec[4] - 0x30;
dbrousse 0:129e8567ddbd 235 int f = bufferRec[5] - 0x30;
dbrousse 0:129e8567ddbd 236 int g = bufferRec[6] - 0x30;
dbrousse 0:129e8567ddbd 237 int anglePositionVerticale = (b * 100) + (c * 10) + d;
dbrousse 0:129e8567ddbd 238 int valeurPositionVerticale = anglePositionVerticale * 4095 / 360;
dbrousse 0:129e8567ddbd 239 setPosition(ID_SERVO_VERTICAL, valeurPositionVerticale);
dbrousse 0:129e8567ddbd 240 int anglePositionHorizontale = (e * 100) + (f * 10) + g;
dbrousse 0:129e8567ddbd 241 int valeurPositionHorizontale = anglePositionHorizontale * 4095 / 360;
dbrousse 3:a3074e835b35 242 wait_us(3200); // Attendre fin de la trame de réponse du Servo Vertical
dbrousse 1:538b13cb6699 243 setPosition(ID_SERVO_HORIZONTALE, valeurPositionHorizontale);
dbrousse 0:129e8567ddbd 244 //Pour Debug
dbrousse 1:538b13cb6699 245 //serialPc.printf("idServ = %d \tvaleurPosition = %d\n",ID_SERVO_VERTICAL, valeurPositionVerticale);
dbrousse 1:538b13cb6699 246 //serialPc.printf("idServ = %d \tvaleurPosition = %d\n",ID_SERVO_HORIZONTALE, valeurPositionHorizontale);
dbrousse 2:43daa01ba06d 247
dbrousse 2:43daa01ba06d 248 //--Solution simple pour retourner la position mais sans tester "positions demandéess=atteintes"
dbrousse 2:43daa01ba06d 249 // test positions atteintes à faire
wumzi 5:a81d551cfffa 250 wait(1); //pour laisser le temps au moteur de se déplacer,
wumzi 5:a81d551cfffa 251
wumzi 5:a81d551cfffa 252 int incr = 0;
wumzi 5:a81d551cfffa 253
wumzi 5:a81d551cfffa 254 int valeurLue = lirePositionServo(ID_SERVO_VERTICAL);
wumzi 5:a81d551cfffa 255 int valeurAngleVertCentiemeDegres = (valeurLue * 100) * 360/4096;
wumzi 5:a81d551cfffa 256 wait(0.2);
wumzi 5:a81d551cfffa 257 valeurLue = lirePositionServo(ID_SERVO_HORIZONTALE);
wumzi 5:a81d551cfffa 258 int valeurAngleHorizontaleCentiemeDegres = (valeurLue * 100) * 360/4096;
wumzi 5:a81d551cfffa 259
wumzi 5:a81d551cfffa 260 /*while ((abs(valeurAngleVertCentiemeDegres - anglePositionVerticale * 100) > 100
wumzi 5:a81d551cfffa 261 || abs(valeurAngleHorizontaleCentiemeDegres - anglePositionHorizontale * 100) > 100)
wumzi 5:a81d551cfffa 262 && incr < 60)
wumzi 5:a81d551cfffa 263 {
wumzi 5:a81d551cfffa 264 wait(1);
wumzi 5:a81d551cfffa 265 int valeurLue = lirePositionServo(ID_SERVO_VERTICAL);
wumzi 5:a81d551cfffa 266 valeurAngleVertCentiemeDegres = (valeurLue * 100) * 360/4096;
wumzi 5:a81d551cfffa 267 wait(0.2);
wumzi 5:a81d551cfffa 268 valeurLue = lirePositionServo(ID_SERVO_HORIZONTALE);
wumzi 5:a81d551cfffa 269 valeurAngleHorizontaleCentiemeDegres = (valeurLue * 100) * 360/4096;
wumzi 5:a81d551cfffa 270 incr++;
wumzi 5:a81d551cfffa 271
wumzi 5:a81d551cfffa 272 //pc.printf(" Goal = %d ; Current = %d", goalHoriz, currentPositionHoriz);
wumzi 5:a81d551cfffa 273 }*/
wumzi 5:a81d551cfffa 274
dbrousse 2:43daa01ba06d 275 bufferRec[0] = '1'; //pour retourner la mesure de la position
dbrousse 2:43daa01ba06d 276 numeroOctetRecu = 2; //pour retourner la mesure de la position
dbrousse 0:129e8567ddbd 277 }
dbrousse 0:129e8567ddbd 278 if ((bufferRec[0] == '1')&&(numeroOctetRecu == 2)) { // si demande de lecture de la position
dbrousse 0:129e8567ddbd 279 led03 = 1; // indic
dbrousse 2:43daa01ba06d 280 int valeurLue = lirePositionServo(ID_SERVO_VERTICAL);
dbrousse 1:538b13cb6699 281 int valeurAngleVertCentiemeDegres = (valeurLue * 100) * 360/4096;
dbrousse 1:538b13cb6699 282 wait(0.2);
dbrousse 2:43daa01ba06d 283 valeurLue = lirePositionServo(ID_SERVO_HORIZONTALE);
dbrousse 1:538b13cb6699 284 int valeurAngleHorizontaleCentiemeDegres = (valeurLue * 100) * 360/4096;
wumzi 5:a81d551cfffa 285
wumzi 5:a81d551cfffa 286
dbrousse 2:43daa01ba06d 287 serialPc.printf("$OK_moteurs_%0#5d_%0#5d/\n",valeurAngleVertCentiemeDegres, valeurAngleHorizontaleCentiemeDegres);
dbrousse 1:538b13cb6699 288
dbrousse 0:129e8567ddbd 289 led03 = 0; //
dbrousse 0:129e8567ddbd 290 }
dbrousse 0:129e8567ddbd 291 if ((bufferRec[0] == '2')&&(numeroOctetRecu == 3)) { // si c'est une commande Laser
dbrousse 0:129e8567ddbd 292 if (bufferRec[1] == '1') {
dbrousse 0:129e8567ddbd 293 //Laser on
dbrousse 0:129e8567ddbd 294 led02 = 1; // indic commande Laser on
dbrousse 0:129e8567ddbd 295 laser = 1;
dbrousse 2:43daa01ba06d 296 serialPc.printf("$OK_laser_1/\n");
dbrousse 0:129e8567ddbd 297 } else {
dbrousse 0:129e8567ddbd 298 //Laser off
dbrousse 0:129e8567ddbd 299 led02 = 0; // indic commande Laser off
dbrousse 0:129e8567ddbd 300 laser = 0;
dbrousse 2:43daa01ba06d 301 serialPc.printf("$OK_laser_0/\n");
dbrousse 0:129e8567ddbd 302 }
dbrousse 0:129e8567ddbd 303 }
dbrousse 0:129e8567ddbd 304 if ((bufferRec[0] == '3')&&(numeroOctetRecu == 11)) { // si commande panneau led supérieur
dbrousse 0:129e8567ddbd 305 led03 = 1; // indic commande panneau led supérieur
dbrousse 0:129e8567ddbd 306 // A terminer
dbrousse 0:129e8567ddbd 307 int b = bufferRec[1] - 0x30;
dbrousse 0:129e8567ddbd 308 int c = bufferRec[2] - 0x30;
dbrousse 0:129e8567ddbd 309 int d = bufferRec[3] - 0x30;
dbrousse 0:129e8567ddbd 310 int e = bufferRec[4] - 0x30;
dbrousse 0:129e8567ddbd 311 int f = bufferRec[5] - 0x30;
dbrousse 0:129e8567ddbd 312 int g = bufferRec[6] - 0x30;
dbrousse 0:129e8567ddbd 313 int h = bufferRec[7] - 0x30;
dbrousse 0:129e8567ddbd 314 int i = bufferRec[8] - 0x30;
dbrousse 0:129e8567ddbd 315 int j = bufferRec[9] - 0x30;
dbrousse 0:129e8567ddbd 316 int colorLed1 = (b*100+c*10+d)*65536+(e*100+f*10+g)*256+(h*100+i*10+j);
dbrousse 0:129e8567ddbd 317 panelLed1.clear();
dbrousse 0:129e8567ddbd 318 for(int n=0;n<N1;n++) {
dbrousse 0:129e8567ddbd 319 panelLed1.setPixel(n,colorLed1);
dbrousse 0:129e8567ddbd 320 }
dbrousse 0:129e8567ddbd 321 panelLed1.write();
dbrousse 0:129e8567ddbd 322
dbrousse 0:129e8567ddbd 323 wait(0.2);
dbrousse 2:43daa01ba06d 324 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]);
dbrousse 0:129e8567ddbd 325 led03 = 0; // indic commande panneau led supérieur
dbrousse 0:129e8567ddbd 326 }
dbrousse 0:129e8567ddbd 327 if ((bufferRec[0] == '4')&&(numeroOctetRecu == 11)) { // si commande panneau led latéral 1
dbrousse 0:129e8567ddbd 328 led03 = 1; // indic commande panneau led
dbrousse 0:129e8567ddbd 329 int b = bufferRec[1] - 0x30;
dbrousse 0:129e8567ddbd 330 int c = bufferRec[2] - 0x30;
dbrousse 0:129e8567ddbd 331 int d = bufferRec[3] - 0x30;
dbrousse 0:129e8567ddbd 332 int e = bufferRec[4] - 0x30;
dbrousse 0:129e8567ddbd 333 int f = bufferRec[5] - 0x30;
dbrousse 0:129e8567ddbd 334 int g = bufferRec[6] - 0x30;
dbrousse 0:129e8567ddbd 335 int h = bufferRec[7] - 0x30;
dbrousse 0:129e8567ddbd 336 int i = bufferRec[8] - 0x30;
dbrousse 0:129e8567ddbd 337 int j = bufferRec[9] - 0x30;
dbrousse 0:129e8567ddbd 338 int valeurRouge = (b*100+c*10+d);
dbrousse 0:129e8567ddbd 339 int valeurVerte = (e*100+f*10+g);
dbrousse 0:129e8567ddbd 340 int valeurBleu = (h*100+i*10+j);
dbrousse 0:129e8567ddbd 341 writeMatriceLed(2,N2,valeurRouge, valeurVerte, valeurBleu);
dbrousse 0:129e8567ddbd 342 wait(0.2);
dbrousse 2:43daa01ba06d 343 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]);
dbrousse 0:129e8567ddbd 344 led03 = 0; // indic commande panneau led
dbrousse 0:129e8567ddbd 345 }
dbrousse 0:129e8567ddbd 346 if ((bufferRec[0] == '5')&&(numeroOctetRecu == 11)) { // si commande panneau led latéral 2
dbrousse 0:129e8567ddbd 347 led03 = 1; // indic commande panneau led
dbrousse 0:129e8567ddbd 348 int b = bufferRec[1] - 0x30;
dbrousse 0:129e8567ddbd 349 int c = bufferRec[2] - 0x30;
dbrousse 0:129e8567ddbd 350 int d = bufferRec[3] - 0x30;
dbrousse 0:129e8567ddbd 351 int e = bufferRec[4] - 0x30;
dbrousse 0:129e8567ddbd 352 int f = bufferRec[5] - 0x30;
dbrousse 0:129e8567ddbd 353 int g = bufferRec[6] - 0x30;
dbrousse 0:129e8567ddbd 354 int h = bufferRec[7] - 0x30;
dbrousse 0:129e8567ddbd 355 int i = bufferRec[8] - 0x30;
dbrousse 0:129e8567ddbd 356 int j = bufferRec[9] - 0x30;
dbrousse 0:129e8567ddbd 357 int valeurRouge = (b*100+c*10+d);
dbrousse 0:129e8567ddbd 358 int valeurVerte = (e*100+f*10+g);
dbrousse 0:129e8567ddbd 359 int valeurBleu = (h*100+i*10+j);
dbrousse 0:129e8567ddbd 360 writeMatriceLed(3,N3,valeurRouge, valeurVerte, valeurBleu);
dbrousse 0:129e8567ddbd 361 wait(0.2);
dbrousse 2:43daa01ba06d 362 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]);
dbrousse 0:129e8567ddbd 363 led03 = 0; // indic commande panneau led
dbrousse 0:129e8567ddbd 364 }
dbrousse 0:129e8567ddbd 365 if ((bufferRec[0] == '6')&&(numeroOctetRecu == 2)) { // si c'est une demande mesure accéléromètre
dbrousse 2:43daa01ba06d 366 //LECTURE ET ENVOI des mesures d'accélérations
dbrousse 0:129e8567ddbd 367 // A Faire
dbrousse 3:a3074e835b35 368 serialPc.printf("AccX = %+-0#5d AccY = %+-0#5d AccZ = %+-0#5d\n",valeurAccX,valeurAccY,valeurAccZ);
dbrousse 0:129e8567ddbd 369 }
dbrousse 3:a3074e835b35 370 if ((bufferRec[0] == '7')&&(numeroOctetRecu == 2)) { // si c'est une demande mesure luminosité
dbrousse 3:a3074e835b35 371
dbrousse 3:a3074e835b35 372 serialPc.printf("$OK_lumiere_%0#4d/\n",valeurLux);
dbrousse 3:a3074e835b35 373 }
dbrousse 0:129e8567ddbd 374 numeroOctetRecu = 0;
dbrousse 0:129e8567ddbd 375 } else {
dbrousse 0:129e8567ddbd 376 numeroOctetRecu++;
dbrousse 0:129e8567ddbd 377 }
dbrousse 0:129e8567ddbd 378 led04 =0;
dbrousse 0:129e8567ddbd 379 }
dbrousse 0:129e8567ddbd 380
dbrousse 0:129e8567ddbd 381 int main()
dbrousse 0:129e8567ddbd 382 {
dbrousse 0:129e8567ddbd 383 uartMX28.baud(57142); // débit standard pour les MX-28 et -106
dbrousse 0:129e8567ddbd 384 serialPc.baud(115200);
dbrousse 0:129e8567ddbd 385 serialPc.attach(&receptionPc); // defini la fonction interruption
dbrousse 0:129e8567ddbd 386 panelLed1.setBrightness(1); // par défaut à 50%
dbrousse 1:538b13cb6699 387 //serialPc.printf("Entrer une commande\n");
dbrousse 0:129e8567ddbd 388 led01 = 1;
dbrousse 0:129e8567ddbd 389 // Clear buffer
dbrousse 0:129e8567ddbd 390 memset(&bufferRec[0], 0, sizeof(bufferRec));
dbrousse 0:129e8567ddbd 391
dbrousse 3:a3074e835b35 392 // Config Accelerometre
dbrousse 3:a3074e835b35 393 char dataLu[2];
dbrousse 3:a3074e835b35 394 dataLu[0]=0x20; // Registre CTRL1
dbrousse 3:a3074e835b35 395 dataLu[1]=0x67; // Accelero ON et data rate = 100Hz
dbrousse 3:a3074e835b35 396 i2cLsm303D.write(addrLsm303D, dataLu, 2);
dbrousse 3:a3074e835b35 397 dataLu[0]=0x26; // Registre CTRL7
dbrousse 3:a3074e835b35 398 dataLu[1]=0x82; // Accelero ON et data rate = 100Hz
dbrousse 3:a3074e835b35 399 i2cLsm303D.write(addrLsm303D, dataLu, 2);
dbrousse 3:a3074e835b35 400
wumzi 5:a81d551cfffa 401 // Gain moteurs
wumzi 5:a81d551cfffa 402 setMotorGainSpeed();
wumzi 5:a81d551cfffa 403
dbrousse 0:129e8567ddbd 404 while(1) {
dbrousse 0:129e8567ddbd 405 led01 = 0;
dbrousse 0:129e8567ddbd 406 wait(0.1);
dbrousse 0:129e8567ddbd 407 led01 = 1;
dbrousse 0:129e8567ddbd 408 wait(0.1);
dbrousse 3:a3074e835b35 409 valeurLux = mesureDeLux(); //mesure cyclique de luminosité
dbrousse 3:a3074e835b35 410 valeurAccX = mesureAcc(0x28);
dbrousse 3:a3074e835b35 411 valeurAccY = mesureAcc(0x2A);
dbrousse 3:a3074e835b35 412 valeurAccZ = mesureAcc(0x2C);
dbrousse 0:129e8567ddbd 413 }
dbrousse 0:129e8567ddbd 414 }