Diff: main.cpp

Revision:

3:96e8e5affaad

Parent:

2:3bb4a324ecf7

Child:

4:6212d7723c16

diff -r 3bb4a324ecf7 -r 96e8e5affaad main.cpp
--- a/main.cpp	Wed Jun 24 14:54:38 2020 +0000
+++ b/main.cpp	Mon Jul 20 07:14:12 2020 +0000
@@ -1,12 +1,27 @@
 #include "mbed.h"
 #include "glibr.h"
+#include <RawSerial.h>
 
-#define SR 0   // 0 = CAN | 1 = serie
+#define SR 1   // 0 = CAN | 1 = serie
+#define DEBUG_COM  1     // 0 = pas de debug | 1 = print de toutes les tramme serie recue et envoyées
+#define DEBUG_SENS 0    // 0 = pas de debug | 1 = print de la couleur (fait pour tester le capteur en lui même)
+
+#if ((DEBUG_COM==1)||(DEBUG_SENS==1))
+#define DEBUG_GENERAL 1
+#else 
+#define DEBUG_GENERAL 0
+#endif
+
+#if (SR==0)
+#define OFFSET_DATA 2 // format CAN
+#else 
+#define OFFSET_DATA 7 // format SR
+#endif 
 
 // Adress
-#define data_adress_sensor  0x00        // Specific to each sensor (between 0x00 and 0xFD)
-#define data_adress_general 0xFF        // will talk to every sensor
-#define adress_color_sensor 0x4B0       // CAN ID same for every sensor (only for CAN)
+#define data_adress_sensor  0x00    // pID specifique a chaque capteur (between 0x00 and 0xFD)
+#define data_adress_general 0xFF    // pID génerale pour communiquer avec tout les capteurs
+#define adress_color_sensor 0x4B0   // CAN ID pareil pour tout les capteur (seulement pour le CAN)
 
 // Request commands
 #define send_RGB       0x00
@@ -29,11 +44,10 @@
 #define SR_MAX     1024
 
 Serial USB_link(USBTX, USBRX);     // USB initialization
-RawSerial tx(PA_9, NC);
-RawSerial rx(NC, PA_10);
-glibr capt1(D4,D5);// I²C initialization : D4 = SDA ; D5 = SCL
+RawSerial sr(PA_9,PA_10);   // SR: PA_9 = TX ; PA_10 = RX  
+glibr capt1(PB_7,PB_6);      // I²C initialization : PB_7 = SDA ; PB_6 = SCL
 CAN can(PA_11, PA_12);      
-PwmOut LED(D9);                   // LED initialization
+PwmOut LED(D9);              
 
 // Buffer CAN
 CANMessage canBuffer[CAN_MAX];
@@ -53,8 +67,8 @@
 uint8_t lenSR = 0, idSR,cmdSR, dataSR=0;
 
 // tramme retour
-char messageSR[15], messageCAN[8];
-int lenRetour = 0;
+char message[15], Len;
+int DataLen = 0;
 
 // capteur et traitement
 uint16_t r,g,b ; // RGB values in 2 bytes
@@ -62,37 +76,46 @@
 char proximity_tresh = 250, color;
 char state;
 
+// PROTOTYPE DE FONCTIONS // 
+/** Fonction initialisant les fréquence et le capteur APDS9960
+ *  @return true si pas d'erreur, false sinon 
+ */
 bool initialization(void);
-/* Fonction initialisant les fréquence et le capteur APDS9960*/
 
+/** Fonction récéptionnant la tramme serie
+ *  et la stockant dans un buffer en attendant traitement
+ */
 void srRead();
-/* Fonction récéptionnant la tramme serie
-et la stockant dans un buffer en attendant traitement */
 
+/** Fonction decryptant la tramme serie recue dans le buffer
+ */
 void srTraitement();
-/* Fonction decryptant la tramme serie recue dans le buffer */
 
-void envoiSR(char commande, char data);
-/* Fonction envoyant une tramme serie au format herkulex */
+/** Fonction receptionnant les messaages CAN dans un Buffer
+ */
+void canRead();
 
-void canRead();
-/* Fonction receptionnant les messaages CAN dans un Buffer */
+/** Fonction traitant le message et envoyant la reponse au format herkulex ou en simple message can
+ *  @param commande : la commande du message recu (voir table com) 
+ *  @param data : la data du message recu si il y en a 
+ */
+void envoi(char commande, char data);
 
-void envoiCAN(const CANMessage &msg);
-/* Fonction traitant le message CAN reçu et repondant si besoin */
+
 
 bool initialization(void)
 {
+#if DEBUG_GENERAL
     // baud init
     USB_link.baud(115200);
-    USB_link.printf("Debut prog\r\n");
+#endif
+
     can.frequency(1000000);
-    rx.baud(115200);
-    tx.baud(115200);
+    sr.baud(115200);
     
     // LED init
     LED.period_ms(10);
-    LED.write(0);
+    LED.write(0.5);
 
     // Sensor init
     if( (capt1.ginit()) && (capt1.enableLightSensor(true)) && (capt1.enableProximitySensor(true)) ) {
@@ -104,7 +127,7 @@
 
 void srRead()
 {
-    srBuffer[srRempli++] = rx.getc();       //stockage nouvel octet dans le buffer
+    srBuffer[srRempli++] = sr.getc();       //stockage nouvel octet dans le buffer
     if (srRempli==SR_MAX) srRempli = 0;     // on recommence au debut du tableau si le max est atteint
 
 
@@ -120,7 +143,9 @@
 {
     uint8_t c = srBuffer[srVide++];     // c prends la valeur d'un octet de la tramme
     if (srVide == SR_MAX) srVide = 0;   // on recommence au debut du tableau si le max est atteint
-
+#if DEBUG_COM
+    USB_link.printf("etat : %d recu : %x\r\n",etatSR,c);
+#endif
     switch (etatSR) {
         case 0: // Verification premier octet header (FF)
             calc_checksumSR1 = c;
@@ -129,7 +154,7 @@
             }
             break;
         case 1: // Verification dexième octet header (FF)
-            calc_checksumSR1 += c;    //update checksum
+            calc_checksumSR1 ^= c;    //update checksum
             if (c==0xFF) {
                 etatSR = 2;
             } else {
@@ -137,19 +162,19 @@
             }
             break;
         case 2: // traitement octet Packet Size
-            calc_checksumSR1 += c;
+            calc_checksumSR1 ^= c;
             lenSR=c;
             if (lenSR<7) etatSR =0; //impossible
             else etatSR = 3;
             break;
         case 3: // traitement octet ID
-            calc_checksumSR1 += c;
+            calc_checksumSR1 ^= c;
             idSR = c;
             if (idSR!= data_adress_sensor) etatSR =0; //le capteur n'est pas concerné
             else etatSR = 4;
             break;
         case 4: // traitement octet CMD
-            calc_checksumSR1 += c;
+            calc_checksumSR1 ^= c;
             cmdSR = c;
             etatSR = 5;
             break;
@@ -167,91 +192,21 @@
             }
             break;
         case 7: // octet data (un seul octet dans notre cas)
-            calc_checksumSR1 += c;
+            calc_checksumSR1 ^= c;
             dataSR=c;
             etatSR =8;
             break;
-        case 8: // verification des checksum et envoi
-            calc_checksumSR1 &=0xFE ;
-            calc_checksumSR2 = (~calc_checksumSR1 & 0xFE);
-            etatSR = 0;
-            if ((checksumSR1 == calc_checksumSR1) && (checksumSR2 == calc_checksumSR2)) {  // Verification validité de la tramme
-                envoiSR(cmdSR,dataSR);// dataSR ne sera utilise que dans les cas de setup 
-            }
-            break;
     }
-
-}
-
-void envoiSR(char commande, char data)
-{
-    int j;
-    messageSR[4]=commande+0x40; // CMD (doc)
-
-    switch (commande) {
-        case send_RGB:
-            messageSR[7] = (char)((r & HIGH)>>8); // data
-            messageSR[8] = (char) (r & LOW);
-            messageSR[9] = (char)((g & HIGH)>>8);
-            messageSR[10]= (char) (g & LOW);
-            messageSR[11]= (char)a ;
-            messageSR[2]=12;    // Packet size
-            break;
-
-        case send_RED:
-            messageSR[7]= (char)((r & HIGH)>>8);
-            messageSR[8]= (char) (r & LOW);
-            messageSR[2]=9;
-            break;
-
-        case send_GREEN:
-            messageSR[7]= (char)((g & HIGH)>>8);
-            messageSR[8]= (char) (g & LOW);
-            messageSR[2]=9;
-            break;
-
-        case send_BLUE:
-            messageSR[7]= (char)((b & HIGH)>>8);
-            messageSR[8]= (char) (b & LOW);
-            messageSR[2]=9;
-            break;
-
-        case send_PROXIMITY:
-            messageSR[7] = a ;
-            messageSR[2]=8;
-            break;
-
-        case send_COLOR:
-            messageSR[7]=color;
-            messageSR[2]=8;
-            break;
-
-        case setup_LED:
-            //LED.write(data/258.0);
-            messageSR[2]=7;
-            break;
-
-        case setup_PROXIMITY_THRESHOLD :
-            proximity_tresh = data;
-            messageSR[2]=7;
-            break;
-    }
-    messageSR[5]=0x00;    // checksum1
-    //calcul des checksums
-    for(j=0; j<messageSR[2]; j++) {
-        if ((j!=5)&&(j!=6)) messageSR[5] += messageSR[j];
-    }
-    messageSR[5] &= 0xFE;   // checksum1
-    messageSR[6] = (~messageSR[5] & 0xFE);//checksum2
-
-    // envoi
-    for (j=0; j<messageSR[2]; j++) {
-        while (!tx.writeable());      // attente bluetooth libre
-        tx.putc(messageSR[j]);        // ecriture octet par octet
+    if (etatSR==8){// verification des checksum et envoi
+        calc_checksumSR1 &=0xFE ;
+        calc_checksumSR2 = (~calc_checksumSR1 & 0xFE);
+        etatSR = 0;
+        if ((checksumSR1 == calc_checksumSR1) && (checksumSR2 == calc_checksumSR2)) {  // Verification validité de la tramme
+            envoi(cmdSR,dataSR);// dataSR ne sera utilise que dans les cas de setup 
+        }           
     }
 }
 
-
 void canRead()
 {
     can.read(canBuffer[canRempli++]);
@@ -265,94 +220,131 @@
     }
 }
 
-void envoiCAN(const CANMessage &msg)
+void envoi(char commande, char data)
 {
-
-    state = msg.data[1];
-     messageCAN[1]= state +0x40;// CMD return
-    switch (state) {
+    int j;
+    message[4]=commande+0x40; // CMD (doc) 
+    
+    // Preparation des datas //
+    switch (commande) {
         case send_RGB:
-            messageCAN[2]= (char)((r & HIGH)>>8);
-            messageCAN[3]= (char) (r & LOW);
-            messageCAN[4]= (char)((g & HIGH)>>8);
-            messageCAN[5]= (char) (g & LOW);
-            messageCAN[6]= (char)a ;
-            lenRetour=7;
+            message[OFFSET_DATA+0] = (char)((r & HIGH)>>8); // data
+            message[OFFSET_DATA+1] = (char) (r & LOW);
+            message[OFFSET_DATA+2] = (char)((g & HIGH)>>8);
+            message[OFFSET_DATA+3] = (char) (g & LOW);
+            message[OFFSET_DATA+4] = (char) a ;
+            DataLen=5;    
             break;
 
         case send_RED:
-            messageCAN[2]= (char)((r & HIGH)>>8);
-            messageCAN[3]= (char) (r & LOW);
-            lenRetour=4;
+            message[OFFSET_DATA+0]= (char)((r & HIGH)>>8);
+            message[OFFSET_DATA+1]= (char) (r & LOW);
+            DataLen=2;
             break;
 
         case send_GREEN:
-            messageCAN[2]= (char)((g & HIGH)>>8);
-            messageCAN[3]= (char) (g & LOW);
-            lenRetour=4;
+            message[OFFSET_DATA+0]= (char)((g & HIGH)>>8);
+            message[OFFSET_DATA+1]= (char) (g & LOW);
+            DataLen=2;
             break;
 
         case send_BLUE:
-            messageCAN[2]= (char)((b & HIGH)>>8);
-            messageCAN[3]= (char) (b & LOW);
-            lenRetour=4;
+            message[OFFSET_DATA+0]= (char)((b & HIGH)>>8);
+            message[OFFSET_DATA+1]= (char) (b & LOW);
+            DataLen=2;
             break;
 
         case send_PROXIMITY:
-            messageCAN[2] = a ;
-            lenRetour=3;
+            message[OFFSET_DATA+0] = (char) a ;
+            DataLen=1;
             break;
 
         case send_COLOR:
-            messageCAN[2]=color;
-            lenRetour=3;
+            message[OFFSET_DATA+0]=color;
+            DataLen=1;
             break;
 
         case setup_LED:
-            //LED.write(msg.data[2]/258.0);
-            lenRetour=0;
+            LED.write(data/258.0); // a modifier : ON/OFF seulement 
+            DataLen=0;
             break;
 
         case setup_PROXIMITY_THRESHOLD :
-            proximity_tresh = msg.data[2];
-            lenRetour=0;
+            proximity_tresh = data;
+            DataLen=0;
             break;
     }
     
-    // envoi si besoin
-    if (lenRetour>0) { 
-        can.write(CANMessage(adress_color_sensor,messageCAN,lenRetour));
+    Len = DataLen+OFFSET_DATA;//calcul longueur msg    
+    
+    
+#if SR==1 // format SR //
+    message[2]= Len;
+        
+    // calcul des checksums //
+    message[5]=0xFF;    // checksum1
+    for(j=1; j<message[2]; j++) {
+        if ((j!=5)&&(j!=6)) message[5] ^= message[j];
     }
+    message[5] &= 0xFE;   // checksum1
+    message[6] = (~message[5] & 0xFE);//checksum2
+
+    // envoi //
+    //sr.enable_output(true);
+    for (j=0; j<message[2]; j++) {
+        while (!sr.writeable());      // attente liaison libre   
+        sr.putc(message[j]);        // ecriture octet par octet
+        wait_us(100); 
+    #if DEBUG_COM
+        USB_link.printf("envoi : %x\r\n",message[j]); 
+    #endif
+    }
+       // sr.diasable_output(true)
+
+#else // format CAN //
+    can.write(CANMessage(adress_color_sensor,message,Len));
+#endif   
 }
 
-
 int main()
 {
-    if (initialization()) USB_link.printf("Init finie \r\n");
+#if DEBUG_GENERAL
+    if (initialization()) USB_link.printf("Init finie\r\n");
     else USB_link.printf("Erreur pendant l'init\r\n");
+#else 
+    initialization();
+#endif
 
     if(SR==0) { // liaison CAN selectionné
+#if DEBUG_COM
+        USB_link.printf("type de com : CAN\r\n");
+        USB_link.printf("ID can : %h\r\ndata ID : %h\r\n",adress_color_sensor,data_adress_sensor);
+#endif        
         can.attach(canRead);
-        // le premier octet est toujours pareil
-        messageCAN[0] = data_adress_sensor;
+        // le premier octet est toujours pareil 
+        message[0] = data_adress_sensor;
 
     } else if (SR==1) { // liaison Serie selectionnée
-        rx.attach(&srRead); 
+#if DEBUG_COM
+        USB_link.printf("type de com : Serie\r\n");
+        USB_link.printf("Id : %x\r\n",data_adress_sensor);
+#endif
+        sr.attach(&srRead); 
         // octets toujours pareil :
-        messageSR[0]=0xff; // Start of packet
-        messageSR[1]=0xff;
-        messageSR[3]= data_adress_sensor; // pID
+        message[0]=0xff; // Start of packet
+        message[1]=0xff;
+        message[3]= data_adress_sensor; // pID
     }
 
     while(1) {
 
-        /* acquisition capteurs */
+        // acquisition capteurs //
         capt1.readRedLight(r);
         capt1.readGreenLight(g);
         capt1.readBlueLight(b);
         capt1.readProximity(a);
 
-        /* calcul couleur */
+        // calcul couleur //
         if (a<proximity_tresh) {
             color = 0 ;  // 0 Rien
         } else if (r > g ) {
@@ -360,15 +352,26 @@
         } else {
             color = 2 ;  // 2 vert
         }
+#if DEBUG_SENS
+        USB_link.printf("color : %hu \r\n", color);
+        wait(0.5);
+#endif    
         
-        /* liaison CAN */
+        
+        // liaison CAN //
         if (canRempli != canVide) { // si le buffer CAN n'est pas vide
             canVide++;
             if (canVide == CAN_MAX) canVide = 0;
-            envoiCAN(canBuffer[canRempli-1]);
+            if ((canBuffer[canRempli-1].id==adress_color_sensor)&((canBuffer[canRempli-1].data[0]==data_adress_general)|(canBuffer[canRempli-1].data[0]==data_adress_sensor))) {
+                if (canBuffer[canRempli-1].len==3){
+                    envoi(canBuffer[canRempli-1].data[1],canBuffer[canRempli-1].data[2]);
+                } else { 
+                    envoi(canBuffer[canRempli-1].data[1],0);
+                }
+            }
         }
 
-        /* liaison serie */
+        // liaison serie //
         if (srRempli != srVide) { // si le buffer serie n'est pas vide
             srTraitement();          // traitement de la tramme sr
         }