Theo Le Paih / Mbed 2 deprecated Lib_Pixy2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

pixy2.cpp@26:a56cc5d715e9, 2019-11-28 (annotated)

Committer:
theolp
Date:
Thu Nov 28 07:39:32 2019 +0000
Revision:
26:a56cc5d715e9
Parent:
25:2a2fcb9e4775
Child:
27:bee63ac9b70b
Avant la tentative de correction de Pixy2_getFeatures()

Who changed what in which revision?

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