Waël Hazami
/
Lib_PIXY2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
pixy2.cpp
00001 #include "pixy2.h" 00002 00003 extern int sommeDeControle,sommeRecue; 00004 extern Serial pc; 00005 00006 PIXY2::PIXY2(PinName tx, PinName rx, int debit) : Pixy2_numBlocks(0), Pixy2_numVectors(0), Pixy2_numIntersections(0), Pixy2_numBarcodes(0) 00007 { 00008 _Pixy2 = new Serial (tx, rx, debit); 00009 _Pixy2->attach (callback(this,&PIXY2::pixy2_getByte)); 00010 etat = idle; 00011 Pixy2_buffer = (Byte*) malloc (0x100); 00012 } 00013 00014 PIXY2::~PIXY2() 00015 { 00016 free (Pixy2_buffer); 00017 } 00018 00019 // POUR DEBUG // 00020 T_Pixy2State PIXY2::getEtat() 00021 { 00022 return this->etat; 00023 } 00024 00025 void PIXY2::affDataSize() 00026 { 00027 pc.printf("dataSize : %d\n\r", this->dataSize); 00028 } 00029 // Pour détail d'une trame (a mettre dans la fct correspondante) : 00030 //pc.printf("pixSync : %d, pixType : %d, pixLength : %d, pixChecksum : %d\n\r", msg->pixSync, msg->pixType, msg->pixLength, msg->pixChecksum); 00031 00032 /* 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. 00033 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... 00034 Les étapes de fonctionnement sont les suivantes : 00035 00036 * idle : La caméra n'a pas été solicité => on envoi le message de requête. 00037 * messageSent : La requête a été transmise, mais la réponse n'est pas encore arrivée. 00038 * receivingHeader : Le mot de synchro a été reçu, le reste de l'entête est en cours de réception. 00039 * receivingData : L'entête a été intégralement reçu (et traité) et on est en train de recevoir la payload. 00040 * dataReceived : La réponse a été intégralement reçue et est disponible pour le traitement (et la libération). 00041 00042 On utilise plusieurs pointeurs : 00043 * wPointer est le pointeur d'écriture des octets reçus dans le buffer de réception 00044 * hPointer est le pointeur de l'entête du message 00045 * dPointer est le pointeur de la zone de données 00046 00047 On rajoute une variable : dataSize, qui définit la quantité de données à recevoir (issue de l'entête) 00048 00049 **** COMMENT CA MARCHE **** 00050 00051 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 00052 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. 00053 Quand on trouve le code, on enregistre le point de départ (c'est hPointer) comme étant la position actuelle du wPointer-1. 00054 A partir de ça, on attend de recevoir les 4 premiers octets de l'entête : 00055 * start of frame (hPointer et hPointer + 1) 00056 * identification du type (hPointer + 2) 00057 * taille des données (hPointer + 3) 00058 Et ensuite si on a une trame avec un checksum (normal) : 00059 * le checksum (hPointer + 4 et hPointer + 5) 00060 * les données (hPointer + 6 et +) 00061 Et si on a une trame sans checksum (pas normal) : 00062 * les données (hPointer + 4 et +) 00063 00064 On continue alors de recevoir des octets jusqu'à ce que wPointer soit égal à dPointer + dataSize - 1 00065 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) 00066 ou pour la même quantité de données mais avec une trame sans checksum 7 (4 + 4 - 1)... 00067 */ 00068 00069 void PIXY2::pixy2_getByte () // Interruption de la pixy2 00070 { 00071 T_Word *buffer; 00072 00073 Pixy2_buffer[wPointer] = _Pixy2->getc(); // On stocke l'octet reçu dans la première case dispo du buffer de réception 00074 00075 00076 switch (etat) { 00077 case messageSent : // Si on a envoyé une requete => on attend un entête 00078 if (wPointer > 0) { // On attend d'avoir reçu 2 octets 00079 buffer = (T_Word*) &Pixy2_buffer[wPointer-1]; // On pointe la structure sur les 2 derniers octets reçus 00080 if ((buffer->mot == PIXY2_CSSYNC) || (buffer->mot == PIXY2_SYNC)) { // Si c'est un mot d'entête 00081 etat = receivingHeader; // On passe à l'état réception de l'entête 00082 hPointer = wPointer - 1; // On initialise le pointeur de l'entête 00083 if (buffer->mot == PIXY2_SYNC) { 00084 frameContainChecksum = 0; // Si c'est un entête sans checksum, on mémorise qu'il n'y a pas de checksum à vérifier 00085 dPointer = hPointer + PIXY2_NCSHEADERSIZE; 00086 } else { 00087 frameContainChecksum = 1; // Sinon, on mémorise qu'il y a un checksum à vérifier 00088 dPointer = hPointer + PIXY2_CSHEADERSIZE; 00089 } 00090 } // Si on n'a pas de mot d'entête on attend d'en trouver un... 00091 } 00092 break; 00093 00094 case receivingHeader : // Si on est en train de recevoir un entête (entre le SYNC et... La fin de l'entête) 00095 if ((frameContainChecksum && ((wPointer - hPointer) == (PIXY2_CSHEADERSIZE - 1))) || (!frameContainChecksum && ((wPointer - hPointer) == (PIXY2_NCSHEADERSIZE - 1)))) { 00096 // 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 00097 etat = receivingData; // On dit que l'on va de recevoir des données 00098 dataSize = Pixy2_buffer[hPointer + 3]; // On enregistre la taille de la payload 00099 if (dataSize == 0) // Si on reçoit rien, on reset le processus 00100 etat = idle; 00101 } 00102 break; 00103 00104 case receivingData : // Si on est en train de recevoir des données. 00105 if (wPointer == ((dataSize - 1) + dPointer)) { 00106 etat = dataReceived; // Quand on a reçu toutes les données promises on dit que c'est OK 00107 } 00108 break; 00109 00110 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. 00111 break; 00112 } 00113 wPointer++; // on pointe la case suivante du buffer de réception 00114 00115 } 00116 00117 T_pixy2ErrorCode PIXY2::pixy2_sndGetVersion (void){ 00118 T_pixy2SendBuffer msg; 00119 int i = 0, dataSize = 0; 00120 msg.frame.header.pixSync = PIXY2_SYNC; 00121 msg.frame.header.pixType = PIXY2_ASK_VERS; 00122 msg.frame.header.pixLength = dataSize; 00123 do { 00124 while(!_Pixy2->writable()); 00125 _Pixy2->putc(msg.data[i]); 00126 i++; 00127 } while (i<(PIXY2_NCSHEADERSIZE+dataSize)); 00128 return PIXY2_OK; 00129 } 00130 00131 T_pixy2ErrorCode PIXY2::pixy2_sndGetResolution (void){ 00132 T_pixy2SendBuffer msg; 00133 int i = 0, dataSize = 1; 00134 msg.frame.header.pixSync = PIXY2_SYNC; 00135 msg.frame.header.pixType = PIXY2_ASK_RESOL; 00136 msg.frame.header.pixLength = dataSize; 00137 msg.frame.data[0] = 0; 00138 do { 00139 while(!_Pixy2->writable()); 00140 _Pixy2->putc(msg.data[i]); 00141 i++; 00142 } while (i<(PIXY2_NCSHEADERSIZE+dataSize)); 00143 return PIXY2_OK; 00144 } 00145 00146 T_pixy2ErrorCode PIXY2::pixy2_sndSetCameraBrightness (Byte brightness){ 00147 T_pixy2SendBuffer msg; 00148 int i = 0, dataSize = 1; 00149 msg.frame.header.pixSync = PIXY2_SYNC; 00150 msg.frame.header.pixType = PIXY2_SET_BRIGHT; 00151 msg.frame.header.pixLength = dataSize; 00152 msg.frame.data[0] = brightness; 00153 do { 00154 while(!_Pixy2->writable()); 00155 _Pixy2->putc(msg.data[i]); 00156 i++; 00157 } while (i<(PIXY2_NCSHEADERSIZE+dataSize)); 00158 return PIXY2_OK; 00159 } 00160 00161 T_pixy2ErrorCode PIXY2::pixy2_sndSetServo (Word s0, Word s1){ 00162 T_pixy2SendBuffer msg; 00163 int i = 0, dataSize = 4; 00164 T_Word tmp; 00165 msg.frame.header.pixSync = PIXY2_SYNC; 00166 msg.frame.header.pixType = PIXY2_SET_SERVOS; 00167 msg.frame.header.pixLength = dataSize; 00168 tmp.mot = s0; 00169 msg.frame.data[0] = tmp.octet[0]; 00170 msg.frame.data[1] = tmp.octet[1]; 00171 tmp.mot = s1; 00172 msg.frame.data[2] = tmp.octet[0]; 00173 msg.frame.data[3] = tmp.octet[1]; 00174 do { 00175 while(!_Pixy2->writable()); 00176 _Pixy2->putc(msg.data[i]); 00177 i++; 00178 } while (i<(PIXY2_NCSHEADERSIZE+dataSize)); 00179 return PIXY2_OK; 00180 } 00181 00182 T_pixy2ErrorCode PIXY2::pixy2_sndSetLED (Byte red, Byte green, Byte blue){ 00183 T_pixy2SendBuffer msg; 00184 int i = 0, dataSize = 3; 00185 msg.frame.header.pixSync = PIXY2_SYNC; 00186 msg.frame.header.pixType = PIXY2_SET_LED; 00187 msg.frame.header.pixLength = dataSize; 00188 msg.frame.data[0] = red; 00189 msg.frame.data[1] = green; 00190 msg.frame.data[2] = blue; 00191 do { 00192 while(!_Pixy2->writable()); 00193 _Pixy2->putc(msg.data[i]); 00194 i++; 00195 } while (i<(PIXY2_NCSHEADERSIZE+dataSize)); 00196 return PIXY2_OK; 00197 } 00198 00199 T_pixy2ErrorCode PIXY2::pixy2_sndSetLamp (Byte upper, Byte lower){ 00200 T_pixy2SendBuffer msg; 00201 int i = 0, dataSize = 2; 00202 msg.frame.header.pixSync = PIXY2_SYNC; 00203 msg.frame.header.pixType = PIXY2_SET_LAMP; 00204 msg.frame.header.pixLength = dataSize; 00205 msg.frame.data[0] = upper; 00206 msg.frame.data[1] = lower; 00207 do { 00208 while(!_Pixy2->writable()); 00209 _Pixy2->putc(msg.data[i]); 00210 i++; 00211 } while (i<(PIXY2_NCSHEADERSIZE+dataSize)); 00212 return PIXY2_OK; 00213 } 00214 00215 T_pixy2ErrorCode PIXY2::pixy2_sndGetFPS (void){ 00216 T_pixy2SendBuffer msg; 00217 int i = 0, dataSize = 0; 00218 msg.frame.header.pixSync = PIXY2_SYNC; 00219 msg.frame.header.pixType = PIXY2_ASK_FPS; 00220 msg.frame.header.pixLength = dataSize; 00221 do { 00222 while(!_Pixy2->writable()); 00223 _Pixy2->putc(msg.data[i]); 00224 i++; 00225 } while (i<(PIXY2_NCSHEADERSIZE+dataSize)); 00226 return PIXY2_OK; 00227 } 00228 00229 T_pixy2ErrorCode PIXY2::pixy2_sndGetBlocks (Byte sigmap, Byte maxBloc){ 00230 T_pixy2SendBuffer msg; 00231 int i = 0, dataSize = 2; 00232 msg.frame.header.pixSync = PIXY2_SYNC; 00233 msg.frame.header.pixType = PIXY2_ASK_BLOC; 00234 msg.frame.header.pixLength = dataSize; 00235 msg.frame.data[0] = sigmap; 00236 msg.frame.data[1] = maxBloc; 00237 do { 00238 while(!_Pixy2->writable()); 00239 _Pixy2->putc(msg.data[i]); 00240 i++; 00241 } while (i<(PIXY2_NCSHEADERSIZE+dataSize)); 00242 return PIXY2_OK; 00243 } 00244 00245 T_pixy2ErrorCode PIXY2::pixy2_sndGetLineFeature (Byte type, Byte feature){ 00246 T_pixy2SendBuffer msg; 00247 int i = 0, dataSize = 2; 00248 msg.frame.header.pixSync = PIXY2_SYNC; 00249 msg.frame.header.pixType = PIXY2_ASK_LINE; 00250 msg.frame.header.pixLength = dataSize; 00251 msg.frame.data[0] = type; 00252 msg.frame.data[1] = feature; 00253 do { 00254 while(!_Pixy2->writable()); 00255 _Pixy2->putc(msg.data[i]); 00256 i++; 00257 } while (i<(PIXY2_NCSHEADERSIZE+dataSize)); 00258 return PIXY2_OK; 00259 } 00260 00261 T_pixy2ErrorCode PIXY2::pixy2_sndSetMode (Byte mode){ 00262 T_pixy2SendBuffer msg; 00263 int i = 0, dataSize = 1; 00264 msg.frame.header.pixSync = PIXY2_SYNC; 00265 msg.frame.header.pixType = PIXY2_SET_MODE; 00266 msg.frame.header.pixLength = dataSize; 00267 msg.frame.data[0] = mode; 00268 do { 00269 while(!_Pixy2->writable()); 00270 _Pixy2->putc(msg.data[i]); 00271 i++; 00272 } while (i<(PIXY2_NCSHEADERSIZE+dataSize)); 00273 return PIXY2_OK; 00274 } 00275 00276 T_pixy2ErrorCode PIXY2::pixy2_sndSetNextTurn (Word angle){ 00277 T_pixy2SendBuffer msg; 00278 int i = 0, dataSize = 2; 00279 T_Word tmp; 00280 tmp.mot = angle; 00281 msg.frame.header.pixSync = PIXY2_SYNC; 00282 msg.frame.header.pixType = PIXY2_SET_TURN; 00283 msg.frame.header.pixLength = dataSize; 00284 tmp.mot = angle; 00285 msg.frame.data[0] = tmp.octet[0]; 00286 msg.frame.data[1] = tmp.octet[1]; 00287 do { 00288 while(!_Pixy2->writable()); 00289 _Pixy2->putc(msg.data[i]); 00290 i++; 00291 } while (i<(PIXY2_NCSHEADERSIZE+dataSize)); 00292 return PIXY2_OK; 00293 } 00294 00295 T_pixy2ErrorCode PIXY2::pixy2_sndSetDefaultTurn (Word angle){ 00296 T_pixy2SendBuffer msg; 00297 int i = 0, dataSize = 2; 00298 T_Word tmp; 00299 tmp.mot = angle; 00300 msg.frame.header.pixSync = PIXY2_SYNC; 00301 msg.frame.header.pixType = PIXY2_SET_DEFTURN; 00302 msg.frame.header.pixLength = dataSize; 00303 tmp.mot = angle; 00304 msg.frame.data[0] = tmp.octet[0]; 00305 msg.frame.data[1] = tmp.octet[1]; 00306 do { 00307 while(!_Pixy2->writable()); 00308 _Pixy2->putc(msg.data[i]); 00309 i++; 00310 } while (i<(PIXY2_NCSHEADERSIZE+dataSize)); 00311 return PIXY2_OK; 00312 } 00313 00314 T_pixy2ErrorCode PIXY2::pixy2_sndSetVector (Byte vectorIndex){ 00315 T_pixy2SendBuffer msg; 00316 int i = 0, dataSize = 1; 00317 msg.frame.header.pixSync = PIXY2_SYNC; 00318 msg.frame.header.pixType = PIXY2_SET_VECTOR; 00319 msg.frame.header.pixLength = dataSize; 00320 msg.frame.data[0] = vectorIndex; 00321 do { 00322 while(!_Pixy2->writable()); 00323 _Pixy2->putc(msg.data[i]); 00324 i++; 00325 } while (i<(PIXY2_NCSHEADERSIZE+dataSize)); 00326 return PIXY2_OK; 00327 } 00328 00329 T_pixy2ErrorCode PIXY2::pixy2_sndReverseVector (void){ 00330 T_pixy2SendBuffer msg; 00331 int i = 0, dataSize = 0; 00332 msg.frame.header.pixSync = PIXY2_SYNC; 00333 msg.frame.header.pixType = PIXY2_SET_REVERSE; 00334 msg.frame.header.pixLength = dataSize; 00335 do { 00336 while(!_Pixy2->writable()); 00337 _Pixy2->putc(msg.data[i]); 00338 i++; 00339 } while (i<(PIXY2_NCSHEADERSIZE+dataSize)); 00340 return PIXY2_OK; 00341 } 00342 00343 T_pixy2ErrorCode PIXY2::pixy2_sndGetRGB (Word x, Word y, Byte saturate){ 00344 T_pixy2SendBuffer msg; 00345 int i = 0, dataSize = 5; 00346 msg.frame.header.pixSync = PIXY2_SYNC; 00347 msg.frame.header.pixType = PIXY2_ASK_VIDEO; 00348 msg.frame.header.pixLength = dataSize; 00349 msg.frame.data[0] = x; 00350 msg.frame.data[1] = y; 00351 msg.frame.data[2] = saturate; 00352 do { 00353 while(!_Pixy2->writable()); 00354 _Pixy2->putc(msg.data[i]); 00355 i++; 00356 } while (i<(PIXY2_NCSHEADERSIZE+dataSize)); 00357 return PIXY2_OK; 00358 } 00359 00360 /* 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é. 00361 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. */ 00362 00363 T_pixy2ErrorCode PIXY2::pixy2_getVersion (T_pixy2Version **ptrVersion){ 00364 00365 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer]; 00366 T_pixy2ErrorCode cr = PIXY2_OK; 00367 00368 switch (etat) { 00369 case idle : // Si la caméra est inactive 00370 wPointer = 0; // On remonte en haut du buffer 00371 cr = PIXY2::pixy2_sndGetVersion(); // On envoie la trame de demande de la version 00372 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte ! 00373 etat = messageSent; // On passe à l'attente du message de réponse 00374 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée 00375 break; 00376 00377 case dataReceived : // Quand on a reçu l'intégralité du message 00378 if (frameContainChecksum) { // Si la trame contient un checksum 00379 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum 00380 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur 00381 } 00382 } 00383 if (msg->pixType == PIXY2_REP_VERS) { // On vérifie que la trame est du type convenable (REPONSE VERSION) 00384 *ptrVersion = (T_pixy2Version*) &Pixy2_buffer[dPointer]; // On mappe le pointeur de structure sur le buffer de réception. 00385 } else { // Si ce n'est pas le bon type 00386 if (msg->pixType == PIXY2_REP_ERROR) { // Cela pourrait être une trame d'erreur 00387 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour 00388 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type. 00389 } 00390 etat = idle; // On annonce que la pixy est libre 00391 break; 00392 00393 default : // Dans tous les autres cas 00394 cr = PIXY2_BUSY; // On signale que la caméra est occupée. 00395 break; 00396 } 00397 return cr; 00398 } 00399 00400 T_pixy2ErrorCode PIXY2::pixy2_getResolution (T_pixy2Resolution **ptrResolution){ 00401 00402 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer]; 00403 T_pixy2ErrorCode cr = PIXY2_OK; 00404 00405 switch (etat) { 00406 case idle : // Si la caméra est inactive 00407 wPointer = 0; // On remonte en haut du buffer 00408 cr = PIXY2::pixy2_sndGetResolution(); // On envoie la trame de demande de la résolution 00409 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte ! 00410 etat = messageSent; // On passe à l'attente du message de réponse 00411 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée 00412 break; 00413 00414 case dataReceived : // Quand on a reçu l'intégralité du message 00415 if (frameContainChecksum) { // Si la trame contient un checksum 00416 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum 00417 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur 00418 } 00419 } 00420 if (msg->pixType == PIXY2_REP_RESOL) { // On vérifie que la trame est du type convenable (REPONSE RESOLUTION) 00421 *ptrResolution = (T_pixy2Resolution*) &Pixy2_buffer[dPointer]; // On mappe le pointeur de structure sur le buffer de réception. 00422 } else { // Si ce n'est pas le bon type 00423 if (msg->pixType == PIXY2_REP_ERROR) { // Cela pourrait être une trame d'erreur 00424 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour 00425 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type. 00426 } 00427 etat = idle; // On annoce que la pixy est libre 00428 break; 00429 00430 default : // Dans tous les autres cas 00431 cr = PIXY2_BUSY; // On signale que la caméra est occupée. 00432 break; 00433 } 00434 return cr; 00435 } 00436 00437 T_pixy2ErrorCode PIXY2::pixy2_setCameraBrightness (Byte brightness){ 00438 00439 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer]; 00440 T_pixy2ErrorCode cr = PIXY2_OK; 00441 00442 switch (etat) { 00443 case idle : // Si la caméra est inactive 00444 wPointer = 0; // On remonte en haut du buffer 00445 cr = PIXY2::pixy2_sndSetCameraBrightness (brightness); // On envoie la trame de règlage de la luminosité 00446 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte ! 00447 etat = messageSent; // On passe à l'attente du message de réponse 00448 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée 00449 break; 00450 00451 case dataReceived : // Quand on a reçu l'intégralité du message 00452 if (frameContainChecksum) { // Si la trame contient un checksum 00453 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum 00454 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur 00455 } 00456 } 00457 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) { 00458 // On vérifie que la trame est du type convenable (ACK ou ERREUR) 00459 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour 00460 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type. 00461 etat = idle; // On annoce que la pixy est libre 00462 break; 00463 00464 default : // Dans tous les autres cas 00465 cr = PIXY2_BUSY; // On signale que la caméra est occupée. 00466 break; 00467 } 00468 return cr; 00469 } 00470 00471 T_pixy2ErrorCode PIXY2::pixy2_setServos (Word s0, Word s1){ 00472 00473 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer]; 00474 T_pixy2ErrorCode cr = PIXY2_OK; 00475 00476 switch (etat) { 00477 case idle : // Si la caméra est inactive 00478 wPointer = 0; // On remonte en haut du buffer 00479 cr = PIXY2::pixy2_sndSetServo (s0, s1); // On envoie la trame de règlage des servos moteurs 00480 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte ! 00481 etat = messageSent; // On passe à l'attente du message de réponse 00482 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée 00483 break; 00484 00485 case dataReceived : // Quand on a reçu l'intégralité du message 00486 if (frameContainChecksum) { // Si la trame contient un checksum 00487 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum 00488 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur 00489 } 00490 } 00491 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) { 00492 // On vérifie que la trame est du type convenable (ACK ou ERREUR) 00493 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour 00494 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type. 00495 etat = idle; // On annoce que la pixy est libre 00496 break; 00497 00498 default : // Dans tous les autres cas 00499 cr = PIXY2_BUSY; // On signale que la caméra est occupée. 00500 break; 00501 } 00502 return cr; 00503 } 00504 00505 T_pixy2ErrorCode PIXY2::pixy2_setLED (Byte red, Byte green, Byte blue){ 00506 00507 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer]; 00508 T_pixy2ErrorCode cr = PIXY2_OK; 00509 00510 switch (etat) { 00511 case idle : // Si la caméra est inactive 00512 wPointer = 0; // On remonte en haut du buffer 00513 cr = PIXY2::pixy2_sndSetLED (red, green, blue); // On envoie la trame de règlage des composantes de la LED RGB 00514 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte ! 00515 etat = messageSent; // On passe à l'attente du message de réponse 00516 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée 00517 break; 00518 00519 case dataReceived : // Quand on a reçu l'intégralité du message 00520 if (frameContainChecksum) { // Si la trame contient un checksum 00521 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum 00522 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur 00523 } 00524 } 00525 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) { 00526 // On vérifie que la trame est du type convenable (ACK ou ERREUR) 00527 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour 00528 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type. 00529 etat = idle; // On annoce que la pixy est libre 00530 break; 00531 00532 default : // Dans tous les autres cas 00533 cr = PIXY2_BUSY; // On signale que la caméra est occupée. 00534 break; 00535 } 00536 return cr; 00537 } 00538 00539 T_pixy2ErrorCode PIXY2::pixy2_setLamp (Byte upper, Byte lower){ 00540 00541 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer]; 00542 T_pixy2ErrorCode cr = PIXY2_OK; 00543 00544 switch (etat) { 00545 case idle : // Si la caméra est inactive 00546 wPointer = 0; // On remonte en haut du buffer 00547 cr = PIXY2::pixy2_sndSetLamp (upper, lower); // On envoie la trame de règlage d'allumage des lumières de contraste 00548 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte ! 00549 etat = messageSent; // On passe à l'attente du message de réponse 00550 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée 00551 break; 00552 00553 case dataReceived : // Quand on a reçu l'intégralité du message 00554 if (frameContainChecksum) { // Si la trame contient un checksum 00555 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum 00556 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur 00557 } 00558 } 00559 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) { 00560 // On vérifie que la trame est du type convenable (ACK ou ERREUR) 00561 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour 00562 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type. 00563 etat = idle; // On annoce que la pixy est libre 00564 break; 00565 00566 default : // Dans tous les autres cas 00567 cr = PIXY2_BUSY; // On signale que la caméra est occupée. 00568 break; 00569 } 00570 return cr; 00571 } 00572 00573 T_pixy2ErrorCode PIXY2::pixy2_getFPS (T_pixy2ReturnCode **framerate){ 00574 00575 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer]; 00576 T_pixy2ErrorCode cr = PIXY2_OK; 00577 00578 switch (etat) { 00579 case idle : // Si la caméra est inactive 00580 wPointer = 0; // On remonte en haut du buffer 00581 cr = PIXY2::pixy2_sndGetFPS(); // On envoie la trame de demande du Framerate 00582 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte ! 00583 etat = messageSent; // On passe à l'attente du message de réponse 00584 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée 00585 break; 00586 00587 case dataReceived : // Quand on a reçu l'intégralité du message 00588 if (frameContainChecksum) { // Si la trame contient un checksum 00589 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum 00590 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur 00591 } 00592 } 00593 if (msg->pixType == PIXY2_REP_FPS) { // On vérifie que la trame est du type convenable (REPONSE FPS) 00594 *framerate = (T_pixy2ReturnCode*) &Pixy2_buffer[dPointer]; // On mappe le pointeur de structure sur le buffer de réception. 00595 } else { // Si ce n'est pas le bon type 00596 if (msg->pixType == PIXY2_REP_ERROR) { // Cela pourrait être une trame d'erreur 00597 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour 00598 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type. 00599 } 00600 etat = idle; // On annoce que la pixy est libre 00601 break; 00602 00603 default : // Dans tous les autres cas 00604 cr = PIXY2_BUSY; // On signale que la caméra est occupée. 00605 break; 00606 } 00607 return cr; 00608 } 00609 00610 T_pixy2ErrorCode PIXY2::pixy2_getBlocks (Byte sigmap, Byte maxBloc){ 00611 00612 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer]; 00613 T_pixy2ErrorCode cr = PIXY2_OK; 00614 00615 switch (etat) { 00616 case idle : // Si la caméra est inactive 00617 wPointer = 0; // On remonte en haut du buffer 00618 cr = PIXY2::pixy2_sndGetBlocks(sigmap, maxBloc); // On envoie la trame de demande de blocs de couleur 00619 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte ! 00620 etat = messageSent; // On passe à l'attente du message de réponse 00621 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée 00622 break; 00623 00624 case dataReceived : // Quand on a reçu l'intégralité du message 00625 if (frameContainChecksum) { // Si la trame contient un checksum 00626 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum 00627 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur 00628 } 00629 } 00630 if (msg->pixType == PIXY2_REP_BLOC) { // On vérifie que la trame est du type convenable (REPONSE BLOCS) 00631 Pixy2_blocks = (T_pixy2Bloc*) &Pixy2_buffer[dPointer]; // On mappe le pointeur de structure sur le buffer de réception. 00632 Pixy2_numBlocks = dataSize / sizeof(T_pixy2Bloc); // On indique le nombre de blocs reçus 00633 } else { // Si ce n'est pas le bon type 00634 if (msg->pixType == PIXY2_REP_ERROR) { // Cela pourrait être une trame d'erreur 00635 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour 00636 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type. 00637 } 00638 etat = idle; // On annoce que la pixy est libre 00639 break; 00640 00641 default : // Dans tous les autres cas 00642 cr = PIXY2_BUSY; // On signale que la caméra est occupée. 00643 break; 00644 } 00645 return cr; 00646 } 00647 00648 T_pixy2ErrorCode PIXY2::pixy2_getFeatures (){ 00649 00650 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer]; 00651 T_pixy2ErrorCode cr = PIXY2_OK; 00652 T_pixy2LineFeature* lineFeature; 00653 int fPointer; // Pointeur sur une feature entière 00654 int fdPointer; // Pointeur sur un élément à l'intérieur d'une feature 00655 00656 if (frameContainChecksum) { // Si la trame contient un checksum 00657 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum 00658 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur 00659 } 00660 } 00661 if (msg->pixType == PIXY2_REP_LINE) { // On vérifie que la trame est du type convenable (REPONSE LIGNE) 00662 fPointer = dPointer; // On pointe sur la premiere feature 00663 do { 00664 lineFeature = (T_pixy2LineFeature*) &Pixy2_buffer[fPointer]; // On mappe le pointeur de structure sur le buffer de réception des features. 00665 if (lineFeature->fType == PIXY2_VECTOR) { // On regarde si le type est vecteur 00666 Pixy2_numVectors = lineFeature->fLength / sizeof(T_pixy2Vector); // Si oui, on compte combien il y a de vecteurs 00667 fdPointer = fPointer + 2; // On pointe sur le premier élément de la feature 00668 Pixy2_vectors = (T_pixy2Vector*) &Pixy2_buffer[fdPointer]; // On mappe le résultat 00669 fPointer += lineFeature->fLength + 2; // On déplace le pointeur de données et on recommence 00670 cr |= PIXY2_VECTOR; 00671 } 00672 if (lineFeature->fType == PIXY2_INTERSECTION) { // On regarde si le type est intersection 00673 Pixy2_numIntersections = lineFeature->fLength / sizeof(T_pixy2Intersection); 00674 // Si oui, on compte combien il y a d'intersections 00675 fdPointer = fPointer + 2; // On pointe sur le premier élément de la feature 00676 Pixy2_intersections = (T_pixy2Intersection*) &Pixy2_buffer[fdPointer]; 00677 // On mappe le résultat sur l'entête de l'intersection 00678 fPointer += lineFeature->fLength + 2; // On déplace le pointeur de données et on recommence 00679 cr |= PIXY2_INTERSECTION; 00680 } 00681 if (lineFeature->fType == PIXY2_BARCODE) { // On regarde si le type est codebarre 00682 Pixy2_numBarcodes = lineFeature->fLength / sizeof(T_pixy2BarCode); 00683 // Si oui, on compte combien il y a de codebarre 00684 fdPointer = fPointer + 2; // On pointe sur le premier élément de la feature 00685 Pixy2_barcodes = (T_pixy2BarCode*) &Pixy2_buffer[fdPointer]; // On mappe le résultat 00686 fPointer += lineFeature->fLength + 2; // On déplace le pointeur de données et on recommence 00687 cr |= PIXY2_BARCODE; 00688 } 00689 } while(fPointer < ((dataSize - 1) + dPointer)); // Tant qu'il y a des données à traiter 00690 } else { // Si ce n'est pas le bon type 00691 if (msg->pixType == PIXY2_REP_ERROR) { // Cela pourrait être une trame d'erreur ou quand on ne reçoit rien 00692 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour 00693 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type. 00694 } 00695 etat = idle; // On annoce que la pixy est libre 00696 return cr; 00697 } 00698 00699 00700 T_pixy2ErrorCode PIXY2::pixy2_getMainFeature (Byte features){ 00701 00702 T_pixy2ErrorCode cr = PIXY2_OK; 00703 00704 switch (etat) { 00705 case idle : // Si la caméra est inactive 00706 wPointer = 0; // On remonte en haut du buffer 00707 cr = PIXY2::pixy2_sndGetLineFeature(0, features); // On envoie la trame de demande de suivi de ligne 00708 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte ! 00709 etat = messageSent; // On passe à l'attente du message de réponse 00710 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée 00711 break; 00712 00713 case dataReceived : // Quand on a reçu l'intégralité du message 00714 cr = PIXY2::pixy2_getFeatures(); // On appelle la fonction de traitement. 00715 break; 00716 00717 default : // Dans tous les autres cas 00718 cr = PIXY2_BUSY; // On signale que la caméra est occupée. 00719 break; 00720 } 00721 return cr; 00722 } 00723 00724 T_pixy2ErrorCode PIXY2::pixy2_getAllFeature (Byte features){ 00725 T_pixy2ErrorCode cr = PIXY2_OK; 00726 00727 switch (etat) { 00728 case idle : // Si la caméra est inactive 00729 wPointer = 0; // On remonte en haut du buffer 00730 cr = PIXY2::pixy2_sndGetLineFeature(1, features); // On envoie la trame de demande de suivi de ligne 00731 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte ! 00732 etat = messageSent; // On passe à l'attente du message de réponse 00733 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée 00734 break; 00735 00736 case dataReceived : // Quand on a reçu l'intégralité du message 00737 cr = PIXY2::pixy2_getFeatures(); // On appelle la fonction de traitement. 00738 break; 00739 00740 default : // Dans tous les autres cas 00741 cr = PIXY2_BUSY; // On signale que la caméra est occupée. 00742 break; 00743 } 00744 return cr; 00745 } 00746 00747 T_pixy2ErrorCode PIXY2::pixy2_setMode (Byte mode) 00748 { 00749 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer]; 00750 T_pixy2ErrorCode cr = PIXY2_OK; 00751 00752 switch (etat) { 00753 case idle : // Si la caméra est inactive 00754 wPointer = 0; // On remonte en haut du buffer 00755 cr = PIXY2::pixy2_sndSetMode (mode); // On envoie la trame de règlage du mode de fonctionnement du suivi de ligne 00756 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte ! 00757 etat = messageSent; // On passe à l'attente du message de réponse 00758 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée 00759 break; 00760 00761 case dataReceived : // Quand on a reçu l'intégralité du message 00762 if (frameContainChecksum) { // Si la trame contient un checksum 00763 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum 00764 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur 00765 } 00766 } 00767 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) { 00768 // On vérifie que la trame est du type convenable (ACK ou ERREUR) 00769 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour 00770 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type. 00771 etat = idle; // On annoce que la pixy est libre 00772 break; 00773 00774 default : // Dans tous les autres cas 00775 cr = PIXY2_BUSY; // On signale que la caméra est occupée. 00776 break; 00777 } 00778 return cr; 00779 } 00780 00781 T_pixy2ErrorCode PIXY2::pixy2_setNextTurn (sWord angle) 00782 { 00783 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer]; 00784 T_pixy2ErrorCode cr = PIXY2_OK; 00785 00786 switch (etat) { 00787 case idle : // Si la caméra est inactive 00788 wPointer = 0; // On remonte en haut du buffer 00789 cr = PIXY2::pixy2_sndSetNextTurn (angle); // On envoie la trame de choix de l'angle du prochain virage 00790 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte ! 00791 etat = messageSent; // On passe à l'attente du message de réponse 00792 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée 00793 break; 00794 00795 case dataReceived : // Quand on a reçu l'intégralité du message 00796 if (frameContainChecksum) { // Si la trame contient un checksum 00797 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum 00798 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur 00799 } 00800 } 00801 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) { 00802 // On vérifie que la trame est du type convenable (ACK ou ERREUR) 00803 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour 00804 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type. 00805 etat = idle; // On annoce que la pixy est libre 00806 break; 00807 00808 default : // Dans tous les autres cas 00809 cr = PIXY2_BUSY; // On signale que la caméra est occupée. 00810 break; 00811 } 00812 return cr; 00813 } 00814 00815 T_pixy2ErrorCode PIXY2::pixy2_setDefaultTurn (sWord angle) 00816 { 00817 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer]; 00818 T_pixy2ErrorCode cr = PIXY2_OK; 00819 00820 switch (etat) { 00821 case idle : // Si la caméra est inactive 00822 wPointer = 0; // On remonte en haut du buffer 00823 cr = PIXY2::pixy2_sndSetDefaultTurn (angle); // On envoie la trame de choix de l'angle par défaut des virages 00824 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte ! 00825 etat = messageSent; // On passe à l'attente du message de réponse 00826 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée 00827 break; 00828 00829 case dataReceived : // Quand on a reçu l'intégralité du message 00830 if (frameContainChecksum) { // Si la trame contient un checksum 00831 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum 00832 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur 00833 } 00834 } 00835 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) { 00836 // On vérifie que la trame est du type convenable (ACK ou ERREUR) 00837 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour 00838 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type. 00839 etat = idle; // On annoce que la pixy est libre 00840 break; 00841 00842 default : // Dans tous les autres cas 00843 cr = PIXY2_BUSY; // On signale que la caméra est occupée. 00844 break; 00845 } 00846 return cr; 00847 } 00848 00849 T_pixy2ErrorCode PIXY2::pixy2_setVector (Byte vectorIndex) 00850 { 00851 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer]; 00852 T_pixy2ErrorCode cr = PIXY2_OK; 00853 00854 switch (etat) { 00855 case idle : // Si la caméra est inactive 00856 wPointer = 0; // On remonte en haut du buffer 00857 cr = PIXY2::pixy2_sndSetVector (vectorIndex); // On envoie la trame de choix du vecteur à suivre 00858 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte ! 00859 etat = messageSent; // On passe à l'attente du message de réponse 00860 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée 00861 break; 00862 00863 case dataReceived : // Quand on a reçu l'intégralité du message 00864 if (frameContainChecksum) { // Si la trame contient un checksum 00865 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum 00866 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur 00867 } 00868 } 00869 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) { 00870 // On vérifie que la trame est du type convenable (ACK ou ERREUR) 00871 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour 00872 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type. 00873 etat = idle; // On annoce que la pixy est libre 00874 break; 00875 00876 default : // Dans tous les autres cas 00877 cr = PIXY2_BUSY; // On signale que la caméra est occupée. 00878 break; 00879 } 00880 return cr; 00881 } 00882 00883 00884 T_pixy2ErrorCode PIXY2::pixy2_ReverseVector (void) 00885 { 00886 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer]; 00887 T_pixy2ErrorCode cr = PIXY2_OK; 00888 00889 switch (etat) { 00890 case idle : // Si la caméra est inactive 00891 wPointer = 0; // On remonte en haut du buffer 00892 cr = PIXY2::pixy2_sndReverseVector (); // On envoie la trame d'inversion de l'image (haut en bas et bas en haut) 00893 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte ! 00894 etat = messageSent; // On passe à l'attente du message de réponse 00895 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée 00896 break; 00897 00898 case dataReceived : // Quand on a reçu l'intégralité du message 00899 if (frameContainChecksum) { // Si la trame contient un checksum 00900 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum 00901 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur 00902 } 00903 } 00904 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) { 00905 // On vérifie que la trame est du type convenable (ACK ou ERREUR) 00906 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour 00907 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type. 00908 etat = idle; // On annoce que la pixy est libre 00909 break; 00910 00911 default : // Dans tous les autres cas 00912 cr = PIXY2_BUSY; // On signale que la caméra est occupée. 00913 break; 00914 } 00915 return cr; 00916 } 00917 00918 T_pixy2ErrorCode PIXY2::pixy2_getRGB (Word x, Word y, Byte saturate, T_pixy2Pixel **pixel){ 00919 00920 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer]; 00921 T_pixy2ErrorCode cr = PIXY2_OK; 00922 00923 switch (etat) { 00924 case idle : // Si la caméra est inactive 00925 wPointer = 0; // On remonte en haut du buffer 00926 cr = PIXY2::pixy2_sndGetRGB(x, y, saturate); // On envoie la trame de demande de la couleur (RGB) d'un carée de pixel 00927 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte ! 00928 etat = messageSent; // On passe à l'attente du message de réponse 00929 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée 00930 break; 00931 00932 case dataReceived : // Quand on a reçu l'intégralité du message 00933 if (frameContainChecksum) { // Si la trame contient un checksum 00934 if (pixy2_validateChecksum (&Pixy2_buffer[hPointer]) != 0) { // On lance la validation du checksum 00935 return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur 00936 } 00937 } 00938 if (msg->pixType == PIXY2_REP_ACK) { // On vérifie que la trame est du type convenable (REPONSE ACK) 00939 *pixel = (T_pixy2Pixel*) &Pixy2_buffer[dPointer]; // On mappe le pointeur de structure sur le buffer de réception. 00940 } else { // Si ce n'est pas le bon type 00941 if (msg->pixType == PIXY2_REP_ERROR) { // Cela pourrait être une trame d'erreur 00942 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour 00943 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type. 00944 } 00945 etat = idle; // On annoce que la pixy est libre 00946 break; 00947 00948 default : // Dans tous les autres cas 00949 cr = PIXY2_BUSY; // On signale que la caméra est occupée. 00950 break; 00951 } 00952 return cr; 00953 } 00954 00955 00956 00957 T_pixy2ErrorCode PIXY2::pixy2_validateChecksum (Byte* tab){ 00958 Word i, sum = 0; 00959 T_Word *tmp; 00960 00961 for (i=0; i<*(tab+3);i++) sum = sum + *(tab + PIXY2_CSHEADERSIZE + i); 00962 tmp = (T_Word*) (tab+4); 00963 00964 if (tmp->mot == sum) return PIXY2_OK; 00965 else { 00966 if (_DEBUG_) { 00967 sommeDeControle = sum; 00968 sommeRecue = tmp->mot; 00969 } 00970 return PIXY2_BAD_CHECKSUM; 00971 } 00972 }
Generated on Wed Jul 13 2022 08:41:45 by
1.7.2