FRC - Hackathon / Pixy

Library for Pixy - CMUCAM5 smart camera

Dependents:   TestPixy FRC_2018 FRC2018_Bis 0hackton_08_06_18 ... more

Fork of Pixy by FRC - Hackathon

Pixy.cpp@4:5982d904f7aa, 2018-06-01 (annotated)

Committer:
haarkon
Date:
Fri Jun 01 10:11:53 2018 +0000
Revision:
4:5982d904f7aa
Parent:
3:1e4c6d7a0053 
Tested Pixy (CMUCAM5) Library (with comments)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
haarkon 0:6f78c735f07c 1 #include "Pixy.h"
haarkon 4:5982d904f7aa 2 /**
haarkon 4:5982d904f7aa 3 * Constructeur de l'objet
haarkon 4:5982d904f7aa 4 */
haarkon 0:6f78c735f07c 5
haarkon 0:6f78c735f07c 6 PIXY::PIXY(PinName tx, PinName rx, int debit)
haarkon 0:6f78c735f07c 7 {
haarkon 0:6f78c735f07c 8 Pixy_CCFrameIsNew = -1;
haarkon 0:6f78c735f07c 9 Pixy_NMFrameIsNew = -1;
haarkon 0:6f78c735f07c 10 FlagPixy = 0;
haarkon 0:6f78c735f07c 11 FlagPixyOverflow = 0;
haarkon 0:6f78c735f07c 12 Pixy_check = -1;
haarkon 0:6f78c735f07c 13
haarkon 3:1e4c6d7a0053 14 _Pixy = new Serial (tx, rx, debit);
haarkon 3:1e4c6d7a0053 15 _Pixy->attach (callback(this,&PIXY::getPixyByte));
haarkon 0:6f78c735f07c 16
haarkon 0:6f78c735f07c 17 }
haarkon 0:6f78c735f07c 18
haarkon 4:5982d904f7aa 19 /**
haarkon 4:5982d904f7aa 20 * Programme d'interruption gérant la réception des octets envoyés par la caméra sur la liaison série
haarkon 4:5982d904f7aa 21 */
haarkon 0:6f78c735f07c 22 void PIXY::getPixyByte ()
haarkon 0:6f78c735f07c 23 {
haarkon 4:5982d904f7aa 24 static T_msgBuffer msgBuffer; // Buffer de réception 32 bits
haarkon 4:5982d904f7aa 25 static T_wordBuffer wordBuffer; // Buffer de réception 16 bits
haarkon 4:5982d904f7aa 26 static T_pixyState PIXY_state = none; // Variable d'etat de la liaison
haarkon 4:5982d904f7aa 27 static Byte byteCount = 0; // Nb d'octets lu
haarkon 4:5982d904f7aa 28 static int PIXY_synced = 0; // Etat de la liaison Pixy <-> carte
haarkon 3:1e4c6d7a0053 29 Word i, somme;
haarkon 3:1e4c6d7a0053 30 Byte dummy;
haarkon 4:5982d904f7aa 31 static Byte PIXY_nbCCObjet = 0, PIXY_wCCObjet = 0; // Variable interne de la FIFO Color Code
haarkon 4:5982d904f7aa 32 static Byte PIXY_nbNMObjet = 0, PIXY_wNMObjet = 0; // Variable interne de la FIFO normale
haarkon 0:6f78c735f07c 33
haarkon 0:6f78c735f07c 34 Pixy_check = 0;
haarkon 0:6f78c735f07c 35
haarkon 4:5982d904f7aa 36 dummy = _Pixy->getc(); // On stocke l'octet reçu dans une variable temporaire
haarkon 3:1e4c6d7a0053 37
haarkon 3:1e4c6d7a0053 38 if (!PIXY_synced) { // Si la camera et la carte ne sont pas synchronisées
haarkon 3:1e4c6d7a0053 39 msgBuffer.octet[byteCount] = dummy; // On stocke l'octet reçu dans la première case dispo du tableau temporaire
haarkon 0:6f78c735f07c 40
haarkon 3:1e4c6d7a0053 41 if (byteCount < 3) { // Tant qu'on n'a pas encore reçu les 4 premier octets
haarkon 3:1e4c6d7a0053 42 byteCount++; // On passe à la case suivante du tableau temporaire
haarkon 3:1e4c6d7a0053 43 } else { // Lorsqu'on a 4 octets
haarkon 3:1e4c6d7a0053 44 if ((msgBuffer.motlong == 0xaa550000)||(msgBuffer.motlong == 0xaa560000)) { // On teste pour voir si on a reçu un entête
haarkon 3:1e4c6d7a0053 45 PIXY_synced = 1; // Si oui : On passe en mode synchronisé
haarkon 3:1e4c6d7a0053 46 PIXY_state = begin; // Et on va à l'aiguillage
haarkon 3:1e4c6d7a0053 47 byteCount = 0; // Et on remet le comptage d'octet à 0
haarkon 3:1e4c6d7a0053 48 } else { // Si on n'a pas trouvé d'entête
haarkon 3:1e4c6d7a0053 49 for (i=1; i<4; i++) msgBuffer.octet[i-1] = msgBuffer.octet[i]; // On décalle les cases du tableau
haarkon 3:1e4c6d7a0053 50 byteCount = 3; // Et on attend le caractère suivant
haarkon 0:6f78c735f07c 51 }
haarkon 0:6f78c735f07c 52 }
haarkon 3:1e4c6d7a0053 53 } else {
haarkon 0:6f78c735f07c 54 switch (PIXY_state) {
haarkon 0:6f78c735f07c 55 case begin : // l'aiguillage est là !
haarkon 3:1e4c6d7a0053 56 wordBuffer.octet[byteCount%2] = dummy; // on stocke les octets reçus
haarkon 0:6f78c735f07c 57 byteCount++;
haarkon 0:6f78c735f07c 58 if (byteCount == 2) { // Quand on a 2 octets
haarkon 3:1e4c6d7a0053 59 switch (wordBuffer.mot) {
haarkon 3:1e4c6d7a0053 60 case 0xaa55 : // Si c'est un bloc normal (code = 0xAA55)
haarkon 3:1e4c6d7a0053 61 PIXY_state = normal; // On part vers le traitement spécifique
haarkon 3:1e4c6d7a0053 62 break;
haarkon 0:6f78c735f07c 63
haarkon 3:1e4c6d7a0053 64 case 0xaa56 : // Si c'est un bloc color Code (code = 0xAA56)
haarkon 3:1e4c6d7a0053 65 PIXY_state = colorCode; // On part vers le traitement spécifique
haarkon 3:1e4c6d7a0053 66 break;
haarkon 0:6f78c735f07c 67
haarkon 3:1e4c6d7a0053 68 case 0 : // Si c'est un debut de trame (code = 0x0000)
haarkon 3:1e4c6d7a0053 69 PIXY_state = doubleZero; // On part vers le traitement spécifique
haarkon 3:1e4c6d7a0053 70 break;
haarkon 0:6f78c735f07c 71
haarkon 3:1e4c6d7a0053 72 default : // Si c'est autre chose
haarkon 3:1e4c6d7a0053 73 PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé.
haarkon 3:1e4c6d7a0053 74 PIXY_state = none; // Ceinture et bretelle
haarkon 3:1e4c6d7a0053 75 break;
haarkon 0:6f78c735f07c 76 }
haarkon 0:6f78c735f07c 77 byteCount = 0;
haarkon 0:6f78c735f07c 78 }
haarkon 0:6f78c735f07c 79 break;
haarkon 0:6f78c735f07c 80
haarkon 0:6f78c735f07c 81 case normal : // Si on a un bloc normal
haarkon 0:6f78c735f07c 82
haarkon 3:1e4c6d7a0053 83 Pixy_NMFIFO[PIXY_wNMObjet].tab[byteCount] = dummy; // On stocke les octets un à un dans la structure Bloc
haarkon 0:6f78c735f07c 84 if (byteCount < 11) { // Tant que la structure n'est pas pleine
haarkon 0:6f78c735f07c 85 byteCount++; // On passe à l'octet suivant
haarkon 0:6f78c735f07c 86 } else { // Quand elle est pleine
haarkon 0:6f78c735f07c 87 byteCount = 0; // On réinitialise
haarkon 4:5982d904f7aa 88 PIXY_state = begin; // On prépare le retour à l'aiguillage
haarkon 0:6f78c735f07c 89
haarkon 0:6f78c735f07c 90 // On calcule la somme de contrôle
haarkon 0:6f78c735f07c 91 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;
haarkon 0:6f78c735f07c 92
haarkon 0:6f78c735f07c 93 if (somme == Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.checksum) { // Si le checksum est bon, on valide la réception
haarkon 0:6f78c735f07c 94 if (PIXY_wNMObjet < 19) PIXY_wNMObjet++; // On incrémente le pointeur d'écriture dans la FIFO Objet
haarkon 0:6f78c735f07c 95 else PIXY_wNMObjet = 0;
haarkon 0:6f78c735f07c 96 if (PIXY_nbNMObjet < 19) PIXY_nbNMObjet++; // On dit que l'on a un objet de plus
haarkon 4:5982d904f7aa 97 else FlagPixyOverflow = 1; // Si on a plus de 20 objets en attente => Overflow
haarkon 0:6f78c735f07c 98 }
haarkon 0:6f78c735f07c 99 }
haarkon 0:6f78c735f07c 100 break;
haarkon 0:6f78c735f07c 101
haarkon 0:6f78c735f07c 102 case colorCode : // Si on a un bloc colorCode
haarkon 0:6f78c735f07c 103
haarkon 0:6f78c735f07c 104 Pixy_CCFIFO[PIXY_wCCObjet].tab[byteCount] = dummy; // On stocke les octets un à un dans la structure CCBloc
haarkon 0:6f78c735f07c 105 if (byteCount < 13) byteCount++; // tant que la structure n'est pas pleine on passe à l'octet suivant
haarkon 0:6f78c735f07c 106 else { // Quand elle est pleine
haarkon 0:6f78c735f07c 107 byteCount = 0; // On réinitialise
haarkon 4:5982d904f7aa 108 PIXY_state = begin; // On prepare le retour à l'aiguillage
haarkon 0:6f78c735f07c 109
haarkon 0:6f78c735f07c 110 // On calcule la somme de contrôle
haarkon 0:6f78c735f07c 111 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;
haarkon 0:6f78c735f07c 112
haarkon 0:6f78c735f07c 113 if (somme == Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.checksum) { // Si le checksum est bon
haarkon 0:6f78c735f07c 114 if (PIXY_wCCObjet < 19) PIXY_wCCObjet++; // On incrémente le pointeur d'écriture dans la FIFO CCObjet
haarkon 0:6f78c735f07c 115 else PIXY_wCCObjet = 0;
haarkon 0:6f78c735f07c 116 if (PIXY_nbCCObjet < 19) PIXY_nbCCObjet++; // On dit que l'on a un objet CC de plus à traiter
haarkon 0:6f78c735f07c 117 else FlagPixyOverflow = 1; // Si on a plus de 20 CC objets (en attente) => Overflow
haarkon 0:6f78c735f07c 118 }
haarkon 0:6f78c735f07c 119 }
haarkon 0:6f78c735f07c 120 break;
haarkon 0:6f78c735f07c 121
haarkon 3:1e4c6d7a0053 122 case doubleZero : // Si on a reçu le code de début d'une nouvelle trame (0000).
haarkon 3:1e4c6d7a0053 123
haarkon 3:1e4c6d7a0053 124 Pixy_NMObjet = PIXY_nbNMObjet; // On met à jour les variables pour le traitement extérieur
haarkon 3:1e4c6d7a0053 125 Pixy_CCObjet = PIXY_nbCCObjet;
haarkon 3:1e4c6d7a0053 126 Pixy_CCFrameIsNew = 1;
haarkon 3:1e4c6d7a0053 127 Pixy_FirstCCObjet = PIXY_wCCObjet;
haarkon 3:1e4c6d7a0053 128 Pixy_NMFrameIsNew = 1;
haarkon 3:1e4c6d7a0053 129 Pixy_FirstNMObjet = PIXY_wNMObjet;
haarkon 3:1e4c6d7a0053 130 FlagPixy = 1; // On valide le traitement de la trame précédente.
haarkon 0:6f78c735f07c 131
haarkon 3:1e4c6d7a0053 132 PIXY_nbCCObjet = 0; // On remet à 0 les variables internes
haarkon 3:1e4c6d7a0053 133 PIXY_nbNMObjet = 0;
haarkon 3:1e4c6d7a0053 134 byteCount = 0;
haarkon 3:1e4c6d7a0053 135
haarkon 4:5982d904f7aa 136 // Comme on est dans une interruption, si on est ici c'est qu'on a reçu le code 0000, mais aussi le premier octet de la suite (le 0x55 du start)
haarkon 4:5982d904f7aa 137 // On va donc le stocker ici dans le tableau de réception du start.
haarkon 4:5982d904f7aa 138 wordBuffer.octet[byteCount%2] = dummy; // on stocke l'octet reçu
haarkon 3:1e4c6d7a0053 139 byteCount++;
haarkon 4:5982d904f7aa 140
haarkon 3:1e4c6d7a0053 141 PIXY_state = waitForStart; // On passe en attente de la deuxième partie du code
haarkon 3:1e4c6d7a0053 142 break;
haarkon 3:1e4c6d7a0053 143
haarkon 3:1e4c6d7a0053 144 case waitForStart : // Si on a reçu le code de début d'une nouvelle trame.
haarkon 3:1e4c6d7a0053 145
haarkon 4:5982d904f7aa 146 wordBuffer.octet[byteCount%2] = dummy; // on stocke l'octet reçu (ça en fait 2 avec celui reçu precedemment
haarkon 0:6f78c735f07c 147 byteCount++;
haarkon 4:5982d904f7aa 148 if (byteCount == 2) { // On verifie quand même si on a bien 2 octets
haarkon 4:5982d904f7aa 149 byteCount = 0; // Si oui, on remet à 0 le comptage
haarkon 3:1e4c6d7a0053 150 if (wordBuffer.mot == 0xaa55) { // On vérifie qu'on a bien reçu l'entête de depart
haarkon 0:6f78c735f07c 151 PIXY_state = begin; // Si c'est le cas, alors tout va bien et on va à l'aiguillage
haarkon 3:1e4c6d7a0053 152 } else { // Si ce n'est pas le cas
haarkon 3:1e4c6d7a0053 153 PIXY_synced = 0; // On n'est plus synchronisés
haarkon 3:1e4c6d7a0053 154 PIXY_state = none; // Ceinture et Bretelles
haarkon 0:6f78c735f07c 155 }
haarkon 0:6f78c735f07c 156 }
haarkon 0:6f78c735f07c 157 break;
haarkon 0:6f78c735f07c 158 }
haarkon 0:6f78c735f07c 159 }
haarkon 0:6f78c735f07c 160 }
haarkon 0:6f78c735f07c 161
haarkon 4:5982d904f7aa 162 /**
haarkon 4:5982d904f7aa 163 * Transmet le nombre d'objets par catégorie dans la dernière trame reçue
haarkon 4:5982d904f7aa 164 */
haarkon 2:90355c600404 165 int PIXY::detectedObject (int* nbNM, int* nbCC)
haarkon 0:6f78c735f07c 166 {
haarkon 2:90355c600404 167 *nbNM = (int)Pixy_NMObjet;
haarkon 0:6f78c735f07c 168 *nbCC = (int)Pixy_CCObjet;
haarkon 0:6f78c735f07c 169 if (Pixy_check!=0) return -1;
haarkon 0:6f78c735f07c 170 if (FlagPixyOverflow) return -2;
haarkon 0:6f78c735f07c 171 return 0;
haarkon 0:6f78c735f07c 172 }
haarkon 0:6f78c735f07c 173
haarkon 4:5982d904f7aa 174 /**
haarkon 4:5982d904f7aa 175 * Permet la lecture du plus ancien bloc CC de la FIFO se trouvant dans la dernière trame reçue
haarkon 4:5982d904f7aa 176 */
haarkon 0:6f78c735f07c 177 T_pixyCCBloc PIXY::getCCBloc (void)
haarkon 0:6f78c735f07c 178 {
haarkon 0:6f78c735f07c 179 T_pixyCCBloc dummy;
haarkon 0:6f78c735f07c 180 static Byte PIXY_rCCObjet = 0;
haarkon 0:6f78c735f07c 181
haarkon 0:6f78c735f07c 182 if (Pixy_CCObjet !=0) {
haarkon 0:6f78c735f07c 183 if (Pixy_CCFrameIsNew) { //Si on est au début d'une nouvelle trame, on initialise le pointeur de lecture
haarkon 0:6f78c735f07c 184 if (Pixy_FirstCCObjet <= Pixy_CCObjet) PIXY_rCCObjet = 20 - Pixy_CCObjet + Pixy_FirstCCObjet ;
haarkon 0:6f78c735f07c 185 else PIXY_rCCObjet = Pixy_FirstCCObjet - Pixy_CCObjet ;
haarkon 0:6f78c735f07c 186 Pixy_CCFrameIsNew = 0;
haarkon 0:6f78c735f07c 187 }
haarkon 0:6f78c735f07c 188 dummy = Pixy_CCFIFO[PIXY_rCCObjet].CCbloc;
haarkon 0:6f78c735f07c 189 if (PIXY_rCCObjet < 19) PIXY_rCCObjet++; // On incrémente le pointeur de lecture dans la FIFO CCObjet
haarkon 0:6f78c735f07c 190 else PIXY_rCCObjet = 0;
haarkon 0:6f78c735f07c 191 Pixy_CCObjet--; // On dit que l'on a un objet CC de moins à traiter
haarkon 0:6f78c735f07c 192 } else {
haarkon 0:6f78c735f07c 193 dummy.signature = 0xFFFF;
haarkon 0:6f78c735f07c 194 }
haarkon 0:6f78c735f07c 195 return dummy;
haarkon 0:6f78c735f07c 196 }
haarkon 0:6f78c735f07c 197
haarkon 4:5982d904f7aa 198 /**
haarkon 4:5982d904f7aa 199 * Permet la lecture du plus ancien bloc NM de la FIFO se trouvant dans la dernière trame reçue
haarkon 4:5982d904f7aa 200 */
haarkon 0:6f78c735f07c 201 T_pixyNMBloc PIXY::getNMBloc (void)
haarkon 0:6f78c735f07c 202 {
haarkon 0:6f78c735f07c 203 T_pixyNMBloc dummy;
haarkon 0:6f78c735f07c 204 static Byte PIXY_rNMObjet = 0;
haarkon 0:6f78c735f07c 205
haarkon 0:6f78c735f07c 206 if (Pixy_NMObjet !=0) {
haarkon 0:6f78c735f07c 207 if (Pixy_NMFrameIsNew) { //Si on est au début d'une nouvelle trame, on initialise le pointeur de lecture
haarkon 0:6f78c735f07c 208 if (Pixy_FirstNMObjet <= Pixy_NMObjet) PIXY_rNMObjet = 20 - Pixy_NMObjet + Pixy_FirstNMObjet ;
haarkon 0:6f78c735f07c 209 else PIXY_rNMObjet = Pixy_FirstNMObjet - Pixy_NMObjet ;
haarkon 0:6f78c735f07c 210 Pixy_NMFrameIsNew = 0;
haarkon 0:6f78c735f07c 211 }
haarkon 0:6f78c735f07c 212 dummy = Pixy_NMFIFO[PIXY_rNMObjet].NMbloc;
haarkon 0:6f78c735f07c 213 if (PIXY_rNMObjet < 19) PIXY_rNMObjet++; // On incrémente le pointeur de lecture dans la FIFO CCObjet
haarkon 0:6f78c735f07c 214 else PIXY_rNMObjet = 0;
haarkon 0:6f78c735f07c 215 Pixy_NMObjet--; // On dit que l'on a un objet CC de moins à traiter
haarkon 0:6f78c735f07c 216 } else {
haarkon 0:6f78c735f07c 217 dummy.signature = 0xFFFF;
haarkon 0:6f78c735f07c 218 }
haarkon 0:6f78c735f07c 219 return dummy;
haarkon 0:6f78c735f07c 220 }
haarkon 0:6f78c735f07c 221
haarkon 4:5982d904f7aa 222 /**
haarkon 4:5982d904f7aa 223 * Permet le réglage de la luminosité
haarkon 4:5982d904f7aa 224 */
haarkon 3:1e4c6d7a0053 225 void PIXY::setBrightness (Byte brightness)
haarkon 0:6f78c735f07c 226 {
haarkon 3:1e4c6d7a0053 227 _Pixy->putc(0); //Start
haarkon 3:1e4c6d7a0053 228 _Pixy->putc(0xFE); //Brightness Level
haarkon 3:1e4c6d7a0053 229 _Pixy->putc(brightness);
haarkon 3:1e4c6d7a0053 230 }
haarkon 0:6f78c735f07c 231
haarkon 4:5982d904f7aa 232 /**
haarkon 4:5982d904f7aa 233 * Indique si une nouvelle image est disponible pour le traitement
haarkon 4:5982d904f7aa 234 */
haarkon 3:1e4c6d7a0053 235 int PIXY::checkNewImage (void)
haarkon 3:1e4c6d7a0053 236 {
haarkon 3:1e4c6d7a0053 237 if (FlagPixy) {
haarkon 3:1e4c6d7a0053 238 FlagPixy = 0;
haarkon 3:1e4c6d7a0053 239 return 1;
haarkon 3:1e4c6d7a0053 240 } else {
haarkon 3:1e4c6d7a0053 241 return 0;
haarkon 3:1e4c6d7a0053 242 }
haarkon 3:1e4c6d7a0053 243 }
haarkon 0:6f78c735f07c 244
haarkon 4:5982d904f7aa 245 /**
haarkon 4:5982d904f7aa 246 * Indique l'état de la liaison entre la carte et la camera
haarkon 4:5982d904f7aa 247 */
haarkon 3:1e4c6d7a0053 248 int PIXY::checkPixy (void)
haarkon 3:1e4c6d7a0053 249 {
haarkon 3:1e4c6d7a0053 250 return Pixy_check;
haarkon 0:6f78c735f07c 251 }