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