Theo Le Paih
/
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.
pixy2.cpp
- Committer:
- haarkon
- Date:
- 2019-03-13
- Revision:
- 18:190e3987e274
- Parent:
- 16:f84b0f970c3e
- Child:
- 19:007d361c8d66
File content as of revision 18:190e3987e274:
#include "pixy2.h"
PIXY2::PIXY2(PinName tx, PinName rx, int debit)
{
_Pixy2 = new Serial (tx, rx, debit);
_Pixy2->attach (callback(this,&PIXY2::pixy2_getByte));
etat = idle;
Pixy2_buffer = (Byte*) malloc (0x100);
}
/* 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.
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...
Les étapes de fonctionnement sont les suivantes :
* idle : La caméra n'a pas été solicité => on envoi le message de requête.
* messageSent : La requête a été transmise, mais la réponse n'est pas encore arrivée.
* receivingHeader : Le mot de synchro a été reçu, le reste de l'entête est en cours de réception.
* headerReceived : L'entête a été intégralement reçu et peut être traité (tache interne) pour définir la payload.
* receivingData : On est en train de recevoir la payload.
* dataReceived : La réponse a été intégralement reçue et est disponible pour le traitement (et la libération).
*/
void PIXY2::pixy2_getByte () // Interruption de la pixy2
{
T_Word *buffer;
Pixy2_buffer[wPointer] = _Pixy2->getc(); // On stocke l'octet reçu dans la première case dispo du buffer de réception
switch (etat) {
case messageSent : // Si on a envoyé une requete => on attend un entête
if (wPointer > 0) { // On attend d'avoir reçu 2 octets
buffer = (T_Word*) &Pixy2_buffer[wPointer-1]; // On pointe la structure sur les 2 derniers octets reçus
if ((buffer->mot == PIXY2_CSSYNC) || (buffer->mot == PIXY2_SYNC)) { // Si c'est un mot d'entête
etat = receivingHeader; // On passe à l'état réception de l'entête
hPointer = wPointer - 1; // On initialise le pointeur de l'entête
if (buffer->mot == PIXY2_SYNC) frameContainChecksum = 0; // Si c'est un entête sans checksum, on mémorise qu'il n'y a pas de checksum à vérifier
else frameContainChecksum = 1; // Sinon, on mémorise qu'il y a un checksum à vérifier
}
}
break;
case receivingHeader : // Si on est en train de recevoir un entête (entre le SYNC et... La fin de l'entête)
if ((frameContainChecksum && ((wPointer - hPointer) == PIXY2_CSHEADERSIZE-1)) || (!frameContainChecksum && ((wPointer - hPointer) == PIXY2_NCSHEADERSIZE-1))) {
// Si on a reçu 6 octets pour une trame avec checksum ou 4 pour une trame sans checksum, c'est à dire un entête complet
etat = receivingData; // On dit que l'on va de recevoir des données
dataSize = Pixy2_buffer[hPointer+3]; // On enregistre la taille de la payload
dPointer = wPointer + 1; // Et on sauvegarde le point de départ de la zone de données
}
break;
case receivingData : // Si on est en train de recevoir des données.
if (wPointer == ((dataSize - 1) + dPointer)) etat = dataReceived; // Quand on a reçu toutes les données promises on dit que c'est OK
break;
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.
break;
}
wPointer++; // on pointe la case suivante du buffer de réception
}
T_pixy2ErrorCode PIXY2::pixy2_sndGetVersion (void){
T_pixy2SendBuffer msg;
int i = 0, dataSize = 0;
msg.frame.header.pixSync = PIXY2_SYNC;
msg.frame.header.pixType = PIXY2_ASK_VERS;
msg.frame.header.pixLength = dataSize;
do {
while(!_Pixy2->writable());
_Pixy2->putc(msg.data[i]);
i++;
} while (i<(PIXY2_NCSHEADERSIZE+dataSize));
return PIXY2_OK;
}
T_pixy2ErrorCode PIXY2::pixy2_sndGetResolution (void){
T_pixy2SendBuffer msg;
int i = 0, dataSize = 1;
msg.frame.header.pixSync = PIXY2_SYNC;
msg.frame.header.pixType = PIXY2_ASK_RESOL;
msg.frame.header.pixLength = dataSize;
msg.frame.data[0] = 0;
do {
while(!_Pixy2->writable());
_Pixy2->putc(msg.data[i]);
i++;
} while (i<(PIXY2_NCSHEADERSIZE+dataSize));
return PIXY2_OK;
}
T_pixy2ErrorCode PIXY2::pixy2_sndSetCameraBrightness (Byte brightness){
T_pixy2SendBuffer msg;
int i = 0, dataSize = 1;
msg.frame.header.pixSync = PIXY2_SYNC;
msg.frame.header.pixType = PIXY2_SET_BRIGHT;
msg.frame.header.pixLength = dataSize;
msg.frame.data[0] = brightness;
do {
while(!_Pixy2->writable());
_Pixy2->putc(msg.data[i]);
i++;
} while (i<(PIXY2_NCSHEADERSIZE+dataSize));
return PIXY2_OK;
}
T_pixy2ErrorCode PIXY2::pixy2_sndSetServo (Word s0, Word s1){
T_pixy2SendBuffer msg;
int i = 0, dataSize = 4;
T_Word tmp;
msg.frame.header.pixSync = PIXY2_SYNC;
msg.frame.header.pixType = PIXY2_SET_SERVOS;
msg.frame.header.pixLength = dataSize;
tmp.mot = s0;
msg.frame.data[0] = tmp.octet[0];
msg.frame.data[1] = tmp.octet[1];
tmp.mot = s1;
msg.frame.data[2] = tmp.octet[0];
msg.frame.data[3] = tmp.octet[1];
do {
while(!_Pixy2->writable());
_Pixy2->putc(msg.data[i]);
i++;
} while (i<(PIXY2_NCSHEADERSIZE+dataSize));
return PIXY2_OK;
}
T_pixy2ErrorCode PIXY2::pixy2_sndSetLED (Byte red, Byte green, Byte blue){
T_pixy2SendBuffer msg;
int i = 0, dataSize = 3;
msg.frame.header.pixSync = PIXY2_SYNC;
msg.frame.header.pixType = PIXY2_SET_LED;
msg.frame.header.pixLength = dataSize;
msg.frame.data[0] = red;
msg.frame.data[1] = green;
msg.frame.data[2] = blue;
do {
while(!_Pixy2->writable());
_Pixy2->putc(msg.data[i]);
i++;
} while (i<(PIXY2_NCSHEADERSIZE+dataSize));
return PIXY2_OK;
}
T_pixy2ErrorCode PIXY2::pixy2_sndSetLamp (Byte upper, Byte lower){
T_pixy2SendBuffer msg;
int i = 0, dataSize = 2;
msg.frame.header.pixSync = PIXY2_SYNC;
msg.frame.header.pixType = PIXY2_SET_LED;
msg.frame.header.pixLength = dataSize;
msg.frame.data[0] = upper;
msg.frame.data[1] = lower;
do {
while(!_Pixy2->writable());
_Pixy2->putc(msg.data[i]);
i++;
} while (i<(PIXY2_NCSHEADERSIZE+dataSize));
return PIXY2_OK;
}
T_pixy2ErrorCode PIXY2::pixy2_sndGetFPS (void){
T_pixy2SendBuffer msg;
int i = 0, dataSize = 0;
msg.frame.header.pixSync = PIXY2_SYNC;
msg.frame.header.pixType = PIXY2_ASK_FPS;
msg.frame.header.pixLength = dataSize;
do {
while(!_Pixy2->writable());
_Pixy2->putc(msg.data[i]);
i++;
} while (i<(PIXY2_NCSHEADERSIZE+dataSize));
return PIXY2_OK;
}
T_pixy2ErrorCode PIXY2::pixy2_sndGetBlocks (Byte sigmap, Byte maxBloc){
T_pixy2SendBuffer msg;
int i = 0, dataSize = 2;
msg.frame.header.pixSync = PIXY2_SYNC;
msg.frame.header.pixType = PIXY2_ASK_BLOC;
msg.frame.header.pixLength = dataSize;
msg.frame.data[0] = sigmap;
msg.frame.data[1] = maxBloc;
do {
while(!_Pixy2->writable());
_Pixy2->putc(msg.data[i]);
i++;
} while (i<(PIXY2_NCSHEADERSIZE+dataSize));
return PIXY2_OK;
}
T_pixy2ErrorCode PIXY2::pixy2_sndGetMainFeature (Byte type, Byte feature){
T_pixy2SendBuffer msg;
int i = 0, dataSize = 2;
msg.frame.header.pixSync = PIXY2_SYNC;
msg.frame.header.pixType = PIXY2_ASK_LINE;
msg.frame.header.pixLength = dataSize;
msg.frame.data[0] = type;
msg.frame.data[1] = feature;
do {
while(!_Pixy2->writable());
_Pixy2->putc(msg.data[i]);
i++;
} while (i<(PIXY2_NCSHEADERSIZE+dataSize));
return PIXY2_OK;
}
T_pixy2ErrorCode PIXY2::pixy2_sndSetMode (Byte mode){
T_pixy2SendBuffer msg;
int i = 0, dataSize = 1;
msg.frame.header.pixSync = PIXY2_SYNC;
msg.frame.header.pixType = PIXY2_SET_MODE;
msg.frame.header.pixLength = dataSize;
msg.frame.data[0] = mode;
do {
while(!_Pixy2->writable());
_Pixy2->putc(msg.data[i]);
i++;
} while (i<(PIXY2_NCSHEADERSIZE+dataSize));
return PIXY2_OK;
}
T_pixy2ErrorCode PIXY2::pixy2_sndSetNexTurn (Word angle){
T_pixy2SendBuffer msg;
int i = 0, dataSize = 2;
T_Word tmp;
tmp.mot = angle;
msg.frame.header.pixSync = PIXY2_SYNC;
msg.frame.header.pixType = PIXY2_SET_TURN;
msg.frame.header.pixLength = dataSize;
tmp.mot = angle;
msg.frame.data[0] = tmp.octet[0];
msg.frame.data[1] = tmp.octet[1];
do {
while(!_Pixy2->writable());
_Pixy2->putc(msg.data[i]);
i++;
} while (i<(PIXY2_NCSHEADERSIZE+dataSize));
return PIXY2_OK;
}
T_pixy2ErrorCode PIXY2::pixy2_sndSetDefaultTurn (Word angle){
T_pixy2SendBuffer msg;
int i = 0, dataSize = 2;
T_Word tmp;
tmp.mot = angle;
msg.frame.header.pixSync = PIXY2_SYNC;
msg.frame.header.pixType = PIXY2_SET_DEFTURN;
msg.frame.header.pixLength = dataSize;
tmp.mot = angle;
msg.frame.data[0] = tmp.octet[0];
msg.frame.data[1] = tmp.octet[1];
do {
while(!_Pixy2->writable());
_Pixy2->putc(msg.data[i]);
i++;
} while (i<(PIXY2_NCSHEADERSIZE+dataSize));
return PIXY2_OK;
}
T_pixy2ErrorCode PIXY2::pixy2_sndSetVector (Byte vectorIndex){
T_pixy2SendBuffer msg;
int i = 0, dataSize = 1;
msg.frame.header.pixSync = PIXY2_SYNC;
msg.frame.header.pixType = PIXY2_SET_VECTOR;
msg.frame.header.pixLength = dataSize;
msg.frame.data[0] = vectorIndex;
do {
while(!_Pixy2->writable());
_Pixy2->putc(msg.data[i]);
i++;
} while (i<(PIXY2_NCSHEADERSIZE+dataSize));
return PIXY2_OK;
}
T_pixy2ErrorCode PIXY2::pixy2_sndReverseVector (void){
T_pixy2SendBuffer msg;
int i = 0, dataSize = 0;
msg.frame.header.pixSync = PIXY2_SYNC;
msg.frame.header.pixType = PIXY2_SET_REVERSE;
msg.frame.header.pixLength = dataSize;
do {
while(!_Pixy2->writable());
_Pixy2->putc(msg.data[i]);
i++;
} while (i<(PIXY2_NCSHEADERSIZE+dataSize));
return PIXY2_OK;
}
T_pixy2ErrorCode PIXY2::pixy2_sndGetRGB (Word x, Word y, Byte saturate){
T_pixy2SendBuffer msg;
int i = 0, dataSize = 3;
msg.frame.header.pixSync = PIXY2_SYNC;
msg.frame.header.pixType = PIXY2_ASK_VIDEO;
msg.frame.header.pixLength = dataSize;
msg.frame.data[0] = x;
msg.frame.data[1] = y;
msg.frame.data[2] = saturate;
do {
while(!_Pixy2->writable());
_Pixy2->putc(msg.data[i]);
i++;
} while (i<(PIXY2_NCSHEADERSIZE+dataSize));
return PIXY2_OK;
}
/* 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é.
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. */
T_pixy2ErrorCode PIXY2::pixy2_getVersion (T_pixy2Version *version){
T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
T_pixy2ErrorCode cr = PIXY2_OK;
switch (etat) {
case idle : // Si la caméra est inactive
wPointer = 0; // On remonte en haut du buffer
cr = PIXY2::pixy2_sndGetVersion(); // On envoie la trame de demande de la version
if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
etat = messageSent; // On passe à l'attente du message de réponse
cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
break;
case dataReceived : // Quand on a reçu l'intégralité du message
if (frameContainChecksum) { // Si la trame contient un checksum
if ( pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
if (msg->pixType == PIXY2_REP_VERS) { // On vérifie que la trame est du type convenable (REPONSE VERSION)
version = (T_pixy2Version*) &Pixy2_buffer[dPointer]; // On mappe le pointeur de structure sur le buffer de réception.
} else { // Si ce n'est pas le bon type
if (msg->pixType == PIXY2_REP_ERROR) { // Cela pourrait être une trame d'erreur
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
}
} else cr = PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur aussi !
} else { // S'il n'y a pas de checksum (pas normal mais on ne sait jamais)
if (msg->pixType == PIXY2_REP_VERS) { // On vérifie que la trame est du type convenable
version = (T_pixy2Version*) &Pixy2_buffer[dPointer]; // Si le type est bon, on mappe le pointeur de structure sur le buffer de réception.
} else {
if (msg->pixType == PIXY2_REP_ERROR) { // Si on reçoit une trame d'erreur
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // On copie le code d'erreur reçu dans la variable de retour
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur
}
}
etat = idle; // On annoce que la pixy est libre
break;
default : // Dans tous les autres cas
cr = PIXY2_BUSY; // On signale que la caméra est occupée.
break;
}
return cr;
}
T_pixy2ErrorCode PIXY2::pixy2_getResolution (T_pixy2Resolution *resolution){
T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
T_pixy2ErrorCode cr = PIXY2_OK;
switch (etat) {
case idle : // Si la caméra est inactive
wPointer = 0; // On remonte en haut du buffer
cr = PIXY2::pixy2_sndGetResolution(); // On envoie la trame de demande de la résolution
if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
etat = messageSent; // On passe à l'attente du message de réponse
cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
break;
case dataReceived : // Quand on a reçu l'intégralité du message
if (frameContainChecksum) { // Si la trame contient un checksum
if ( pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
if (msg->pixType == PIXY2_REP_RESOL) { // On vérifie que la trame est du type convenable (REPONSE RESOLUTION)
resolution = (T_pixy2Resolution*) &Pixy2_buffer[dPointer]; // On mappe le pointeur de structure sur le buffer de réception.
} else { // Si ce n'est pas le bon type
if (msg->pixType == PIXY2_REP_ERROR) { // Cela pourrait être une trame d'erreur
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
}
} else cr = PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur aussi !
} else { // S'il n'y a pas de checksum (pas normal mais on ne sait jamais)
if (msg->pixType == PIXY2_REP_RESOL) { // On vérifie que la trame est du type convenable (REPONSE RESOLUTION)
resolution = (T_pixy2Resolution*) &Pixy2_buffer[dPointer]; // Si le type est bon, on mappe le pointeur de structure sur le buffer de réception.
} else {
if (msg->pixType == PIXY2_REP_ERROR) { // Si on reçoit une trame d'erreur
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // On copie le code d'erreur reçu dans la variable de retour
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur
}
}
etat = idle; // On annoce que la pixy est libre
break;
default : // Dans tous les autres cas
cr = PIXY2_BUSY; // On signale que la caméra est occupée.
break;
}
return cr;
}
T_pixy2ErrorCode PIXY2::pixy2_setCameraBrightness (Byte brightness){
T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
T_pixy2ErrorCode cr = PIXY2_OK;
switch (etat) {
case idle : // Si la caméra est inactive
wPointer = 0; // On remonte en haut du buffer
cr = PIXY2::pixy2_sndSetCameraBrightness (brightness); // On envoie la trame de règlage de la luminosité
if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
etat = messageSent; // On passe à l'attente du message de réponse
cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
break;
case dataReceived : // Quand on a reçu l'intégralité du message
if (frameContainChecksum) { // Si la trame contient un checksum
if ( pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
// On vérifie que la trame est du type convenable (ACK ou ERREUR)
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
} else cr = PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur aussi !
} else { // S'il n'y a pas de checksum (pas normal mais on ne sait jamais)
if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
// On vérifie que la trame est du type convenable (ACK ou ERREUR)
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
}
etat = idle; // On annoce que la pixy est libre
break;
default : // Dans tous les autres cas
cr = PIXY2_BUSY; // On signale que la caméra est occupée.
break;
}
return cr;
}
T_pixy2ErrorCode PIXY2::pixy2_setServos (Word s0, Word s1){
T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
T_pixy2ErrorCode cr = PIXY2_OK;
switch (etat) {
case idle : // Si la caméra est inactive
wPointer = 0; // On remonte en haut du buffer
cr = PIXY2::pixy2_sndSetServo (s0, s1); // On envoie la trame de règlage des servos moteurs
if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
etat = messageSent; // On passe à l'attente du message de réponse
cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
break;
case dataReceived : // Quand on a reçu l'intégralité du message
if (frameContainChecksum) { // Si la trame contient un checksum
if ( pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
// On vérifie que la trame est du type convenable (ACK ou ERREUR)
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
} else cr = PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur aussi !
} else { // S'il n'y a pas de checksum (pas normal mais on ne sait jamais)
if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
// On vérifie que la trame est du type convenable (ACK ou ERREUR)
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
}
etat = idle; // On annoce que la pixy est libre
break;
default : // Dans tous les autres cas
cr = PIXY2_BUSY; // On signale que la caméra est occupée.
break;
}
return cr;
}
T_pixy2ErrorCode PIXY2::pixy2_setLED (Byte red, Byte green, Byte blue){
T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
T_pixy2ErrorCode cr = PIXY2_OK;
switch (etat) {
case idle : // Si la caméra est inactive
wPointer = 0; // On remonte en haut du buffer
cr = PIXY2::pixy2_sndSetLED (red, green, blue); // On envoie la trame de règlage des composantes de la LED RGB
if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
etat = messageSent; // On passe à l'attente du message de réponse
cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
break;
case dataReceived : // Quand on a reçu l'intégralité du message
if (frameContainChecksum) { // Si la trame contient un checksum
if ( pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
// On vérifie que la trame est du type convenable (ACK ou ERREUR)
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
} else cr = PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur aussi !
} else { // S'il n'y a pas de checksum (pas normal mais on ne sait jamais)
if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
// On vérifie que la trame est du type convenable (ACK ou ERREUR)
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
}
etat = idle; // On annoce que la pixy est libre
break;
default : // Dans tous les autres cas
cr = PIXY2_BUSY; // On signale que la caméra est occupée.
break;
}
return cr;
}
T_pixy2ErrorCode PIXY2::pixy2_setLamp (Byte upper, Byte lower){
T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
T_pixy2ErrorCode cr = PIXY2_OK;
switch (etat) {
case idle : // Si la caméra est inactive
wPointer = 0; // On remonte en haut du buffer
cr = PIXY2::pixy2_sndSetLamp (upper, lower); // On envoie la trame de règlage d'allumage des lumières de contraste
if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
etat = messageSent; // On passe à l'attente du message de réponse
cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
break;
case dataReceived : // Quand on a reçu l'intégralité du message
if (frameContainChecksum) { // Si la trame contient un checksum
if ( pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
// On vérifie que la trame est du type convenable (ACK ou ERREUR)
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
} else cr = PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur aussi !
} else { // S'il n'y a pas de checksum (pas normal mais on ne sait jamais)
if ((msg->pixType == PIXY2_REP_ACK) || (msg->pixType == PIXY2_REP_ERROR)) {
// On vérifie que la trame est du type convenable (ACK ou ERREUR)
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
}
etat = idle; // On annoce que la pixy est libre
break;
default : // Dans tous les autres cas
cr = PIXY2_BUSY; // On signale que la caméra est occupée.
break;
}
return cr;
}
T_pixy2ErrorCode PIXY2::pixy2_getFPS (T_pixy2ReturnCode *framerate){
T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
T_pixy2ErrorCode cr = PIXY2_OK;
switch (etat) {
case idle : // Si la caméra est inactive
wPointer = 0; // On remonte en haut du buffer
cr = PIXY2::pixy2_sndGetFPS(); // On envoie la trame de demande du Framerate
if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
etat = messageSent; // On passe à l'attente du message de réponse
cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
break;
case dataReceived : // Quand on a reçu l'intégralité du message
if (frameContainChecksum) { // Si la trame contient un checksum
if ( pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
if (msg->pixType == PIXY2_REP_FPS) { // On vérifie que la trame est du type convenable (REPONSE FPS)
framerate = (T_pixy2ReturnCode*) &Pixy2_buffer[dPointer]; // On mappe le pointeur de structure sur le buffer de réception.
} else { // Si ce n'est pas le bon type
if (msg->pixType == PIXY2_REP_ERROR) { // Cela pourrait être une trame d'erreur
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
}
} else cr = PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur aussi !
} else { // S'il n'y a pas de checksum (pas normal mais on ne sait jamais)
if (msg->pixType == PIXY2_REP_FPS) { // On vérifie que la trame est du type convenable (REPONSE FPS)
framerate = (T_pixy2ReturnCode*) &Pixy2_buffer[dPointer]; // Si le type est bon, on mappe le pointeur de structure sur le buffer de réception.
} else {
if (msg->pixType == PIXY2_REP_ERROR) { // Si on reçoit une trame d'erreur
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // On copie le code d'erreur reçu dans la variable de retour
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur
}
}
etat = idle; // On annoce que la pixy est libre
break;
default : // Dans tous les autres cas
cr = PIXY2_BUSY; // On signale que la caméra est occupée.
break;
}
return cr;
}
T_pixy2ErrorCode PIXY2::pixy2_getBlocks (Byte sigmap, Byte maxBloc){
T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
T_pixy2ErrorCode cr = PIXY2_OK;
switch (etat) {
case idle : // Si la caméra est inactive
wPointer = 0; // On remonte en haut du buffer
cr = PIXY2::pixy2_sndGetBlocks(sigmap, maxBloc); // On envoie la trame de demande de blocs de couleur
if (cr!= PIXY2_OK) return cr; // S'il y a une erreur lors de l'envoi on ejecte !
etat = messageSent; // On passe à l'attente du message de réponse
cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
break;
case dataReceived : // Quand on a reçu l'intégralité du message
if (frameContainChecksum) { // Si la trame contient un checksum
if ( pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
if (msg->pixType == PIXY2_REP_BLOC) { // On vérifie que la trame est du type convenable (REPONSE BLOCS)
Pixy2_blocks = (T_pixy2Bloc*) &Pixy2_buffer[dPointer]; // On mappe le pointeur de structure sur le buffer de réception.
Pixy2_numBlocks = dataSize / sizeof(T_pixy2Bloc); // On indique le nombre de blocs reçus
} else { // Si ce n'est pas le bon type
if (msg->pixType == PIXY2_REP_ERROR) { // Cela pourrait être une trame d'erreur
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // Si c'est le cas, on copie le code d'erreur reçu dans la variable de retour
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur de type.
}
} else cr = PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur aussi !
} else { // S'il n'y a pas de checksum (pas normal mais on ne sait jamais)
if (msg->pixType == PIXY2_REP_BLOC) { // On vérifie que la trame est du type convenable (REPONSE BLOCS)
Pixy2_blocks = (T_pixy2Bloc*) &Pixy2_buffer[dPointer]; // On mappe le pointeur de structure sur le buffer de réception.
Pixy2_numBlocks = dataSize / sizeof(T_pixy2Bloc); // On indique le nombre de blocs reçus
} else {
if (msg->pixType == PIXY2_REP_ERROR) { // Si on reçoit une trame d'erreur
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[dPointer]; // On copie le code d'erreur reçu dans la variable de retour
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur
}
}
etat = idle; // On annoce que la pixy est libre
break;
default : // Dans tous les autres cas
cr = PIXY2_BUSY; // On signale que la caméra est occupée.
break;
}
return cr;
}
T_pixy2ErrorCode PIXY2::pixy2_getMainFeature (Byte features){return PIXY2_OK;}
T_pixy2ErrorCode PIXY2::pixy2_getAllFeature (Byte features){return PIXY2_OK;}
T_pixy2ErrorCode PIXY2::pixy2_setMode (Byte mode){return PIXY2_OK;}
T_pixy2ErrorCode PIXY2::pixy2_setNexTurn (sWord angle){return PIXY2_OK;}
T_pixy2ErrorCode PIXY2::pixy2_setDefaultTurn (sWord angle){return PIXY2_OK;}
T_pixy2ErrorCode PIXY2::pixy2_setVector (Byte vectorIndex){return PIXY2_OK;}
T_pixy2ErrorCode PIXY2::pixy2_ReverseVector (void){return PIXY2_OK;}
T_pixy2ErrorCode PIXY2::pixy2_getRGB (Word x, Word y, Byte saturate, T_pixy2Pixel *pixel){return PIXY2_OK;}
T_pixy2ErrorCode PIXY2::pixy2_validateChecksum (Byte* tab){
Word i, sum = 0;
T_Word *tmp;
for (i=0; i<*(tab+3);i++) sum = sum + *(tab + PIXY2_CSHEADERSIZE + i);
tmp = (T_Word*) (tab+4);
if (tmp->mot == sum) return PIXY2_OK;
else return PIXY2_BAD_CHECKSUM;
}