FRC - Hackathon
Programme de test à priori fonctionnel avec la version 2018 de la carte
Dependencies: PwmIn mbed Encoder_Nucleo_16_bits
Diff: main.cpp
- Revision:
- 3:3dfe26a10796
- Parent:
- 2:1d440e938c44
- Child:
- 4:f65b0905e4ce
--- a/main.cpp Wed May 31 06:39:30 2017 +0000
+++ b/main.cpp Wed May 31 07:57:18 2017 +0000
@@ -55,7 +55,7 @@
Word y;
Word width;
Word height;
-} T_pixyBloc;
+} T_pixyNMBloc;
typedef union {
Byte tab[14];
@@ -64,8 +64,8 @@
typedef union {
Byte tab[12];
- T_pixyBloc bloc;
-} T_pixyData;
+ T_pixyNMBloc NMbloc;
+} T_pixyNMData;
@@ -208,22 +208,21 @@
*/
// Structure de temps
-lWord Tick = 0;
+lWord Tick = 0;
// Sémaphore d'interruption
-int FlagUS1 = 0, FlagUS2 = 0, FlagUS3 = 0, FlagPixy = 0, FlagPixyOverflow = 0;
-int FlagTick = 0, FlagTickLed = 0, BPPressed = 0;
-int Pixy_check = -1;
+int FlagUS1 = 0, FlagUS2 = 0, FlagUS3 = 0, FlagPixy = 0, FlagPixyOverflow = 0;
+int FlagTick = 0, FlagTickLed = 0, BPPressed = 0;
+int Pixy_check = -1;
// Dialogue avec la Pixy
-T_pixyCCData Pixy_CCFIFO[20];
-T_pixyData Pixy_FIFO[20];
-Byte Pixy_saveNbCCObjet;
-Byte Pixy_saveNbObjet;
+T_pixyCCData Pixy_CCFIFO[20];
+T_pixyNMData Pixy_NMFIFO[20];
+Byte Pixy_CCObjet, Pixy_NMObjet;
// Gestion des capteurs Ultrason
-long Echo1Start, Echo2Start, Echo3Start;
-double DistUS1, DistUS2, DistUS3;
+long Echo1Start, Echo2Start, Echo3Start;
+double DistUS1, DistUS2, DistUS3;
/** Liste des interruptions
*
@@ -278,17 +277,16 @@
{
static T_tmpBuffer tmpBuffer;
static T_structBuffer msgBuffer;
- static T_pixyState Pixy_state = none;
+ static T_pixyState PIXY_state = none;
static Byte byteCount = 0;
- static int Pixy_synced = 0;
- static int dummy;
+ static int PIXY_synced = 0, dummy;
int i, somme;
- static Byte Pixy_nbCCObjet = 0, Pixy_wCCObjet = 0;
- static Byte Pixy_nbObjet = 0, Pixy_wObjet = 0;
+ static Byte PIXY_nbCCObjet = 0, PIXY_wCCObjet = 0;
+ static Byte PIXY_nbNMObjet = 0, PIXY_wNMObjet = 0;
Pixy_check = 0;
- if (!Pixy_synced) { // On n'a pas trouvé le START (0x55aa0000)
+ if (!PIXY_synced) { // On n'a pas trouvé le START (0x55aa0000)
tmpBuffer.tab[byteCount] = Pixy.getc(); // On stocke l'octet reçu dans la première case dispo du tableau temporaire
if (byteCount < 3) { // Si on n'a pas encore reçu les 4 premier octets
@@ -298,16 +296,16 @@
for (i=1; i<4; i++) tmpBuffer.tab[i-1] = tmpBuffer.tab[i]; // On décalle les cases du tableau
byteCount = 3; // Et on attend le caractère suivant
} else { // Si on a trouvé le code de START
- Pixy_synced = 1; // On passe en mode synchronisé
- Pixy_state = begin;
+ PIXY_synced = 1; // On passe en mode synchronisé
+ PIXY_state = begin;
byteCount = 0;
}
}
}
- if (Pixy_synced) {
+ if (PIXY_synced) {
- switch (Pixy_state) {
+ switch (PIXY_state) {
case begin : // l'aiguillage est là !
msgBuffer.tab[byteCount%2] = Pixy.getc(); // on stocke les octets reçus
@@ -315,19 +313,19 @@
if (byteCount == 2) { // Quand on a 2 octets
if (msgBuffer.mot == 0xaa55) { // Si c'est un bloc normal (code 0xAA55)
- Pixy_state = normal; // On part vers le traitement spécifique
+ PIXY_state = normal; // On part vers le traitement spécifique
}
if (msgBuffer.mot == 0xaa56) { // Si c'est un bloc Color Code (code 0xAA56)
- Pixy_state = colorCode; // On part vers le traitement spécifique
+ PIXY_state = colorCode; // On part vers le traitement spécifique
}
if (msgBuffer.mot == 0) { // Si on a un debut de trame (code 0000)
- Pixy_state = doubleZero; // On part vers le traitement spécifique
+ PIXY_state = doubleZero; // On part vers le traitement spécifique
}
- if ((Pixy_state == begin) || (Pixy_state == none)) { // Si c'est autre chose
- Pixy_synced = 0; // C'est qu'on est perdu donc plus synchronisé.
- Pixy_state = none; // Ceinture et bretelle
+ if ((PIXY_state == begin) || (PIXY_state == none)) { // Si c'est autre chose
+ PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé.
+ PIXY_state = none; // Ceinture et bretelle
}
byteCount = 0;
}
@@ -335,19 +333,19 @@
case normal : // Si on a un bloc normal
- Pixy_FIFO[Pixy_wObjet].tab[byteCount] = Pixy.getc(); // On stocke les octets un à un dans la structure Bloc
+ Pixy_NMFIFO[PIXY_wNMObjet].tab[byteCount] = Pixy.getc(); // On stocke les octets un à un dans la structure Bloc
if (byteCount < 11) { // Tant que la structure n'est pas pleine
byteCount++; // On passe à l'octet suivant
} else { // Quand elle est pleine
byteCount = 0; // On réinitialise
- Pixy_state = begin; // On retourne à l'aiguillage
+ PIXY_state = begin; // On retourne à l'aiguillage
// On calcule la somme de contrôle
- somme = Pixy_FIFO[Pixy_wObjet].bloc.signature + Pixy_FIFO[Pixy_wObjet].bloc.x + Pixy_FIFO[Pixy_wObjet].bloc.y + Pixy_FIFO[Pixy_wObjet].bloc.width + Pixy_FIFO[Pixy_wObjet].bloc.height;
+ 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;
- if (somme == Pixy_FIFO[Pixy_wObjet].bloc.checksum) { // Si le checksum est bon, on valide la réception
- if (Pixy_wObjet < 19) Pixy_wObjet++; // On incrémente le pointeur d'écriture dans la FIFO Objet
- else Pixy_wObjet = 0;
- if (Pixy_nbObjet < 19) Pixy_nbObjet++; // On dit que l'on a un objet CC de plus
+ if (somme == Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.checksum) { // Si le checksum est bon, on valide la réception
+ if (PIXY_wNMObjet < 19) PIXY_wNMObjet++; // On incrémente le pointeur d'écriture dans la FIFO Objet
+ else PIXY_wNMObjet = 0;
+ if (PIXY_nbNMObjet < 19) PIXY_nbNMObjet++; // On dit que l'on a un objet CC de plus
else FlagPixyOverflow = 1; // Si on a plus de 20 CC objets (en attente) => Overflow
}
}
@@ -355,18 +353,18 @@
case colorCode : // Si on a un bloc colorCode
- Pixy_CCFIFO[Pixy_wCCObjet].tab[byteCount] = dummy; // On stocke les octets un à un dans la structure CCBloc
+ Pixy_CCFIFO[PIXY_wCCObjet].tab[byteCount] = dummy; // On stocke les octets un à un dans la structure CCBloc
if (byteCount < 13) byteCount++; // tant que la structure n'est pas pleine on passe à l'octet suivant
else { // Quand elle est pleine
byteCount = 0; // On réinitialise
- Pixy_state = begin; // On retourne à l'aiguillage
+ PIXY_state = begin; // On retourne à l'aiguillage
// On calcule la somme de contrôle
- 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;
+ 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;
- if (somme == Pixy_CCFIFO[Pixy_wCCObjet].CCbloc.checksum) { // Si le checksum est bon
- if (Pixy_wCCObjet < 19) Pixy_wCCObjet++; // On incrémente le pointeur d'écriture dans la FIFO CCObjet
- else Pixy_wCCObjet = 0;
- if (Pixy_nbCCObjet < 19) Pixy_nbCCObjet++; // On dit que l'on a un objet CC de plus à traiter
+ if (somme == Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.checksum) { // Si le checksum est bon
+ if (PIXY_wCCObjet < 19) PIXY_wCCObjet++; // On incrémente le pointeur d'écriture dans la FIFO CCObjet
+ else PIXY_wCCObjet = 0;
+ if (PIXY_nbCCObjet < 19) PIXY_nbCCObjet++; // On dit que l'on a un objet CC de plus à traiter
else FlagPixyOverflow = 1; // Si on a plus de 20 CC objets (en attente) => Overflow
}
}
@@ -374,21 +372,19 @@
case doubleZero : // Si on a reçu le code de début d'une nouvelle trame.
- Pixy_saveNbCCObjet = Pixy_nbCCObjet;
- Pixy_saveNbObjet = Pixy_nbObjet;
- Pixy_nbCCObjet = 0;
- Pixy_nbObjet = 0;
- FlagPixy = 1; // On valide le traitement de la trame précédente.
-
msgBuffer.tab[byteCount%2] = Pixy.getc(); // on stocke les octets reçus
byteCount++;
if (byteCount == 2) { // Quand on a 2 octets
if (msgBuffer.mot == 0xaa55) { // On doit impérativement trouver le code 0xAA55
- Pixy_state = begin; // Si c'est le cas, alors tout va bien et on va à l'aiguillage
-
+ PIXY_state = begin; // Si c'est le cas, alors tout va bien et on va à l'aiguillage
+ Pixy_NMObjet = PIXY_nbNMObjet; // On met à jour les variables pour le traitement
+ Pixy_CCObjet = PIXY_nbCCObjet;
+ PIXY_nbCCObjet = 0;
+ PIXY_nbNMObjet = 0;
+ FlagPixy = 1; // On valide le traitement de la trame précédente.
} else { // Si on trouve autre chose
- Pixy_synced = 0; // C'est qu'on est perdu donc plus synchronisé.
- Pixy_state = none; // Ceinture et bretelle
+ PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé.
+ PIXY_state = none; // Ceinture et bretelle
}
byteCount = 0;
}
@@ -403,7 +399,7 @@
int I2C_check = -1, BOUSSOLE_check = -1 /*, SPI2_check = -1, SPI3_check = -1, MOTG_check = -1, MOTD_check = -1*/;
- Byte PIXY_rCCObjet = 0, PIXY_rObjet = 0;
+ Byte PIXY_rCCObjet = 0, PIXY_rNMObjet = 0;
int PIXY_objet;
double SERVO_angle = 0, SERVO_angleMAX = 180, SERVO_angleMIN = 0;
@@ -414,11 +410,11 @@
char BOUSSOLE_status[1] = {0};
char I2C_registerValue[4];
+ double BOUSSOLE_periode;
double CAP_I2C, CAP_PWM;
double SD1_val, SD2_val, LD1_val, LD2_val, CNY1_val, CNY2_val, CNY3_val, Vbat_val;
double SD1_dist, SD2_dist, LD1_dist, LD2_dist;
- double periode;
times.reset();
times.start();
@@ -539,9 +535,9 @@
Pc.printf (" FAIL\n");
}
- periode = PWMB.period();
+ BOUSSOLE_periode = PWMB.period();
Pc.printf ("\rVerif de la PWM :");
- if ((periode > 0.11) || (periode < 0.06)) {
+ if ((BOUSSOLE_periode > 0.11) || (BOUSSOLE_periode < 0.06)) {
Pc.printf (" FAIL\n\n");
} else {
Pc.printf (" OK\n\n");
@@ -663,9 +659,6 @@
Led1 = !Led1;
FlagTickLed = 0;
}
-
-
-
} while(!Pc.readable());
MENU_choix = Pc.getc();
break;
@@ -725,29 +718,28 @@
do {
if (FlagPixy) {
- Led2 = 1;
if (FlagPixyOverflow) {
Pc.printf ("\rSome Data were lost\n");
FlagPixyOverflow = 0;
}
- for (PIXY_objet = 0; PIXY_objet < Pixy_saveNbObjet; PIXY_objet++) {
- Pc.printf ("\robj #%hd/%hd : sig = %hd : X=%5hd, Y=%5hd (W=%5hd, H=%5hd)\n", PIXY_objet+1, Pixy_saveNbObjet, Pixy_FIFO[PIXY_rObjet].bloc.signature, Pixy_FIFO[PIXY_rObjet].bloc.x, Pixy_FIFO[PIXY_rObjet].bloc.y, Pixy_FIFO[PIXY_rObjet].bloc.width, Pixy_FIFO[PIXY_rObjet].bloc.height);
+ for (PIXY_objet = 0; PIXY_objet < Pixy_NMObjet; PIXY_objet++) {
+ Pc.printf ("\rNMobj #%hd/%hd : sig = %hd : X=%5hd, Y=%5hd (W=%5hd, H=%5hd)\n", PIXY_objet+1, Pixy_NMObjet, Pixy_NMFIFO[PIXY_rNMObjet].NMbloc.signature, Pixy_NMFIFO[PIXY_rNMObjet].NMbloc.x, Pixy_NMFIFO[PIXY_rNMObjet].NMbloc.y, Pixy_NMFIFO[PIXY_rNMObjet].NMbloc.width, Pixy_NMFIFO[PIXY_rNMObjet].NMbloc.height);
- if (PIXY_rObjet<19) PIXY_rObjet++;
- else PIXY_rObjet = 0;
+ if (PIXY_rNMObjet<19) PIXY_rNMObjet++;
+ else PIXY_rNMObjet = 0;
}
- Pixy_saveNbObjet = 0;
+ Pixy_NMObjet = 0;
- for (PIXY_objet = 0; PIXY_objet < Pixy_saveNbCCObjet; PIXY_objet++) {
- Pc.printf ("\rCCobj #%hd/%hd : sig = %hd : X=%5hd, Y=%5hd (W=%5hd, H=%5hd)\n", PIXY_objet+1, Pixy_saveNbCCObjet, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.signature, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.x, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.y, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.width, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.height);
+ for (PIXY_objet = 0; PIXY_objet < Pixy_CCObjet; PIXY_objet++) {
+ Pc.printf ("\rCCobj #%hd/%hd : sig = %hd : X=%5hd, Y=%5hd (W=%5hd, H=%5hd)\n", PIXY_objet+1, Pixy_CCObjet, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.signature, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.x, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.y, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.width, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.height);
if (PIXY_rCCObjet<19) PIXY_rCCObjet++;
else PIXY_rCCObjet = 0;
}
- Pixy_saveNbCCObjet = 0;
+ Pixy_CCObjet = 0;
+ Pc.printf("\n\r");
FlagPixy = 0;
- Led2 = 0;
}
if (FlagTickLed) {