CRAC Team
pixy2 smart camera library www.pixycam.com
Pixy2 Library project (using UART interface)
pixy2.cpp
- Committer:
- haarkon
- Date:
- 2019-03-12
- Revision:
- 14:c0eda1e35a00
- Parent:
- 12:7493191dd1dc
- Child:
- 16:f84b0f970c3e
File content as of revision 14:c0eda1e35a00:
#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;
rPointer = 0;
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
{
static Byte startPoint;
T_Word *buffer;
Pixy2_buffer[wPointer] = _Pixy2->getc(); // On stocke l'octet reçu dans la première case dispo du tableau temporaire
wPointer++;
switch (etat) {
case messageSent : // Si on a envoyé une requete => on attend un entête
if ( (wPointer - rPointer) >= 2) { // On attend d'avoir reçu 2 octets
startPoint = wPointer - 1;
buffer = (T_Word*) &Pixy2_buffer[startPoint]; //On pointe la structure sur les 2 derniers octets reçus
if ((buffer->mot == PIXY2_CSSYNC) || (buffer->mot == PIXY2_SYNC)) {
etat = receivingHeader; // On passe à l'état réception de l'entête
rPointer = startPoint;
if (buffer->mot == PIXY2_SYNC) frameContainChecksum = 0; // On mémorise qu'il n'y a pas de checksum à vérifier
else frameContainChecksum = 1; // 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 - rPointer) == PIXY2_CSHEADERSIZE)) || (!frameContainChecksum && ((wPointer - rPointer) == PIXY2_NCSHEADERSIZE))) {
//Si on a reçu 6 octets pour une trame avec checksum ou 4 pour une trame sans checksum
etat = headerReceived; //On passe à entête reçu
dataSize = Pixy2_buffer[startPoint+3]; // Et on lit la taille de la payload
}
break;
case headerReceived : // Si on vient de recevoir le premier octet de données
startPoint = wPointer; // On enregistre sa position
etat = receivingData; // Et on dit que l'on est en train de recevoir des données
break;
case receivingData : // Si on est en train de recevoir des données.
if (wPointer == (dataSize + startPoint)) 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;
}
}
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 = 0;
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 = 1;
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 = 1;
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 = 1;
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 = 1;
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 = 1;
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 = 0;
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 = 1;
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 = 1;
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 = 1;
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 = 1;
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 = 1;
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 = 1;
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 = 0;
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 = 1;
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[rPointer];
T_pixy2ErrorCode cr = PIXY2_OK;
switch (etat) {
case idle : // Si la caméra est inactive
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
rPointer = wPointer; // On enregistre la position du pointeur de FIFO
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[rPointer]) ) { // On lance la validation du checksum
if (msg->pixType == PIXY2_REP_VERS) { // On vérifie que la trame est du type convenable
version = (T_pixy2Version*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
// On mappe le pointeur de version sur la donnée stockée dans la FIFO (ligne précédente)
} else {
if (msg->pixType == PIXY2_ERROR) { // Si on reçoit une trame d'erreur
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
// 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
}
} 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[rPointer + PIXY2_NCSHEADERSIZE];
// Si le type est bon, on mappe le pointeur de résultat sur la FIFO (ligne précédente)
} else {
if (msg->pixType == PIXY2_ERROR) { // Si on reçoit une trame d'erreur
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
// 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; // Et 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[rPointer];
T_pixy2ErrorCode cr = PIXY2_OK;
switch (etat) {
case idle : // Si la caméra est inactive
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
rPointer = wPointer; // On enregistre la position du pointeur de FIFO
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[rPointer]) ) { // On lance la validation du checksum
if (msg->pixType == PIXY2_REP_RESOL) { // On vérifie que la trame est du type convenable
resolution = (T_pixy2Resolution*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
// Si le checksum et le type sont bon, on mappe le pointeur de résultat sur la FIFO (ligne précédente)
} else {
if (msg->pixType == PIXY2_ERROR) { // Si on reçoit une trame d'erreur
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
// 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
}
} 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
resolution = (T_pixy2Resolution*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
// Si le type est bon, on mappe le pointeur de résultat sur la FIFO (ligne précédente)
} else {
if (msg->pixType == PIXY2_ERROR) { // Si on reçoit une trame d'erreur
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
// 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; // Et 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[rPointer];
T_pixy2ErrorCode cr = PIXY2_OK;
T_pixy2ReturnCode retour;
switch (etat) {
case idle : // Si la caméra est inactive
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
rPointer = wPointer; // On enregistre la position du pointeur de FIFO
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[rPointer]) ) { // On lance la validation du checksum
if (msg->pixType == PIXY2_ACK) { // On vérifie que la trame est du type convenable
retour = *(T_pixy2ReturnCode*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
// Si le checksum et le type sont bon, on mappe le pointeur de résultat sur la FIFO (ligne précédente)
if (retour.pixReturn != PIXY2_OK) cr = (T_pixy2ErrorCode) retour.pixReturn;
// Si la Pixy n'aquite pas l'ordre, on retourne l'erreur signalée
} else {
if (msg->pixType == PIXY2_ERROR) { // Si on reçoit une trame d'erreur
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
// On mappe le résultat sur la donnée stockée dans la FIFO (ligne précédente)
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur
}
} 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_ACK) { // On vérifie que la trame est du type convenable
retour = *(T_pixy2ReturnCode*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
// Si le type est bon, on mappe le pointeur de résultat sur la FIFO (ligne précédente)
if (retour.pixReturn != PIXY2_OK) cr = (T_pixy2ErrorCode) retour.pixReturn;
// Si la Pixy n'aquite pas l'ordre, on retourne l'erreur signalée
} else {
if (msg->pixType == PIXY2_ERROR) { // Si on reçoit une trame d'erreur
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
// On mappe le résultat sur la donnée stockée dans la FIFO (ligne précédente)
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur
}
}
etat = idle; // Et on annoce que la pixy est libre
break;
default : // Dans tous les autres cas
cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
break;
}
return cr;
}
T_pixy2ErrorCode PIXY2::pixy2_setServos (Word s0, Word s1){
T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[rPointer];
T_pixy2ErrorCode cr = PIXY2_OK;
T_pixy2ReturnCode retour;
switch (etat) {
case idle : // Si la caméra est inactive
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
rPointer = wPointer; // On enregistre la position du pointeur de FIFO
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[rPointer]) ) { // On lance la validation du checksum
if (msg->pixType == PIXY2_ACK) { // On vérifie que la trame est du type convenable
retour = *(T_pixy2ReturnCode*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
// Si le checksum et le type sont bon, on mappe le pointeur de résultat sur la FIFO (ligne précédente)
if (retour.pixReturn != PIXY2_OK) cr = (T_pixy2ErrorCode) retour.pixReturn;
// Si la Pixy n'aquite pas l'ordre, on retourne l'erreur signalée
} else {
if (msg->pixType == PIXY2_ERROR) { // Si on reçoit une trame d'erreur
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
// On mappe le résultat sur la donnée stockée dans la FIFO (ligne précédente)
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur
}
} 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_ACK) { // On vérifie que la trame est du type convenable
retour = *(T_pixy2ReturnCode*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
// Si le type est bon, on mappe le pointeur de résultat sur la FIFO (ligne précédente)
if (retour.pixReturn != PIXY2_OK) cr = (T_pixy2ErrorCode) retour.pixReturn;
// Si la Pixy n'aquite pas l'ordre, on retourne l'erreur signalée
} else {
if (msg->pixType == PIXY2_ERROR) { // Si on reçoit une trame d'erreur
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
// On mappe le résultat sur la donnée stockée dans la FIFO (ligne précédente)
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur
}
}
etat = idle; // Et on annoce que la pixy est libre
break;
default : // Dans tous les autres cas
cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
break;
}
return cr;
}
T_pixy2ErrorCode PIXY2::pixy2_setLED (Byte red, Byte green, Byte blue){
T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[rPointer];
T_pixy2ErrorCode cr = PIXY2_OK;
T_pixy2ReturnCode retour;
switch (etat) {
case idle : // Si la caméra est inactive
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
etat = messageSent; // On passe à l'attente du message de réponse
rPointer = wPointer; // On enregistre la position du pointeur de FIFO
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[rPointer]) ) { // On lance la validation du checksum
if (msg->pixType == PIXY2_ACK) { // On vérifie que la trame est du type convenable
retour = *(T_pixy2ReturnCode*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
// Si le checksum et le type sont bon, on mappe le pointeur de résultat sur la FIFO (ligne précédente)
if (retour.pixReturn != PIXY2_OK) cr = (T_pixy2ErrorCode) retour.pixReturn;
// Si la Pixy n'aquite pas l'ordre, on retourne l'erreur signalée
} else {
if (msg->pixType == PIXY2_ERROR) { // Si on reçoit une trame d'erreur
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
// On mappe le résultat sur la donnée stockée dans la FIFO (ligne précédente)
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur
}
} 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_ACK) { // On vérifie que la trame est du type convenable
retour = *(T_pixy2ReturnCode*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
// Si le type est bon, on mappe le pointeur de résultat sur la FIFO (ligne précédente)
if (retour.pixReturn != PIXY2_OK) cr = (T_pixy2ErrorCode) retour.pixReturn;
// Si la Pixy n'aquite pas l'ordre, on retourne l'erreur signalée
} else {
if (msg->pixType == PIXY2_ERROR) { // Si on reçoit une trame d'erreur
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
// On mappe le résultat sur la donnée stockée dans la FIFO (ligne précédente)
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur
}
}
etat = idle; // Et on annoce que la pixy est libre
break;
default : // Dans tous les autres cas
cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
break;
}
return cr;
}
T_pixy2ErrorCode PIXY2::pixy2_setLamp (Byte upper, Byte lower){
T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[rPointer];
T_pixy2ErrorCode cr = PIXY2_OK;
T_pixy2ReturnCode retour;
switch (etat) {
case idle : // Si la caméra est inactive
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
etat = messageSent; // On passe à l'attente du message de réponse
rPointer = wPointer; // On enregistre la position du pointeur de FIFO
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[rPointer]) ) { // On lance la validation du checksum
if (msg->pixType == PIXY2_ACK) { // On vérifie que la trame est du type convenable
retour = *(T_pixy2ReturnCode*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
// Si le checksum et le type sont bon, on mappe le pointeur de résultat sur la FIFO (ligne précédente)
if (retour.pixReturn != PIXY2_OK) cr = (T_pixy2ErrorCode) retour.pixReturn;
// Si la Pixy n'aquite pas l'ordre, on retourne l'erreur signalée
} else {
if (msg->pixType == PIXY2_ERROR) { // Si on reçoit une trame d'erreur
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
// On mappe le résultat sur la donnée stockée dans la FIFO (ligne précédente)
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur
}
} 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_ACK) { // On vérifie que la trame est du type convenable
retour = *(T_pixy2ReturnCode*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
// Si le type est bon, on mappe le pointeur de résultat sur la FIFO (ligne précédente)
if (retour.pixReturn != PIXY2_OK) cr = (T_pixy2ErrorCode) retour.pixReturn;
// Si la Pixy n'aquite pas l'ordre, on retourne l'erreur signalée
} else {
if (msg->pixType == PIXY2_ERROR) { // Si on reçoit une trame d'erreur
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
// On mappe le résultat sur la donnée stockée dans la FIFO (ligne précédente)
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur
}
}
etat = idle; // Et on annoce que la pixy est libre
break;
default : // Dans tous les autres cas
cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
break;
}
return cr;
}
T_pixy2ErrorCode PIXY2::pixy2_getFPS (T_pixy2ReturnCode *framerate){
T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[rPointer];
T_pixy2ErrorCode cr = PIXY2_OK;
switch (etat) {
case idle : // Si la caméra est inactive
cr = PIXY2::pixy2_sndGetFPS(); // 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
etat = messageSent; // On passe à l'attente du message de réponse
rPointer = wPointer; // On enregistre la position du pointeur de FIFO
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[rPointer]) ) { // On lance la validation du checksum
if (msg->pixType == PIXY2_ACK) { // On vérifie que la trame est du type convenable
framerate = (T_pixy2ReturnCode*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
// Si le checksum et le type sont bon, on mappe le pointeur de résultat sur la FIFO (ligne précédente)
} else {
if (msg->pixType == PIXY2_ERROR) { // Si on reçoit une trame d'erreur
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
// On mappe le résultat sur la donnée stockée dans la FIFO (ligne précédente)
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur
}
} 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_ACK) { // On vérifie que la trame est du type convenable
framerate = (T_pixy2ReturnCode*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
// Si le type est bon, on mappe le pointeur de résultat sur la FIFO (ligne précédente)
} else {
if (msg->pixType == PIXY2_ERROR) { // Si on reçoit une trame d'erreur
cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
// On mappe le résultat sur la donnée stockée dans la FIFO (ligne précédente)
}else cr = PIXY2_TYPE_ERROR; // Si le type ne correspond à rien de normal on signale une erreur
}
}
etat = idle; // Et on annoce que la pixy est libre
break;
default : // Dans tous les autres cas
cr = PIXY2_BUSY; // On signale à l'utilisateur que la caméra est maintenant occupée
break;
}
return cr;
}
T_pixy2ErrorCode PIXY2::pixy2_getBlocks (Byte sigmap, Byte maxBloc){return PIXY2_OK;}
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 (Word angle){return PIXY2_OK;}
T_pixy2ErrorCode PIXY2::pixy2_setDefaultTurn (Word 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;
}