CRAC Team / Pixy2-HA

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?

UserRevisionLine numberNew 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 }