File content as of revision 5:b91bf2db2ea8:

#include "mbed.h"
#include "glibr.h"

#define SR 1   // 0 = CAN | 1 = serie
#define DEBUG_COM  0     // 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  0x01    // 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
#define send_RED       0x01
#define send_GREEN     0x02
#define send_BLUE      0x03
#define send_PROXIMITY 0x04
#define send_COLOR     0x05

// Setup commands
#define setup_LED 0x08
#define setup_PROXIMITY_THRESHOLD 0x09

// Masks
#define HIGH 0xFF00
#define LOW  0x00FF

// Buffer
#define CAN_MAX     256
#define SR_MAX     1024

Serial USB_link(USBTX, USBRX);     // USB initialization
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);              
DigitalIn srTx(PA_9, PullUp);

// Buffer CAN
CANMessage canBuffer[CAN_MAX];
int canRempli = 0;
int canVide = 0;
int canPerdu = 0;

//Buffer SR
uint8_t srBuffer[SR_MAX];
int srRempli = 0;
int srVide = 0;
int srPerdu = 0;

// traitement tramme SR
int etatSR = 0;
uint8_t checksumSR1 = 0,checksumSR2 = 0,calc_checksumSR1=0,calc_checksumSR2=0;
uint8_t lenSR = 0, idSR,cmdSR, dataSR=0;

// tramme retour
char message[15], Len;
int DataLen = 0;

// capteur et traitement
uint16_t r,g,b ; // RGB values in 2 bytes
uint8_t a ;      // proximity value in 1 byte
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 récéptionnant la tramme serie
 *  et la stockant dans un buffer en attendant traitement
 */
void srRead();

/** Fonction decryptant la tramme serie recue dans le buffer
 */
void srTraitement();

/** Fonction receptionnant les messaages CAN dans un Buffer
 */
void canRead();

/** 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);






bool initialization(void)
{
#if DEBUG_GENERAL
    // baud init
    USB_link.baud(115200);
#endif

    can.frequency(1000000);
    sr.baud(115200);
    
    // LED init
    LED.period_ms(10);
    LED.write(0.5);

    // Sensor init
    if( (capt1.ginit()) && (capt1.enableLightSensor(true)) && (capt1.enableProximitySensor(true)) ) {
        return true;
    } else {
        return false;
    }
}


int main()
{
#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
        int adColor = adress_color_sensor;
        int adSensor = data_adress_sensor;
        USB_link.printf("type de com : CAN\r\n");
        USB_link.printf("ID can : %03X\r\ndata ID : %02X\r\n", adColor, adSensor);
#endif        
        can.attach(canRead);
        // le premier octet est toujours pareil 
        message[0] = data_adress_sensor;

    }
    else if (SR==1) { // liaison Serie selectionnée
#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 :
        message[0]=0xff; // Start of packet
        message[1]=0xff;
        message[3]= data_adress_sensor; // pID
    }

    while(1) {

        // acquisition capteurs //
        capt1.readRedLight(r);
        capt1.readGreenLight(g);
        capt1.readBlueLight(b);
        capt1.readProximity(a);

        // calcul couleur //
        if (a<proximity_tresh) {
            color = 0 ;  // 0 Rien
        }
        else if ((r > g )&&(r > b )) {
            color = 1 ;  // 1 rouge
        }
        else if ((g > r )&&(g > b )) {
            color = 2 ;  // 2 vert
        }
        else if ((b > r )&&(b > g )) {
            color = 3 ;  // 3 bleu
        }
        else {
            color = 4 ;  // 4 noir ou blanc
        }
#if DEBUG_SENS
        USB_link.printf("rouge : %x, vert : %x, bleu : %x    ",r, g, b);
        USB_link.printf("color : %x \r\n", color);
        USB_link.printf("color : %hu \r\n", color);
        wait(0.5);
#endif    
        
        
        // liaison CAN //
        if (canRempli != canVide) { // si le buffer CAN n'est pas vide
            canVide++;
            if (canVide == CAN_MAX) canVide = 0;
            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 //
        if (srRempli != srVide) { // si le buffer serie n'est pas vide
            srTraitement();          // traitement de la tramme sr
        }
    }
}


void srRead()
{
    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


    if (srRempli == srVide) { // buffer plein on perd un caractère (le premier recu)
        srVide++;   // le message commence donc un octet plus tard
        if (srVide == SR_MAX) srVide = 0;   // on recommence au debut du tableau si le max est atteint

        srPerdu++;  // mise en memoire : un message perdu
    }
}

void srTraitement()
{
    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)
            if (c==0xFF) {
                etatSR = 1;
            }
            break;
        case 1: // Verification dexième octet header (FF)
            calc_checksumSR1 = 0;
            if (c==0xFF) {
                etatSR = 2;
            } else {
                etatSR = 0;
            }
            break;
        case 2: // traitement octet Packet Size
            calc_checksumSR1 ^= c;
            lenSR=c;
            if (lenSR<7) etatSR =0; //impossible
            else etatSR = 3;
            break;
        case 3: // traitement octet ID
            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;
            cmdSR = c;
            etatSR = 5;
            break;
        case 5: // traitement octet checkSum1
            checksumSR1 = c;
            etatSR = 6;
            break;
        case 6: // traitement octet checkSum2
            checksumSR2 = c;
            if (lenSR>7) {
                etatSR =7;// si le message comporte des datas
            } else {
                dataSR = 0x00;
                etatSR=8;
            }
            break;
        case 7: // octet data (un seul octet dans notre cas)
            calc_checksumSR1 ^= c;
            dataSR=c;
            etatSR =8;
            break;
    }
    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++]);
    if (canRempli==CAN_MAX) {
        canRempli = 0;
    }
    if (canRempli == canVide) { // buffer plein on perd un message
        canVide++;
        if (canVide == CAN_MAX) canVide = 0;
        canPerdu++;
    }
}

void envoi(char commande, char data)
{
#if SR==0 // mode CAN    
    message[1]=commande+0x40; // CMD (doc) 
#endif

    // Preparation des datas //
    switch (commande) {
        case send_RGB:
            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)((b & HIGH)>>8);
            message[OFFSET_DATA+5] = (char) (b & LOW);
            DataLen=6;    
            break;

        case send_RED:
            message[OFFSET_DATA+0]= (char)((r & HIGH)>>8);
            message[OFFSET_DATA+1]= (char) (r & LOW);
            DataLen=2;
            break;

        case send_GREEN:
            message[OFFSET_DATA+0]= (char)((g & HIGH)>>8);
            message[OFFSET_DATA+1]= (char) (g & LOW);
            DataLen=2;
            break;

        case send_BLUE:
            message[OFFSET_DATA+0]= (char)((b & HIGH)>>8);
            message[OFFSET_DATA+1]= (char) (b & LOW);
            DataLen=2;
            break;

        case send_PROXIMITY:
            message[OFFSET_DATA+0] = (char) a ;
            DataLen=1;
            break;

        case send_COLOR:
            message[OFFSET_DATA+0] = color;
            DataLen=1;
            break;

        case setup_LED:
            LED.write(data/255.0); // a modifier : ON/OFF seulement 
            DataLen=0;
            break;

        case setup_PROXIMITY_THRESHOLD :
            proximity_tresh = data;
            DataLen=0;
            break;
    }
    
    Len = DataLen+OFFSET_DATA;//calcul longueur msg    
    
    
#if SR==1 // format SR //
    message[2]= Len;
    message[4]=commande+0x40; // CMD (doc)
    int j;
    // calcul des checksums //
    message[5]=0;    // checksum1
    for(j=2; 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.disable_output(true)

#else // format CAN //
    can.write(CANMessage(adress_color_sensor,message,Len));
#endif   
}