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