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@3:1e4c6d7a0053, 2018-05-31 (annotated)

Committer:
haarkon
Date:
Thu May 31 17:36:49 2018 +0000
Revision:
3:1e4c6d7a0053
Parent:
2:90355c600404
Child:
4:5982d904f7aa
Child:
6:f16f250fe2cb
Puxy HelloWorld program

Who changed what in which revision?

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