NXPCup_Cachan / Mbed 2 deprecated NXPCup

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_lib/PIXY2.cpp@0:8743b606abc3, 2020-02-11 (annotated)

Committer:
Wael_H
Date:
Tue Feb 11 15:41:45 2020 +0000
Revision:
0:8743b606abc3
Child:
1:a9af73d5abd4
Version direction a peu pres

Who changed what in which revision?

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