FRC - Hackathon Pixy
Pixy.cpp@7:c809db7aa1c0, 2019-04-17 (annotated)
- Committer:
- haarkon
- Date:
- Wed Apr 17 10:14:41 2019 +0000
- Revision:
- 7:c809db7aa1c0
- Parent:
- 6:f16f250fe2cb
Moved more raw numbers to symbolic constant for parametric code evolution.
Who changed what in which revision?
User | Revision | Line number | New 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 | 5:d395fe0fb0e0 | 11 | _Pixy = new Serial (tx, rx, debit); |
haarkon | 5:d395fe0fb0e0 | 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 | 5:d395fe0fb0e0 | 19 | static T_msgBuffer msgBuffer; |
haarkon | 5:d395fe0fb0e0 | 20 | static T_wordBuffer wordBuffer; |
haarkon | 0:6f78c735f07c | 21 | static T_pixyState PIXY_state = none; |
haarkon | 0:6f78c735f07c | 22 | static Byte byteCount = 0; |
haarkon | 5:d395fe0fb0e0 | 23 | static int PIXY_synced = 0; |
haarkon | 5:d395fe0fb0e0 | 24 | Word i, somme; |
haarkon | 5:d395fe0fb0e0 | 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 | 5:d395fe0fb0e0 | 31 | dummy = _Pixy->getc(); // On stocke l'octet reçu dans une variable temporaire |
haarkon | 5:d395fe0fb0e0 | 32 | |
haarkon | 5:d395fe0fb0e0 | 33 | if (!PIXY_synced) { // Si la camera et la carte ne sont pas synchronisées |
haarkon | 5:d395fe0fb0e0 | 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 | 5:d395fe0fb0e0 | 36 | if (byteCount < 3) { // Tant qu'on n'a pas encore reçu les 4 premier octets |
haarkon | 5:d395fe0fb0e0 | 37 | byteCount++; // On passe à la case suivante du tableau temporaire |
haarkon | 5:d395fe0fb0e0 | 38 | } else { // Lorsqu'on a 4 octets |
haarkon | 5:d395fe0fb0e0 | 39 | if ((msgBuffer.motlong == 0xaa550000)||(msgBuffer.motlong == 0xaa560000)) { // On teste pour voir si on a reçu un entête |
haarkon | 5:d395fe0fb0e0 | 40 | PIXY_synced = 1; // Si oui : On passe en mode synchronisé |
haarkon | 5:d395fe0fb0e0 | 41 | PIXY_state = begin; // Et on va à l'aiguillage |
haarkon | 5:d395fe0fb0e0 | 42 | byteCount = 0; // Et on remet le comptage d'octet à 0 |
haarkon | 5:d395fe0fb0e0 | 43 | } else { // Si on n'a pas trouvé d'entête |
haarkon | 5:d395fe0fb0e0 | 44 | for (i=1; i<4; i++) msgBuffer.octet[i-1] = msgBuffer.octet[i]; // On décalle les cases du tableau |
haarkon | 5:d395fe0fb0e0 | 45 | byteCount = 3; // Et on attend le caractère suivant |
haarkon | 0:6f78c735f07c | 46 | } |
haarkon | 0:6f78c735f07c | 47 | } |
haarkon | 5:d395fe0fb0e0 | 48 | } else { |
haarkon | 0:6f78c735f07c | 49 | switch (PIXY_state) { |
haarkon | 0:6f78c735f07c | 50 | case begin : // l'aiguillage est là ! |
haarkon | 5:d395fe0fb0e0 | 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 | 5:d395fe0fb0e0 | 54 | switch (wordBuffer.mot) { |
haarkon | 7:c809db7aa1c0 | 55 | case NM_BLOCCODE : // Si c'est un bloc normal (code = 0xAA55) |
haarkon | 5:d395fe0fb0e0 | 56 | PIXY_state = normal; // On part vers le traitement spécifique |
haarkon | 5:d395fe0fb0e0 | 57 | break; |
haarkon | 0:6f78c735f07c | 58 | |
haarkon | 7:c809db7aa1c0 | 59 | case CC_BLOCCODE : // Si c'est un bloc color Code (code = 0xAA56) |
haarkon | 5:d395fe0fb0e0 | 60 | PIXY_state = colorCode; // On part vers le traitement spécifique |
haarkon | 5:d395fe0fb0e0 | 61 | break; |
haarkon | 0:6f78c735f07c | 62 | |
haarkon | 5:d395fe0fb0e0 | 63 | case 0 : // Si c'est un debut de trame (code = 0x0000) |
haarkon | 5:d395fe0fb0e0 | 64 | PIXY_state = doubleZero; // On part vers le traitement spécifique |
haarkon | 5:d395fe0fb0e0 | 65 | break; |
haarkon | 0:6f78c735f07c | 66 | |
haarkon | 5:d395fe0fb0e0 | 67 | default : // Si c'est autre chose |
haarkon | 5:d395fe0fb0e0 | 68 | PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé. |
haarkon | 5:d395fe0fb0e0 | 69 | PIXY_state = none; // Ceinture et bretelle |
haarkon | 5:d395fe0fb0e0 | 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 | 5:d395fe0fb0e0 | 78 | Pixy_NMFIFO[PIXY_wNMObjet].tab[byteCount] = dummy; // On stocke les octets un à un dans la structure Bloc |
haarkon | 7:c809db7aa1c0 | 79 | if (byteCount < (NM_BLOCSIZE-1)) { // 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 | 6:f16f250fe2cb | 89 | if (PIXY_wNMObjet < (NM_MAXOBJECT-1)) PIXY_wNMObjet++; // On incrémente le pointeur d'écriture dans la FIFO Objet |
haarkon | 6:f16f250fe2cb | 90 | else PIXY_wNMObjet = 0; |
haarkon | 6:f16f250fe2cb | 91 | if (PIXY_nbNMObjet < (NM_MAXOBJECT-1)) PIXY_nbNMObjet++; // On dit que l'on a un objet de plus |
haarkon | 6:f16f250fe2cb | 92 | else FlagPixyOverflow = 1; // Si on a plus de NM_MAXOBJECT 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 | 7:c809db7aa1c0 | 100 | if (byteCount < (CC_BLOCSIZE-1)) 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 | 6:f16f250fe2cb | 109 | if (PIXY_wCCObjet < (CC_MAXOBJECT-1)) PIXY_wCCObjet++; // On incrémente le pointeur d'écriture dans la FIFO CCObjet |
haarkon | 6:f16f250fe2cb | 110 | else PIXY_wCCObjet = 0; |
haarkon | 6:f16f250fe2cb | 111 | if (PIXY_nbCCObjet < (CC_MAXOBJECT-1)) PIXY_nbCCObjet++; // On dit que l'on a un objet CC de plus à traiter |
haarkon | 6:f16f250fe2cb | 112 | else FlagPixyOverflow = 1; // Si on a plus de CC_MAXOBJECT CC objets (en attente) => Overflow |
haarkon | 0:6f78c735f07c | 113 | } |
haarkon | 0:6f78c735f07c | 114 | } |
haarkon | 0:6f78c735f07c | 115 | break; |
haarkon | 0:6f78c735f07c | 116 | |
haarkon | 5:d395fe0fb0e0 | 117 | case doubleZero : // Si on a reçu le code de début d'une nouvelle trame (0000). |
haarkon | 5:d395fe0fb0e0 | 118 | |
haarkon | 5:d395fe0fb0e0 | 119 | Pixy_NMObjet = PIXY_nbNMObjet; // On met à jour les variables pour le traitement extérieur |
haarkon | 5:d395fe0fb0e0 | 120 | Pixy_CCObjet = PIXY_nbCCObjet; |
haarkon | 5:d395fe0fb0e0 | 121 | Pixy_CCFrameIsNew = 1; |
haarkon | 5:d395fe0fb0e0 | 122 | Pixy_FirstCCObjet = PIXY_wCCObjet; |
haarkon | 5:d395fe0fb0e0 | 123 | Pixy_NMFrameIsNew = 1; |
haarkon | 5:d395fe0fb0e0 | 124 | Pixy_FirstNMObjet = PIXY_wNMObjet; |
haarkon | 5:d395fe0fb0e0 | 125 | FlagPixy = 1; // On valide le traitement de la trame précédente. |
haarkon | 0:6f78c735f07c | 126 | |
haarkon | 5:d395fe0fb0e0 | 127 | PIXY_nbCCObjet = 0; // On remet à 0 les variables internes |
haarkon | 5:d395fe0fb0e0 | 128 | PIXY_nbNMObjet = 0; |
haarkon | 5:d395fe0fb0e0 | 129 | byteCount = 0; |
haarkon | 5:d395fe0fb0e0 | 130 | |
haarkon | 5:d395fe0fb0e0 | 131 | wordBuffer.octet[byteCount%2] = dummy; // on stocke les octets reçus |
haarkon | 5:d395fe0fb0e0 | 132 | byteCount++; |
haarkon | 5:d395fe0fb0e0 | 133 | PIXY_state = waitForStart; // On passe en attente de la deuxième partie du code |
haarkon | 5:d395fe0fb0e0 | 134 | break; |
haarkon | 5:d395fe0fb0e0 | 135 | |
haarkon | 5:d395fe0fb0e0 | 136 | case waitForStart : // Si on a reçu le code de début d'une nouvelle trame. |
haarkon | 5:d395fe0fb0e0 | 137 | |
haarkon | 5:d395fe0fb0e0 | 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 | 5:d395fe0fb0e0 | 141 | byteCount = 0; |
haarkon | 7:c809db7aa1c0 | 142 | if (wordBuffer.mot == NM_BLOCCODE) { // 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 | 5:d395fe0fb0e0 | 144 | } else { // Si ce n'est pas le cas |
haarkon | 5:d395fe0fb0e0 | 145 | PIXY_synced = 0; // On n'est plus synchronisés |
haarkon | 5:d395fe0fb0e0 | 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 | 6:f16f250fe2cb | 170 | if (Pixy_FirstCCObjet <= Pixy_CCObjet) PIXY_rCCObjet = CC_MAXOBJECT - 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 | 6:f16f250fe2cb | 175 | if (PIXY_rCCObjet < (CC_MAXOBJECT-1)) PIXY_rCCObjet++; // On incrémente le pointeur de lecture dans la FIFO CCObjet |
haarkon | 6:f16f250fe2cb | 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 | 6:f16f250fe2cb | 191 | if (Pixy_FirstNMObjet <= Pixy_NMObjet) PIXY_rNMObjet = NM_MAXOBJECT - 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 | 6:f16f250fe2cb | 196 | if (PIXY_rNMObjet < (NM_MAXOBJECT-1)) PIXY_rNMObjet++; // On incrémente le pointeur de lecture dans la FIFO CCObjet |
haarkon | 6:f16f250fe2cb | 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 | 5:d395fe0fb0e0 | 205 | void PIXY::setBrightness (Byte brightness) |
haarkon | 0:6f78c735f07c | 206 | { |
haarkon | 5:d395fe0fb0e0 | 207 | _Pixy->putc(0); //Start |
haarkon | 5:d395fe0fb0e0 | 208 | _Pixy->putc(0xFE); //Brightness Level |
haarkon | 5:d395fe0fb0e0 | 209 | _Pixy->putc(brightness); |
haarkon | 5:d395fe0fb0e0 | 210 | } |
haarkon | 0:6f78c735f07c | 211 | |
haarkon | 5:d395fe0fb0e0 | 212 | int PIXY::checkNewImage (void) |
haarkon | 5:d395fe0fb0e0 | 213 | { |
haarkon | 5:d395fe0fb0e0 | 214 | if (FlagPixy) { |
haarkon | 5:d395fe0fb0e0 | 215 | FlagPixy = 0; |
haarkon | 5:d395fe0fb0e0 | 216 | return 1; |
haarkon | 5:d395fe0fb0e0 | 217 | } else { |
haarkon | 5:d395fe0fb0e0 | 218 | return 0; |
haarkon | 5:d395fe0fb0e0 | 219 | } |
haarkon | 5:d395fe0fb0e0 | 220 | } |
haarkon | 0:6f78c735f07c | 221 | |
haarkon | 5:d395fe0fb0e0 | 222 | int PIXY::checkPixy (void) |
haarkon | 5:d395fe0fb0e0 | 223 | { |
haarkon | 5:d395fe0fb0e0 | 224 | return Pixy_check; |
haarkon | 0:6f78c735f07c | 225 | } |