Jonas DOREL
Automate
Dependencies: Encoder_Nucleo_16_bits PwmIn mbed
Fork of
TestBoard
by 
IUT DE CACHAN GE1
Diff: function_library.h
- Revision:
- 8:ad8b64ca548d
diff -r 075ba099309b -r ad8b64ca548d function_library.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/function_library.h Fri Jun 09 17:06:05 2017 +0000
@@ -0,0 +1,275 @@
+//Variables
+
+Serial Pixy (PA_0, PA_1, 230400);
+Serial Pc (PA_2, PA_3, 921600);
+
+AnalogIn CNY1 (PC_4);
+AnalogIn CNY2 (PA_7);
+AnalogIn CNY3 (PA_6);
+AnalogIn LD1 (PC_0);
+AnalogIn LD2 (PC_1);
+AnalogIn SD1 (PB_0);
+AnalogIn SD2 (PA_4);
+AnalogIn Vbat (PC_5);
+
+DigitalOut Led1 (PA_5);
+DigitalOut Led2 (PD_2);
+DigitalOut Trig1 (PC_3);
+DigitalOut Trig2 (PC_2);
+DigitalOut Trig3 (PA_9);
+DigitalOut En (PC_9);
+DigitalOut SensG (PC_8);
+DigitalOut SensD (PC_6);
+DigitalOut SS (PA_12);
+
+InterruptIn Echo1 (PA_10);
+InterruptIn Echo2 (PA_11);
+InterruptIn Echo3 (PB_12);
+InterruptIn BP (PC_13);
+InterruptIn IG (PC_7);
+InterruptIn PWMG (PB_2);
+InterruptIn PWMD (PB_1);
+
+PwmIn PWMB (PA_15);
+
+PwmOut Pwm_MG (PB_10);
+PwmOut Pwm_MD (PB_3);
+PwmOut Servo (PA_8);
+
+I2C Bus_I2C (PB_9, PB_8);
+
+SPI SPIG (PC_12, PC_11, PC_10);
+SPI SPID (PB_15, PB_14, PB_13);
+
+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 tick;
+Timer times;
+
+/** Liste des variables globales
+ *
+ * Tick -> (long) Compte le nombre de MS écoulé et déclenche l'exécution de la boucle en fonction du temps écoulé.
+ * FlagIG -> (int) Indication de la présence de fronts sur l'index de l'encodeur de la roue gauche
+ * FlagID -> (int) Indication de la présence de fronts sur l'index de l'encodeur de la roue droite
+ * EchoXStart -> (long) Donne le temps en µs de début de l'impulsion d'écho de l'US n°X
+ * DistUSX -> (float) Donne la distance en mm mesurée par l'US n°X
+ */
+
+// Structure de temps
+lWord Tick = 0, Gperiod, Dperiod;
+
+// Sémaphore d'interruption
+int FlagUS1 = 0, FlagUS2 = 0, FlagUS3 = 0, FlagPixy = 0, FlagPixyOverflow = 0;
+int FlagTick = 0, FlagTickLed = 0, BPPressed = 0, nbTurnG = 0, nbTurnD = 0;
+int Pixy_check = -1;
+
+// Dialogue avec la Pixy
+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;
+
+/** Liste des interruptions
+ *
+ */
+
+void tickTime() {
+ Tick++;
+ FlagTick = 1;
+ if ((Tick%100)==0) FlagTickLed = 1;
+}
+
+void BPevent () {
+ BPPressed = 1;
+}
+
+void PWM_motGRise (void) {
+ static lWord oldTime;
+ lWord now;
+ now = times.read_us();
+ Gperiod = now-oldTime;
+ oldTime = now;
+ nbTurnG++;
+}
+
+void PWM_motDRise (void) {
+ static lWord oldTime;
+ lWord now;
+ now = times.read_us();
+ Dperiod = now-oldTime;
+ oldTime = now;
+ nbTurnD++;
+}
+
+void echo1Rise () {
+ Echo1Start = times.read_us();
+}
+
+void echo2Rise () {
+ Echo2Start = times.read_us();
+}
+
+void echo3Rise () {
+ Echo3Start = times.read_us();
+}
+
+void echo1Fall () {
+ DistUS1 = (double)(times.read_us() - Echo1Start)/5.8;
+ FlagUS1 = 1;
+}
+
+void echo2Fall () {
+ DistUS2 = (double)(times.read_us() - Echo2Start)/5.8;
+ FlagUS2 = 1;
+}
+
+void echo3Fall () {
+ 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, dummy;
+ int i, somme;
+ 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)
+ 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;
+ }
+ }
+ }
+
+ 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_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
+ // On calcule la somme de contrôle
+ 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_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
+ }
+ }
+ 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.
+
+ 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_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
+ }
+ byteCount = 0;
+ }
+ break;
+ }
+ }
+}
+
+void ir_read(double SD2,double LD2,double *SD2_dist,double *LD2_dist) {
+ double SD2_val, LD2_val;
+
+ SD2_val = SD2;
+ LD2_val = LD2;
+
+ if (SD2_val < 0.06) {
+ SD2_val = 0;
+ *SD2_dist = 40;
+ } else {
+ *SD2_dist = 11.611/(SD2_val*3.3-0.0237);
+ if (*SD2_dist > 40) *SD2_dist = 40;
+ }
+
+ if (LD2_val < 0.1) {
+ LD2_val = 0;
+ *LD2_dist = 150;
+ } else {
+ *LD2_dist = 59.175/(LD2_val*3.3-0.0275);
+ if (*LD2_dist > 150) *LD2_dist = 150;
+ }
+}
\ No newline at end of file