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@18:190e3987e274, 2019-03-13 (annotated)

Committer:
haarkon
Date:
Wed Mar 13 16:27:14 2019 +0000
Revision:
18:190e3987e274
Parent:
16:f84b0f970c3e
Child:
19:007d361c8d66
important revision of functions

Who changed what in which revision?

UserRevisionLine numberNew contents of line
haarkon 0:dde1b9d6c9d6 1 #include "pixy2.h"
haarkon 0:dde1b9d6c9d6 2
haarkon 0:dde1b9d6c9d6 3 PIXY2::PIXY2(PinName tx, PinName rx, int debit)
haarkon 0:dde1b9d6c9d6 4 {
haarkon 0:dde1b9d6c9d6 5 _Pixy2 = new Serial (tx, rx, debit);
haarkon 0:dde1b9d6c9d6 6 _Pixy2->attach (callback(this,&PIXY2::pixy2_getByte));
haarkon 0:dde1b9d6c9d6 7 etat = idle;
haarkon 0:dde1b9d6c9d6 8 Pixy2_buffer = (Byte*) malloc (0x100);
haarkon 0:dde1b9d6c9d6 9 }
haarkon 0:dde1b9d6c9d6 10
haarkon 3:6ba43af28440 11 /* 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 12 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 13 Les étapes de fonctionnement sont les suivantes :
haarkon 3:6ba43af28440 14
haarkon 3:6ba43af28440 15 * idle : La caméra n'a pas été solicité => on envoi le message de requête.
haarkon 3:6ba43af28440 16 * messageSent : La requête a été transmise, mais la réponse n'est pas encore arrivée.
haarkon 3:6ba43af28440 17 * receivingHeader : Le mot de synchro a été reçu, le reste de l'entête est en cours de réception.
haarkon 3:6ba43af28440 18 * headerReceived : L'entête a été intégralement reçu et peut être traité (tache interne) pour définir la payload.
haarkon 3:6ba43af28440 19 * receivingData : On est en train de recevoir la payload.
haarkon 3:6ba43af28440 20 * dataReceived : La réponse a été intégralement reçue et est disponible pour le traitement (et la libération).
haarkon 3:6ba43af28440 21 */
haarkon 3:6ba43af28440 22
haarkon 0:dde1b9d6c9d6 23 void PIXY2::pixy2_getByte () // Interruption de la pixy2
haarkon 0:dde1b9d6c9d6 24 {
haarkon 0:dde1b9d6c9d6 25 T_Word *buffer;
haarkon 0:dde1b9d6c9d6 26
haarkon 18:190e3987e274 27 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 28
haarkon 0:dde1b9d6c9d6 29 switch (etat) {
haarkon 3:6ba43af28440 30 case messageSent : // Si on a envoyé une requete => on attend un entête
haarkon 18:190e3987e274 31 if (wPointer > 0) { // On attend d'avoir reçu 2 octets
haarkon 18:190e3987e274 32 buffer = (T_Word*) &Pixy2_buffer[wPointer-1]; // On pointe la structure sur les 2 derniers octets reçus
haarkon 18:190e3987e274 33 if ((buffer->mot == PIXY2_CSSYNC) || (buffer->mot == PIXY2_SYNC)) { // Si c'est un mot d'entête
haarkon 3:6ba43af28440 34 etat = receivingHeader; // On passe à l'état réception de l'entête
haarkon 18:190e3987e274 35 hPointer = wPointer - 1; // On initialise le pointeur de l'entête
haarkon 18:190e3987e274 36 if (buffer->mot == PIXY2_SYNC) frameContainChecksum = 0; // Si c'est un entête sans checksum, on mémorise qu'il n'y a pas de checksum à vérifier
haarkon 18:190e3987e274 37 else frameContainChecksum = 1; // Sinon, on mémorise qu'il y a un checksum à vérifier
haarkon 0:dde1b9d6c9d6 38 }
haarkon 0:dde1b9d6c9d6 39 }
haarkon 0:dde1b9d6c9d6 40 break;
haarkon 0:dde1b9d6c9d6 41
haarkon 18:190e3987e274 42 case receivingHeader : // Si on est en train de recevoir un entête (entre le SYNC et... La fin de l'entête)
haarkon 18:190e3987e274 43 if ((frameContainChecksum && ((wPointer - hPointer) == PIXY2_CSHEADERSIZE-1)) || (!frameContainChecksum && ((wPointer - hPointer) == PIXY2_NCSHEADERSIZE-1))) {
haarkon 18:190e3987e274 44 // 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 45 etat = receivingData; // On dit que l'on va de recevoir des données
haarkon 18:190e3987e274 46 dataSize = Pixy2_buffer[hPointer+3]; // On enregistre la taille de la payload
haarkon 18:190e3987e274 47 dPointer = wPointer + 1; // Et on sauvegarde le point de départ de la zone de données
haarkon 0:dde1b9d6c9d6 48 }
haarkon 0:dde1b9d6c9d6 49 break;
haarkon 0:dde1b9d6c9d6 50
haarkon 3:6ba43af28440 51 case receivingData : // Si on est en train de recevoir des données.
haarkon 18:190e3987e274 52 if (wPointer == ((dataSize - 1) + dPointer)) etat = dataReceived; // Quand on a reçu toutes les données promises on dit que c'est OK
haarkon 0:dde1b9d6c9d6 53 break;
haarkon 0:dde1b9d6c9d6 54
haarkon 0:dde1b9d6c9d6 55 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 56 break;
haarkon 0:dde1b9d6c9d6 57 }
haarkon 18:190e3987e274 58 wPointer++; // on pointe la case suivante du buffer de réception
haarkon 18:190e3987e274 59
haarkon 0:dde1b9d6c9d6 60 }
haarkon 0:dde1b9d6c9d6 61
haarkon 11:68ef9f85e40e 62 T_pixy2ErrorCode PIXY2::pixy2_sndGetVersion (void){
haarkon 10:ea759846c2d5 63 T_pixy2SendBuffer msg;
haarkon 0:dde1b9d6c9d6 64 int i = 0, dataSize = 0;
haarkon 0:dde1b9d6c9d6 65 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:dde1b9d6c9d6 66 msg.frame.header.pixType = PIXY2_ASK_VERS;
haarkon 18:190e3987e274 67 msg.frame.header.pixLength = dataSize;
haarkon 0:dde1b9d6c9d6 68 do {
haarkon 0:dde1b9d6c9d6 69 while(!_Pixy2->writable());
haarkon 0:dde1b9d6c9d6 70 _Pixy2->putc(msg.data[i]);
haarkon 0:dde1b9d6c9d6 71 i++;
haarkon 0:dde1b9d6c9d6 72 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 5:28e47187bb5c 73 return PIXY2_OK;
haarkon 0:dde1b9d6c9d6 74 }
haarkon 0:dde1b9d6c9d6 75
haarkon 11:68ef9f85e40e 76 T_pixy2ErrorCode PIXY2::pixy2_sndGetResolution (void){
haarkon 10:ea759846c2d5 77 T_pixy2SendBuffer msg;
haarkon 0:dde1b9d6c9d6 78 int i = 0, dataSize = 1;
haarkon 0:dde1b9d6c9d6 79 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:dde1b9d6c9d6 80 msg.frame.header.pixType = PIXY2_ASK_RESOL;
haarkon 18:190e3987e274 81 msg.frame.header.pixLength = dataSize;
haarkon 0:dde1b9d6c9d6 82 msg.frame.data[0] = 0;
haarkon 0:dde1b9d6c9d6 83 do {
haarkon 0:dde1b9d6c9d6 84 while(!_Pixy2->writable());
haarkon 0:dde1b9d6c9d6 85 _Pixy2->putc(msg.data[i]);
haarkon 0:dde1b9d6c9d6 86 i++;
haarkon 0:dde1b9d6c9d6 87 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 5:28e47187bb5c 88 return PIXY2_OK;
haarkon 0:dde1b9d6c9d6 89 }
haarkon 0:dde1b9d6c9d6 90
haarkon 11:68ef9f85e40e 91 T_pixy2ErrorCode PIXY2::pixy2_sndSetCameraBrightness (Byte brightness){
haarkon 10:ea759846c2d5 92 T_pixy2SendBuffer msg;
haarkon 0:dde1b9d6c9d6 93 int i = 0, dataSize = 1;
haarkon 0:dde1b9d6c9d6 94 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:dde1b9d6c9d6 95 msg.frame.header.pixType = PIXY2_SET_BRIGHT;
haarkon 18:190e3987e274 96 msg.frame.header.pixLength = dataSize;
haarkon 0:dde1b9d6c9d6 97 msg.frame.data[0] = brightness;
haarkon 0:dde1b9d6c9d6 98 do {
haarkon 0:dde1b9d6c9d6 99 while(!_Pixy2->writable());
haarkon 0:dde1b9d6c9d6 100 _Pixy2->putc(msg.data[i]);
haarkon 0:dde1b9d6c9d6 101 i++;
haarkon 0:dde1b9d6c9d6 102 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 5:28e47187bb5c 103 return PIXY2_OK;
haarkon 0:dde1b9d6c9d6 104 }
haarkon 0:dde1b9d6c9d6 105
haarkon 11:68ef9f85e40e 106 T_pixy2ErrorCode PIXY2::pixy2_sndSetServo (Word s0, Word s1){
haarkon 10:ea759846c2d5 107 T_pixy2SendBuffer msg;
haarkon 0:dde1b9d6c9d6 108 int i = 0, dataSize = 4;
haarkon 0:dde1b9d6c9d6 109 T_Word tmp;
haarkon 0:dde1b9d6c9d6 110 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:dde1b9d6c9d6 111 msg.frame.header.pixType = PIXY2_SET_SERVOS;
haarkon 18:190e3987e274 112 msg.frame.header.pixLength = dataSize;
haarkon 0:dde1b9d6c9d6 113 tmp.mot = s0;
haarkon 0:dde1b9d6c9d6 114 msg.frame.data[0] = tmp.octet[0];
haarkon 0:dde1b9d6c9d6 115 msg.frame.data[1] = tmp.octet[1];
haarkon 0:dde1b9d6c9d6 116 tmp.mot = s1;
haarkon 0:dde1b9d6c9d6 117 msg.frame.data[2] = tmp.octet[0];
haarkon 0:dde1b9d6c9d6 118 msg.frame.data[3] = tmp.octet[1];
haarkon 0:dde1b9d6c9d6 119 do {
haarkon 0:dde1b9d6c9d6 120 while(!_Pixy2->writable());
haarkon 0:dde1b9d6c9d6 121 _Pixy2->putc(msg.data[i]);
haarkon 0:dde1b9d6c9d6 122 i++;
haarkon 0:dde1b9d6c9d6 123 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 5:28e47187bb5c 124 return PIXY2_OK;
haarkon 0:dde1b9d6c9d6 125 }
haarkon 0:dde1b9d6c9d6 126
haarkon 11:68ef9f85e40e 127 T_pixy2ErrorCode PIXY2::pixy2_sndSetLED (Byte red, Byte green, Byte blue){
haarkon 10:ea759846c2d5 128 T_pixy2SendBuffer msg;
haarkon 0:dde1b9d6c9d6 129 int i = 0, dataSize = 3;
haarkon 0:dde1b9d6c9d6 130 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:dde1b9d6c9d6 131 msg.frame.header.pixType = PIXY2_SET_LED;
haarkon 18:190e3987e274 132 msg.frame.header.pixLength = dataSize;
haarkon 0:dde1b9d6c9d6 133 msg.frame.data[0] = red;
haarkon 0:dde1b9d6c9d6 134 msg.frame.data[1] = green;
haarkon 0:dde1b9d6c9d6 135 msg.frame.data[2] = blue;
haarkon 0:dde1b9d6c9d6 136 do {
haarkon 0:dde1b9d6c9d6 137 while(!_Pixy2->writable());
haarkon 0:dde1b9d6c9d6 138 _Pixy2->putc(msg.data[i]);
haarkon 0:dde1b9d6c9d6 139 i++;
haarkon 0:dde1b9d6c9d6 140 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 5:28e47187bb5c 141 return PIXY2_OK;
haarkon 0:dde1b9d6c9d6 142 }
haarkon 0:dde1b9d6c9d6 143
haarkon 11:68ef9f85e40e 144 T_pixy2ErrorCode PIXY2::pixy2_sndSetLamp (Byte upper, Byte lower){
haarkon 10:ea759846c2d5 145 T_pixy2SendBuffer msg;
haarkon 0:dde1b9d6c9d6 146 int i = 0, dataSize = 2;
haarkon 0:dde1b9d6c9d6 147 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:dde1b9d6c9d6 148 msg.frame.header.pixType = PIXY2_SET_LED;
haarkon 18:190e3987e274 149 msg.frame.header.pixLength = dataSize;
haarkon 0:dde1b9d6c9d6 150 msg.frame.data[0] = upper;
haarkon 0:dde1b9d6c9d6 151 msg.frame.data[1] = lower;
haarkon 0:dde1b9d6c9d6 152 do {
haarkon 0:dde1b9d6c9d6 153 while(!_Pixy2->writable());
haarkon 0:dde1b9d6c9d6 154 _Pixy2->putc(msg.data[i]);
haarkon 0:dde1b9d6c9d6 155 i++;
haarkon 0:dde1b9d6c9d6 156 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 5:28e47187bb5c 157 return PIXY2_OK;
haarkon 0:dde1b9d6c9d6 158 }
haarkon 0:dde1b9d6c9d6 159
haarkon 11:68ef9f85e40e 160 T_pixy2ErrorCode PIXY2::pixy2_sndGetFPS (void){
haarkon 10:ea759846c2d5 161 T_pixy2SendBuffer msg;
haarkon 0:dde1b9d6c9d6 162 int i = 0, dataSize = 0;
haarkon 0:dde1b9d6c9d6 163 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:dde1b9d6c9d6 164 msg.frame.header.pixType = PIXY2_ASK_FPS;
haarkon 18:190e3987e274 165 msg.frame.header.pixLength = dataSize;
haarkon 0:dde1b9d6c9d6 166 do {
haarkon 0:dde1b9d6c9d6 167 while(!_Pixy2->writable());
haarkon 0:dde1b9d6c9d6 168 _Pixy2->putc(msg.data[i]);
haarkon 0:dde1b9d6c9d6 169 i++;
haarkon 0:dde1b9d6c9d6 170 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 5:28e47187bb5c 171 return PIXY2_OK;
haarkon 0:dde1b9d6c9d6 172 }
haarkon 0:dde1b9d6c9d6 173
haarkon 11:68ef9f85e40e 174 T_pixy2ErrorCode PIXY2::pixy2_sndGetBlocks (Byte sigmap, Byte maxBloc){
haarkon 10:ea759846c2d5 175 T_pixy2SendBuffer msg;
haarkon 0:dde1b9d6c9d6 176 int i = 0, dataSize = 2;
haarkon 0:dde1b9d6c9d6 177 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:dde1b9d6c9d6 178 msg.frame.header.pixType = PIXY2_ASK_BLOC;
haarkon 18:190e3987e274 179 msg.frame.header.pixLength = dataSize;
haarkon 0:dde1b9d6c9d6 180 msg.frame.data[0] = sigmap;
haarkon 0:dde1b9d6c9d6 181 msg.frame.data[1] = maxBloc;
haarkon 0:dde1b9d6c9d6 182 do {
haarkon 0:dde1b9d6c9d6 183 while(!_Pixy2->writable());
haarkon 0:dde1b9d6c9d6 184 _Pixy2->putc(msg.data[i]);
haarkon 0:dde1b9d6c9d6 185 i++;
haarkon 0:dde1b9d6c9d6 186 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 5:28e47187bb5c 187 return PIXY2_OK;
haarkon 0:dde1b9d6c9d6 188 }
haarkon 0:dde1b9d6c9d6 189
haarkon 11:68ef9f85e40e 190 T_pixy2ErrorCode PIXY2::pixy2_sndGetMainFeature (Byte type, Byte feature){
haarkon 10:ea759846c2d5 191 T_pixy2SendBuffer msg;
haarkon 0:dde1b9d6c9d6 192 int i = 0, dataSize = 2;
haarkon 0:dde1b9d6c9d6 193 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:dde1b9d6c9d6 194 msg.frame.header.pixType = PIXY2_ASK_LINE;
haarkon 18:190e3987e274 195 msg.frame.header.pixLength = dataSize;
haarkon 0:dde1b9d6c9d6 196 msg.frame.data[0] = type;
haarkon 0:dde1b9d6c9d6 197 msg.frame.data[1] = feature;
haarkon 0:dde1b9d6c9d6 198 do {
haarkon 0:dde1b9d6c9d6 199 while(!_Pixy2->writable());
haarkon 0:dde1b9d6c9d6 200 _Pixy2->putc(msg.data[i]);
haarkon 0:dde1b9d6c9d6 201 i++;
haarkon 0:dde1b9d6c9d6 202 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 5:28e47187bb5c 203 return PIXY2_OK;
haarkon 0:dde1b9d6c9d6 204 }
haarkon 0:dde1b9d6c9d6 205
haarkon 11:68ef9f85e40e 206 T_pixy2ErrorCode PIXY2::pixy2_sndSetMode (Byte mode){
haarkon 10:ea759846c2d5 207 T_pixy2SendBuffer msg;
haarkon 0:dde1b9d6c9d6 208 int i = 0, dataSize = 1;
haarkon 0:dde1b9d6c9d6 209 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:dde1b9d6c9d6 210 msg.frame.header.pixType = PIXY2_SET_MODE;
haarkon 18:190e3987e274 211 msg.frame.header.pixLength = dataSize;
haarkon 0:dde1b9d6c9d6 212 msg.frame.data[0] = mode;
haarkon 0:dde1b9d6c9d6 213 do {
haarkon 0:dde1b9d6c9d6 214 while(!_Pixy2->writable());
haarkon 0:dde1b9d6c9d6 215 _Pixy2->putc(msg.data[i]);
haarkon 0:dde1b9d6c9d6 216 i++;
haarkon 0:dde1b9d6c9d6 217 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 5:28e47187bb5c 218 return PIXY2_OK;
haarkon 0:dde1b9d6c9d6 219 }
haarkon 0:dde1b9d6c9d6 220
haarkon 11:68ef9f85e40e 221 T_pixy2ErrorCode PIXY2::pixy2_sndSetNexTurn (Word angle){
haarkon 10:ea759846c2d5 222 T_pixy2SendBuffer msg;
haarkon 0:dde1b9d6c9d6 223 int i = 0, dataSize = 2;
haarkon 0:dde1b9d6c9d6 224 T_Word tmp;
haarkon 0:dde1b9d6c9d6 225 tmp.mot = angle;
haarkon 0:dde1b9d6c9d6 226 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:dde1b9d6c9d6 227 msg.frame.header.pixType = PIXY2_SET_TURN;
haarkon 18:190e3987e274 228 msg.frame.header.pixLength = dataSize;
haarkon 0:dde1b9d6c9d6 229 tmp.mot = angle;
haarkon 0:dde1b9d6c9d6 230 msg.frame.data[0] = tmp.octet[0];
haarkon 0:dde1b9d6c9d6 231 msg.frame.data[1] = tmp.octet[1];
haarkon 0:dde1b9d6c9d6 232 do {
haarkon 0:dde1b9d6c9d6 233 while(!_Pixy2->writable());
haarkon 0:dde1b9d6c9d6 234 _Pixy2->putc(msg.data[i]);
haarkon 0:dde1b9d6c9d6 235 i++;
haarkon 0:dde1b9d6c9d6 236 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 5:28e47187bb5c 237 return PIXY2_OK;
haarkon 0:dde1b9d6c9d6 238 }
haarkon 0:dde1b9d6c9d6 239
haarkon 11:68ef9f85e40e 240 T_pixy2ErrorCode PIXY2::pixy2_sndSetDefaultTurn (Word angle){
haarkon 10:ea759846c2d5 241 T_pixy2SendBuffer msg;
haarkon 0:dde1b9d6c9d6 242 int i = 0, dataSize = 2;
haarkon 0:dde1b9d6c9d6 243 T_Word tmp;
haarkon 0:dde1b9d6c9d6 244 tmp.mot = angle;
haarkon 0:dde1b9d6c9d6 245 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:dde1b9d6c9d6 246 msg.frame.header.pixType = PIXY2_SET_DEFTURN;
haarkon 18:190e3987e274 247 msg.frame.header.pixLength = dataSize;
haarkon 0:dde1b9d6c9d6 248 tmp.mot = angle;
haarkon 0:dde1b9d6c9d6 249 msg.frame.data[0] = tmp.octet[0];
haarkon 0:dde1b9d6c9d6 250 msg.frame.data[1] = tmp.octet[1];
haarkon 0:dde1b9d6c9d6 251 do {
haarkon 0:dde1b9d6c9d6 252 while(!_Pixy2->writable());
haarkon 0:dde1b9d6c9d6 253 _Pixy2->putc(msg.data[i]);
haarkon 0:dde1b9d6c9d6 254 i++;
haarkon 0:dde1b9d6c9d6 255 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 5:28e47187bb5c 256 return PIXY2_OK;
haarkon 0:dde1b9d6c9d6 257 }
haarkon 0:dde1b9d6c9d6 258
haarkon 11:68ef9f85e40e 259 T_pixy2ErrorCode PIXY2::pixy2_sndSetVector (Byte vectorIndex){
haarkon 10:ea759846c2d5 260 T_pixy2SendBuffer msg;
haarkon 0:dde1b9d6c9d6 261 int i = 0, dataSize = 1;
haarkon 0:dde1b9d6c9d6 262 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:dde1b9d6c9d6 263 msg.frame.header.pixType = PIXY2_SET_VECTOR;
haarkon 18:190e3987e274 264 msg.frame.header.pixLength = dataSize;
haarkon 0:dde1b9d6c9d6 265 msg.frame.data[0] = vectorIndex;
haarkon 0:dde1b9d6c9d6 266 do {
haarkon 0:dde1b9d6c9d6 267 while(!_Pixy2->writable());
haarkon 0:dde1b9d6c9d6 268 _Pixy2->putc(msg.data[i]);
haarkon 0:dde1b9d6c9d6 269 i++;
haarkon 0:dde1b9d6c9d6 270 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 5:28e47187bb5c 271 return PIXY2_OK;
haarkon 0:dde1b9d6c9d6 272 }
haarkon 0:dde1b9d6c9d6 273
haarkon 11:68ef9f85e40e 274 T_pixy2ErrorCode PIXY2::pixy2_sndReverseVector (void){
haarkon 10:ea759846c2d5 275 T_pixy2SendBuffer msg;
haarkon 0:dde1b9d6c9d6 276 int i = 0, dataSize = 0;
haarkon 0:dde1b9d6c9d6 277 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:dde1b9d6c9d6 278 msg.frame.header.pixType = PIXY2_SET_REVERSE;
haarkon 18:190e3987e274 279 msg.frame.header.pixLength = dataSize;
haarkon 0:dde1b9d6c9d6 280 do {
haarkon 0:dde1b9d6c9d6 281 while(!_Pixy2->writable());
haarkon 0:dde1b9d6c9d6 282 _Pixy2->putc(msg.data[i]);
haarkon 0:dde1b9d6c9d6 283 i++;
haarkon 0:dde1b9d6c9d6 284 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 5:28e47187bb5c 285 return PIXY2_OK;
haarkon 0:dde1b9d6c9d6 286 }
haarkon 0:dde1b9d6c9d6 287
haarkon 11:68ef9f85e40e 288 T_pixy2ErrorCode PIXY2::pixy2_sndGetRGB (Word x, Word y, Byte saturate){
haarkon 10:ea759846c2d5 289 T_pixy2SendBuffer msg;
haarkon 0:dde1b9d6c9d6 290 int i = 0, dataSize = 3;
haarkon 0:dde1b9d6c9d6 291 msg.frame.header.pixSync = PIXY2_SYNC;
haarkon 0:dde1b9d6c9d6 292 msg.frame.header.pixType = PIXY2_ASK_VIDEO;
haarkon 18:190e3987e274 293 msg.frame.header.pixLength = dataSize;
haarkon 0:dde1b9d6c9d6 294 msg.frame.data[0] = x;
haarkon 0:dde1b9d6c9d6 295 msg.frame.data[1] = y;
haarkon 0:dde1b9d6c9d6 296 msg.frame.data[2] = saturate;
haarkon 0:dde1b9d6c9d6 297 do {
haarkon 0:dde1b9d6c9d6 298 while(!_Pixy2->writable());
haarkon 0:dde1b9d6c9d6 299 _Pixy2->putc(msg.data[i]);
haarkon 0:dde1b9d6c9d6 300 i++;
haarkon 0:dde1b9d6c9d6 301 } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
haarkon 5:28e47187bb5c 302 return PIXY2_OK;
haarkon 0:dde1b9d6c9d6 303 }
haarkon 0:dde1b9d6c9d6 304
haarkon 0:dde1b9d6c9d6 305 /* 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 306 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 307
haarkon 11:68ef9f85e40e 308 T_pixy2ErrorCode PIXY2::pixy2_getVersion (T_pixy2Version *version){
haarkon 0:dde1b9d6c9d6 309
haarkon 18:190e3987e274 310 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
haarkon 11:68ef9f85e40e 311 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 0:dde1b9d6c9d6 312
haarkon 0:dde1b9d6c9d6 313 switch (etat) {
haarkon 3:6ba43af28440 314 case idle : // Si la caméra est inactive
haarkon 18:190e3987e274 315 wPointer = 0; // On remonte en haut du buffer
haarkon 14:c0eda1e35a00 316 cr = PIXY2::pixy2_sndGetVersion(); // On envoie la trame de demande de la version
haarkon 14:c0eda1e35a00 317 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 3:6ba43af28440 318 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 5:28e47187bb5c 319 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 0:dde1b9d6c9d6 320 break;
haarkon 0:dde1b9d6c9d6 321
haarkon 3:6ba43af28440 322 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 3:6ba43af28440 323 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 18:190e3987e274 324 if ( pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
haarkon 18:190e3987e274 325 if (msg->pixType == PIXY2_REP_VERS) { // On vérifie que la trame est du type convenable (REPONSE VERSION)
haarkon 18:190e3987e274 326 version = (T_pixy2Version*) &Pixy2_buffer[dPointer]; // On mappe le pointeur de structure sur le buffer de réception.
haarkon 18:190e3987e274 327 } else { // Si ce n'est pas le bon type
haarkon 18:190e3987e274 328 if (msg->pixType == PIXY2_REP_ERROR) { // Cela pourrait être une trame d'erreur
haarkon 18:190e3987e274 329 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 18:190e3987e274 330 }else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 10:ea759846c2d5 331 }
haarkon 5:28e47187bb5c 332 } else cr = PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur aussi !
haarkon 3:6ba43af28440 333 } else { // S'il n'y a pas de checksum (pas normal mais on ne sait jamais)
haarkon 3:6ba43af28440 334 if (msg->pixType == PIXY2_REP_VERS) { // On vérifie que la trame est du type convenable
haarkon 18:190e3987e274 335 version = (T_pixy2Version*) &Pixy2_buffer[dPointer]; // Si le type est bon, on mappe le pointeur de structure sur le buffer de réception.
haarkon 10:ea759846c2d5 336 } else {
haarkon 18:190e3987e274 337 if (msg->pixType == PIXY2_REP_ERROR) { // Si on reçoit une trame d'erreur
haarkon 18:190e3987e274 338 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // On copie le code d'erreur reçu dans la variable de retour
haarkon 10:ea759846c2d5 339 }else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur
haarkon 10:ea759846c2d5 340 }
haarkon 0:dde1b9d6c9d6 341 }
haarkon 18:190e3987e274 342 etat = idle; // On annoce que la pixy est libre
haarkon 0:dde1b9d6c9d6 343 break;
haarkon 0:dde1b9d6c9d6 344
haarkon 3:6ba43af28440 345 default : // Dans tous les autres cas
haarkon 5:28e47187bb5c 346 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 0:dde1b9d6c9d6 347 break;
haarkon 0:dde1b9d6c9d6 348 }
haarkon 0:dde1b9d6c9d6 349 return cr;
haarkon 0:dde1b9d6c9d6 350 }
haarkon 0:dde1b9d6c9d6 351
haarkon 11:68ef9f85e40e 352 T_pixy2ErrorCode PIXY2::pixy2_getResolution (T_pixy2Resolution *resolution){
haarkon 3:6ba43af28440 353
haarkon 18:190e3987e274 354 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
haarkon 11:68ef9f85e40e 355 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 3:6ba43af28440 356
haarkon 3:6ba43af28440 357 switch (etat) {
haarkon 3:6ba43af28440 358 case idle : // Si la caméra est inactive
haarkon 18:190e3987e274 359 wPointer = 0; // On remonte en haut du buffer
haarkon 14:c0eda1e35a00 360 cr = PIXY2::pixy2_sndGetResolution(); // On envoie la trame de demande de la résolution
haarkon 14:c0eda1e35a00 361 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 3:6ba43af28440 362 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 5:28e47187bb5c 363 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 3:6ba43af28440 364 break;
haarkon 3:6ba43af28440 365
haarkon 3:6ba43af28440 366 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 3:6ba43af28440 367 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 18:190e3987e274 368 if ( pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
haarkon 18:190e3987e274 369 if (msg->pixType == PIXY2_REP_RESOL) { // On vérifie que la trame est du type convenable (REPONSE RESOLUTION)
haarkon 18:190e3987e274 370 resolution = (T_pixy2Resolution*) &Pixy2_buffer[dPointer]; // On mappe le pointeur de structure sur le buffer de réception.
haarkon 18:190e3987e274 371 } else { // Si ce n'est pas le bon type
haarkon 18:190e3987e274 372 if (msg->pixType == PIXY2_REP_ERROR) { // Cela pourrait être une trame d'erreur
haarkon 18:190e3987e274 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
haarkon 18:190e3987e274 374 }else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 12:7493191dd1dc 375 }
haarkon 12:7493191dd1dc 376 } else cr = PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur aussi !
haarkon 12:7493191dd1dc 377 } else { // S'il n'y a pas de checksum (pas normal mais on ne sait jamais)
haarkon 18:190e3987e274 378 if (msg->pixType == PIXY2_REP_RESOL) { // On vérifie que la trame est du type convenable (REPONSE RESOLUTION)
haarkon 18:190e3987e274 379 resolution = (T_pixy2Resolution*) &Pixy2_buffer[dPointer]; // Si le type est bon, on mappe le pointeur de structure sur le buffer de réception.
haarkon 12:7493191dd1dc 380 } else {
haarkon 18:190e3987e274 381 if (msg->pixType == PIXY2_REP_ERROR) { // Si on reçoit une trame d'erreur
haarkon 18:190e3987e274 382 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // On copie le code d'erreur reçu dans la variable de retour
haarkon 12:7493191dd1dc 383 }else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur
haarkon 12:7493191dd1dc 384 }
haarkon 12:7493191dd1dc 385 }
haarkon 18:190e3987e274 386 etat = idle; // On annoce que la pixy est libre
haarkon 18:190e3987e274 387 break;
haarkon 18:190e3987e274 388
haarkon 18:190e3987e274 389 default : // Dans tous les autres cas
haarkon 18:190e3987e274 390 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 18:190e3987e274 391 break;
haarkon 18:190e3987e274 392 }
haarkon 18:190e3987e274 393 return cr;
haarkon 18:190e3987e274 394 }
haarkon 18:190e3987e274 395
haarkon 18:190e3987e274 396 T_pixy2ErrorCode PIXY2::pixy2_setCameraBrightness (Byte brightness){
haarkon 18:190e3987e274 397
haarkon 18:190e3987e274 398 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
haarkon 18:190e3987e274 399 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 18:190e3987e274 400
haarkon 18:190e3987e274 401 switch (etat) {
haarkon 18:190e3987e274 402 case idle : // Si la caméra est inactive
haarkon 18:190e3987e274 403 wPointer = 0; // On remonte en haut du buffer
haarkon 18:190e3987e274 404 cr = PIXY2::pixy2_sndSetCameraBrightness (brightness); // On envoie la trame de règlage de la luminosité
haarkon 18:190e3987e274 405 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 18:190e3987e274 406 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 18:190e3987e274 407 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 18:190e3987e274 408 break;
haarkon 18:190e3987e274 409
haarkon 18:190e3987e274 410 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 18:190e3987e274 411 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 18:190e3987e274 412 if ( pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
haarkon 18:190e3987e274 413 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
haarkon 18:190e3987e274 414 // On vérifie que la trame est du type convenable (ACK ou ERREUR)
haarkon 18:190e3987e274 415 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 18:190e3987e274 416 }else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 18:190e3987e274 417 } else cr = PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur aussi !
haarkon 18:190e3987e274 418 } else { // S'il n'y a pas de checksum (pas normal mais on ne sait jamais)
haarkon 18:190e3987e274 419 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
haarkon 18:190e3987e274 420 // On vérifie que la trame est du type convenable (ACK ou ERREUR)
haarkon 18:190e3987e274 421 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 18:190e3987e274 422 }else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 18:190e3987e274 423 }
haarkon 18:190e3987e274 424 etat = idle; // On annoce que la pixy est libre
haarkon 18:190e3987e274 425 break;
haarkon 18:190e3987e274 426
haarkon 18:190e3987e274 427 default : // Dans tous les autres cas
haarkon 18:190e3987e274 428 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 18:190e3987e274 429 break;
haarkon 18:190e3987e274 430 }
haarkon 18:190e3987e274 431 return cr;
haarkon 18:190e3987e274 432 }
haarkon 18:190e3987e274 433
haarkon 18:190e3987e274 434 T_pixy2ErrorCode PIXY2::pixy2_setServos (Word s0, Word s1){
haarkon 18:190e3987e274 435
haarkon 18:190e3987e274 436 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
haarkon 18:190e3987e274 437 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 18:190e3987e274 438
haarkon 18:190e3987e274 439 switch (etat) {
haarkon 18:190e3987e274 440 case idle : // Si la caméra est inactive
haarkon 18:190e3987e274 441 wPointer = 0; // On remonte en haut du buffer
haarkon 18:190e3987e274 442 cr = PIXY2::pixy2_sndSetServo (s0, s1); // On envoie la trame de règlage des servos moteurs
haarkon 18:190e3987e274 443 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 18:190e3987e274 444 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 18:190e3987e274 445 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 18:190e3987e274 446 break;
haarkon 18:190e3987e274 447
haarkon 18:190e3987e274 448 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 18:190e3987e274 449 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 18:190e3987e274 450 if ( pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
haarkon 18:190e3987e274 451 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
haarkon 18:190e3987e274 452 // On vérifie que la trame est du type convenable (ACK ou ERREUR)
haarkon 18:190e3987e274 453 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 18:190e3987e274 454 }else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 18:190e3987e274 455 } else cr = PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur aussi !
haarkon 18:190e3987e274 456 } else { // S'il n'y a pas de checksum (pas normal mais on ne sait jamais)
haarkon 18:190e3987e274 457 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
haarkon 18:190e3987e274 458 // On vérifie que la trame est du type convenable (ACK ou ERREUR)
haarkon 18:190e3987e274 459 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 18:190e3987e274 460 }else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 18:190e3987e274 461 }
haarkon 18:190e3987e274 462 etat = idle; // On annoce que la pixy est libre
haarkon 12:7493191dd1dc 463 break;
haarkon 12:7493191dd1dc 464
haarkon 12:7493191dd1dc 465 default : // Dans tous les autres cas
haarkon 12:7493191dd1dc 466 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 12:7493191dd1dc 467 break;
haarkon 12:7493191dd1dc 468 }
haarkon 12:7493191dd1dc 469 return cr;
haarkon 12:7493191dd1dc 470 }
haarkon 12:7493191dd1dc 471
haarkon 18:190e3987e274 472 T_pixy2ErrorCode PIXY2::pixy2_setLED (Byte red, Byte green, Byte blue){
haarkon 12:7493191dd1dc 473
haarkon 18:190e3987e274 474 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
haarkon 12:7493191dd1dc 475 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 12:7493191dd1dc 476
haarkon 12:7493191dd1dc 477 switch (etat) {
haarkon 12:7493191dd1dc 478 case idle : // Si la caméra est inactive
haarkon 18:190e3987e274 479 wPointer = 0; // On remonte en haut du buffer
haarkon 18:190e3987e274 480 cr = PIXY2::pixy2_sndSetLED (red, green, blue); // On envoie la trame de règlage des composantes de la LED RGB
haarkon 14:c0eda1e35a00 481 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 12:7493191dd1dc 482 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 12:7493191dd1dc 483 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 12:7493191dd1dc 484 break;
haarkon 12:7493191dd1dc 485
haarkon 12:7493191dd1dc 486 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 12:7493191dd1dc 487 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 18:190e3987e274 488 if ( pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
haarkon 18:190e3987e274 489 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
haarkon 18:190e3987e274 490 // On vérifie que la trame est du type convenable (ACK ou ERREUR)
haarkon 18:190e3987e274 491 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 18:190e3987e274 492 }else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 12:7493191dd1dc 493 } else cr = PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur aussi !
haarkon 12:7493191dd1dc 494 } else { // S'il n'y a pas de checksum (pas normal mais on ne sait jamais)
haarkon 18:190e3987e274 495 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
haarkon 18:190e3987e274 496 // On vérifie que la trame est du type convenable (ACK ou ERREUR)
haarkon 18:190e3987e274 497 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 18:190e3987e274 498 }else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 12:7493191dd1dc 499 }
haarkon 18:190e3987e274 500 etat = idle; // On annoce que la pixy est libre
haarkon 3:6ba43af28440 501 break;
haarkon 3:6ba43af28440 502
haarkon 3:6ba43af28440 503 default : // Dans tous les autres cas
haarkon 18:190e3987e274 504 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 12:7493191dd1dc 505 break;
haarkon 12:7493191dd1dc 506 }
haarkon 12:7493191dd1dc 507 return cr;
haarkon 12:7493191dd1dc 508 }
haarkon 12:7493191dd1dc 509
haarkon 12:7493191dd1dc 510 T_pixy2ErrorCode PIXY2::pixy2_setLamp (Byte upper, Byte lower){
haarkon 12:7493191dd1dc 511
haarkon 18:190e3987e274 512 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
haarkon 12:7493191dd1dc 513 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 12:7493191dd1dc 514
haarkon 12:7493191dd1dc 515 switch (etat) {
haarkon 12:7493191dd1dc 516 case idle : // Si la caméra est inactive
haarkon 18:190e3987e274 517 wPointer = 0; // On remonte en haut du buffer
haarkon 14:c0eda1e35a00 518 cr = PIXY2::pixy2_sndSetLamp (upper, lower); // On envoie la trame de règlage d'allumage des lumières de contraste
haarkon 18:190e3987e274 519 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 12:7493191dd1dc 520 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 12:7493191dd1dc 521 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 12:7493191dd1dc 522 break;
haarkon 12:7493191dd1dc 523
haarkon 12:7493191dd1dc 524 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 12:7493191dd1dc 525 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 18:190e3987e274 526 if ( pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
haarkon 18:190e3987e274 527 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
haarkon 18:190e3987e274 528 // On vérifie que la trame est du type convenable (ACK ou ERREUR)
haarkon 18:190e3987e274 529 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 18:190e3987e274 530 }else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 12:7493191dd1dc 531 } else cr = PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur aussi !
haarkon 12:7493191dd1dc 532 } else { // S'il n'y a pas de checksum (pas normal mais on ne sait jamais)
haarkon 18:190e3987e274 533 if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
haarkon 18:190e3987e274 534 // On vérifie que la trame est du type convenable (ACK ou ERREUR)
haarkon 18:190e3987e274 535 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 18:190e3987e274 536 }else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 12:7493191dd1dc 537 }
haarkon 18:190e3987e274 538 etat = idle; // On annoce que la pixy est libre
haarkon 12:7493191dd1dc 539 break;
haarkon 12:7493191dd1dc 540
haarkon 12:7493191dd1dc 541 default : // Dans tous les autres cas
haarkon 18:190e3987e274 542 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 12:7493191dd1dc 543 break;
haarkon 12:7493191dd1dc 544 }
haarkon 12:7493191dd1dc 545 return cr;
haarkon 12:7493191dd1dc 546 }
haarkon 12:7493191dd1dc 547
haarkon 12:7493191dd1dc 548 T_pixy2ErrorCode PIXY2::pixy2_getFPS (T_pixy2ReturnCode *framerate){
haarkon 12:7493191dd1dc 549
haarkon 18:190e3987e274 550 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
haarkon 12:7493191dd1dc 551 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 12:7493191dd1dc 552
haarkon 12:7493191dd1dc 553 switch (etat) {
haarkon 12:7493191dd1dc 554 case idle : // Si la caméra est inactive
haarkon 18:190e3987e274 555 wPointer = 0; // On remonte en haut du buffer
haarkon 18:190e3987e274 556 cr = PIXY2::pixy2_sndGetFPS(); // On envoie la trame de demande du Framerate
haarkon 18:190e3987e274 557 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 12:7493191dd1dc 558 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 12:7493191dd1dc 559 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 12:7493191dd1dc 560 break;
haarkon 12:7493191dd1dc 561
haarkon 12:7493191dd1dc 562 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 12:7493191dd1dc 563 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 18:190e3987e274 564 if ( pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
haarkon 18:190e3987e274 565 if (msg->pixType == PIXY2_REP_FPS) { // On vérifie que la trame est du type convenable (REPONSE FPS)
haarkon 18:190e3987e274 566 framerate = (T_pixy2ReturnCode*) &Pixy2_buffer[dPointer]; // On mappe le pointeur de structure sur le buffer de réception.
haarkon 18:190e3987e274 567 } else { // Si ce n'est pas le bon type
haarkon 18:190e3987e274 568 if (msg->pixType == PIXY2_REP_ERROR) { // Cela pourrait être une trame d'erreur
haarkon 18:190e3987e274 569 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 18:190e3987e274 570 }else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 12:7493191dd1dc 571 }
haarkon 12:7493191dd1dc 572 } else cr = PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur aussi !
haarkon 12:7493191dd1dc 573 } else { // S'il n'y a pas de checksum (pas normal mais on ne sait jamais)
haarkon 18:190e3987e274 574 if (msg->pixType == PIXY2_REP_FPS) { // On vérifie que la trame est du type convenable (REPONSE FPS)
haarkon 18:190e3987e274 575 framerate = (T_pixy2ReturnCode*) &Pixy2_buffer[dPointer]; // Si le type est bon, on mappe le pointeur de structure sur le buffer de réception.
haarkon 12:7493191dd1dc 576 } else {
haarkon 18:190e3987e274 577 if (msg->pixType == PIXY2_REP_ERROR) { // Si on reçoit une trame d'erreur
haarkon 18:190e3987e274 578 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // On copie le code d'erreur reçu dans la variable de retour
haarkon 18:190e3987e274 579 }else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur
haarkon 12:7493191dd1dc 580 }
haarkon 12:7493191dd1dc 581 }
haarkon 18:190e3987e274 582 etat = idle; // On annoce que la pixy est libre
haarkon 12:7493191dd1dc 583 break;
haarkon 12:7493191dd1dc 584
haarkon 12:7493191dd1dc 585 default : // Dans tous les autres cas
haarkon 18:190e3987e274 586 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 12:7493191dd1dc 587 break;
haarkon 12:7493191dd1dc 588 }
haarkon 12:7493191dd1dc 589 return cr;
haarkon 12:7493191dd1dc 590 }
haarkon 12:7493191dd1dc 591
haarkon 18:190e3987e274 592 T_pixy2ErrorCode PIXY2::pixy2_getBlocks (Byte sigmap, Byte maxBloc){
haarkon 18:190e3987e274 593
haarkon 18:190e3987e274 594 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
haarkon 18:190e3987e274 595 T_pixy2ErrorCode cr = PIXY2_OK;
haarkon 18:190e3987e274 596
haarkon 18:190e3987e274 597 switch (etat) {
haarkon 18:190e3987e274 598 case idle : // Si la caméra est inactive
haarkon 18:190e3987e274 599 wPointer = 0; // On remonte en haut du buffer
haarkon 18:190e3987e274 600 cr = PIXY2::pixy2_sndGetBlocks(sigmap, maxBloc); // On envoie la trame de demande de blocs de couleur
haarkon 18:190e3987e274 601 if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
haarkon 18:190e3987e274 602 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 18:190e3987e274 603 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 18:190e3987e274 604 break;
haarkon 18:190e3987e274 605
haarkon 18:190e3987e274 606 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 18:190e3987e274 607 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 18:190e3987e274 608 if ( pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
haarkon 18:190e3987e274 609 if (msg->pixType == PIXY2_REP_BLOC) { // On vérifie que la trame est du type convenable (REPONSE BLOCS)
haarkon 18:190e3987e274 610 Pixy2_blocks = (T_pixy2Bloc*) &Pixy2_buffer[dPointer]; // On mappe le pointeur de structure sur le buffer de réception.
haarkon 18:190e3987e274 611 Pixy2_numBlocks = dataSize / sizeof(T_pixy2Bloc); // On indique le nombre de blocs reçus
haarkon 18:190e3987e274 612 } else { // Si ce n'est pas le bon type
haarkon 18:190e3987e274 613 if (msg->pixType == PIXY2_REP_ERROR) { // Cela pourrait être une trame d'erreur
haarkon 18:190e3987e274 614 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
haarkon 18:190e3987e274 615 }else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
haarkon 18:190e3987e274 616 }
haarkon 18:190e3987e274 617 } else cr = PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur aussi !
haarkon 18:190e3987e274 618 } else { // S'il n'y a pas de checksum (pas normal mais on ne sait jamais)
haarkon 18:190e3987e274 619 if (msg->pixType == PIXY2_REP_BLOC) { // On vérifie que la trame est du type convenable (REPONSE BLOCS)
haarkon 18:190e3987e274 620 Pixy2_blocks = (T_pixy2Bloc*) &Pixy2_buffer[dPointer]; // On mappe le pointeur de structure sur le buffer de réception.
haarkon 18:190e3987e274 621 Pixy2_numBlocks = dataSize / sizeof(T_pixy2Bloc); // On indique le nombre de blocs reçus
haarkon 18:190e3987e274 622 } else {
haarkon 18:190e3987e274 623 if (msg->pixType == PIXY2_REP_ERROR) { // Si on reçoit une trame d'erreur
haarkon 18:190e3987e274 624 cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // On copie le code d'erreur reçu dans la variable de retour
haarkon 18:190e3987e274 625 }else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur
haarkon 18:190e3987e274 626 }
haarkon 18:190e3987e274 627 }
haarkon 18:190e3987e274 628 etat = idle; // On annoce que la pixy est libre
haarkon 18:190e3987e274 629 break;
haarkon 18:190e3987e274 630
haarkon 18:190e3987e274 631 default : // Dans tous les autres cas
haarkon 18:190e3987e274 632 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 18:190e3987e274 633 break;
haarkon 18:190e3987e274 634 }
haarkon 18:190e3987e274 635 return cr;
haarkon 18:190e3987e274 636 }
haarkon 18:190e3987e274 637
haarkon 12:7493191dd1dc 638 T_pixy2ErrorCode PIXY2::pixy2_getMainFeature (Byte features){return PIXY2_OK;}
haarkon 12:7493191dd1dc 639 T_pixy2ErrorCode PIXY2::pixy2_getAllFeature (Byte features){return PIXY2_OK;}
haarkon 12:7493191dd1dc 640 T_pixy2ErrorCode PIXY2::pixy2_setMode (Byte mode){return PIXY2_OK;}
haarkon 16:f84b0f970c3e 641 T_pixy2ErrorCode PIXY2::pixy2_setNexTurn (sWord angle){return PIXY2_OK;}
haarkon 16:f84b0f970c3e 642 T_pixy2ErrorCode PIXY2::pixy2_setDefaultTurn (sWord angle){return PIXY2_OK;}
haarkon 12:7493191dd1dc 643 T_pixy2ErrorCode PIXY2::pixy2_setVector (Byte vectorIndex){return PIXY2_OK;}
haarkon 12:7493191dd1dc 644 T_pixy2ErrorCode PIXY2::pixy2_ReverseVector (void){return PIXY2_OK;}
haarkon 12:7493191dd1dc 645 T_pixy2ErrorCode PIXY2::pixy2_getRGB (Word x, Word y, Byte saturate, T_pixy2Pixel *pixel){return PIXY2_OK;}
haarkon 0:dde1b9d6c9d6 646
haarkon 11:68ef9f85e40e 647 T_pixy2ErrorCode PIXY2::pixy2_validateChecksum (Byte* tab){
haarkon 1:dd81f4065b6b 648 Word i, sum = 0;
haarkon 1:dd81f4065b6b 649 T_Word *tmp;
haarkon 0:dde1b9d6c9d6 650
haarkon 1:dd81f4065b6b 651 for (i=0; i<*(tab+3);i++) sum = sum + *(tab + PIXY2_CSHEADERSIZE + i);
haarkon 1:dd81f4065b6b 652 tmp = (T_Word*) (tab+4);
haarkon 12:7493191dd1dc 653 if (tmp->mot == sum) return PIXY2_OK;
haarkon 5:28e47187bb5c 654 else return PIXY2_BAD_CHECKSUM;
haarkon 1:dd81f4065b6b 655 }