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:
- 2:1d440e938c44
- Parent:
- 1:d95546f84105
- Child:
- 3:3dfe26a10796
--- a/main.cpp Mon May 29 16:23:47 2017 +0000
+++ b/main.cpp Wed May 31 06:39:30 2017 +0000
@@ -11,7 +11,14 @@
#include "PwmIn.h"
#include "Nucleo_Encoder_16_bits.h"
-#define BOUSSOLE_adress 0xC0
+#define BOUSSOLE_adress 0xC0
+#define PIXY_adress 0x54
+
+#define CC_BLOCSIZE 14
+#define N_BLOCSIZE 12
+
+#define N_BLOCCODE 0xAA55
+#define CC_BLOCCODE 0xAA56
typedef unsigned char Byte;
typedef unsigned short Word;
@@ -19,10 +26,17 @@
typedef enum {S_monte = 1, S_descente = 0} T_SERVODIR;
+typedef enum {none, begin, normal, colorCode, doubleZero} T_pixyState;
+
typedef union {
- lWord header;
+ lWord mot;
Byte tab[4];
-} T_tmpbuffer;
+} T_tmpBuffer;
+
+typedef union {
+ Word mot;
+ Byte tab[2];
+} T_structBuffer;
typedef struct {
Word checksum;
@@ -31,16 +45,30 @@
Word y;
Word width;
Word height;
- Word angle; // angle is only available for color coded blocks
+ Word angle;
} T_pixyCCBloc;
+typedef struct {
+ Word checksum;
+ Word signature;
+ Word x;
+ Word y;
+ Word width;
+ Word height;
+} T_pixyBloc;
typedef union {
Byte tab[14];
T_pixyCCBloc CCbloc;
+} T_pixyCCData;
+
+typedef union {
+ Byte tab[12];
+ T_pixyBloc bloc;
} T_pixyData;
+
/** Liste des objets
*
* Serial #4 Pixy
@@ -167,8 +195,8 @@
Nucleo_Encoder_16_bits Gauche (TIM4); // A = PB_6, B = PB_7
Nucleo_Encoder_16_bits Droite (TIM3); // A = PB_4, B = PB_5
-Ticker timer;
-Timer temps;
+Ticker tick;
+Timer times;
/** Liste des variables globales
*
@@ -180,15 +208,18 @@
*/
// 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 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_pixyData Pixy_FIFO[20];
-Byte Pixy_nbObjet = 0, Pixy_wObjet = 0, Pixy_rObjet = 0;
+T_pixyCCData Pixy_CCFIFO[20];
+T_pixyData Pixy_FIFO[20];
+Byte Pixy_saveNbCCObjet;
+Byte Pixy_saveNbObjet;
// Gestion des capteurs Ultrason
long Echo1Start, Echo2Start, Echo3Start;
@@ -212,87 +243,176 @@
void echo1Rise ()
{
- Echo1Start = temps.read_us();
+ Echo1Start = times.read_us();
}
void echo2Rise ()
{
- Echo2Start = temps.read_us();
+ Echo2Start = times.read_us();
}
void echo3Rise ()
{
- Echo3Start = temps.read_us();
+ Echo3Start = times.read_us();
}
void echo1Fall ()
{
- DistUS1 = (double)(temps.read_us() - Echo1Start)/5.8;
+ DistUS1 = (double)(times.read_us() - Echo1Start)/5.8;
FlagUS1 = 1;
}
void echo2Fall ()
{
- DistUS2 = (double)(temps.read_us() - Echo2Start)/5.8;
+ DistUS2 = (double)(times.read_us() - Echo2Start)/5.8;
FlagUS2 = 1;
}
void echo3Fall ()
{
- DistUS3 = (double)(temps.read_us() - Echo3Start)/5.8;
+ DistUS3 = (double)(times.read_us() - Echo3Start)/5.8;
FlagUS3 = 1;
}
void getPixyByte ()
{
+ static T_tmpBuffer tmpBuffer;
+ static T_structBuffer msgBuffer;
+ static T_pixyState Pixy_state = none;
+ static Byte byteCount = 0;
+ static int Pixy_synced = 0;
+ static int dummy;
+ int i, somme;
+ static Byte Pixy_nbCCObjet = 0, Pixy_wCCObjet = 0;
+ static Byte Pixy_nbObjet = 0, Pixy_wObjet = 0;
- static T_tmpbuffer tmpBuffer;
- static Byte bytecount = 0;
- static int Pixy_synced = 0;
- int i;
+ Pixy_check = 0;
- Led2 = ! Led2;
- if (!Pixy_synced) { // On n'a pas trouvé le départ (0x55aa55aa ou 0x55aa56aa)
- 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
- bytecount++; // On passe à la case suivante du tableau temporaire
- } else { // Si on a 4 octets de données
- if (tmpBuffer.header == 0x55aa56aa) { // Si on a un Color Code Bloc
- if (Pixy_wObjet < 19) Pixy_wObjet++; // On incrémente le pointeur d'écriture dans la FIFO Objet
- else Pixy_wObjet = 0;
- Pixy_nbObjet++; // On dit que l'on a un objet de plus
- if (Pixy_nbObjet >= 20) FlagPixyOverflow = 1; // Si on a plus de 20 objets (en attente) => Overflow
- Pixy_synced = 1; // Et on peut dire que l'on a synchronisé la Pixy
- bytecount = 0;
- } else { // Si on n'a pas vu le header d'un Color Code Bloc
- for (i=1; i<4; i++) tmpBuffer.tab[i-1] = tmpBuffer.tab[i]; // On décalle les cases du tableau
+ 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
+ byteCount++; // On passe à la case suivante du tableau temporaire
+ } else { // Lorsqu'on a 4 octets
+ if (tmpBuffer.mot != 0xaa550000) { // Si le code n'est pas le code de START
+ 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;
+ byteCount = 0;
}
}
- } else { // La Pixy est synchronisée
- Pixy_FIFO[Pixy_wObjet].tab[bytecount] = Pixy.getc(); // On stocke les octets un à un dans la structure CCBloc
- if (bytecount < 14) bytecount++; // tant que la structure n'est pas pleine
- else { // Quand elle est pleine
- bytecount = 0; // On réinitialise
- Pixy_synced = 0;
- FlagPixy = 1; // Et on signale au main qu'un objet est pret à être analysé.
+ }
+
+ if (Pixy_synced) {
+
+ switch (Pixy_state) {
+ case begin : // l'aiguillage est là !
+
+ 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) { // Si c'est un bloc normal (code 0xAA55)
+ 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
+ }
+
+ if (msgBuffer.mot == 0) { // Si on a un debut de trame (code 0000)
+ 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
+ }
+ byteCount = 0;
+ }
+ break;
+
+ 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
+ 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
+ // 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;
+
+ 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
+ else FlagPixyOverflow = 1; // Si on a plus de 20 CC objets (en attente) => Overflow
+ }
+ }
+ break;
+
+ 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
+ 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
+ // 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;
+
+ 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
+ }
+ }
+ break;
+
+ 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
+
+ } else { // Si on trouve autre chose
+ Pixy_synced = 0; // C'est qu'on est perdu donc plus synchronisé.
+ Pixy_state = none; // Ceinture et bretelle
+ }
+ byteCount = 0;
+ }
+ break;
}
}
}
+
int main()
{
- int I2C_check = -1, BOUSSOLE_check = -1 /*, SPI2_check = -1, SPI3_check = -1, PIXY_check = -1, MOTG_check = -1, MOTD_check = -1*/;
- int phase = 0;
+ 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;
+ int PIXY_objet;
double SERVO_angle = 0, SERVO_angleMAX = 180, SERVO_angleMIN = 0;
Word SERVO_pos;
T_SERVODIR SERVO_dir = S_monte;
- Byte PIXY_red = 0, PIXY_green = 0, PIXY_blue = 0;
char MENU_choix = 0;
- char BOUSSOLE_status[1] = {0}/*, BOUSSOLE_bearingWord[1] = {2}*/;
+ char BOUSSOLE_status[1] = {0};
char I2C_registerValue[4];
double CAP_I2C, CAP_PWM;
@@ -300,11 +420,10 @@
double SD1_dist, SD2_dist, LD1_dist, LD2_dist;
double periode;
- temps.reset();
- temps.start();
+ times.reset();
+ times.start();
-
- timer.attach(&tickTime, 0.001);
+ tick.attach(&tickTime, 0.001);
Bus_I2C.frequency (100000);
@@ -441,12 +560,12 @@
CAP_PWM = ((PWMB.pulsewidth()*1000)-1)*10;
Pc.printf ("\r PWM = %4.1lf", CAP_PWM);
}
- //if (I2C_check==0) {
+ if (I2C_check==0) {
Bus_I2C.write(BOUSSOLE_adress,BOUSSOLE_status, 1, true);
Bus_I2C.read (BOUSSOLE_adress,I2C_registerValue,4);
CAP_I2C = (double)(((unsigned short)I2C_registerValue[2]<<8)+(unsigned short)I2C_registerValue[3])/10.0;
Pc.printf ("\r\t\t I2C = %4.1lf", CAP_I2C);
- //}
+ }
}
} while(!Pc.readable());
MENU_choix = Pc.getc();
@@ -544,8 +663,8 @@
Led1 = !Led1;
FlagTickLed = 0;
}
-
-
+
+
} while(!Pc.readable());
MENU_choix = Pc.getc();
@@ -600,86 +719,35 @@
case 9 :
Pc.printf ("\n\n\rPixy\n");
Pc.printf ("\rAppuyez sur Entree pour quitter\n");
+
+ if (Pixy_check == 0) Pc.printf ("\n\rPIXY is ALIVE\n");
+ else Pc.printf ("\n\rPIXY don't talk\n");
+
do {
- if (((Tick%50)==0) && FlagTick) {
- FlagTick = 0;
- switch (phase) {
- case 0 :
- PIXY_red += 5;
- if (PIXY_red == 255) {
- phase = 1;
- //Pc.printf ("\rPhase 1");
- }
- break;
- case 1 :
- PIXY_green += 5;
- if (PIXY_green == 255) {
- phase = 2;
- //Pc.printf ("\rPhase 2");
- }
- break;
- case 2 :
- PIXY_red -= 5;
- if (PIXY_red == 0) {
- phase = 3;
- //Pc.printf ("\rPhase 3");
- }
- break;
- case 3 :
- PIXY_blue += 5;
- if (PIXY_blue == 255) {
- phase = 4;
- //Pc.printf ("\rPhase 4");
- }
- break;
- case 4 :
- PIXY_green -= 5;
- if (PIXY_green == 0) {
- phase = 5;
- //Pc.printf ("\rPhase 5");
- }
- break;
- case 5 :
- PIXY_red += 5;
- if (PIXY_red == 255) {
- phase = 6;
- //Pc.printf ("\rPhase 6");
- }
- break;
- case 6 :
- PIXY_green += 5;
- if (PIXY_green == 255) {
- phase = 7;
- //Pc.printf ("\rPhase 7");
- }
- break;
- case 7 :
- PIXY_red -= 5;
- PIXY_green -= 5;
- PIXY_blue -=5;
- if (PIXY_red == 0) {
- phase = 0;
- //Pc.printf ("\rPhase 0");
- }
- break;
+ if (FlagPixy) {
+ Led2 = 1;
+
+ if (FlagPixyOverflow) {
+ Pc.printf ("\rSome Data were lost\n");
+ FlagPixyOverflow = 0;
}
- Pixy.putc(0);
- Pixy.putc(0xFD);
- Pixy.putc(PIXY_red);
- Pixy.putc(PIXY_green);
- Pixy.putc(PIXY_blue);
+ 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);
+
+ if (PIXY_rObjet<19) PIXY_rObjet++;
+ else PIXY_rObjet = 0;
+ }
+ Pixy_saveNbObjet = 0;
- while (Pixy_nbObjet) {
- Pc.printf ("\r%5hd = %5hd,%5hd : %5hdx%5hd", Pixy_FIFO[Pixy_rObjet].CCbloc.signature, Pixy_FIFO[Pixy_rObjet].CCbloc.x, Pixy_FIFO[Pixy_rObjet].CCbloc.y, Pixy_FIFO[Pixy_rObjet].CCbloc.width, Pixy_FIFO[Pixy_rObjet].CCbloc.height);
-
- if (Pixy_nbObjet>1) Pc.printf("\n");
-
- if (Pixy_rObjet<19) Pixy_rObjet++;
- else Pixy_rObjet = 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);
- Pixy_nbObjet--;
- }
+ if (PIXY_rCCObjet<19) PIXY_rCCObjet++;
+ else PIXY_rCCObjet = 0;
+ }
+ Pixy_saveNbCCObjet = 0;
FlagPixy = 0;
+ Led2 = 0;
}
if (FlagTickLed) {