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.
Diff: pixy2.cpp
- Revision:
- 19:007d361c8d66
- Parent:
- 18:190e3987e274
- Child:
- 20:8e3adcaab290
--- a/pixy2.cpp Wed Mar 13 16:27:14 2019 +0000
+++ b/pixy2.cpp Mon Apr 01 12:34:14 2019 +0000
@@ -8,6 +8,11 @@
Pixy2_buffer = (Byte*) malloc (0x100);
}
+PIXY2::~PIXY2()
+{
+ free (Pixy2_buffer);
+}
+
/* 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 :
@@ -15,9 +20,34 @@
* 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.
+ * receivingData : L'entête a été intégralement reçu (et traité) et 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).
+
+ On utilise plusieurs pointeurs :
+ * wPointer est le pointeur d'écriture des octets reçus dans le buffer de réception
+ * hPointer est le pointeur de l'entête du message
+ * dPointer est le pointeur de la zone de données
+
+ On rajoute une variable : dataSize, qui définit la quantité de données à recevoir (issue de l'entête)
+
+ **** COMMENT CA MARCHE ****
+
+ Au début wPointer = 0. Quand wPointer est supérieur à 1 (donc qu'on a reçu au moins 2 octets), on regarde si on a reçu le code de début de trame 0xC1AF
+ Si ce n'est pas le cas, on continue de recevoir les octets et on les stock jusqu'à trouver le code de début de trame.
+ Quand on trouve le code, on enregistre le point de départ (c'est hPointer) comme étant la position actuelle du wPointer-1.
+ A partir de ça, on attend de recevoir les 4 premiers octets de l'entête :
+ * start of frame (hPointer et hPointer + 1)
+ * identification du type (hPointer + 2)
+ * taille des données (hPointer + 3)
+ Et ensuite si on a une trame avec un checksum (normal) :
+ * le checksum (hPointer + 4 et hPointer + 5)
+ * les données (hPointer + 6 et +)
+ Et si on a une trame sans checksum (pas normal) :
+ * les données (hPointer + 4 et +)
+
+ On continue alors de recevoir des octets jusqu'à ce que wPointer soit égal à dPointer + dataSize - 1
+ par exemple si la data fait 4 octets et une trame avec checksum, on reçoit tant que wPointer est inférieur à 9 (6 + 4 - 1)
+ ou pour la même quantité de données mais avec une trame sans checksum 7 (4 + 4 - 1)...
*/
void PIXY2::pixy2_getByte () // Interruption de la pixy2
@@ -33,18 +63,22 @@
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
- }
+ 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
+ dPointer = hPointer + 4;
+ } else {
+ frameContainChecksum = 1; // Sinon, on mémorise qu'il y a un checksum à vérifier
+ dPointer = hPointer + 6;
+ }
+ } // Si on n'a pas de mot d'entête on attend d'en trouver un...
}
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))) {
+ 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
+ dataSize = Pixy2_buffer[hPointer + 3]; // On enregistre la taille de la payload
}
break;
@@ -55,7 +89,7 @@
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
+ wPointer++; // on pointe la case suivante du buffer de réception
}
@@ -187,7 +221,7 @@
return PIXY2_OK;
}
-T_pixy2ErrorCode PIXY2::pixy2_sndGetMainFeature (Byte type, Byte feature){
+T_pixy2ErrorCode PIXY2::pixy2_sndGetLineFeature (Byte type, Byte feature){
T_pixy2SendBuffer msg;
int i = 0, dataSize = 2;
msg.frame.header.pixSync = PIXY2_SYNC;
@@ -218,7 +252,7 @@
return PIXY2_OK;
}
-T_pixy2ErrorCode PIXY2::pixy2_sndSetNexTurn (Word angle){
+T_pixy2ErrorCode PIXY2::pixy2_sndSetNextTurn (Word angle){
T_pixy2SendBuffer msg;
int i = 0, dataSize = 2;
T_Word tmp;
@@ -321,24 +355,17 @@
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
+ if ( !pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
+ return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
}
}
+ 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.
+ }
etat = idle; // On annoce que la pixy est libre
break;
@@ -365,24 +392,17 @@
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
+ if ( !pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
+ return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
}
}
+ 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.
+ }
etat = idle; // On annoce que la pixy est libre
break;
@@ -409,18 +429,14 @@
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)) {
+ if ( !pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
+ return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
+ }
+ }
+ 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.
- }
+ 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;
@@ -447,18 +463,14 @@
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)) {
+ if ( !pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
+ return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
+ }
+ }
+ 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.
- }
+ 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;
@@ -485,18 +497,14 @@
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)) {
+ if ( !pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
+ return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
+ }
+ }
+ 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.
- }
+ 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;
@@ -523,18 +531,14 @@
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)) {
+ if ( !pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
+ return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
+ }
+ }
+ 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.
- }
+ 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;
@@ -561,24 +565,17 @@
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
+ if ( !pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
+ return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
}
}
+ 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.
+ }
etat = idle; // On annoce que la pixy est libre
break;
@@ -605,26 +602,18 @@
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
+ if ( !pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
+ return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
}
}
+ 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.
+ }
etat = idle; // On annoce que la pixy est libre
break;
@@ -635,13 +624,275 @@
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_getFeatures (){
+
+ T_pixy2RcvHeader *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[hPointer];
+ T_pixy2ErrorCode cr = PIXY2_OK;
+ T_pixy2LineFeature* lineFeature;
+ int fPointer;
+
+ if (frameContainChecksum) { // Si la trame contient un checksum
+ if ( !pixy2_validateChecksum (&Pixy2_buffer[hPointer]) ) { // On lance la validation du checksum
+ return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
+ }
+ }
+ if (msg->pixType == PIXY2_REP_LINE) { // On vérifie que la trame est du type convenable (REPONSE LIGNE)
+ fPointer = dPointer; // On pointe sur la premiere feature
+ do {
+ lineFeature = (T_pixy2LineFeature*) &Pixy2_buffer[fPointer]; // On mappe le pointeur de structure sur le buffer de réception des features.
+ if (lineFeature->fType == PIXY2_VECTOR) { // On regarde si le type est vecteur
+ Pixy2_numVectors = lineFeature->fLength / sizeof(T_pixy2Vector);
+ // Si oui, on compte combien il y a de vecteurs
+ Pixy2_vectors = (T_pixy2Vector*) &Pixy2_buffer[fPointer]; // On mappe le résultat
+ fPointer += lineFeature->fLength; // On déplace le pointeur de données et on recommence
+ cr |= PIXY2_VECTOR;
+ }
+
+ if (lineFeature->fType == PIXY2_BARCODE) { // On regarde si le type est codebarre
+ Pixy2_numBarcodes = lineFeature->fLength / sizeof(T_pixy2BarCode);
+ // Si oui, on compte combien il y a de codebarre
+ Pixy2_barcodes = (T_pixy2BarCode*) &Pixy2_buffer[fPointer]; // On mappe le résultat
+ fPointer += lineFeature->fLength; // On déplace le pointeur de données et on recommence
+ cr |= PIXY2_BARCODE;
+ }
+ if (lineFeature->fType == PIXY2_INTERSECTION) { // On regarde si le type est intersection
+ Pixy2_numIntersections = lineFeature->fLength / sizeof(T_pixy2Intersection);
+ // Si oui, on compte combien il y a d'intersections
+ Pixy2_intersections = (T_pixy2Intersection*) &Pixy2_buffer[fPointer];
+ // On mappe le résultat sur l'entête de l'intersection
+ fPointer += lineFeature->fLength; // On déplace le pointeur de données et on recommence
+ cr |= PIXY2_INTERSECTION;
+ }
+ } while(fPointer < ((dataSize - 1) + dPointer)); // Tant qu'il y a des données à traiter
+ } 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.
+ }
+ etat = idle; // On annoce que la pixy est libre
+ return cr;
+}
+
+
+T_pixy2ErrorCode PIXY2::pixy2_getMainFeature (Byte features){
+
+ 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_sndGetLineFeature(0, features); // On envoie la trame de demande de suivi de ligne
+ 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
+ cr = PIXY2::pixy2_getFeatures(); // On appelle la fonction de traitement.
+ 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_getAllFeature (Byte features){
+ 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_sndGetLineFeature(1, features); // On envoie la trame de demande de suivi de ligne
+ 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
+ cr = PIXY2::pixy2_getFeatures(); // On appelle la fonction de traitement.
+ 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_setMode (Byte mode)
+{
+ 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_sndSetMode (mode); // On envoie la trame de règlage du mode de fonctionnement du suivi de ligne
+ 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
+ return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
+ }
+ }
+ 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_setNextTurn (sWord angle)
+{
+ 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_sndSetNextTurn (angle); // On envoie la trame de choix de l'angle du prochain virage
+ 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
+ return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
+ }
+ }
+ 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_setDefaultTurn (sWord angle)
+{
+ 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_sndSetDefaultTurn (angle); // On envoie la trame de choix de l'angle par défaut des virages
+ 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
+ return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
+ }
+ }
+ 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_setVector (Byte vectorIndex)
+{
+ 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_sndSetVector (vectorIndex); // On envoie la trame de choix du vecteur à suivre
+ 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
+ return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
+ }
+ }
+ 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_ReverseVector (void)
+{
+ 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_sndReverseVector (); // On envoie la trame d'inversion de l'image (haut en bas et bas en haut)
+ 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
+ return PIXY2_BAD_CHECKSUM; // Si le checksum est faux on retourne une erreur
+ }
+ }
+ 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_getRGB (Word x, Word y, Byte saturate, T_pixy2Pixel *pixel){return PIXY2_OK;}
T_pixy2ErrorCode PIXY2::pixy2_validateChecksum (Byte* tab){