H A / Pixy2_UART

Fully functionnal PIXY2 lib for UART communication Made by IUT de Cachan (Hugues & Wael)

pixy2.cpp@0:0bf9aab1408e, 2020-07-22 (annotated)

Committer:
haarkon
Date:
Wed Jul 22 09:29:59 2020 +0000
Revision:
0:0bf9aab1408e
added comments

Who changed what in which revision?

UserRevisionLine numberNew contents of line
haarkon 0:0bf9aab1408e 1 #include "pixy2.h"
haarkon 0:0bf9aab1408e 2
haarkon 0:0bf9aab1408e 3 extern int sommeDeControle,sommeRecue;
haarkon 0:0bf9aab1408e 4 extern Serial pc;
haarkon 0:0bf9aab1408e 5
haarkon 0:0bf9aab1408e 6 PIXY2::PIXY2(PinName tx, PinName rx, int debit) : Pixy2_numBlocks(0), Pixy2_numVectors(0), Pixy2_numIntersections(0), Pixy2_numBarcodes(0)
haarkon 0:0bf9aab1408e 7 {
haarkon 0:0bf9aab1408e 8 _Pixy2 = new Serial (tx, rx, debit);
haarkon 0:0bf9aab1408e 9 _Pixy2->attach (callback(this,&PIXY2::pixy2_getByte));
haarkon 0:0bf9aab1408e 10 etat = idle;
haarkon 0:0bf9aab1408e 11 Pixy2_buffer = (Byte*) malloc (0x100);
haarkon 0:0bf9aab1408e 12 }
haarkon 0:0bf9aab1408e 13
haarkon 0:0bf9aab1408e 14 PIXY2::~PIXY2()
haarkon 0:0bf9aab1408e 15 {
haarkon 0:0bf9aab1408e 16 free (Pixy2_buffer);
haarkon 0:0bf9aab1408e 17 }
haarkon 0:0bf9aab1408e 18
haarkon 0:0bf9aab1408e 19 // POUR DEBUG //
haarkon 0:0bf9aab1408e 20 T_Pixy2State PIXY2::getEtat()
haarkon 0:0bf9aab1408e 21 {
haarkon 0:0bf9aab1408e 22 return this->etat;
haarkon 0:0bf9aab1408e 23 }
haarkon 0:0bf9aab1408e 24
haarkon 0:0bf9aab1408e 25 void PIXY2::affDataSize()
haarkon 0:0bf9aab1408e 26 {
haarkon 0:0bf9aab1408e 27 pc.printf("dataSize : %d\n\r", this->dataSize);
haarkon 0:0bf9aab1408e 28 }
haarkon 0:0bf9aab1408e 29 // Pour détail d'une trame (a mettre dans la fct correspondante) :
haarkon 0:0bf9aab1408e 30 //pc.printf("pixSync : %d, pixType : %d, pixLength : %d, pixChecksum : %d\n\r", msg->pixSync, msg->pixType, msg->pixLength, msg->pixChecksum);
haarkon 0:0bf9aab1408e 31
haarkon 0:0bf9aab1408e 32 /* Le programme utilise l'interruption de la liaison série pour recevoir le message et avancer la machine d'état pour permettre au programme de ne pas être bloquant en réception.
haarkon 0:0bf9aab1408e 33 Lorsqu'on appelle une fonction, elle retourne le code -2 pour signifier que le message de réponse de la caméra n'est pas completement reçu...
haarkon 0:0bf9aab1408e 34 Les étapes de fonctionnement sont les suivantes :
haarkon 0:0bf9aab1408e 35
haarkon 0:0bf9aab1408e 36 * idle : La caméra n'a pas été solicité => on peut envoyer le message de requête.
haarkon 0:0bf9aab1408e 37 * messageSent : La requête a été transmise, mais la réponse n'est pas encore arrivée.
haarkon 0:0bf9aab1408e 38 * receivingHeader : Le mot de synchro a été reçu, le reste de l'entête est en cours de réception.
haarkon 0:0bf9aab1408e 39 * receivingData : L'entête a été intégralement reçu (et traité) et on est en train de recevoir la payload.
haarkon 0:0bf9aab1408e 40 * dataReceived : La réponse a été intégralement reçue et est disponible pour le traitement (et la libération).
haarkon 0:0bf9aab1408e 41
haarkon 0:0bf9aab1408e 42 On utilise plusieurs pointeurs :
haarkon 0:0bf9aab1408e 43 * wPointer est le pointeur d'écriture des octets reçus dans le buffer de réception
haarkon 0:0bf9aab1408e 44 * hPointer est le pointeur de l'entête du message
haarkon 0:0bf9aab1408e 45 * dPointer est le pointeur de la zone de données
haarkon 0:0bf9aab1408e 46
haarkon 0:0bf9aab1408e 47 On rajoute une variable : dataSize, qui définit la quantité de données à recevoir (issue de l'entête)
haarkon 0:0bf9aab1408e 48
haarkon 0:0bf9aab1408e 49 **** COMMENT CA MARCHE ****
haarkon 0:0bf9aab1408e 50
haarkon 0:0bf9aab1408e 51 Au début wPointer = 0. Quand wPointer est supérieur à 1 (donc qu'on a reçu au moins 2 octets), on regarde si on a reçu le code de début de trame 0xC1AF
haarkon 0:0bf9aab1408e 52 Si ce n'est pas le cas, on continue de recevoir les octets et on les stock jusqu'à trouver le code de début de trame.
haarkon 0:0bf9aab1408e 53 Quand on trouve le code, on enregistre le point de départ (c'est hPointer) comme étant la position actuelle du wPointer-1.
haarkon 0:0bf9aab1408e 54 A partir de ça, on attend de recevoir les 6 premiers octets de l'entête :
haarkon 0:0bf9aab1408e 55 * start of frame (hPointer et hPointer + 1)
haarkon 0:0bf9aab1408e 56 * identification du type (hPointer + 2)
haarkon 0:0bf9aab1408e 57 * taille des données (hPointer + 3)
haarkon 0:0bf9aab1408e 58 Et ensuite si on a une trame avec un checksum (normal) :
haarkon 0:0bf9aab1408e 59 * le checksum (hPointer + 4 et hPointer + 5)
haarkon 0:0bf9aab1408e 60 * les données (hPointer + 6 et suivant)
haarkon 0:0bf9aab1408e 61 Et si on a une trame sans checksum (pas normal) :
haarkon 0:0bf9aab1408e 62 * les données (hPointer + 4 et suivant)
haarkon 0:0bf9aab1408e 63
haarkon 0:0bf9aab1408e 64 On continue alors de recevoir des octets jusqu'à ce que wPointer soit égal à dPointer + dataSize - 1
haarkon 0:0bf9aab1408e 65 par exemple si la data fait 4 octets et une trame avec checksum, on reçoit tant que wPointer est inférieur à 9 (6 + 4 - 1)
haarkon 0:0bf9aab1408e 66 ou pour la même quantité de données mais avec une trame sans checksum 7 (4 + 4 - 1)...
haarkon 0:0bf9aab1408e 67
haarkon 0:0bf9aab1408e 68 Automate des fonctions.
haarkon 0:0bf9aab1408e 69
haarkon 0:0bf9aab1408e 70 appel de la fonction
haarkon 0:0bf9aab1408e 71 /--- publique ---\ /-------------------- géré par l'interruption -------------------\
haarkon 0:0bf9aab1408e 72
haarkon 0:0bf9aab1408e 73 |------| envoi |---------| start |-----------| header |-----------| trame |----------|
haarkon 0:0bf9aab1408e 74 | | ordre | | reçu | receiving | reçu | receiving | reçue | data |
haarkon 0:0bf9aab1408e 75 | Idle |----+---->| msgSent |----+--->| header |----+---->| data |----+--->| received |---\
haarkon 0:0bf9aab1408e 76 | | | | C1AF | | 6o | | (data | | |
haarkon 0:0bf9aab1408e 77 |------| |---------|(ou C1AE)|-----------| |-----------| size) |----------| |
haarkon 0:0bf9aab1408e 78 | |
haarkon 0:0bf9aab1408e 79 \-------------------<---------------------------+---------------------------<------------------/
haarkon 0:0bf9aab1408e 80 lecture des données reçues
haarkon 0:0bf9aab1408e 81
haarkon 0:0bf9aab1408e 82 \------------------------------- appel de la fonction publique ------------------------------/
haarkon 0:0bf9aab1408e 83
haarkon 0:0bf9aab1408e 84 Pour l'utilisateur seul l'appel de la fonction publique est nécessaire.
haarkon 0:0bf9aab1408e 85 Tant qu'il récupère un code de retour -1, cela signifie que la tâche n'est pas achevée
haarkon 0:0bf9aab1408e 86 Quand le code reçu est 0, cela signifie que le résultat est disponible
haarkon 0:0bf9aab1408e 87 Toutes les autres valeurs signifient une erreur
haarkon 0:0bf9aab1408e 88 */
haarkon 0:0bf9aab1408e 89
haarkon 0:0bf9aab1408e 90 void PIXY2::pixy2_getByte () // Interruption de la pixy2
haarkon 0:0bf9aab1408e 91 {
haarkon 0:0bf9aab1408e 92 T_Word *buffer;
haarkon 0:0bf9aab1408e 93
haarkon 0:0bf9aab1408e 94 Pixy2_buffer[wPointer] = _Pixy2->getc(); // On stocke l'octet reçu dans la première case dispo du buffer de réception
haarkon 0:0bf9aab1408e 95
haarkon 0:0bf9aab1408e 96
haarkon 0:0bf9aab1408e 97 switch (etat) {
haarkon 0:0bf9aab1408e 98 case messageSent : // Si on a envoyé une requete => on attend un entête
haarkon 0:0bf9aab1408e 99 if (wPointer > 0) { // On attend d'avoir reçu 2 octets
haarkon 0:0bf9aab1408e 100 buffer = (T_Word*) &Pixy2_buffer[wPointer-1]; // On pointe la structure sur les 2 derniers octets reçus
haarkon 0:0bf9aab1408e 101 if ((buffer->mot == PIXY2_CSSYNC) || (buffer->mot == PIXY2_SYNC)) { // Si c'est un mot d'entête
haarkon 0:0bf9aab1408e 102 etat = receivingHeader; // On passe à l'état réception de l'entête
haarkon 0:0bf9aab1408e 103 hPointer = wPointer - 1; // On initialise le pointeur de l'entête
haarkon 0:0bf9aab1408e 104 if (buffer->mot == PIXY2_SYNC) {
haarkon 0:0bf9aab1408e 105 frameContainChecksum = 0; // Si c'est un entête sans checksum, on mémorise qu'il n'y a pas de checksum à vérifier
haarkon 0:0bf9aab1408e 106 dPointer = hPointer + PIXY2_NCSHEADERSIZE;
haarkon 0:0bf9aab1408e 107 } else {
haarkon 0:0bf9aab1408e 108 frameContainChecksum = 1; // Sinon, on mémorise qu'il y a un checksum à vérifier
haarkon 0:0bf9aab1408e 109 dPointer = hPointer + PIXY2_CSHEADERSIZE;
haarkon 0:0bf9aab1408e 110 }
haarkon 0:0bf9aab1408e 111 } // Si on n'a pas de mot d'entête on attend d'en trouver un...
haarkon 0:0bf9aab1408e 112 }
haarkon 0:0bf9aab1408e 113 break;
haarkon 0:0bf9aab1408e 114
haarkon 0:0bf9aab1408e 115 case receivingHeader : // Si on est en train de recevoir un entête (entre le SYNC et... La fin de l'entête)
haarkon 0:0bf9aab1408e 116 if ((frameContainChecksum && ((wPointer - hPointer) == (PIXY2_CSHEADERSIZE - 1))) || (!frameContainChecksum && ((wPointer - hPointer) == (PIXY2_NCSHEADERSIZE - 1)))) {
haarkon 0:0bf9aab1408e 117 // Si on a reçu 6 octets pour une trame avec checksum ou 4 pour une trame sans checksum, c'est à dire un entête complet
haarkon 0:0bf9aab1408e 118 etat = receivingData; // On dit que l'on va de recevoir des données
haarkon 0:0bf9aab1408e 119 dataSize = Pixy2_buffer[hPointer + 3]; // On enregistre la taille de la payload
haarkon 0:0bf9aab1408e 120 if (dataSize == 0) // Si on ne doit recevoir qu'un entête, on a terminé
haarkon 0:0bf9aab1408e 121 etat = idle; // On revient à l'état d'attente d'ordre
haarkon 0:0bf9aab1408e 122 }
haarkon 0:0bf9aab1408e 123 break;
haarkon 0:0bf9aab1408e 124
haarkon 0:0bf9aab1408e 125 case receivingData : // Si on est en train de recevoir des données.
haarkon 0:0bf9aab1408e 126 if (wPointer == ((dataSize - 1) + dPointer)) { // Quand on a reçu toutes les données
haarkon 0:0bf9aab1408e 127 etat = dataReceived; // On dit que c'est OK pour leur traitement
haarkon 0:0bf9aab1408e 128 }
haarkon 0:0bf9aab1408e 129 break;
haarkon 0:0bf9aab1408e 130
haarkon 0:0bf9aab1408e 131 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.
haarkon 0:0bf9aab1408e 132 break;
haarkon 0:0bf9aab1408e 133 }
haarkon 0:0bf9aab1408e 134 wPointer++; // on pointe la case suivante du buffer de réception
haarkon 0:0bf9aab1408e 135
haarkon 0:0bf9aab1408e 136 }
haarkon 0:0bf9aab1408e 137
haarkon 0:0bf9aab1408e 138 T_pixy2ErrorCode PIXY2::pixy2_sndGetVersion (void){
haarkon 0:0bf9aab1408e 139 T_pixy2SendBuffer msg;
haarkon 0:0bf9aab1408e 140 int i = 0, dataSize = 0;
haarkon 0:0bf9aab1408e 141 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:0bf9aab1408e 142 msg.frame.header.pixType = PIXY2_ASK_VERS;
haarkon 0:0bf9aab1408e 143 msg.frame.header.pixLength = dataSize;
haarkon 0:0bf9aab1408e 144 do {
haarkon 0:0bf9aab1408e 145 while(!_Pixy2->writable());
haarkon 0:0bf9aab1408e 146 _Pixy2->putc(msg.data[i]);
haarkon 0:0bf9aab1408e 147 i++;
haarkon 0:0bf9aab1408e 148 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 0:0bf9aab1408e 149 return PIXY2_OK;
haarkon 0:0bf9aab1408e 150 }
haarkon 0:0bf9aab1408e 151
haarkon 0:0bf9aab1408e 152 T_pixy2ErrorCode PIXY2::pixy2_sndGetResolution (void){
haarkon 0:0bf9aab1408e 153 T_pixy2SendBuffer msg;
haarkon 0:0bf9aab1408e 154 int i = 0, dataSize = 1;
haarkon 0:0bf9aab1408e 155 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:0bf9aab1408e 156 msg.frame.header.pixType = PIXY2_ASK_RESOL;
haarkon 0:0bf9aab1408e 157 msg.frame.header.pixLength = dataSize;
haarkon 0:0bf9aab1408e 158 msg.frame.data[0] = 0;
haarkon 0:0bf9aab1408e 159 do {
haarkon 0:0bf9aab1408e 160 while(!_Pixy2->writable());
haarkon 0:0bf9aab1408e 161 _Pixy2->putc(msg.data[i]);
haarkon 0:0bf9aab1408e 162 i++;
haarkon 0:0bf9aab1408e 163 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 0:0bf9aab1408e 164 return PIXY2_OK;
haarkon 0:0bf9aab1408e 165 }
haarkon 0:0bf9aab1408e 166
haarkon 0:0bf9aab1408e 167 T_pixy2ErrorCode PIXY2::pixy2_sndSetCameraBrightness (Byte brightness){
haarkon 0:0bf9aab1408e 168 T_pixy2SendBuffer msg;
haarkon 0:0bf9aab1408e 169 int i = 0, dataSize = 1;
haarkon 0:0bf9aab1408e 170 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:0bf9aab1408e 171 msg.frame.header.pixType = PIXY2_SET_BRIGHT;
haarkon 0:0bf9aab1408e 172 msg.frame.header.pixLength = dataSize;
haarkon 0:0bf9aab1408e 173 msg.frame.data[0] = brightness;
haarkon 0:0bf9aab1408e 174 do {
haarkon 0:0bf9aab1408e 175 while(!_Pixy2->writable());
haarkon 0:0bf9aab1408e 176 _Pixy2->putc(msg.data[i]);
haarkon 0:0bf9aab1408e 177 i++;
haarkon 0:0bf9aab1408e 178 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 0:0bf9aab1408e 179 return PIXY2_OK;
haarkon 0:0bf9aab1408e 180 }
haarkon 0:0bf9aab1408e 181
haarkon 0:0bf9aab1408e 182 T_pixy2ErrorCode PIXY2::pixy2_sndSetServo (Word s0, Word s1){
haarkon 0:0bf9aab1408e 183 T_pixy2SendBuffer msg;
haarkon 0:0bf9aab1408e 184 int i = 0, dataSize = 4;
haarkon 0:0bf9aab1408e 185 T_Word tmp;
haarkon 0:0bf9aab1408e 186 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:0bf9aab1408e 187 msg.frame.header.pixType = PIXY2_SET_SERVOS;
haarkon 0:0bf9aab1408e 188 msg.frame.header.pixLength = dataSize;
haarkon 0:0bf9aab1408e 189 tmp.mot = s0;
haarkon 0:0bf9aab1408e 190 msg.frame.data[0] = tmp.octet[0];
haarkon 0:0bf9aab1408e 191 msg.frame.data[1] = tmp.octet[1];
haarkon 0:0bf9aab1408e 192 tmp.mot = s1;
haarkon 0:0bf9aab1408e 193 msg.frame.data[2] = tmp.octet[0];
haarkon 0:0bf9aab1408e 194 msg.frame.data[3] = tmp.octet[1];
haarkon 0:0bf9aab1408e 195 do {
haarkon 0:0bf9aab1408e 196 while(!_Pixy2->writable());
haarkon 0:0bf9aab1408e 197 _Pixy2->putc(msg.data[i]);
haarkon 0:0bf9aab1408e 198 i++;
haarkon 0:0bf9aab1408e 199 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 0:0bf9aab1408e 200 return PIXY2_OK;
haarkon 0:0bf9aab1408e 201 }
haarkon 0:0bf9aab1408e 202
haarkon 0:0bf9aab1408e 203 T_pixy2ErrorCode PIXY2::pixy2_sndSetLED (Byte red, Byte green, Byte blue){
haarkon 0:0bf9aab1408e 204 T_pixy2SendBuffer msg;
haarkon 0:0bf9aab1408e 205 int i = 0, dataSize = 3;
haarkon 0:0bf9aab1408e 206 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:0bf9aab1408e 207 msg.frame.header.pixType = PIXY2_SET_LED;
haarkon 0:0bf9aab1408e 208 msg.frame.header.pixLength = dataSize;
haarkon 0:0bf9aab1408e 209 msg.frame.data[0] = red;
haarkon 0:0bf9aab1408e 210 msg.frame.data[1] = green;
haarkon 0:0bf9aab1408e 211 msg.frame.data[2] = blue;
haarkon 0:0bf9aab1408e 212 do {
haarkon 0:0bf9aab1408e 213 while(!_Pixy2->writable());
haarkon 0:0bf9aab1408e 214 _Pixy2->putc(msg.data[i]);
haarkon 0:0bf9aab1408e 215 i++;
haarkon 0:0bf9aab1408e 216 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 0:0bf9aab1408e 217 return PIXY2_OK;
haarkon 0:0bf9aab1408e 218 }
haarkon 0:0bf9aab1408e 219
haarkon 0:0bf9aab1408e 220 T_pixy2ErrorCode PIXY2::pixy2_sndSetLamp (Byte upper, Byte lower){
haarkon 0:0bf9aab1408e 221 T_pixy2SendBuffer msg;
haarkon 0:0bf9aab1408e 222 int i = 0, dataSize = 2;
haarkon 0:0bf9aab1408e 223 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:0bf9aab1408e 224 msg.frame.header.pixType = PIXY2_SET_LAMP;
haarkon 0:0bf9aab1408e 225 msg.frame.header.pixLength = dataSize;
haarkon 0:0bf9aab1408e 226 msg.frame.data[0] = upper;
haarkon 0:0bf9aab1408e 227 msg.frame.data[1] = lower;
haarkon 0:0bf9aab1408e 228 do {
haarkon 0:0bf9aab1408e 229 while(!_Pixy2->writable());
haarkon 0:0bf9aab1408e 230 _Pixy2->putc(msg.data[i]);
haarkon 0:0bf9aab1408e 231 i++;
haarkon 0:0bf9aab1408e 232 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 0:0bf9aab1408e 233 return PIXY2_OK;
haarkon 0:0bf9aab1408e 234 }
haarkon 0:0bf9aab1408e 235
haarkon 0:0bf9aab1408e 236 T_pixy2ErrorCode PIXY2::pixy2_sndGetFPS (void){
haarkon 0:0bf9aab1408e 237 T_pixy2SendBuffer msg;
haarkon 0:0bf9aab1408e 238 int i = 0, dataSize = 0;
haarkon 0:0bf9aab1408e 239 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:0bf9aab1408e 240 msg.frame.header.pixType = PIXY2_ASK_FPS;
haarkon 0:0bf9aab1408e 241 msg.frame.header.pixLength = dataSize;
haarkon 0:0bf9aab1408e 242 do {
haarkon 0:0bf9aab1408e 243 while(!_Pixy2->writable());
haarkon 0:0bf9aab1408e 244 _Pixy2->putc(msg.data[i]);
haarkon 0:0bf9aab1408e 245 i++;
haarkon 0:0bf9aab1408e 246 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 0:0bf9aab1408e 247 return PIXY2_OK;
haarkon 0:0bf9aab1408e 248 }
haarkon 0:0bf9aab1408e 249
haarkon 0:0bf9aab1408e 250 T_pixy2ErrorCode PIXY2::pixy2_sndGetBlocks (Byte sigmap, Byte maxBloc){
haarkon 0:0bf9aab1408e 251 T_pixy2SendBuffer msg;
haarkon 0:0bf9aab1408e 252 int i = 0, dataSize = 2;
haarkon 0:0bf9aab1408e 253 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:0bf9aab1408e 254 msg.frame.header.pixType = PIXY2_ASK_BLOC;
haarkon 0:0bf9aab1408e 255 msg.frame.header.pixLength = dataSize;
haarkon 0:0bf9aab1408e 256 msg.frame.data[0] = sigmap;
haarkon 0:0bf9aab1408e 257 msg.frame.data[1] = maxBloc;
haarkon 0:0bf9aab1408e 258 do {
haarkon 0:0bf9aab1408e 259 while(!_Pixy2->writable());
haarkon 0:0bf9aab1408e 260 _Pixy2->putc(msg.data[i]);
haarkon 0:0bf9aab1408e 261 i++;
haarkon 0:0bf9aab1408e 262 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 0:0bf9aab1408e 263 return PIXY2_OK;
haarkon 0:0bf9aab1408e 264 }
haarkon 0:0bf9aab1408e 265
haarkon 0:0bf9aab1408e 266 T_pixy2ErrorCode PIXY2::pixy2_sndGetLineFeature (Byte type, Byte feature){
haarkon 0:0bf9aab1408e 267 T_pixy2SendBuffer msg;
haarkon 0:0bf9aab1408e 268 int i = 0, dataSize = 2;
haarkon 0:0bf9aab1408e 269 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:0bf9aab1408e 270 msg.frame.header.pixType = PIXY2_ASK_LINE;
haarkon 0:0bf9aab1408e 271 msg.frame.header.pixLength = dataSize;
haarkon 0:0bf9aab1408e 272 msg.frame.data[0] = type;
haarkon 0:0bf9aab1408e 273 msg.frame.data[1] = feature;
haarkon 0:0bf9aab1408e 274 do {
haarkon 0:0bf9aab1408e 275 while(!_Pixy2->writable());
haarkon 0:0bf9aab1408e 276 _Pixy2->putc(msg.data[i]);
haarkon 0:0bf9aab1408e 277 i++;
haarkon 0:0bf9aab1408e 278 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 0:0bf9aab1408e 279 return PIXY2_OK;
haarkon 0:0bf9aab1408e 280 }
haarkon 0:0bf9aab1408e 281
haarkon 0:0bf9aab1408e 282 T_pixy2ErrorCode PIXY2::pixy2_sndSetMode (Byte mode){
haarkon 0:0bf9aab1408e 283 T_pixy2SendBuffer msg;
haarkon 0:0bf9aab1408e 284 int i = 0, dataSize = 1;
haarkon 0:0bf9aab1408e 285 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:0bf9aab1408e 286 msg.frame.header.pixType = PIXY2_SET_MODE;
haarkon 0:0bf9aab1408e 287 msg.frame.header.pixLength = dataSize;
haarkon 0:0bf9aab1408e 288 msg.frame.data[0] = mode;
haarkon 0:0bf9aab1408e 289 do {
haarkon 0:0bf9aab1408e 290 while(!_Pixy2->writable());
haarkon 0:0bf9aab1408e 291 _Pixy2->putc(msg.data[i]);
haarkon 0:0bf9aab1408e 292 i++;
haarkon 0:0bf9aab1408e 293 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 0:0bf9aab1408e 294 return PIXY2_OK;
haarkon 0:0bf9aab1408e 295 }
haarkon 0:0bf9aab1408e 296
haarkon 0:0bf9aab1408e 297 T_pixy2ErrorCode PIXY2::pixy2_sndSetNextTurn (Word angle){
haarkon 0:0bf9aab1408e 298 T_pixy2SendBuffer msg;
haarkon 0:0bf9aab1408e 299 int i = 0, dataSize = 2;
haarkon 0:0bf9aab1408e 300 T_Word tmp;
haarkon 0:0bf9aab1408e 301 tmp.mot = angle;
haarkon 0:0bf9aab1408e 302 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:0bf9aab1408e 303 msg.frame.header.pixType = PIXY2_SET_TURN;
haarkon 0:0bf9aab1408e 304 msg.frame.header.pixLength = dataSize;
haarkon 0:0bf9aab1408e 305 tmp.mot = angle;
haarkon 0:0bf9aab1408e 306 msg.frame.data[0] = tmp.octet[0];
haarkon 0:0bf9aab1408e 307 msg.frame.data[1] = tmp.octet[1];
haarkon 0:0bf9aab1408e 308 do {
haarkon 0:0bf9aab1408e 309 while(!_Pixy2->writable());
haarkon 0:0bf9aab1408e 310 _Pixy2->putc(msg.data[i]);
haarkon 0:0bf9aab1408e 311 i++;
haarkon 0:0bf9aab1408e 312 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 0:0bf9aab1408e 313 return PIXY2_OK;
haarkon 0:0bf9aab1408e 314 }
haarkon 0:0bf9aab1408e 315
haarkon 0:0bf9aab1408e 316 T_pixy2ErrorCode PIXY2::pixy2_sndSetDefaultTurn (Word angle){
haarkon 0:0bf9aab1408e 317 T_pixy2SendBuffer msg;
haarkon 0:0bf9aab1408e 318 int i = 0, dataSize = 2;
haarkon 0:0bf9aab1408e 319 T_Word tmp;
haarkon 0:0bf9aab1408e 320 tmp.mot = angle;
haarkon 0:0bf9aab1408e 321 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:0bf9aab1408e 322 msg.frame.header.pixType = PIXY2_SET_DEFTURN;
haarkon 0:0bf9aab1408e 323 msg.frame.header.pixLength = dataSize;
haarkon 0:0bf9aab1408e 324 tmp.mot = angle;
haarkon 0:0bf9aab1408e 325 msg.frame.data[0] = tmp.octet[0];
haarkon 0:0bf9aab1408e 326 msg.frame.data[1] = tmp.octet[1];
haarkon 0:0bf9aab1408e 327 do {
haarkon 0:0bf9aab1408e 328 while(!_Pixy2->writable());
haarkon 0:0bf9aab1408e 329 _Pixy2->putc(msg.data[i]);
haarkon 0:0bf9aab1408e 330 i++;
haarkon 0:0bf9aab1408e 331 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 0:0bf9aab1408e 332 return PIXY2_OK;
haarkon 0:0bf9aab1408e 333 }
haarkon 0:0bf9aab1408e 334
haarkon 0:0bf9aab1408e 335 T_pixy2ErrorCode PIXY2::pixy2_sndSetVector (Byte vectorIndex){
haarkon 0:0bf9aab1408e 336 T_pixy2SendBuffer msg;
haarkon 0:0bf9aab1408e 337 int i = 0, dataSize = 1;
haarkon 0:0bf9aab1408e 338 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:0bf9aab1408e 339 msg.frame.header.pixType = PIXY2_SET_VECTOR;
haarkon 0:0bf9aab1408e 340 msg.frame.header.pixLength = dataSize;
haarkon 0:0bf9aab1408e 341 msg.frame.data[0] = vectorIndex;
haarkon 0:0bf9aab1408e 342 do {
haarkon 0:0bf9aab1408e 343 while(!_Pixy2->writable());
haarkon 0:0bf9aab1408e 344 _Pixy2->putc(msg.data[i]);
haarkon 0:0bf9aab1408e 345 i++;
haarkon 0:0bf9aab1408e 346 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 0:0bf9aab1408e 347 return PIXY2_OK;
haarkon 0:0bf9aab1408e 348 }
haarkon 0:0bf9aab1408e 349
haarkon 0:0bf9aab1408e 350 T_pixy2ErrorCode PIXY2::pixy2_sndReverseVector (void){
haarkon 0:0bf9aab1408e 351 T_pixy2SendBuffer msg;
haarkon 0:0bf9aab1408e 352 int i = 0, dataSize = 0;
haarkon 0:0bf9aab1408e 353 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:0bf9aab1408e 354 msg.frame.header.pixType = PIXY2_SET_REVERSE;
haarkon 0:0bf9aab1408e 355 msg.frame.header.pixLength = dataSize;
haarkon 0:0bf9aab1408e 356 do {
haarkon 0:0bf9aab1408e 357 while(!_Pixy2->writable());
haarkon 0:0bf9aab1408e 358 _Pixy2->putc(msg.data[i]);
haarkon 0:0bf9aab1408e 359 i++;
haarkon 0:0bf9aab1408e 360 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 0:0bf9aab1408e 361 return PIXY2_OK;
haarkon 0:0bf9aab1408e 362 }
haarkon 0:0bf9aab1408e 363
haarkon 0:0bf9aab1408e 364 T_pixy2ErrorCode PIXY2::pixy2_sndGetRGB (Word x, Word y, Byte saturate){
haarkon 0:0bf9aab1408e 365 T_pixy2SendBuffer msg;
haarkon 0:0bf9aab1408e 366 int i = 0, dataSize = 5;
haarkon 0:0bf9aab1408e 367 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:0bf9aab1408e 368 msg.frame.header.pixType = PIXY2_ASK_VIDEO;
haarkon 0:0bf9aab1408e 369 msg.frame.header.pixLength = dataSize;
haarkon 0:0bf9aab1408e 370 msg.frame.data[0] = x;
haarkon 0:0bf9aab1408e 371 msg.frame.data[1] = y;
haarkon 0:0bf9aab1408e 372 msg.frame.data[2] = saturate;
haarkon 0:0bf9aab1408e 373 do {
haarkon 0:0bf9aab1408e 374 while(!_Pixy2->writable());
haarkon 0:0bf9aab1408e 375 _Pixy2->putc(msg.data[i]);
haarkon 0:0bf9aab1408e 376 i++;
haarkon 0:0bf9aab1408e 377 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 0:0bf9aab1408e 378 return PIXY2_OK;
haarkon 0:0bf9aab1408e 379 }
haarkon 0:0bf9aab1408e 380
haarkon 0:0bf9aab1408e 381 /* 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é.
haarkon 0:0bf9aab1408e 382 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. */
haarkon 0:0bf9aab1408e 383
haarkon 0:0bf9aab1408e 384 T_pixy2ErrorCode PIXY2::pixy2_getVersion (T_pixy2Version **ptrVersion){
haarkon 0:0bf9aab1408e 385
haarkon 0:0bf9aab1408e 386 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
haarkon 0:0bf9aab1408e 387 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 0:0bf9aab1408e 388
haarkon 0:0bf9aab1408e 389 switch (etat) {
haarkon 0:0bf9aab1408e 390 case idle : // Si la caméra est inactive
haarkon 0:0bf9aab1408e 391 wPointer = 0; // On remonte en haut du buffer
haarkon 0:0bf9aab1408e 392 cr = PIXY2::pixy2_sndGetVersion(); // On envoie la trame de demande de la version
haarkon 0:0bf9aab1408e 393 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 0:0bf9aab1408e 394 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 0:0bf9aab1408e 395 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 0:0bf9aab1408e 396 break;
haarkon 0:0bf9aab1408e 397
haarkon 0:0bf9aab1408e 398 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 0:0bf9aab1408e 399 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 0:0bf9aab1408e 400 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum
haarkon 0:0bf9aab1408e 401 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
haarkon 0:0bf9aab1408e 402 }
haarkon 0:0bf9aab1408e 403 }
haarkon 0:0bf9aab1408e 404 if (msg->pixType == PIXY2_REP_VERS) { // On vérifie que la trame est du type convenable (REPONSE VERSION)
haarkon 0:0bf9aab1408e 405 *ptrVersion = (T_pixy2Version*) &Pixy2_buffer[dPointer]; // On mappe le pointeur de structure sur le buffer de réception.
haarkon 0:0bf9aab1408e 406 } else { // Si ce n'est pas le bon type
haarkon 0:0bf9aab1408e 407 if (msg->pixType == PIXY2_REP_ERROR) { // Cela pourrait être une trame d'erreur
haarkon 0:0bf9aab1408e 408 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
haarkon 0:0bf9aab1408e 409 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 0:0bf9aab1408e 410 }
haarkon 0:0bf9aab1408e 411 etat = idle; // On annonce que la pixy est libre
haarkon 0:0bf9aab1408e 412 break;
haarkon 0:0bf9aab1408e 413
haarkon 0:0bf9aab1408e 414 default : // Dans tous les autres cas
haarkon 0:0bf9aab1408e 415 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 0:0bf9aab1408e 416 break;
haarkon 0:0bf9aab1408e 417 }
haarkon 0:0bf9aab1408e 418 return cr;
haarkon 0:0bf9aab1408e 419 }
haarkon 0:0bf9aab1408e 420
haarkon 0:0bf9aab1408e 421 T_pixy2ErrorCode PIXY2::pixy2_getResolution (T_pixy2Resolution **ptrResolution){
haarkon 0:0bf9aab1408e 422
haarkon 0:0bf9aab1408e 423 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
haarkon 0:0bf9aab1408e 424 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 0:0bf9aab1408e 425
haarkon 0:0bf9aab1408e 426 switch (etat) {
haarkon 0:0bf9aab1408e 427 case idle : // Si la caméra est inactive
haarkon 0:0bf9aab1408e 428 wPointer = 0; // On remonte en haut du buffer
haarkon 0:0bf9aab1408e 429 cr = PIXY2::pixy2_sndGetResolution(); // On envoie la trame de demande de la résolution
haarkon 0:0bf9aab1408e 430 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 0:0bf9aab1408e 431 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 0:0bf9aab1408e 432 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 0:0bf9aab1408e 433 break;
haarkon 0:0bf9aab1408e 434
haarkon 0:0bf9aab1408e 435 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 0:0bf9aab1408e 436 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 0:0bf9aab1408e 437 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum
haarkon 0:0bf9aab1408e 438 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
haarkon 0:0bf9aab1408e 439 }
haarkon 0:0bf9aab1408e 440 }
haarkon 0:0bf9aab1408e 441 if (msg->pixType == PIXY2_REP_RESOL) { // On vérifie que la trame est du type convenable (REPONSE RESOLUTION)
haarkon 0:0bf9aab1408e 442 *ptrResolution = (T_pixy2Resolution*) &Pixy2_buffer[dPointer]; // On mappe le pointeur de structure sur le buffer de réception.
haarkon 0:0bf9aab1408e 443 } else { // Si ce n'est pas le bon type
haarkon 0:0bf9aab1408e 444 if (msg->pixType == PIXY2_REP_ERROR) { // Cela pourrait être une trame d'erreur
haarkon 0:0bf9aab1408e 445 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
haarkon 0:0bf9aab1408e 446 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 0:0bf9aab1408e 447 }
haarkon 0:0bf9aab1408e 448 etat = idle; // On annoce que la pixy est libre
haarkon 0:0bf9aab1408e 449 break;
haarkon 0:0bf9aab1408e 450
haarkon 0:0bf9aab1408e 451 default : // Dans tous les autres cas
haarkon 0:0bf9aab1408e 452 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 0:0bf9aab1408e 453 break;
haarkon 0:0bf9aab1408e 454 }
haarkon 0:0bf9aab1408e 455 return cr;
haarkon 0:0bf9aab1408e 456 }
haarkon 0:0bf9aab1408e 457
haarkon 0:0bf9aab1408e 458 T_pixy2ErrorCode PIXY2::pixy2_setCameraBrightness (Byte brightness){
haarkon 0:0bf9aab1408e 459
haarkon 0:0bf9aab1408e 460 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
haarkon 0:0bf9aab1408e 461 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 0:0bf9aab1408e 462
haarkon 0:0bf9aab1408e 463 switch (etat) {
haarkon 0:0bf9aab1408e 464 case idle : // Si la caméra est inactive
haarkon 0:0bf9aab1408e 465 wPointer = 0; // On remonte en haut du buffer
haarkon 0:0bf9aab1408e 466 cr = PIXY2::pixy2_sndSetCameraBrightness (brightness); // On envoie la trame de règlage de la luminosité
haarkon 0:0bf9aab1408e 467 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 0:0bf9aab1408e 468 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 0:0bf9aab1408e 469 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 0:0bf9aab1408e 470 break;
haarkon 0:0bf9aab1408e 471
haarkon 0:0bf9aab1408e 472 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 0:0bf9aab1408e 473 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 0:0bf9aab1408e 474 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum
haarkon 0:0bf9aab1408e 475 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
haarkon 0:0bf9aab1408e 476 }
haarkon 0:0bf9aab1408e 477 }
haarkon 0:0bf9aab1408e 478 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
haarkon 0:0bf9aab1408e 479 // On vérifie que la trame est du type convenable (ACK ou ERREUR)
haarkon 0:0bf9aab1408e 480 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
haarkon 0:0bf9aab1408e 481 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 0:0bf9aab1408e 482 etat = idle; // On annoce que la pixy est libre
haarkon 0:0bf9aab1408e 483 break;
haarkon 0:0bf9aab1408e 484
haarkon 0:0bf9aab1408e 485 default : // Dans tous les autres cas
haarkon 0:0bf9aab1408e 486 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 0:0bf9aab1408e 487 break;
haarkon 0:0bf9aab1408e 488 }
haarkon 0:0bf9aab1408e 489 return cr;
haarkon 0:0bf9aab1408e 490 }
haarkon 0:0bf9aab1408e 491
haarkon 0:0bf9aab1408e 492 T_pixy2ErrorCode PIXY2::pixy2_setServos (Word s0, Word s1){
haarkon 0:0bf9aab1408e 493
haarkon 0:0bf9aab1408e 494 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
haarkon 0:0bf9aab1408e 495 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 0:0bf9aab1408e 496
haarkon 0:0bf9aab1408e 497 switch (etat) {
haarkon 0:0bf9aab1408e 498 case idle : // Si la caméra est inactive
haarkon 0:0bf9aab1408e 499 wPointer = 0; // On remonte en haut du buffer
haarkon 0:0bf9aab1408e 500 cr = PIXY2::pixy2_sndSetServo (s0, s1); // On envoie la trame de règlage des servos moteurs
haarkon 0:0bf9aab1408e 501 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 0:0bf9aab1408e 502 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 0:0bf9aab1408e 503 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 0:0bf9aab1408e 504 break;
haarkon 0:0bf9aab1408e 505
haarkon 0:0bf9aab1408e 506 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 0:0bf9aab1408e 507 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 0:0bf9aab1408e 508 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum
haarkon 0:0bf9aab1408e 509 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
haarkon 0:0bf9aab1408e 510 }
haarkon 0:0bf9aab1408e 511 }
haarkon 0:0bf9aab1408e 512 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
haarkon 0:0bf9aab1408e 513 // On vérifie que la trame est du type convenable (ACK ou ERREUR)
haarkon 0:0bf9aab1408e 514 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
haarkon 0:0bf9aab1408e 515 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 0:0bf9aab1408e 516 etat = idle; // On annoce que la pixy est libre
haarkon 0:0bf9aab1408e 517 break;
haarkon 0:0bf9aab1408e 518
haarkon 0:0bf9aab1408e 519 default : // Dans tous les autres cas
haarkon 0:0bf9aab1408e 520 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 0:0bf9aab1408e 521 break;
haarkon 0:0bf9aab1408e 522 }
haarkon 0:0bf9aab1408e 523 return cr;
haarkon 0:0bf9aab1408e 524 }
haarkon 0:0bf9aab1408e 525
haarkon 0:0bf9aab1408e 526 T_pixy2ErrorCode PIXY2::pixy2_setLED (Byte red, Byte green, Byte blue){
haarkon 0:0bf9aab1408e 527
haarkon 0:0bf9aab1408e 528 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
haarkon 0:0bf9aab1408e 529 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 0:0bf9aab1408e 530
haarkon 0:0bf9aab1408e 531 switch (etat) {
haarkon 0:0bf9aab1408e 532 case idle : // Si la caméra est inactive
haarkon 0:0bf9aab1408e 533 wPointer = 0; // On remonte en haut du buffer
haarkon 0:0bf9aab1408e 534 cr = PIXY2::pixy2_sndSetLED (red, green, blue); // On envoie la trame de règlage des composantes de la LED RGB
haarkon 0:0bf9aab1408e 535 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 0:0bf9aab1408e 536 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 0:0bf9aab1408e 537 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 0:0bf9aab1408e 538 break;
haarkon 0:0bf9aab1408e 539
haarkon 0:0bf9aab1408e 540 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 0:0bf9aab1408e 541 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 0:0bf9aab1408e 542 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum
haarkon 0:0bf9aab1408e 543 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
haarkon 0:0bf9aab1408e 544 }
haarkon 0:0bf9aab1408e 545 }
haarkon 0:0bf9aab1408e 546 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
haarkon 0:0bf9aab1408e 547 // On vérifie que la trame est du type convenable (ACK ou ERREUR)
haarkon 0:0bf9aab1408e 548 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
haarkon 0:0bf9aab1408e 549 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 0:0bf9aab1408e 550 etat = idle; // On annoce que la pixy est libre
haarkon 0:0bf9aab1408e 551 break;
haarkon 0:0bf9aab1408e 552
haarkon 0:0bf9aab1408e 553 default : // Dans tous les autres cas
haarkon 0:0bf9aab1408e 554 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 0:0bf9aab1408e 555 break;
haarkon 0:0bf9aab1408e 556 }
haarkon 0:0bf9aab1408e 557 return cr;
haarkon 0:0bf9aab1408e 558 }
haarkon 0:0bf9aab1408e 559
haarkon 0:0bf9aab1408e 560 T_pixy2ErrorCode PIXY2::pixy2_setLamp (Byte upper, Byte lower){
haarkon 0:0bf9aab1408e 561
haarkon 0:0bf9aab1408e 562 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
haarkon 0:0bf9aab1408e 563 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 0:0bf9aab1408e 564
haarkon 0:0bf9aab1408e 565 switch (etat) {
haarkon 0:0bf9aab1408e 566 case idle : // Si la caméra est inactive
haarkon 0:0bf9aab1408e 567 wPointer = 0; // On remonte en haut du buffer
haarkon 0:0bf9aab1408e 568 cr = PIXY2::pixy2_sndSetLamp (upper, lower); // On envoie la trame de règlage d'allumage des lumières de contraste
haarkon 0:0bf9aab1408e 569 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 0:0bf9aab1408e 570 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 0:0bf9aab1408e 571 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 0:0bf9aab1408e 572 break;
haarkon 0:0bf9aab1408e 573
haarkon 0:0bf9aab1408e 574 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 0:0bf9aab1408e 575 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 0:0bf9aab1408e 576 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum
haarkon 0:0bf9aab1408e 577 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
haarkon 0:0bf9aab1408e 578 }
haarkon 0:0bf9aab1408e 579 }
haarkon 0:0bf9aab1408e 580 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
haarkon 0:0bf9aab1408e 581 // On vérifie que la trame est du type convenable (ACK ou ERREUR)
haarkon 0:0bf9aab1408e 582 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
haarkon 0:0bf9aab1408e 583 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 0:0bf9aab1408e 584 etat = idle; // On annoce que la pixy est libre
haarkon 0:0bf9aab1408e 585 break;
haarkon 0:0bf9aab1408e 586
haarkon 0:0bf9aab1408e 587 default : // Dans tous les autres cas
haarkon 0:0bf9aab1408e 588 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 0:0bf9aab1408e 589 break;
haarkon 0:0bf9aab1408e 590 }
haarkon 0:0bf9aab1408e 591 return cr;
haarkon 0:0bf9aab1408e 592 }
haarkon 0:0bf9aab1408e 593
haarkon 0:0bf9aab1408e 594 T_pixy2ErrorCode PIXY2::pixy2_getFPS (T_pixy2ReturnCode **framerate){
haarkon 0:0bf9aab1408e 595
haarkon 0:0bf9aab1408e 596 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
haarkon 0:0bf9aab1408e 597 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 0:0bf9aab1408e 598
haarkon 0:0bf9aab1408e 599 switch (etat) {
haarkon 0:0bf9aab1408e 600 case idle : // Si la caméra est inactive
haarkon 0:0bf9aab1408e 601 wPointer = 0; // On remonte en haut du buffer
haarkon 0:0bf9aab1408e 602 cr = PIXY2::pixy2_sndGetFPS(); // On envoie la trame de demande du Framerate
haarkon 0:0bf9aab1408e 603 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 0:0bf9aab1408e 604 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 0:0bf9aab1408e 605 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 0:0bf9aab1408e 606 break;
haarkon 0:0bf9aab1408e 607
haarkon 0:0bf9aab1408e 608 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 0:0bf9aab1408e 609 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 0:0bf9aab1408e 610 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum
haarkon 0:0bf9aab1408e 611 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
haarkon 0:0bf9aab1408e 612 }
haarkon 0:0bf9aab1408e 613 }
haarkon 0:0bf9aab1408e 614 if (msg->pixType == PIXY2_REP_FPS) { // On vérifie que la trame est du type convenable (REPONSE FPS)
haarkon 0:0bf9aab1408e 615 *framerate = (T_pixy2ReturnCode*) &Pixy2_buffer[dPointer]; // On mappe le pointeur de structure sur le buffer de réception.
haarkon 0:0bf9aab1408e 616 } else { // Si ce n'est pas le bon type
haarkon 0:0bf9aab1408e 617 if (msg->pixType == PIXY2_REP_ERROR) { // Cela pourrait être une trame d'erreur
haarkon 0:0bf9aab1408e 618 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
haarkon 0:0bf9aab1408e 619 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 0:0bf9aab1408e 620 }
haarkon 0:0bf9aab1408e 621 etat = idle; // On annoce que la pixy est libre
haarkon 0:0bf9aab1408e 622 break;
haarkon 0:0bf9aab1408e 623
haarkon 0:0bf9aab1408e 624 default : // Dans tous les autres cas
haarkon 0:0bf9aab1408e 625 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 0:0bf9aab1408e 626 break;
haarkon 0:0bf9aab1408e 627 }
haarkon 0:0bf9aab1408e 628 return cr;
haarkon 0:0bf9aab1408e 629 }
haarkon 0:0bf9aab1408e 630
haarkon 0:0bf9aab1408e 631 T_pixy2ErrorCode PIXY2::pixy2_getBlocks (Byte sigmap, Byte maxBloc){
haarkon 0:0bf9aab1408e 632
haarkon 0:0bf9aab1408e 633 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
haarkon 0:0bf9aab1408e 634 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 0:0bf9aab1408e 635
haarkon 0:0bf9aab1408e 636 switch (etat) {
haarkon 0:0bf9aab1408e 637 case idle : // Si la caméra est inactive
haarkon 0:0bf9aab1408e 638 wPointer = 0; // On remonte en haut du buffer
haarkon 0:0bf9aab1408e 639 cr = PIXY2::pixy2_sndGetBlocks(sigmap, maxBloc); // On envoie la trame de demande de blocs de couleur
haarkon 0:0bf9aab1408e 640 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 0:0bf9aab1408e 641 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 0:0bf9aab1408e 642 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 0:0bf9aab1408e 643 break;
haarkon 0:0bf9aab1408e 644
haarkon 0:0bf9aab1408e 645 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 0:0bf9aab1408e 646 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 0:0bf9aab1408e 647 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum
haarkon 0:0bf9aab1408e 648 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
haarkon 0:0bf9aab1408e 649 }
haarkon 0:0bf9aab1408e 650 }
haarkon 0:0bf9aab1408e 651 if (msg->pixType == PIXY2_REP_BLOC) { // On vérifie que la trame est du type convenable (REPONSE BLOCS)
haarkon 0:0bf9aab1408e 652 Pixy2_blocks = (T_pixy2Bloc*) &Pixy2_buffer[dPointer]; // On mappe le pointeur de structure sur le buffer de réception.
haarkon 0:0bf9aab1408e 653 Pixy2_numBlocks = dataSize / sizeof(T_pixy2Bloc); // On indique le nombre de blocs reçus
haarkon 0:0bf9aab1408e 654 } else { // Si ce n'est pas le bon type
haarkon 0:0bf9aab1408e 655 if (msg->pixType == PIXY2_REP_ERROR) { // Cela pourrait être une trame d'erreur
haarkon 0:0bf9aab1408e 656 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
haarkon 0:0bf9aab1408e 657 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 0:0bf9aab1408e 658 }
haarkon 0:0bf9aab1408e 659 etat = idle; // On annoce que la pixy est libre
haarkon 0:0bf9aab1408e 660 break;
haarkon 0:0bf9aab1408e 661
haarkon 0:0bf9aab1408e 662 default : // Dans tous les autres cas
haarkon 0:0bf9aab1408e 663 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 0:0bf9aab1408e 664 break;
haarkon 0:0bf9aab1408e 665 }
haarkon 0:0bf9aab1408e 666 return cr;
haarkon 0:0bf9aab1408e 667 }
haarkon 0:0bf9aab1408e 668
haarkon 0:0bf9aab1408e 669 T_pixy2ErrorCode PIXY2::pixy2_getFeatures (){
haarkon 0:0bf9aab1408e 670
haarkon 0:0bf9aab1408e 671 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
haarkon 0:0bf9aab1408e 672 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 0:0bf9aab1408e 673 T_pixy2LineFeature* lineFeature;
haarkon 0:0bf9aab1408e 674 int fPointer; // Pointeur sur une feature entière
haarkon 0:0bf9aab1408e 675 int fdPointer; // Pointeur sur un élément à l'intérieur d'une feature
haarkon 0:0bf9aab1408e 676
haarkon 0:0bf9aab1408e 677 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 0:0bf9aab1408e 678 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum
haarkon 0:0bf9aab1408e 679 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
haarkon 0:0bf9aab1408e 680 }
haarkon 0:0bf9aab1408e 681 }
haarkon 0:0bf9aab1408e 682 if (msg->pixType == PIXY2_REP_LINE) { // On vérifie que la trame est du type convenable (REPONSE LIGNE)
haarkon 0:0bf9aab1408e 683 fPointer = dPointer; // On pointe sur la premiere feature
haarkon 0:0bf9aab1408e 684 do {
haarkon 0:0bf9aab1408e 685 lineFeature = (T_pixy2LineFeature*) &Pixy2_buffer[fPointer]; // On mappe le pointeur de structure sur le buffer de réception des features.
haarkon 0:0bf9aab1408e 686 if (lineFeature->fType == PIXY2_VECTOR) { // On regarde si le type est vecteur
haarkon 0:0bf9aab1408e 687 Pixy2_numVectors = lineFeature->fLength / sizeof(T_pixy2Vector); // Si oui, on compte combien il y a de vecteurs
haarkon 0:0bf9aab1408e 688 fdPointer = fPointer + 2; // On pointe sur le premier élément de la feature
haarkon 0:0bf9aab1408e 689 Pixy2_vectors = (T_pixy2Vector*) &Pixy2_buffer[fdPointer]; // On mappe le résultat
haarkon 0:0bf9aab1408e 690 fPointer += lineFeature->fLength + 2; // On déplace le pointeur de données et on recommence
haarkon 0:0bf9aab1408e 691 cr |= PIXY2_VECTOR;
haarkon 0:0bf9aab1408e 692 }
haarkon 0:0bf9aab1408e 693 if (lineFeature->fType == PIXY2_INTERSECTION) { // On regarde si le type est intersection
haarkon 0:0bf9aab1408e 694 Pixy2_numIntersections = lineFeature->fLength / sizeof(T_pixy2Intersection);
haarkon 0:0bf9aab1408e 695 // Si oui, on compte combien il y a d'intersections
haarkon 0:0bf9aab1408e 696 fdPointer = fPointer + 2; // On pointe sur le premier élément de la feature
haarkon 0:0bf9aab1408e 697 Pixy2_intersections = (T_pixy2Intersection*) &Pixy2_buffer[fdPointer];
haarkon 0:0bf9aab1408e 698 // On mappe le résultat sur l'entête de l'intersection
haarkon 0:0bf9aab1408e 699 fPointer += lineFeature->fLength + 2; // On déplace le pointeur de données et on recommence
haarkon 0:0bf9aab1408e 700 cr |= PIXY2_INTERSECTION;
haarkon 0:0bf9aab1408e 701 }
haarkon 0:0bf9aab1408e 702 if (lineFeature->fType == PIXY2_BARCODE) { // On regarde si le type est codebarre
haarkon 0:0bf9aab1408e 703 Pixy2_numBarcodes = lineFeature->fLength / sizeof(T_pixy2BarCode);
haarkon 0:0bf9aab1408e 704 // Si oui, on compte combien il y a de codebarre
haarkon 0:0bf9aab1408e 705 fdPointer = fPointer + 2; // On pointe sur le premier élément de la feature
haarkon 0:0bf9aab1408e 706 Pixy2_barcodes = (T_pixy2BarCode*) &Pixy2_buffer[fdPointer]; // On mappe le résultat
haarkon 0:0bf9aab1408e 707 fPointer += lineFeature->fLength + 2; // On déplace le pointeur de données et on recommence
haarkon 0:0bf9aab1408e 708 cr |= PIXY2_BARCODE;
haarkon 0:0bf9aab1408e 709 }
haarkon 0:0bf9aab1408e 710 } while(fPointer < ((dataSize - 1) + dPointer)); // Tant qu'il y a des données à traiter
haarkon 0:0bf9aab1408e 711 } else { // Si ce n'est pas le bon type
haarkon 0:0bf9aab1408e 712 if (msg->pixType == PIXY2_REP_ERROR) { // Cela pourrait être une trame d'erreur ou quand on ne reçoit rien
haarkon 0:0bf9aab1408e 713 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
haarkon 0:0bf9aab1408e 714 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 0:0bf9aab1408e 715 }
haarkon 0:0bf9aab1408e 716 etat = idle; // On annoce que la pixy est libre
haarkon 0:0bf9aab1408e 717 return cr;
haarkon 0:0bf9aab1408e 718 }
haarkon 0:0bf9aab1408e 719
haarkon 0:0bf9aab1408e 720
haarkon 0:0bf9aab1408e 721 T_pixy2ErrorCode PIXY2::pixy2_getMainFeature (Byte features){
haarkon 0:0bf9aab1408e 722
haarkon 0:0bf9aab1408e 723 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 0:0bf9aab1408e 724
haarkon 0:0bf9aab1408e 725 switch (etat) {
haarkon 0:0bf9aab1408e 726 case idle : // Si la caméra est inactive
haarkon 0:0bf9aab1408e 727 wPointer = 0; // On remonte en haut du buffer
haarkon 0:0bf9aab1408e 728 cr = PIXY2::pixy2_sndGetLineFeature(0, features); // On envoie la trame de demande de suivi de ligne
haarkon 0:0bf9aab1408e 729 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 0:0bf9aab1408e 730 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 0:0bf9aab1408e 731 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 0:0bf9aab1408e 732 break;
haarkon 0:0bf9aab1408e 733
haarkon 0:0bf9aab1408e 734 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 0:0bf9aab1408e 735 cr = PIXY2::pixy2_getFeatures(); // On appelle la fonction de traitement.
haarkon 0:0bf9aab1408e 736 break;
haarkon 0:0bf9aab1408e 737
haarkon 0:0bf9aab1408e 738 default : // Dans tous les autres cas
haarkon 0:0bf9aab1408e 739 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 0:0bf9aab1408e 740 break;
haarkon 0:0bf9aab1408e 741 }
haarkon 0:0bf9aab1408e 742 return cr;
haarkon 0:0bf9aab1408e 743 }
haarkon 0:0bf9aab1408e 744
haarkon 0:0bf9aab1408e 745 T_pixy2ErrorCode PIXY2::pixy2_getAllFeature (Byte features){
haarkon 0:0bf9aab1408e 746 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 0:0bf9aab1408e 747
haarkon 0:0bf9aab1408e 748 switch (etat) {
haarkon 0:0bf9aab1408e 749 case idle : // Si la caméra est inactive
haarkon 0:0bf9aab1408e 750 wPointer = 0; // On remonte en haut du buffer
haarkon 0:0bf9aab1408e 751 cr = PIXY2::pixy2_sndGetLineFeature(1, features); // On envoie la trame de demande de suivi de ligne
haarkon 0:0bf9aab1408e 752 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 0:0bf9aab1408e 753 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 0:0bf9aab1408e 754 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 0:0bf9aab1408e 755 break;
haarkon 0:0bf9aab1408e 756
haarkon 0:0bf9aab1408e 757 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 0:0bf9aab1408e 758 cr = PIXY2::pixy2_getFeatures(); // On appelle la fonction de traitement.
haarkon 0:0bf9aab1408e 759 break;
haarkon 0:0bf9aab1408e 760
haarkon 0:0bf9aab1408e 761 default : // Dans tous les autres cas
haarkon 0:0bf9aab1408e 762 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 0:0bf9aab1408e 763 break;
haarkon 0:0bf9aab1408e 764 }
haarkon 0:0bf9aab1408e 765 return cr;
haarkon 0:0bf9aab1408e 766 }
haarkon 0:0bf9aab1408e 767
haarkon 0:0bf9aab1408e 768 T_pixy2ErrorCode PIXY2::pixy2_setMode (Byte mode)
haarkon 0:0bf9aab1408e 769 {
haarkon 0:0bf9aab1408e 770 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
haarkon 0:0bf9aab1408e 771 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 0:0bf9aab1408e 772
haarkon 0:0bf9aab1408e 773 switch (etat) {
haarkon 0:0bf9aab1408e 774 case idle : // Si la caméra est inactive
haarkon 0:0bf9aab1408e 775 wPointer = 0; // On remonte en haut du buffer
haarkon 0:0bf9aab1408e 776 cr = PIXY2::pixy2_sndSetMode (mode); // On envoie la trame de règlage du mode de fonctionnement du suivi de ligne
haarkon 0:0bf9aab1408e 777 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 0:0bf9aab1408e 778 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 0:0bf9aab1408e 779 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 0:0bf9aab1408e 780 break;
haarkon 0:0bf9aab1408e 781
haarkon 0:0bf9aab1408e 782 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 0:0bf9aab1408e 783 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 0:0bf9aab1408e 784 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum
haarkon 0:0bf9aab1408e 785 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
haarkon 0:0bf9aab1408e 786 }
haarkon 0:0bf9aab1408e 787 }
haarkon 0:0bf9aab1408e 788 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
haarkon 0:0bf9aab1408e 789 // On vérifie que la trame est du type convenable (ACK ou ERREUR)
haarkon 0:0bf9aab1408e 790 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
haarkon 0:0bf9aab1408e 791 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 0:0bf9aab1408e 792 etat = idle; // On annoce que la pixy est libre
haarkon 0:0bf9aab1408e 793 break;
haarkon 0:0bf9aab1408e 794
haarkon 0:0bf9aab1408e 795 default : // Dans tous les autres cas
haarkon 0:0bf9aab1408e 796 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 0:0bf9aab1408e 797 break;
haarkon 0:0bf9aab1408e 798 }
haarkon 0:0bf9aab1408e 799 return cr;
haarkon 0:0bf9aab1408e 800 }
haarkon 0:0bf9aab1408e 801
haarkon 0:0bf9aab1408e 802 T_pixy2ErrorCode PIXY2::pixy2_setNextTurn (sWord angle)
haarkon 0:0bf9aab1408e 803 {
haarkon 0:0bf9aab1408e 804 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
haarkon 0:0bf9aab1408e 805 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 0:0bf9aab1408e 806
haarkon 0:0bf9aab1408e 807 switch (etat) {
haarkon 0:0bf9aab1408e 808 case idle : // Si la caméra est inactive
haarkon 0:0bf9aab1408e 809 wPointer = 0; // On remonte en haut du buffer
haarkon 0:0bf9aab1408e 810 cr = PIXY2::pixy2_sndSetNextTurn (angle); // On envoie la trame de choix de l'angle du prochain virage
haarkon 0:0bf9aab1408e 811 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 0:0bf9aab1408e 812 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 0:0bf9aab1408e 813 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 0:0bf9aab1408e 814 break;
haarkon 0:0bf9aab1408e 815
haarkon 0:0bf9aab1408e 816 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 0:0bf9aab1408e 817 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 0:0bf9aab1408e 818 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum
haarkon 0:0bf9aab1408e 819 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
haarkon 0:0bf9aab1408e 820 }
haarkon 0:0bf9aab1408e 821 }
haarkon 0:0bf9aab1408e 822 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
haarkon 0:0bf9aab1408e 823 // On vérifie que la trame est du type convenable (ACK ou ERREUR)
haarkon 0:0bf9aab1408e 824 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
haarkon 0:0bf9aab1408e 825 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 0:0bf9aab1408e 826 etat = idle; // On annoce que la pixy est libre
haarkon 0:0bf9aab1408e 827 break;
haarkon 0:0bf9aab1408e 828
haarkon 0:0bf9aab1408e 829 default : // Dans tous les autres cas
haarkon 0:0bf9aab1408e 830 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 0:0bf9aab1408e 831 break;
haarkon 0:0bf9aab1408e 832 }
haarkon 0:0bf9aab1408e 833 return cr;
haarkon 0:0bf9aab1408e 834 }
haarkon 0:0bf9aab1408e 835
haarkon 0:0bf9aab1408e 836 T_pixy2ErrorCode PIXY2::pixy2_setDefaultTurn (sWord angle)
haarkon 0:0bf9aab1408e 837 {
haarkon 0:0bf9aab1408e 838 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
haarkon 0:0bf9aab1408e 839 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 0:0bf9aab1408e 840
haarkon 0:0bf9aab1408e 841 switch (etat) {
haarkon 0:0bf9aab1408e 842 case idle : // Si la caméra est inactive
haarkon 0:0bf9aab1408e 843 wPointer = 0; // On remonte en haut du buffer
haarkon 0:0bf9aab1408e 844 cr = PIXY2::pixy2_sndSetDefaultTurn (angle); // On envoie la trame de choix de l'angle par défaut des virages
haarkon 0:0bf9aab1408e 845 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 0:0bf9aab1408e 846 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 0:0bf9aab1408e 847 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 0:0bf9aab1408e 848 break;
haarkon 0:0bf9aab1408e 849
haarkon 0:0bf9aab1408e 850 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 0:0bf9aab1408e 851 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 0:0bf9aab1408e 852 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum
haarkon 0:0bf9aab1408e 853 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
haarkon 0:0bf9aab1408e 854 }
haarkon 0:0bf9aab1408e 855 }
haarkon 0:0bf9aab1408e 856 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
haarkon 0:0bf9aab1408e 857 // On vérifie que la trame est du type convenable (ACK ou ERREUR)
haarkon 0:0bf9aab1408e 858 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
haarkon 0:0bf9aab1408e 859 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 0:0bf9aab1408e 860 etat = idle; // On annoce que la pixy est libre
haarkon 0:0bf9aab1408e 861 break;
haarkon 0:0bf9aab1408e 862
haarkon 0:0bf9aab1408e 863 default : // Dans tous les autres cas
haarkon 0:0bf9aab1408e 864 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 0:0bf9aab1408e 865 break;
haarkon 0:0bf9aab1408e 866 }
haarkon 0:0bf9aab1408e 867 return cr;
haarkon 0:0bf9aab1408e 868 }
haarkon 0:0bf9aab1408e 869
haarkon 0:0bf9aab1408e 870 T_pixy2ErrorCode PIXY2::pixy2_setVector (Byte vectorIndex)
haarkon 0:0bf9aab1408e 871 {
haarkon 0:0bf9aab1408e 872 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
haarkon 0:0bf9aab1408e 873 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 0:0bf9aab1408e 874
haarkon 0:0bf9aab1408e 875 switch (etat) {
haarkon 0:0bf9aab1408e 876 case idle : // Si la caméra est inactive
haarkon 0:0bf9aab1408e 877 wPointer = 0; // On remonte en haut du buffer
haarkon 0:0bf9aab1408e 878 cr = PIXY2::pixy2_sndSetVector (vectorIndex); // On envoie la trame de choix du vecteur à suivre
haarkon 0:0bf9aab1408e 879 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 0:0bf9aab1408e 880 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 0:0bf9aab1408e 881 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 0:0bf9aab1408e 882 break;
haarkon 0:0bf9aab1408e 883
haarkon 0:0bf9aab1408e 884 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 0:0bf9aab1408e 885 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 0:0bf9aab1408e 886 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum
haarkon 0:0bf9aab1408e 887 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
haarkon 0:0bf9aab1408e 888 }
haarkon 0:0bf9aab1408e 889 }
haarkon 0:0bf9aab1408e 890 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
haarkon 0:0bf9aab1408e 891 // On vérifie que la trame est du type convenable (ACK ou ERREUR)
haarkon 0:0bf9aab1408e 892 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
haarkon 0:0bf9aab1408e 893 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 0:0bf9aab1408e 894 etat = idle; // On annoce que la pixy est libre
haarkon 0:0bf9aab1408e 895 break;
haarkon 0:0bf9aab1408e 896
haarkon 0:0bf9aab1408e 897 default : // Dans tous les autres cas
haarkon 0:0bf9aab1408e 898 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 0:0bf9aab1408e 899 break;
haarkon 0:0bf9aab1408e 900 }
haarkon 0:0bf9aab1408e 901 return cr;
haarkon 0:0bf9aab1408e 902 }
haarkon 0:0bf9aab1408e 903
haarkon 0:0bf9aab1408e 904
haarkon 0:0bf9aab1408e 905 T_pixy2ErrorCode PIXY2::pixy2_ReverseVector (void)
haarkon 0:0bf9aab1408e 906 {
haarkon 0:0bf9aab1408e 907 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
haarkon 0:0bf9aab1408e 908 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 0:0bf9aab1408e 909
haarkon 0:0bf9aab1408e 910 switch (etat) {
haarkon 0:0bf9aab1408e 911 case idle : // Si la caméra est inactive
haarkon 0:0bf9aab1408e 912 wPointer = 0; // On remonte en haut du buffer
haarkon 0:0bf9aab1408e 913 cr = PIXY2::pixy2_sndReverseVector (); // On envoie la trame d'inversion de l'image (haut en bas et bas en haut)
haarkon 0:0bf9aab1408e 914 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 0:0bf9aab1408e 915 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 0:0bf9aab1408e 916 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 0:0bf9aab1408e 917 break;
haarkon 0:0bf9aab1408e 918
haarkon 0:0bf9aab1408e 919 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 0:0bf9aab1408e 920 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 0:0bf9aab1408e 921 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum
haarkon 0:0bf9aab1408e 922 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
haarkon 0:0bf9aab1408e 923 }
haarkon 0:0bf9aab1408e 924 }
haarkon 0:0bf9aab1408e 925 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
haarkon 0:0bf9aab1408e 926 // On vérifie que la trame est du type convenable (ACK ou ERREUR)
haarkon 0:0bf9aab1408e 927 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
haarkon 0:0bf9aab1408e 928 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 0:0bf9aab1408e 929 etat = idle; // On annoce que la pixy est libre
haarkon 0:0bf9aab1408e 930 break;
haarkon 0:0bf9aab1408e 931
haarkon 0:0bf9aab1408e 932 default : // Dans tous les autres cas
haarkon 0:0bf9aab1408e 933 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 0:0bf9aab1408e 934 break;
haarkon 0:0bf9aab1408e 935 }
haarkon 0:0bf9aab1408e 936 return cr;
haarkon 0:0bf9aab1408e 937 }
haarkon 0:0bf9aab1408e 938
haarkon 0:0bf9aab1408e 939 T_pixy2ErrorCode PIXY2::pixy2_getRGB (Word x, Word y, Byte saturate, T_pixy2Pixel **pixel){
haarkon 0:0bf9aab1408e 940
haarkon 0:0bf9aab1408e 941 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
haarkon 0:0bf9aab1408e 942 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 0:0bf9aab1408e 943
haarkon 0:0bf9aab1408e 944 switch (etat) {
haarkon 0:0bf9aab1408e 945 case idle : // Si la caméra est inactive
haarkon 0:0bf9aab1408e 946 wPointer = 0; // On remonte en haut du buffer
haarkon 0:0bf9aab1408e 947 cr = PIXY2::pixy2_sndGetRGB(x, y, saturate); // On envoie la trame de demande de la couleur (RGB) d'un carée de pixel
haarkon 0:0bf9aab1408e 948 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 0:0bf9aab1408e 949 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 0:0bf9aab1408e 950 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 0:0bf9aab1408e 951 break;
haarkon 0:0bf9aab1408e 952
haarkon 0:0bf9aab1408e 953 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 0:0bf9aab1408e 954 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 0:0bf9aab1408e 955 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum
haarkon 0:0bf9aab1408e 956 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
haarkon 0:0bf9aab1408e 957 }
haarkon 0:0bf9aab1408e 958 }
haarkon 0:0bf9aab1408e 959 if (msg->pixType == PIXY2_REP_ACK) { // On vérifie que la trame est du type convenable (REPONSE ACK)
haarkon 0:0bf9aab1408e 960 *pixel = (T_pixy2Pixel*) &Pixy2_buffer[dPointer]; // On mappe le pointeur de structure sur le buffer de réception.
haarkon 0:0bf9aab1408e 961 } else { // Si ce n'est pas le bon type
haarkon 0:0bf9aab1408e 962 if (msg->pixType == PIXY2_REP_ERROR) { // Cela pourrait être une trame d'erreur
haarkon 0:0bf9aab1408e 963 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
haarkon 0:0bf9aab1408e 964 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 0:0bf9aab1408e 965 }
haarkon 0:0bf9aab1408e 966 etat = idle; // On annoce que la pixy est libre
haarkon 0:0bf9aab1408e 967 break;
haarkon 0:0bf9aab1408e 968
haarkon 0:0bf9aab1408e 969 default : // Dans tous les autres cas
haarkon 0:0bf9aab1408e 970 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 0:0bf9aab1408e 971 break;
haarkon 0:0bf9aab1408e 972 }
haarkon 0:0bf9aab1408e 973 return cr;
haarkon 0:0bf9aab1408e 974 }
haarkon 0:0bf9aab1408e 975
haarkon 0:0bf9aab1408e 976
haarkon 0:0bf9aab1408e 977
haarkon 0:0bf9aab1408e 978 T_pixy2ErrorCode PIXY2::pixy2_validateChecksum (Byte* tab){
haarkon 0:0bf9aab1408e 979 Word i, sum = 0;
haarkon 0:0bf9aab1408e 980 T_Word *tmp;
haarkon 0:0bf9aab1408e 981
haarkon 0:0bf9aab1408e 982 for (i=0; i<*(tab+3);i++) sum = sum + *(tab + PIXY2_CSHEADERSIZE + i);
haarkon 0:0bf9aab1408e 983 tmp = (T_Word*) (tab+4);
haarkon 0:0bf9aab1408e 984
haarkon 0:0bf9aab1408e 985 if (tmp->mot == sum) return PIXY2_OK;
haarkon 0:0bf9aab1408e 986 else {
haarkon 0:0bf9aab1408e 987 if (_DEBUG_) {
haarkon 0:0bf9aab1408e 988 sommeDeControle = sum;
haarkon 0:0bf9aab1408e 989 sommeRecue = tmp->mot;
haarkon 0:0bf9aab1408e 990 }
haarkon 0:0bf9aab1408e 991 return PIXY2_BAD_CHECKSUM;
haarkon 0:0bf9aab1408e 992 }
haarkon 0:0bf9aab1408e 993 }