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@2:90355c600404, 2018-05-22 (annotated)

Committer:
haarkon
Date:
Tue May 22 17:15:30 2018 +0000
Revision:
2:90355c600404
Parent:
0:6f78c735f07c
Child:
3:1e4c6d7a0053
added callback treatment of interrupt

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 0:6f78c735f07c 11 _cmucam = new Serial (tx, rx, debit);
haarkon 2:90355c600404 12 _cmucam->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 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 2:90355c600404 143 int PIXY::detectedObject (int* nbNM, int* nbCC)
haarkon 0:6f78c735f07c 144 {
haarkon 2:90355c600404 145 *nbNM = (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 }