Diff: pixy2.cpp

Revision:

0:dde1b9d6c9d6

Child:

1:dd81f4065b6b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pixy2.cpp	Thu Feb 28 08:34:15 2019 +0000
@@ -0,0 +1,359 @@
+#include "pixy2.h"
+
+PIXY2::PIXY2(PinName tx, PinName rx, int debit)
+{
+    _Pixy2 = new Serial (tx, rx, debit);
+    _Pixy2->attach (callback(this,&PIXY2::pixy2_getByte));
+    etat = idle;
+    rPointer = 0;
+    Pixy2_buffer = (Byte*) malloc (0x100);
+}
+
+void PIXY2::pixy2_getByte ()    // Interruption de la pixy2
+{
+    static Byte             startPoint;
+    T_Word                  *buffer;
+    
+    Pixy2_buffer[wPointer] = _Pixy2->getc();                                 // On stocke l'octet reçu dans la première case dispo du tableau temporaire
+    wPointer++;
+    
+    switch (etat) {
+        case messageSent : // Si on a envoyé une requete => on attend un entête
+            if ( (wPointer - rPointer) >= 2) { // On attend d'avoir reçu 2 octets
+                startPoint = wPointer - 1;
+                buffer = (T_Word*) &Pixy2_buffer[startPoint]; //On pointe la structure sur les 2 derniers octets reçus
+                if ((buffer->mot == PIXY2_CSSYNC) || (buffer->mot == PIXY2_SYNC)) {
+                    etat = receivingHeader;     // On passe à l'état réception de l'entête
+                    rPointer = startPoint;
+                    if (buffer->mot == PIXY2_SYNC)      frameContainChecksum = 0;   // On mémorise qu'il n'y a pas de checksum à vérifier
+                    else                                frameContainChecksum = 1;   // On mémorise qu'il y a un checksum à vérifier
+                }
+            }
+            break;
+
+        case receivingHeader :  //Si on est en train de recevoir un entête (entre le SYNC et... La fin de l'entête)
+            if ((frameContainChecksum && ((wPointer - rPointer) == PIXY2_CSHEADERSIZE)) || (!frameContainChecksum && ((wPointer - rPointer) == PIXY2_NCSHEADERSIZE))) {
+                //Si on a reçu 6 octets pour une trame avec checksum ou 4 pour une trame sans checksum
+                etat = headerReceived;          //On passe à entête reçu
+                dataSize = Pixy2_buffer[startPoint+3]; // Et on lit la taille de la payload
+            }
+            break;
+
+        case headerReceived :   // Si on vient de recevoir le premier octet de données
+            startPoint = wPointer;  // On enregistre sa position
+            etat = receivingData;   // Et on dit que l'on est en train de recevoir des données
+            break;
+
+        case receivingData :    // Si on est en train de recevoir des données.
+            if (wPointer == (dataSize + startPoint)) etat = dataReceived; //Quand on a reçu toutes les données promises on dit que c'est OK
+            break;
+
+        default : // On ne traite volontairement ici pas tous les cas, en particulier idle et dataReceived. C'est à la fonction de le faire ! Le reste est lié à des réceptions de données.
+            break;
+    }
+}
+
+int PIXY2::pixy2_sndGetVersion (void){
+    T_pixy2sendBuffer   msg;
+    int                 i = 0, dataSize = 0;
+    msg.frame.header.pixSync = PIXY2_SYNC;
+    msg.frame.header.pixType = PIXY2_ASK_VERS;
+    msg.frame.header.pixLength = 0;
+    do {
+        while(!_Pixy2->writable());
+        _Pixy2->putc(msg.data[i]);
+        i++;
+    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
+    return 0;
+}
+
+int PIXY2::pixy2_sndGetResolution (void){
+    T_pixy2sendBuffer   msg;
+    int                 i = 0, dataSize = 1;
+    msg.frame.header.pixSync = PIXY2_SYNC;
+    msg.frame.header.pixType = PIXY2_ASK_RESOL;
+    msg.frame.header.pixLength = 1;
+    msg.frame.data[0] = 0;
+    do {
+        while(!_Pixy2->writable());
+        _Pixy2->putc(msg.data[i]);
+        i++;
+    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
+    return 0;
+}
+
+int PIXY2::pixy2_sndSetCameraBrightness (Byte brightness){
+    T_pixy2sendBuffer   msg;
+    int                 i = 0, dataSize = 1;
+    msg.frame.header.pixSync = PIXY2_SYNC;
+    msg.frame.header.pixType = PIXY2_SET_BRIGHT;
+    msg.frame.header.pixLength = 1;
+    msg.frame.data[0] = brightness;
+    do {
+        while(!_Pixy2->writable());
+        _Pixy2->putc(msg.data[i]);
+        i++;
+    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
+    return 0;
+}
+
+int PIXY2::pixy2_sndSetServo (Word s0, Word s1){
+    T_pixy2sendBuffer   msg;
+    int                 i = 0, dataSize = 4;
+    T_Word              tmp;
+    msg.frame.header.pixSync = PIXY2_SYNC;
+    msg.frame.header.pixType = PIXY2_SET_SERVOS;
+    msg.frame.header.pixLength = 1;
+    tmp.mot = s0;
+    msg.frame.data[0] = tmp.octet[0];
+    msg.frame.data[1] = tmp.octet[1];
+    tmp.mot = s1;
+    msg.frame.data[2] = tmp.octet[0];
+    msg.frame.data[3] = tmp.octet[1];
+    do {
+        while(!_Pixy2->writable());
+        _Pixy2->putc(msg.data[i]);
+        i++;
+    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
+    return 0;
+}
+
+int PIXY2::pixy2_sndSetLED (Byte red, Byte green, Byte blue){
+    T_pixy2sendBuffer   msg;
+    int                 i = 0, dataSize = 3;
+    msg.frame.header.pixSync = PIXY2_SYNC;
+    msg.frame.header.pixType = PIXY2_SET_LED;
+    msg.frame.header.pixLength = 1;
+    msg.frame.data[0] = red;
+    msg.frame.data[1] = green;
+    msg.frame.data[2] = blue;
+    do {
+        while(!_Pixy2->writable());
+        _Pixy2->putc(msg.data[i]);
+        i++;
+    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
+    return 0;
+}
+
+int PIXY2::pixy2_sndSetLamp (Byte upper, Byte lower){
+    T_pixy2sendBuffer   msg;
+    int                 i = 0, dataSize = 2;
+    msg.frame.header.pixSync = PIXY2_SYNC;
+    msg.frame.header.pixType = PIXY2_SET_LED;
+    msg.frame.header.pixLength = 1;
+    msg.frame.data[0] = upper;
+    msg.frame.data[1] = lower;
+    do {
+        while(!_Pixy2->writable());
+        _Pixy2->putc(msg.data[i]);
+        i++;
+    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
+    return 0;
+}
+
+int PIXY2::pixy2_sndGetFPS (void){
+    T_pixy2sendBuffer   msg;
+    int                 i = 0, dataSize = 0;
+    msg.frame.header.pixSync = PIXY2_SYNC;
+    msg.frame.header.pixType = PIXY2_ASK_FPS;
+    msg.frame.header.pixLength = 0;
+    do {
+        while(!_Pixy2->writable());
+        _Pixy2->putc(msg.data[i]);
+        i++;
+    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
+    return 0;
+}
+
+int PIXY2::pixy2_sndGetBlocks (Byte sigmap, Byte maxBloc){
+    T_pixy2sendBuffer   msg;
+    int                 i = 0, dataSize = 2;
+    msg.frame.header.pixSync = PIXY2_SYNC;
+    msg.frame.header.pixType = PIXY2_ASK_BLOC;
+    msg.frame.header.pixLength = 1;
+    msg.frame.data[0] = sigmap;
+    msg.frame.data[1] = maxBloc;
+    do {
+        while(!_Pixy2->writable());
+        _Pixy2->putc(msg.data[i]);
+        i++;
+    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
+    return 0;
+}
+
+int PIXY2::pixy2_sndGetMainFeature (Byte type, Byte feature){
+    T_pixy2sendBuffer   msg;
+    int                 i = 0, dataSize = 2;
+    msg.frame.header.pixSync = PIXY2_SYNC;
+    msg.frame.header.pixType = PIXY2_ASK_LINE;
+    msg.frame.header.pixLength = 1;
+    msg.frame.data[0] = type;
+    msg.frame.data[1] = feature;
+    do {
+        while(!_Pixy2->writable());
+        _Pixy2->putc(msg.data[i]);
+        i++;
+    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
+    return 0;
+}
+
+int PIXY2::pixy2_sndSetMode (Byte mode){
+    T_pixy2sendBuffer   msg;
+    int                 i = 0, dataSize = 1;
+    msg.frame.header.pixSync = PIXY2_SYNC;
+    msg.frame.header.pixType = PIXY2_SET_MODE;
+    msg.frame.header.pixLength = 1;
+    msg.frame.data[0] = mode;
+    do {
+        while(!_Pixy2->writable());
+        _Pixy2->putc(msg.data[i]);
+        i++;
+    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
+    return 0;
+}
+
+int PIXY2::pixy2_sndSetNexTurn (Word angle){
+    T_pixy2sendBuffer   msg;
+    int                 i = 0, dataSize = 2;
+    T_Word              tmp;
+    tmp.mot = angle;
+    msg.frame.header.pixSync = PIXY2_SYNC;
+    msg.frame.header.pixType = PIXY2_SET_TURN;
+    msg.frame.header.pixLength = 1;
+    tmp.mot = angle;
+    msg.frame.data[0] = tmp.octet[0];
+    msg.frame.data[1] = tmp.octet[1];
+    do {
+        while(!_Pixy2->writable());
+        _Pixy2->putc(msg.data[i]);
+        i++;
+    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
+    return 0;
+}
+
+int PIXY2::pixy2_sndSetDefaultTurn (Word angle){
+    T_pixy2sendBuffer   msg;
+    int                 i = 0, dataSize = 2;
+    T_Word              tmp;
+    tmp.mot = angle;
+    msg.frame.header.pixSync = PIXY2_SYNC;
+    msg.frame.header.pixType = PIXY2_SET_DEFTURN;
+    msg.frame.header.pixLength = 1;
+    tmp.mot = angle;
+    msg.frame.data[0] = tmp.octet[0];
+    msg.frame.data[1] = tmp.octet[1];
+    do {
+        while(!_Pixy2->writable());
+        _Pixy2->putc(msg.data[i]);
+        i++;
+    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
+    return 0;
+}
+
+int PIXY2::pixy2_sndSetVector (Byte vectorIndex){
+    T_pixy2sendBuffer   msg;
+    int                 i = 0, dataSize = 1;
+    msg.frame.header.pixSync = PIXY2_SYNC;
+    msg.frame.header.pixType = PIXY2_SET_VECTOR;
+    msg.frame.header.pixLength = 1;
+    msg.frame.data[0] = vectorIndex;
+    do {
+        while(!_Pixy2->writable());
+        _Pixy2->putc(msg.data[i]);
+        i++;
+    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
+    return 0;
+}
+
+int PIXY2::pixy2_sndReverseVector (void){
+    T_pixy2sendBuffer   msg;
+    int                 i = 0, dataSize = 0;
+    msg.frame.header.pixSync = PIXY2_SYNC;
+    msg.frame.header.pixType = PIXY2_SET_REVERSE;
+    msg.frame.header.pixLength = 0;
+    do {
+        while(!_Pixy2->writable());
+        _Pixy2->putc(msg.data[i]);
+        i++;
+    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
+    return 0;
+}
+
+int PIXY2::pixy2_sndGetRGB (Word x, Word y, Byte saturate){
+    T_pixy2sendBuffer   msg;
+    int                 i = 0, dataSize = 3;
+    msg.frame.header.pixSync = PIXY2_SYNC;
+    msg.frame.header.pixType = PIXY2_ASK_VIDEO;
+    msg.frame.header.pixLength = 1;
+    msg.frame.data[0] = x;
+    msg.frame.data[1] = y;
+    msg.frame.data[2] = saturate;
+    do {
+        while(!_Pixy2->writable());
+        _Pixy2->putc(msg.data[i]);
+        i++;
+    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
+    return 0;
+}
+
+/*  La fonction est bloquante à l'envoi (pas vraiment le choix), mais elle est non bloquante en réception. On essayera de faire une fonction non bloquante en envoi avec write, mais c'est pas la priorité.
+Le principe c'est de stocker dans un buffer circulaire les données au fur et à mesure qu'elle sont reçues et de traiter uniquement en castant les infos. Pour cela, il faut recevoir et stocker. */
+
+int PIXY2::pixy2_getVersion (T_Pixy2Version *version){
+
+    T_pixy2RcvHeader    *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[rPointer];
+    int                 cr = 0;
+    
+    switch (etat) {
+        case idle :
+            PIXY2::pixy2_sndGetVersion();
+            etat = messageSent;
+            rPointer = wPointer;
+            cr = -2;                        //Busy
+            break;
+            
+        case dataReceived :
+            if (frameContainChecksum) {
+                if ( pixy2_validateChecksum (&Pixy2_buffer[rPointer]) ) {
+                    if (msg->pixType == PIXY2_REP_VERS) {
+                        version = (T_Pixy2Version*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
+                        etat = idle;
+                    } else cr = -7;   // Type Error
+                } else cr = -3;       // Checksum Error
+            } else {
+                if (msg->pixType == PIXY2_REP_VERS) {
+                    version = (T_Pixy2Version*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
+                    etat = idle;
+                } else cr = -7;       // Type Error
+            }
+            break;
+
+        default :
+            cr = -1;                // Busy
+            break;
+    }
+    return cr;
+}
+
+int PIXY2::pixy2_getResolution (T_Pixy2Resolution *resolution){return 0;}
+int PIXY2::pixy2_setCameraBrightness (Byte brightness){return 0;}
+int PIXY2::pixy2_setServos (Word s0, Word s1){return 0;} 
+int PIXY2::pixy2_setLED (Byte red, Byte green, Byte blue){return 0;}
+int PIXY2::pixy2_setLamp (Byte upper, Byte lower){return 0;}
+int PIXY2::pixy2_getFPS (T_pixy2Framerate framerate){return 0;}
+int PIXY2::pixy2_getBlocks (Byte sigmap, Byte maxBloc){return 0;}
+int PIXY2::pixy2_getMainFeature (Byte type, Byte features){return 0;}
+int PIXY2::pixy2_getAllFeature (Byte features){return 0;}
+int PIXY2::pixy2_setMode (Byte mode){return 0;}
+int PIXY2::pixy2_setNexTurn (Word angle){return 0;}
+int PIXY2::pixy2_setDefaultTurn (Word angle){return 0;}
+int PIXY2::pixy2_setVector (Byte vectorIndex){return 0;}
+int PIXY2::pixy2_ReverseVector (void){return 0;}
+int PIXY2::pixy2_getRGB (Word x, Word y, Byte saturate, T_pixy2Pixel *pixel){return 0;}
+
+int PIXY2::pixy2_validateChecksum (Byte* tab){
+    Word sum;
+    
+    
+    return 1;}
+