FRC - Hackathon
Library for Pixy - CMUCAM5 smart camera
Dependents: TestPixy FRC_2018 FRC2018_Bis 0hackton_08_06_18 ... more
Fork of
Pixy
by 
FRC - Hackathon
Diff: Pixy.cpp
- Revision:
- 0:6f78c735f07c
- Child:
- 2:90355c600404
diff -r 000000000000 -r 6f78c735f07c Pixy.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Pixy.cpp Sun May 20 08:18:49 2018 +0000
@@ -0,0 +1,204 @@
+#include "Pixy.h"
+
+PIXY::PIXY(PinName tx, PinName rx, int debit)
+{
+ Pixy_CCFrameIsNew = -1;
+ Pixy_NMFrameIsNew = -1;
+ FlagPixy = 0;
+ FlagPixyOverflow = 0;
+ Pixy_check = -1;
+
+ _cmucam = new Serial (tx, rx, debit);
+ _cmucam->attach (this,&PIXY::getPixyByte);
+
+}
+
+
+void PIXY::getPixyByte ()
+{
+ static T_tmpBuffer tmpBuffer;
+ static T_structBuffer msgBuffer;
+ static T_pixyState PIXY_state = none;
+ static Byte byteCount = 0;
+ static int PIXY_synced = 0, dummy;
+ int i, somme;
+ static Byte PIXY_nbCCObjet = 0, PIXY_wCCObjet = 0;
+ static Byte PIXY_nbNMObjet = 0, PIXY_wNMObjet = 0;
+
+ Pixy_check = 0;
+
+ if (!PIXY_synced) { // On n'a pas trouvé le START (0x55aa0000)
+ tmpBuffer.tab[byteCount] = _cmucam->getc(); // On stocke l'octet reçu dans la première case dispo du tableau temporaire
+
+ if (byteCount < 3) { // Si on n'a pas encore reçu les 4 premier octets
+ byteCount++; // On passe à la case suivante du tableau temporaire
+ } else { // Lorsqu'on a 4 octets
+ if (tmpBuffer.mot != 0xaa550000) { // Si le code n'est pas le code de START
+ for (i=1; i<4; i++) tmpBuffer.tab[i-1] = tmpBuffer.tab[i]; // On décalle les cases du tableau
+ byteCount = 3; // Et on attend le caractère suivant
+ } else { // Si on a trouvé le code de START
+ PIXY_synced = 1; // On passe en mode synchronisé
+ PIXY_state = begin;
+ byteCount = 0;
+ }
+ }
+ }
+
+ if (PIXY_synced) {
+
+ switch (PIXY_state) {
+ case begin : // l'aiguillage est là !
+
+ msgBuffer.tab[byteCount%2] = _cmucam->getc(); // on stocke les octets reçus
+ byteCount++;
+ if (byteCount == 2) { // Quand on a 2 octets
+
+ if (msgBuffer.mot == 0xaa55) { // Si c'est un bloc normal (code 0xAA55)
+ PIXY_state = normal; // On part vers le traitement spécifique
+ }
+
+ if (msgBuffer.mot == 0xaa56) { // Si c'est un bloc Color Code (code 0xAA56)
+ PIXY_state = colorCode; // On part vers le traitement spécifique
+ }
+
+ if (msgBuffer.mot == 0) { // Si on a un debut de trame (code 0000)
+ PIXY_state = doubleZero; // On part vers le traitement spécifique
+ }
+ if ((PIXY_state == begin) || (PIXY_state == none)) { // Si c'est autre chose
+ PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé.
+ PIXY_state = none; // Ceinture et bretelle
+ }
+ byteCount = 0;
+ }
+ break;
+
+ case normal : // Si on a un bloc normal
+
+ Pixy_NMFIFO[PIXY_wNMObjet].tab[byteCount] = _cmucam->getc(); // On stocke les octets un à un dans la structure Bloc
+ if (byteCount < 11) { // Tant que la structure n'est pas pleine
+ byteCount++; // On passe à l'octet suivant
+ } else { // Quand elle est pleine
+ byteCount = 0; // On réinitialise
+ PIXY_state = begin; // On retourne à l'aiguillage
+
+ // On calcule la somme de contrôle
+ somme = Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.signature + Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.x + Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.y + Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.width + Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.height;
+
+ if (somme == Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.checksum) { // Si le checksum est bon, on valide la réception
+ if (PIXY_wNMObjet < 19) PIXY_wNMObjet++; // On incrémente le pointeur d'écriture dans la FIFO Objet
+ else PIXY_wNMObjet = 0;
+ if (PIXY_nbNMObjet < 19) PIXY_nbNMObjet++; // On dit que l'on a un objet de plus
+ else FlagPixyOverflow = 1; // Si on a plus de 20 objets (en attente) => Overflow
+ }
+ }
+ break;
+
+ case colorCode : // Si on a un bloc colorCode
+
+ Pixy_CCFIFO[PIXY_wCCObjet].tab[byteCount] = dummy; // On stocke les octets un à un dans la structure CCBloc
+ if (byteCount < 13) byteCount++; // tant que la structure n'est pas pleine on passe à l'octet suivant
+ else { // Quand elle est pleine
+ byteCount = 0; // On réinitialise
+ PIXY_state = begin; // On retourne à l'aiguillage
+
+ // On calcule la somme de contrôle
+ somme = Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.signature + Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.x + Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.y + Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.width + Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.height + Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.angle;
+
+ if (somme == Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.checksum) { // Si le checksum est bon
+ if (PIXY_wCCObjet < 19) PIXY_wCCObjet++; // On incrémente le pointeur d'écriture dans la FIFO CCObjet
+ else PIXY_wCCObjet = 0;
+ if (PIXY_nbCCObjet < 19) PIXY_nbCCObjet++; // On dit que l'on a un objet CC de plus à traiter
+ else FlagPixyOverflow = 1; // Si on a plus de 20 CC objets (en attente) => Overflow
+ }
+ }
+ break;
+
+ case doubleZero : // Si on a reçu le code de début d'une nouvelle trame.
+
+ msgBuffer.tab[byteCount%2] = _cmucam->getc(); // on stocke les octets reçus
+ byteCount++;
+ if (byteCount == 2) { // Quand on a 2 octets
+ if (msgBuffer.mot == 0xaa55) { // On doit impérativement trouver le code 0xAA55
+ PIXY_state = begin; // Si c'est le cas, alors tout va bien et on va à l'aiguillage
+ Pixy_NMObjet = PIXY_nbNMObjet; // On met à jour les variables pour le traitement
+ Pixy_CCObjet = PIXY_nbCCObjet;
+ PIXY_nbCCObjet = 0;
+ PIXY_nbNMObjet = 0;
+ Pixy_CCFrameIsNew = 1;
+ Pixy_FirstCCObjet = PIXY_wCCObjet;
+ Pixy_NMFrameIsNew = 1;
+ Pixy_FirstNMObjet = PIXY_wNMObjet;
+ FlagPixy = 1; // On valide le traitement de la trame précédente.
+ } else { // Si on trouve autre chose
+ PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé.
+ PIXY_state = none; // Ceinture et bretelle
+ }
+ byteCount = 0;
+ }
+ break;
+ }
+ }
+}
+
+int PIXY::detectedObject (int* nbNormal, int* nbCC)
+{
+ *nbNormal = (int)Pixy_NMObjet;
+ *nbCC = (int)Pixy_CCObjet;
+ if (Pixy_check!=0) return -1;
+ if (FlagPixyOverflow) return -2;
+ return 0;
+}
+
+T_pixyCCBloc PIXY::getCCBloc (void)
+{
+ T_pixyCCBloc dummy;
+ static Byte PIXY_rCCObjet = 0;
+
+ if (Pixy_CCObjet !=0) {
+ if (Pixy_CCFrameIsNew) { //Si on est au début d'une nouvelle trame, on initialise le pointeur de lecture
+ if (Pixy_FirstCCObjet <= Pixy_CCObjet) PIXY_rCCObjet = 20 - Pixy_CCObjet + Pixy_FirstCCObjet ;
+ else PIXY_rCCObjet = Pixy_FirstCCObjet - Pixy_CCObjet ;
+ Pixy_CCFrameIsNew = 0;
+ }
+ dummy = Pixy_CCFIFO[PIXY_rCCObjet].CCbloc;
+ if (PIXY_rCCObjet < 19) PIXY_rCCObjet++; // On incrémente le pointeur de lecture dans la FIFO CCObjet
+ else PIXY_rCCObjet = 0;
+ Pixy_CCObjet--; // On dit que l'on a un objet CC de moins à traiter
+ } else {
+ dummy.signature = 0xFFFF;
+ }
+ return dummy;
+}
+
+T_pixyNMBloc PIXY::getNMBloc (void)
+{
+ T_pixyNMBloc dummy;
+ static Byte PIXY_rNMObjet = 0;
+
+ if (Pixy_NMObjet !=0) {
+ if (Pixy_NMFrameIsNew) { //Si on est au début d'une nouvelle trame, on initialise le pointeur de lecture
+ if (Pixy_FirstNMObjet <= Pixy_NMObjet) PIXY_rNMObjet = 20 - Pixy_NMObjet + Pixy_FirstNMObjet ;
+ else PIXY_rNMObjet = Pixy_FirstNMObjet - Pixy_NMObjet ;
+ Pixy_NMFrameIsNew = 0;
+ }
+ dummy = Pixy_NMFIFO[PIXY_rNMObjet].NMbloc;
+ if (PIXY_rNMObjet < 19) PIXY_rNMObjet++; // On incrémente le pointeur de lecture dans la FIFO CCObjet
+ else PIXY_rNMObjet = 0;
+ Pixy_NMObjet--; // On dit que l'on a un objet CC de moins à traiter
+ } else {
+ dummy.signature = 0xFFFF;
+ }
+ return dummy;
+}
+
+int PIXY::setBrightness (Byte brightness)
+{
+
+ Byte PixyMsgBuf[3];
+
+ PixyMsgBuf[0] = 0x00; //Start
+ PixyMsgBuf[1] = 0xFE; //Brightness Level
+ PixyMsgBuf[2] = brightness;
+
+ return _cmucam->write(PixyMsgBuf,3,NULL,0);
+}
\ No newline at end of file