FRC - Hackathon Pixy
Pixy.cpp@0:6f78c735f07c, 2018-05-20 (annotated)
- Committer:
- haarkon
- Date:
- Sun May 20 08:18:49 2018 +0000
- Revision:
- 0:6f78c735f07c
- Child:
- 2:90355c600404
Untested Pixy - CMUCAM5 library
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 | 0:6f78c735f07c | 11 | _cmucam = new Serial (tx, rx, debit); |
haarkon | 0:6f78c735f07c | 12 | _cmucam->attach (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 | 0:6f78c735f07c | 19 | static T_tmpBuffer tmpBuffer; |
haarkon | 0:6f78c735f07c | 20 | static T_structBuffer msgBuffer; |
haarkon | 0:6f78c735f07c | 21 | static T_pixyState PIXY_state = none; |
haarkon | 0:6f78c735f07c | 22 | static Byte byteCount = 0; |
haarkon | 0:6f78c735f07c | 23 | static int PIXY_synced = 0, dummy; |
haarkon | 0:6f78c735f07c | 24 | int i, somme; |
haarkon | 0:6f78c735f07c | 25 | static Byte PIXY_nbCCObjet = 0, PIXY_wCCObjet = 0; |
haarkon | 0:6f78c735f07c | 26 | static Byte PIXY_nbNMObjet = 0, PIXY_wNMObjet = 0; |
haarkon | 0:6f78c735f07c | 27 | |
haarkon | 0:6f78c735f07c | 28 | Pixy_check = 0; |
haarkon | 0:6f78c735f07c | 29 | |
haarkon | 0:6f78c735f07c | 30 | if (!PIXY_synced) { // On n'a pas trouvé le START (0x55aa0000) |
haarkon | 0:6f78c735f07c | 31 | tmpBuffer.tab[byteCount] = _cmucam->getc(); // On stocke l'octet reçu dans la première case dispo du tableau temporaire |
haarkon | 0:6f78c735f07c | 32 | |
haarkon | 0:6f78c735f07c | 33 | if (byteCount < 3) { // Si on n'a pas encore reçu les 4 premier octets |
haarkon | 0:6f78c735f07c | 34 | byteCount++; // On passe à la case suivante du tableau temporaire |
haarkon | 0:6f78c735f07c | 35 | } else { // Lorsqu'on a 4 octets |
haarkon | 0:6f78c735f07c | 36 | if (tmpBuffer.mot != 0xaa550000) { // Si le code n'est pas le code de START |
haarkon | 0:6f78c735f07c | 37 | for (i=1; i<4; i++) tmpBuffer.tab[i-1] = tmpBuffer.tab[i]; // On décalle les cases du tableau |
haarkon | 0:6f78c735f07c | 38 | byteCount = 3; // Et on attend le caractère suivant |
haarkon | 0:6f78c735f07c | 39 | } else { // Si on a trouvé le code de START |
haarkon | 0:6f78c735f07c | 40 | PIXY_synced = 1; // On passe en mode synchronisé |
haarkon | 0:6f78c735f07c | 41 | PIXY_state = begin; |
haarkon | 0:6f78c735f07c | 42 | byteCount = 0; |
haarkon | 0:6f78c735f07c | 43 | } |
haarkon | 0:6f78c735f07c | 44 | } |
haarkon | 0:6f78c735f07c | 45 | } |
haarkon | 0:6f78c735f07c | 46 | |
haarkon | 0:6f78c735f07c | 47 | if (PIXY_synced) { |
haarkon | 0:6f78c735f07c | 48 | |
haarkon | 0:6f78c735f07c | 49 | switch (PIXY_state) { |
haarkon | 0:6f78c735f07c | 50 | case begin : // l'aiguillage est là ! |
haarkon | 0:6f78c735f07c | 51 | |
haarkon | 0:6f78c735f07c | 52 | msgBuffer.tab[byteCount%2] = _cmucam->getc(); // on stocke les octets reçus |
haarkon | 0:6f78c735f07c | 53 | byteCount++; |
haarkon | 0:6f78c735f07c | 54 | if (byteCount == 2) { // Quand on a 2 octets |
haarkon | 0:6f78c735f07c | 55 | |
haarkon | 0:6f78c735f07c | 56 | if (msgBuffer.mot == 0xaa55) { // Si c'est un bloc normal (code 0xAA55) |
haarkon | 0:6f78c735f07c | 57 | PIXY_state = normal; // On part vers le traitement spécifique |
haarkon | 0:6f78c735f07c | 58 | } |
haarkon | 0:6f78c735f07c | 59 | |
haarkon | 0:6f78c735f07c | 60 | if (msgBuffer.mot == 0xaa56) { // Si c'est un bloc Color Code (code 0xAA56) |
haarkon | 0:6f78c735f07c | 61 | PIXY_state = colorCode; // On part vers le traitement spécifique |
haarkon | 0:6f78c735f07c | 62 | } |
haarkon | 0:6f78c735f07c | 63 | |
haarkon | 0:6f78c735f07c | 64 | if (msgBuffer.mot == 0) { // Si on a un debut de trame (code 0000) |
haarkon | 0:6f78c735f07c | 65 | PIXY_state = doubleZero; // On part vers le traitement spécifique |
haarkon | 0:6f78c735f07c | 66 | } |
haarkon | 0:6f78c735f07c | 67 | if ((PIXY_state == begin) || (PIXY_state == none)) { // Si c'est autre chose |
haarkon | 0:6f78c735f07c | 68 | PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé. |
haarkon | 0:6f78c735f07c | 69 | PIXY_state = none; // Ceinture et bretelle |
haarkon | 0:6f78c735f07c | 70 | } |
haarkon | 0:6f78c735f07c | 71 | byteCount = 0; |
haarkon | 0:6f78c735f07c | 72 | } |
haarkon | 0:6f78c735f07c | 73 | break; |
haarkon | 0:6f78c735f07c | 74 | |
haarkon | 0:6f78c735f07c | 75 | case normal : // Si on a un bloc normal |
haarkon | 0:6f78c735f07c | 76 | |
haarkon | 0:6f78c735f07c | 77 | Pixy_NMFIFO[PIXY_wNMObjet].tab[byteCount] = _cmucam->getc(); // On stocke les octets un à un dans la structure Bloc |
haarkon | 0:6f78c735f07c | 78 | if (byteCount < 11) { // Tant que la structure n'est pas pleine |
haarkon | 0:6f78c735f07c | 79 | byteCount++; // On passe à l'octet suivant |
haarkon | 0:6f78c735f07c | 80 | } else { // Quand elle est pleine |
haarkon | 0:6f78c735f07c | 81 | byteCount = 0; // On réinitialise |
haarkon | 0:6f78c735f07c | 82 | PIXY_state = begin; // On retourne à l'aiguillage |
haarkon | 0:6f78c735f07c | 83 | |
haarkon | 0:6f78c735f07c | 84 | // On calcule la somme de contrôle |
haarkon | 0:6f78c735f07c | 85 | 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 | 86 | |
haarkon | 0:6f78c735f07c | 87 | if (somme == Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.checksum) { // Si le checksum est bon, on valide la réception |
haarkon | 0:6f78c735f07c | 88 | if (PIXY_wNMObjet < 19) PIXY_wNMObjet++; // On incrémente le pointeur d'écriture dans la FIFO Objet |
haarkon | 0:6f78c735f07c | 89 | else PIXY_wNMObjet = 0; |
haarkon | 0:6f78c735f07c | 90 | if (PIXY_nbNMObjet < 19) PIXY_nbNMObjet++; // On dit que l'on a un objet de plus |
haarkon | 0:6f78c735f07c | 91 | else FlagPixyOverflow = 1; // Si on a plus de 20 objets (en attente) => Overflow |
haarkon | 0:6f78c735f07c | 92 | } |
haarkon | 0:6f78c735f07c | 93 | } |
haarkon | 0:6f78c735f07c | 94 | break; |
haarkon | 0:6f78c735f07c | 95 | |
haarkon | 0:6f78c735f07c | 96 | case colorCode : // Si on a un bloc colorCode |
haarkon | 0:6f78c735f07c | 97 | |
haarkon | 0:6f78c735f07c | 98 | Pixy_CCFIFO[PIXY_wCCObjet].tab[byteCount] = dummy; // On stocke les octets un à un dans la structure CCBloc |
haarkon | 0:6f78c735f07c | 99 | if (byteCount < 13) byteCount++; // tant que la structure n'est pas pleine on passe à l'octet suivant |
haarkon | 0:6f78c735f07c | 100 | else { // Quand elle est pleine |
haarkon | 0:6f78c735f07c | 101 | byteCount = 0; // On réinitialise |
haarkon | 0:6f78c735f07c | 102 | PIXY_state = begin; // On retourne à l'aiguillage |
haarkon | 0:6f78c735f07c | 103 | |
haarkon | 0:6f78c735f07c | 104 | // On calcule la somme de contrôle |
haarkon | 0:6f78c735f07c | 105 | 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 | 106 | |
haarkon | 0:6f78c735f07c | 107 | if (somme == Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.checksum) { // Si le checksum est bon |
haarkon | 0:6f78c735f07c | 108 | if (PIXY_wCCObjet < 19) PIXY_wCCObjet++; // On incrémente le pointeur d'écriture dans la FIFO CCObjet |
haarkon | 0:6f78c735f07c | 109 | else PIXY_wCCObjet = 0; |
haarkon | 0:6f78c735f07c | 110 | if (PIXY_nbCCObjet < 19) PIXY_nbCCObjet++; // On dit que l'on a un objet CC de plus à traiter |
haarkon | 0:6f78c735f07c | 111 | else FlagPixyOverflow = 1; // Si on a plus de 20 CC objets (en attente) => Overflow |
haarkon | 0:6f78c735f07c | 112 | } |
haarkon | 0:6f78c735f07c | 113 | } |
haarkon | 0:6f78c735f07c | 114 | break; |
haarkon | 0:6f78c735f07c | 115 | |
haarkon | 0:6f78c735f07c | 116 | case doubleZero : // Si on a reçu le code de début d'une nouvelle trame. |
haarkon | 0:6f78c735f07c | 117 | |
haarkon | 0:6f78c735f07c | 118 | msgBuffer.tab[byteCount%2] = _cmucam->getc(); // on stocke les octets reçus |
haarkon | 0:6f78c735f07c | 119 | byteCount++; |
haarkon | 0:6f78c735f07c | 120 | if (byteCount == 2) { // Quand on a 2 octets |
haarkon | 0:6f78c735f07c | 121 | if (msgBuffer.mot == 0xaa55) { // On doit impérativement trouver le code 0xAA55 |
haarkon | 0:6f78c735f07c | 122 | PIXY_state = begin; // Si c'est le cas, alors tout va bien et on va à l'aiguillage |
haarkon | 0:6f78c735f07c | 123 | Pixy_NMObjet = PIXY_nbNMObjet; // On met à jour les variables pour le traitement |
haarkon | 0:6f78c735f07c | 124 | Pixy_CCObjet = PIXY_nbCCObjet; |
haarkon | 0:6f78c735f07c | 125 | PIXY_nbCCObjet = 0; |
haarkon | 0:6f78c735f07c | 126 | PIXY_nbNMObjet = 0; |
haarkon | 0:6f78c735f07c | 127 | Pixy_CCFrameIsNew = 1; |
haarkon | 0:6f78c735f07c | 128 | Pixy_FirstCCObjet = PIXY_wCCObjet; |
haarkon | 0:6f78c735f07c | 129 | Pixy_NMFrameIsNew = 1; |
haarkon | 0:6f78c735f07c | 130 | Pixy_FirstNMObjet = PIXY_wNMObjet; |
haarkon | 0:6f78c735f07c | 131 | FlagPixy = 1; // On valide le traitement de la trame précédente. |
haarkon | 0:6f78c735f07c | 132 | } else { // Si on trouve autre chose |
haarkon | 0:6f78c735f07c | 133 | PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé. |
haarkon | 0:6f78c735f07c | 134 | PIXY_state = none; // Ceinture et bretelle |
haarkon | 0:6f78c735f07c | 135 | } |
haarkon | 0:6f78c735f07c | 136 | byteCount = 0; |
haarkon | 0:6f78c735f07c | 137 | } |
haarkon | 0:6f78c735f07c | 138 | break; |
haarkon | 0:6f78c735f07c | 139 | } |
haarkon | 0:6f78c735f07c | 140 | } |
haarkon | 0:6f78c735f07c | 141 | } |
haarkon | 0:6f78c735f07c | 142 | |
haarkon | 0:6f78c735f07c | 143 | int PIXY::detectedObject (int* nbNormal, int* nbCC) |
haarkon | 0:6f78c735f07c | 144 | { |
haarkon | 0:6f78c735f07c | 145 | *nbNormal = (int)Pixy_NMObjet; |
haarkon | 0:6f78c735f07c | 146 | *nbCC = (int)Pixy_CCObjet; |
haarkon | 0:6f78c735f07c | 147 | if (Pixy_check!=0) return -1; |
haarkon | 0:6f78c735f07c | 148 | if (FlagPixyOverflow) return -2; |
haarkon | 0:6f78c735f07c | 149 | return 0; |
haarkon | 0:6f78c735f07c | 150 | } |
haarkon | 0:6f78c735f07c | 151 | |
haarkon | 0:6f78c735f07c | 152 | T_pixyCCBloc PIXY::getCCBloc (void) |
haarkon | 0:6f78c735f07c | 153 | { |
haarkon | 0:6f78c735f07c | 154 | T_pixyCCBloc dummy; |
haarkon | 0:6f78c735f07c | 155 | static Byte PIXY_rCCObjet = 0; |
haarkon | 0:6f78c735f07c | 156 | |
haarkon | 0:6f78c735f07c | 157 | if (Pixy_CCObjet !=0) { |
haarkon | 0:6f78c735f07c | 158 | if (Pixy_CCFrameIsNew) { //Si on est au début d'une nouvelle trame, on initialise le pointeur de lecture |
haarkon | 0:6f78c735f07c | 159 | if (Pixy_FirstCCObjet <= Pixy_CCObjet) PIXY_rCCObjet = 20 - Pixy_CCObjet + Pixy_FirstCCObjet ; |
haarkon | 0:6f78c735f07c | 160 | else PIXY_rCCObjet = Pixy_FirstCCObjet - Pixy_CCObjet ; |
haarkon | 0:6f78c735f07c | 161 | Pixy_CCFrameIsNew = 0; |
haarkon | 0:6f78c735f07c | 162 | } |
haarkon | 0:6f78c735f07c | 163 | dummy = Pixy_CCFIFO[PIXY_rCCObjet].CCbloc; |
haarkon | 0:6f78c735f07c | 164 | if (PIXY_rCCObjet < 19) PIXY_rCCObjet++; // On incrémente le pointeur de lecture dans la FIFO CCObjet |
haarkon | 0:6f78c735f07c | 165 | else PIXY_rCCObjet = 0; |
haarkon | 0:6f78c735f07c | 166 | Pixy_CCObjet--; // On dit que l'on a un objet CC de moins à traiter |
haarkon | 0:6f78c735f07c | 167 | } else { |
haarkon | 0:6f78c735f07c | 168 | dummy.signature = 0xFFFF; |
haarkon | 0:6f78c735f07c | 169 | } |
haarkon | 0:6f78c735f07c | 170 | return dummy; |
haarkon | 0:6f78c735f07c | 171 | } |
haarkon | 0:6f78c735f07c | 172 | |
haarkon | 0:6f78c735f07c | 173 | T_pixyNMBloc PIXY::getNMBloc (void) |
haarkon | 0:6f78c735f07c | 174 | { |
haarkon | 0:6f78c735f07c | 175 | T_pixyNMBloc dummy; |
haarkon | 0:6f78c735f07c | 176 | static Byte PIXY_rNMObjet = 0; |
haarkon | 0:6f78c735f07c | 177 | |
haarkon | 0:6f78c735f07c | 178 | if (Pixy_NMObjet !=0) { |
haarkon | 0:6f78c735f07c | 179 | if (Pixy_NMFrameIsNew) { //Si on est au début d'une nouvelle trame, on initialise le pointeur de lecture |
haarkon | 0:6f78c735f07c | 180 | if (Pixy_FirstNMObjet <= Pixy_NMObjet) PIXY_rNMObjet = 20 - Pixy_NMObjet + Pixy_FirstNMObjet ; |
haarkon | 0:6f78c735f07c | 181 | else PIXY_rNMObjet = Pixy_FirstNMObjet - Pixy_NMObjet ; |
haarkon | 0:6f78c735f07c | 182 | Pixy_NMFrameIsNew = 0; |
haarkon | 0:6f78c735f07c | 183 | } |
haarkon | 0:6f78c735f07c | 184 | dummy = Pixy_NMFIFO[PIXY_rNMObjet].NMbloc; |
haarkon | 0:6f78c735f07c | 185 | if (PIXY_rNMObjet < 19) PIXY_rNMObjet++; // On incrémente le pointeur de lecture dans la FIFO CCObjet |
haarkon | 0:6f78c735f07c | 186 | else PIXY_rNMObjet = 0; |
haarkon | 0:6f78c735f07c | 187 | Pixy_NMObjet--; // On dit que l'on a un objet CC de moins à traiter |
haarkon | 0:6f78c735f07c | 188 | } else { |
haarkon | 0:6f78c735f07c | 189 | dummy.signature = 0xFFFF; |
haarkon | 0:6f78c735f07c | 190 | } |
haarkon | 0:6f78c735f07c | 191 | return dummy; |
haarkon | 0:6f78c735f07c | 192 | } |
haarkon | 0:6f78c735f07c | 193 | |
haarkon | 0:6f78c735f07c | 194 | int PIXY::setBrightness (Byte brightness) |
haarkon | 0:6f78c735f07c | 195 | { |
haarkon | 0:6f78c735f07c | 196 | |
haarkon | 0:6f78c735f07c | 197 | Byte PixyMsgBuf[3]; |
haarkon | 0:6f78c735f07c | 198 | |
haarkon | 0:6f78c735f07c | 199 | PixyMsgBuf[0] = 0x00; //Start |
haarkon | 0:6f78c735f07c | 200 | PixyMsgBuf[1] = 0xFE; //Brightness Level |
haarkon | 0:6f78c735f07c | 201 | PixyMsgBuf[2] = brightness; |
haarkon | 0:6f78c735f07c | 202 | |
haarkon | 0:6f78c735f07c | 203 | return _cmucam->write(PixyMsgBuf,3,NULL,0); |
haarkon | 0:6f78c735f07c | 204 | } |