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@12:7493191dd1dc, 2019-03-12 (annotated)

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