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

Committer:
haarkon
Date:
Tue Mar 12 14:06:53 2019 +0000
Revision:
10:ea759846c2d5
Parent:
6:e9e3aa6f1a55
Child:
11:68ef9f85e40e
error handling

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 6:e9e3aa6f1a55 68 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 6:e9e3aa6f1a55 82 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 6:e9e3aa6f1a55 97 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 6:e9e3aa6f1a55 112 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 6:e9e3aa6f1a55 133 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 6:e9e3aa6f1a55 150 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 6:e9e3aa6f1a55 166 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 6:e9e3aa6f1a55 180 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 6:e9e3aa6f1a55 196 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 6:e9e3aa6f1a55 212 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 6:e9e3aa6f1a55 227 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 6:e9e3aa6f1a55 246 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 6:e9e3aa6f1a55 265 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 6:e9e3aa6f1a55 280 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 6:e9e3aa6f1a55 294 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 6:e9e3aa6f1a55 314 Pixy2ErrorCode PIXY2::pixy2_getVersion (T_Pixy2Version *version){
haarkon 0:dde1b9d6c9d6 315
haarkon 0:dde1b9d6c9d6 316 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[rPointer];
haarkon 10:ea759846c2d5 317 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 0:dde1b9d6c9d6 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 3:6ba43af28440 333 etat = idle; // Et on annoce que la pixy est libre
haarkon 10:ea759846c2d5 334 } else {
haarkon 10:ea759846c2d5 335 if (msg->pixType == PIXY2_ERROR) { // Si on reçoit une trame d'erreur
haarkon 10:ea759846c2d5 336 cr = *(Pixy2ErrorCode*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
haarkon 10:ea759846c2d5 337 // On mappe le résultat sur la donnée stockée dans la FIFO (ligne précédente)
haarkon 10:ea759846c2d5 338 }else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur
haarkon 10:ea759846c2d5 339 }
haarkon 5:28e47187bb5c 340 } else cr = PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur aussi !
haarkon 3:6ba43af28440 341 } else { // S'il n'y a pas de checksum (pas normal mais on ne sait jamais)
haarkon 3:6ba43af28440 342 if (msg->pixType == PIXY2_REP_VERS) { // On vérifie que la trame est du type convenable
haarkon 0:dde1b9d6c9d6 343 version = (T_Pixy2Version*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
haarkon 3:6ba43af28440 344 // Si le checksum et le type sont bon, on mappe le pointeur de résultat sur la FIFO (ligne précédente)
haarkon 3:6ba43af28440 345 etat = idle; // Et on annoce que la pixy est libre
haarkon 10:ea759846c2d5 346 } else {
haarkon 10:ea759846c2d5 347 if (msg->pixType == PIXY2_ERROR) { // Si on reçoit une trame d'erreur
haarkon 10:ea759846c2d5 348 cr = *(Pixy2ErrorCode*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
haarkon 10:ea759846c2d5 349 // On mappe le résultat sur la donnée stockée dans la FIFO (ligne précédente)
haarkon 10:ea759846c2d5 350 }else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur
haarkon 10:ea759846c2d5 351 }
haarkon 0:dde1b9d6c9d6 352 }
haarkon 0:dde1b9d6c9d6 353 break;
haarkon 0:dde1b9d6c9d6 354
haarkon 3:6ba43af28440 355 default : // Dans tous les autres cas
haarkon 5:28e47187bb5c 356 cr = PIXY2_BUSY; // On signale que la caméra est occupée.
haarkon 0:dde1b9d6c9d6 357 break;
haarkon 0:dde1b9d6c9d6 358 }
haarkon 0:dde1b9d6c9d6 359 return cr;
haarkon 0:dde1b9d6c9d6 360 }
haarkon 0:dde1b9d6c9d6 361
haarkon 6:e9e3aa6f1a55 362 Pixy2ErrorCode PIXY2::pixy2_getResolution (T_Pixy2Resolution *resolution){
haarkon 3:6ba43af28440 363
haarkon 3:6ba43af28440 364 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[rPointer];
haarkon 10:ea759846c2d5 365 Pixy2ErrorCode cr = PIXY2_OK;
haarkon 3:6ba43af28440 366
haarkon 3:6ba43af28440 367 switch (etat) {
haarkon 3:6ba43af28440 368 case idle : // Si la caméra est inactive
haarkon 3:6ba43af28440 369 PIXY2::pixy2_sndGetResolution(); // On envoie la trame de demande de la résolution
haarkon 3:6ba43af28440 370 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 3:6ba43af28440 371 rPointer = wPointer; // On enregistre la position du pointeur de FIFO
haarkon 5:28e47187bb5c 372 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 3:6ba43af28440 373 break;
haarkon 3:6ba43af28440 374
haarkon 3:6ba43af28440 375 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 3:6ba43af28440 376 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 3:6ba43af28440 377 if ( pixy2_validateChecksum (&Pixy2_buffer[rPointer]) ) { // On lance la validation du checksum
haarkon 3:6ba43af28440 378 if (msg->pixType == PIXY2_REP_RESOL) { // On vérifie que la trame est du type convenable
haarkon 3:6ba43af28440 379 resolution = (T_Pixy2Resolution*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
haarkon 3:6ba43af28440 380 // Si le checksum et le type sont bon, on mappe le pointeur de résultat sur la FIFO (ligne précédente)
haarkon 3:6ba43af28440 381 etat = idle; // Et on annoce que la pixy est libre
haarkon 10:ea759846c2d5 382 } else {
haarkon 10:ea759846c2d5 383 if (msg->pixType == PIXY2_ERROR) { // Si on reçoit une trame d'erreur
haarkon 10:ea759846c2d5 384 cr = *(Pixy2ErrorCode*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
haarkon 10:ea759846c2d5 385 // On mappe le résultat sur la donnée stockée dans la FIFO (ligne précédente)
haarkon 10:ea759846c2d5 386 }else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur
haarkon 10:ea759846c2d5 387 }
haarkon 5:28e47187bb5c 388 } else cr = PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur aussi !
haarkon 3:6ba43af28440 389 } else { // S'il n'y a pas de checksum (pas normal mais on ne sait jamais)
haarkon 3:6ba43af28440 390 if (msg->pixType == PIXY2_REP_RESOL) { // On vérifie que la trame est du type convenable
haarkon 3:6ba43af28440 391 resolution = (T_Pixy2Resolution*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
haarkon 3:6ba43af28440 392 // Si le checksum et le type sont bon, on mappe le pointeur de résultat sur la FIFO (ligne précédente)
haarkon 3:6ba43af28440 393 etat = idle; // Et on annoce que la pixy est libre
haarkon 10:ea759846c2d5 394 } else {
haarkon 10:ea759846c2d5 395 if (msg->pixType == PIXY2_ERROR) { // Si on reçoit une trame d'erreur
haarkon 10:ea759846c2d5 396 cr = *(Pixy2ErrorCode*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
haarkon 10:ea759846c2d5 397 // On mappe le résultat sur la donnée stockée dans la FIFO (ligne précédente)
haarkon 10:ea759846c2d5 398 }else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur
haarkon 10:ea759846c2d5 399 }
haarkon 3:6ba43af28440 400 }
haarkon 3:6ba43af28440 401 break;
haarkon 3:6ba43af28440 402
haarkon 3:6ba43af28440 403 default : // Dans tous les autres cas
haarkon 5:28e47187bb5c 404 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 3:6ba43af28440 405 break;
haarkon 3:6ba43af28440 406 }
haarkon 3:6ba43af28440 407 return cr;
haarkon 3:6ba43af28440 408 }
haarkon 3:6ba43af28440 409
haarkon 3:6ba43af28440 410
haarkon 6:e9e3aa6f1a55 411 Pixy2ErrorCode PIXY2::pixy2_setCameraBrightness (Byte brightness){return 0;}
haarkon 6:e9e3aa6f1a55 412 Pixy2ErrorCode PIXY2::pixy2_setServos (Word s0, Word s1){return 0;}
haarkon 6:e9e3aa6f1a55 413 Pixy2ErrorCode PIXY2::pixy2_setLED (Byte red, Byte green, Byte blue){return 0;}
haarkon 6:e9e3aa6f1a55 414 Pixy2ErrorCode PIXY2::pixy2_setLamp (Byte upper, Byte lower){return 0;}
haarkon 10:ea759846c2d5 415 Pixy2ErrorCode PIXY2::pixy2_getFPS (T_pixy2ReturnCode *framerate){return 0;}
haarkon 6:e9e3aa6f1a55 416 Pixy2ErrorCode PIXY2::pixy2_getBlocks (Byte sigmap, Byte maxBloc){return 0;}
haarkon 10:ea759846c2d5 417 Pixy2ErrorCode PIXY2::pixy2_getMainFeature (Byte features){return 0;}
haarkon 6:e9e3aa6f1a55 418 Pixy2ErrorCode PIXY2::pixy2_getAllFeature (Byte features){return 0;}
haarkon 6:e9e3aa6f1a55 419 Pixy2ErrorCode PIXY2::pixy2_setMode (Byte mode){return 0;}
haarkon 6:e9e3aa6f1a55 420 Pixy2ErrorCode PIXY2::pixy2_setNexTurn (Word angle){return 0;}
haarkon 6:e9e3aa6f1a55 421 Pixy2ErrorCode PIXY2::pixy2_setDefaultTurn (Word angle){return 0;}
haarkon 6:e9e3aa6f1a55 422 Pixy2ErrorCode PIXY2::pixy2_setVector (Byte vectorIndex){return 0;}
haarkon 6:e9e3aa6f1a55 423 Pixy2ErrorCode PIXY2::pixy2_ReverseVector (void){return 0;}
haarkon 6:e9e3aa6f1a55 424 Pixy2ErrorCode PIXY2::pixy2_getRGB (Word x, Word y, Byte saturate, T_pixy2Pixel *pixel){return 0;}
haarkon 0:dde1b9d6c9d6 425
haarkon 6:e9e3aa6f1a55 426 Pixy2ErrorCode PIXY2::pixy2_validateChecksum (Byte* tab){
haarkon 1:dd81f4065b6b 427 Word i, sum = 0;
haarkon 1:dd81f4065b6b 428 T_Word *tmp;
haarkon 0:dde1b9d6c9d6 429
haarkon 1:dd81f4065b6b 430 for (i=0; i<*(tab+3);i++) sum = sum + *(tab + PIXY2_CSHEADERSIZE + i);
haarkon 1:dd81f4065b6b 431 tmp = (T_Word*) (tab+4);
haarkon 1:dd81f4065b6b 432 if (tmp->mot == sum) return 0;
haarkon 5:28e47187bb5c 433 else return PIXY2_BAD_CHECKSUM;
haarkon 1:dd81f4065b6b 434 }