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@5:28e47187bb5c, 2019-03-11 (annotated)

Committer:
haarkon
Date:
Mon Mar 11 17:18:25 2019 +0000
Revision:
5:28e47187bb5c
Parent:
3:6ba43af28440
Child:
6:e9e3aa6f1a55
Last release of the day (still very imcomplete)

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 0:dde1b9d6c9d6 68 int PIXY2::pixy2_sndGetVersion (void){
haarkon 0:dde1b9d6c9d6 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 0:dde1b9d6c9d6 82 int PIXY2::pixy2_sndGetResolution (void){
haarkon 0:dde1b9d6c9d6 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 0:dde1b9d6c9d6 97 int PIXY2::pixy2_sndSetCameraBrightness (Byte brightness){
haarkon 0:dde1b9d6c9d6 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 0:dde1b9d6c9d6 112 int PIXY2::pixy2_sndSetServo (Word s0, Word s1){
haarkon 0:dde1b9d6c9d6 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 0:dde1b9d6c9d6 133 int PIXY2::pixy2_sndSetLED (Byte red, Byte green, Byte blue){
haarkon 0:dde1b9d6c9d6 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 0:dde1b9d6c9d6 150 int PIXY2::pixy2_sndSetLamp (Byte upper, Byte lower){
haarkon 0:dde1b9d6c9d6 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 0:dde1b9d6c9d6 166 int PIXY2::pixy2_sndGetFPS (void){
haarkon 0:dde1b9d6c9d6 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 0:dde1b9d6c9d6 180 int PIXY2::pixy2_sndGetBlocks (Byte sigmap, Byte maxBloc){
haarkon 0:dde1b9d6c9d6 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 0:dde1b9d6c9d6 196 int PIXY2::pixy2_sndGetMainFeature (Byte type, Byte feature){
haarkon 0:dde1b9d6c9d6 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 0:dde1b9d6c9d6 212 int PIXY2::pixy2_sndSetMode (Byte mode){
haarkon 0:dde1b9d6c9d6 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 0:dde1b9d6c9d6 227 int PIXY2::pixy2_sndSetNexTurn (Word angle){
haarkon 0:dde1b9d6c9d6 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 0:dde1b9d6c9d6 246 int PIXY2::pixy2_sndSetDefaultTurn (Word angle){
haarkon 0:dde1b9d6c9d6 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 0:dde1b9d6c9d6 265 int PIXY2::pixy2_sndSetVector (Byte vectorIndex){
haarkon 0:dde1b9d6c9d6 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 0:dde1b9d6c9d6 280 int PIXY2::pixy2_sndReverseVector (void){
haarkon 0:dde1b9d6c9d6 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 0:dde1b9d6c9d6 294 int PIXY2::pixy2_sndGetRGB (Word x, Word y, Byte saturate){
haarkon 0:dde1b9d6c9d6 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 0:dde1b9d6c9d6 314 int PIXY2::pixy2_getVersion (T_Pixy2Version *version){
haarkon 0:dde1b9d6c9d6 315
haarkon 0:dde1b9d6c9d6 316 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[rPointer];
haarkon 5:28e47187bb5c 317 int 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 3:6ba43af28440 332 // 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 333 etat = idle; // Et on annoce que la pixy est libre
haarkon 5:28e47187bb5c 334 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond pas on signale une erreur
haarkon 5:28e47187bb5c 335 } else cr = PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur aussi !
haarkon 3:6ba43af28440 336 } else { // S'il n'y a pas de checksum (pas normal mais on ne sait jamais)
haarkon 3:6ba43af28440 337 if (msg->pixType == PIXY2_REP_VERS) { // On vérifie que la trame est du type convenable
haarkon 0:dde1b9d6c9d6 338 version = (T_Pixy2Version*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
haarkon 3:6ba43af28440 339 // 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 340 etat = idle; // Et on annoce que la pixy est libre
haarkon 5:28e47187bb5c 341 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond pas on signale une erreur
haarkon 0:dde1b9d6c9d6 342 }
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 3:6ba43af28440 352 int PIXY2::pixy2_getResolution (T_Pixy2Resolution *resolution){
haarkon 3:6ba43af28440 353
haarkon 3:6ba43af28440 354 T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[rPointer];
haarkon 5:28e47187bb5c 355 int 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 3:6ba43af28440 359 PIXY2::pixy2_sndGetResolution(); // On envoie la trame de demande de la résolution
haarkon 3:6ba43af28440 360 etat = messageSent; // On passe à l'attente du message de réponse
haarkon 3:6ba43af28440 361 rPointer = wPointer; // On enregistre la position du pointeur de FIFO
haarkon 5:28e47187bb5c 362 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 3:6ba43af28440 363 break;
haarkon 3:6ba43af28440 364
haarkon 3:6ba43af28440 365 case dataReceived : // Quand on a reçu l'intégralité du message
haarkon 3:6ba43af28440 366 if (frameContainChecksum) { // Si la trame contient un checksum
haarkon 3:6ba43af28440 367 if ( pixy2_validateChecksum (&Pixy2_buffer[rPointer]) ) { // On lance la validation du checksum
haarkon 3:6ba43af28440 368 if (msg->pixType == PIXY2_REP_RESOL) { // On vérifie que la trame est du type convenable
haarkon 3:6ba43af28440 369 resolution = (T_Pixy2Resolution*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
haarkon 3:6ba43af28440 370 // 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 371 etat = idle; // Et on annoce que la pixy est libre
haarkon 5:28e47187bb5c 372 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond pas on signale une erreur
haarkon 5:28e47187bb5c 373 } else cr = PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur aussi !
haarkon 3:6ba43af28440 374 } else { // S'il n'y a pas de checksum (pas normal mais on ne sait jamais)
haarkon 3:6ba43af28440 375 if (msg->pixType == PIXY2_REP_RESOL) { // On vérifie que la trame est du type convenable
haarkon 3:6ba43af28440 376 resolution = (T_Pixy2Resolution*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
haarkon 3:6ba43af28440 377 // 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 378 etat = idle; // Et on annoce que la pixy est libre
haarkon 5:28e47187bb5c 379 } else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond pas on signale une erreur
haarkon 3:6ba43af28440 380 }
haarkon 3:6ba43af28440 381 break;
haarkon 3:6ba43af28440 382
haarkon 3:6ba43af28440 383 default : // Dans tous les autres cas
haarkon 5:28e47187bb5c 384 cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
haarkon 3:6ba43af28440 385 break;
haarkon 3:6ba43af28440 386 }
haarkon 3:6ba43af28440 387 return cr;
haarkon 3:6ba43af28440 388 }
haarkon 3:6ba43af28440 389
haarkon 3:6ba43af28440 390
haarkon 0:dde1b9d6c9d6 391 int PIXY2::pixy2_setCameraBrightness (Byte brightness){return 0;}
haarkon 0:dde1b9d6c9d6 392 int PIXY2::pixy2_setServos (Word s0, Word s1){return 0;}
haarkon 0:dde1b9d6c9d6 393 int PIXY2::pixy2_setLED (Byte red, Byte green, Byte blue){return 0;}
haarkon 0:dde1b9d6c9d6 394 int PIXY2::pixy2_setLamp (Byte upper, Byte lower){return 0;}
haarkon 1:dd81f4065b6b 395 int PIXY2::pixy2_getFPS (T_pixy2returnCode *framerate){return 0;}
haarkon 0:dde1b9d6c9d6 396 int PIXY2::pixy2_getBlocks (Byte sigmap, Byte maxBloc){return 0;}
haarkon 0:dde1b9d6c9d6 397 int PIXY2::pixy2_getMainFeature (Byte type, Byte features){return 0;}
haarkon 0:dde1b9d6c9d6 398 int PIXY2::pixy2_getAllFeature (Byte features){return 0;}
haarkon 0:dde1b9d6c9d6 399 int PIXY2::pixy2_setMode (Byte mode){return 0;}
haarkon 0:dde1b9d6c9d6 400 int PIXY2::pixy2_setNexTurn (Word angle){return 0;}
haarkon 0:dde1b9d6c9d6 401 int PIXY2::pixy2_setDefaultTurn (Word angle){return 0;}
haarkon 0:dde1b9d6c9d6 402 int PIXY2::pixy2_setVector (Byte vectorIndex){return 0;}
haarkon 0:dde1b9d6c9d6 403 int PIXY2::pixy2_ReverseVector (void){return 0;}
haarkon 0:dde1b9d6c9d6 404 int PIXY2::pixy2_getRGB (Word x, Word y, Byte saturate, T_pixy2Pixel *pixel){return 0;}
haarkon 0:dde1b9d6c9d6 405
haarkon 0:dde1b9d6c9d6 406 int PIXY2::pixy2_validateChecksum (Byte* tab){
haarkon 1:dd81f4065b6b 407 Word i, sum = 0;
haarkon 1:dd81f4065b6b 408 T_Word *tmp;
haarkon 0:dde1b9d6c9d6 409
haarkon 1:dd81f4065b6b 410 for (i=0; i<*(tab+3);i++) sum = sum + *(tab + PIXY2_CSHEADERSIZE + i);
haarkon 1:dd81f4065b6b 411 tmp = (T_Word*) (tab+4);
haarkon 1:dd81f4065b6b 412 if (tmp->mot == sum) return 0;
haarkon 5:28e47187bb5c 413 else return PIXY2_BAD_CHECKSUM;
haarkon 1:dd81f4065b6b 414 }