Actionneurs

Partie Robotech hors-projet

chef/Makefile

@@ -11,7 +11,7 @@ CFLAGS_CUSTOM += -g
 ## Générateurs de drapeaux pour les bibliothèques
 PKG_CONFIG=pkg-config
 ## Nom des objets communs
-OBJS=buttons CF debug diagnostics i2c imu ihm lcd motor movement parcours points position
+OBJS=actionneurs buttons CA CF debug diagnostics i2c imu ihm lcd motor movement parcours points position
 OBJS_O=$(addprefix obj/,$(addsuffix .o,$(OBJS)))
 
 # VARIABLES AUTOMATIQUES

chef/src/CA.c

@@ -0,0 +1,232 @@
+#include "CA.h"
+#include <fcntl.h>   // O_*
+#include <stdio.h>   // printf...
+#include <stdlib.h>  // stuff
+#include <string.h>  // memcpy
+#include <strings.h> // bzero
+#include <unistd.h>  // read(), write()...
+
+int arduino;
+pthread_mutex_t sSendCA;
+pthread_t tReaderAC;
+
+rxHandler rxHandlersAC[256];
+bool pret;
+
+
+void printDataCA(void* data, size_t size)
+{
+    printf(" ");
+    unsigned char* p = data;
+    for (size_t i = 0; i < size; i++) {
+        if (*p >= ' ' && *p <= '~') {
+            printf("  %c", *p);
+        } else {
+            printf(" %02x", *p);
+        }
+        p++;
+    }
+    printf("\n");
+}
+
+void configureArduino()
+{
+    // Connection au port série
+    printf("Connexion à %s... ", ARDUINO_PORTNAME);
+    fflush(stdout);
+    arduino = open(ARDUINO_PORTNAME, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
+    if (arduino < 0) {
+        printf("Échec !\n");
+        exit(1);
+    }
+
+    // Configuration du port série
+    fcntl(arduino, F_SETFL, O_RDWR);
+
+    struct termios cfg;
+
+    tcgetattr(arduino, &cfg);
+
+    cfmakeraw(&cfg);
+    cfsetispeed(&cfg, CA_BAUDRATE);
+    cfsetospeed(&cfg, CA_BAUDRATE);
+
+    cfg.c_cflag |= (CLOCAL | CREAD);
+    cfg.c_cflag &= ~PARENB;
+    cfg.c_cflag &= ~CSTOPB;
+    cfg.c_cflag &= ~CSIZE;
+    cfg.c_cflag |= CS8;
+    cfg.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+    cfg.c_oflag &= ~OPOST;
+
+    cfg.c_cc[VMIN] = 0;
+    cfg.c_cc[VTIME] = 10;
+
+    if (tcsetattr(arduino, TCSANOW, &cfg) < 0) {
+        perror("serialConfig.tcsetattr");
+        exit(1);
+    }
+
+    int status;
+    ioctl(arduino, TIOCMGET, &status);
+
+    status |= TIOCM_DTR;
+    status |= TIOCM_RTS;
+
+    ioctl(arduino, TIOCMSET, &status);
+
+    usleep(10 * 1000);
+
+    // Flush
+    unsigned char trash[1024];
+    read(arduino, &trash, sizeof(trash));
+
+    printf("OK!\n");
+}
+
+void deconfigureArduino()
+{
+    close(arduino);
+    printf("Déconnecté\n");
+}
+
+void registerRxHandlerCA(unsigned char code, rxHandler handler)
+{
+    rxHandlersAC[code] = handler;
+}
+
+void* TaskReaderAC(void* pdata)
+{
+    (void)pdata;
+    unsigned char code;
+    for (;;) {
+        code = readByteCA();
+
+#ifdef PRINTRAWDATA
+        printf("↓");
+        printDataCA(&code, sizeof(code));
+#endif
+        rxHandler handler = rxHandlersAC[code];
+        if (handler != NULL) {
+            handler();
+        } else {
+            printf("Code inconnu: %x (%c)\n", code, code);
+        }
+    }
+    return NULL;
+}
+
+void onA2CD_ERR()
+{
+    struct A2CD_ERRs s;
+    readCA(&s, sizeof(struct A2CD_ERRs));
+    printf("Erreur reçue : %c (%2x)\n", s.code, s.code);
+}
+
+void setPretCA()
+{
+    pret = true;
+}
+
+void doNothingCA()
+{
+}
+
+void configureCA()
+{
+    configureArduino();
+    for (int i = 0; i < 256; i++) {
+        rxHandlersAC[i] = NULL;
+    }
+
+    pthread_mutex_init(&sSendCA, NULL);
+    pthread_create(&tReaderAC, NULL, TaskReaderAC, NULL);
+
+    printf("Attente de réponse du Arduino... ");
+    fflush(stdout);
+    // Dans le cas où on aurait laissé l'Arduino en attente de donnée,
+    // on envoie des pings en boucle jusqu'à ce qu'il nous réponde.
+    pret = false;
+    registerRxHandlerCA(C2AD_PING, setPretCA);
+    while (!pret) {
+        sendCA(C2AD_PING, NULL, 0);
+        usleep(100 * 1000);
+    }
+    registerRxHandlerCA(C2AD_PING, doNothingCA); // TODO
+    registerRxHandlerCA(C2AD_PING, NULL);
+    printf("OK !\n");
+
+    registerRxHandlerCA(A2CD_ERR, onA2CD_ERR);
+}
+
+void deconfigureCA()
+{
+    deconfigureArduino();
+}
+
+void sendByteCA(unsigned char data)
+{
+    write(arduino, &data, sizeof(data));
+
+#ifdef PRINTRAWDATA
+    printf("↑");
+    printDataCA(&data, sizeof(data));
+#endif
+}
+
+void sendCA(unsigned char code, void* data, size_t size)
+{
+    pthread_mutex_lock(&sSendCA);
+
+    sendByteCA(code);
+    if (size > 0) {
+        unsigned char* p = data;
+        for (size_t i = 0; i < size; i++) {
+            write(arduino, p, sizeof(unsigned char));
+            p++;
+        }
+        // Envoyer plus d'un octet d'un coup curieusement il aime pas ça du tout
+    }
+    pthread_mutex_unlock(&sSendCA);
+
+#ifdef PRINTRAWDATA
+    if (size > 0) {
+        printf("↑");
+        printDataCA(data, size);
+    }
+#endif
+}
+
+unsigned char readByteCA()
+{
+    unsigned char c;
+
+    while (read(arduino, &c, sizeof(c)) < 1) {
+        sleep(0);
+    }
+    return c;
+
+#ifdef PRINTRAWDATA
+    printf("↓");
+    printDataCA(&c, sizeof(c));
+#endif
+}
+
+void readCA(void* data, size_t size)
+{
+    size_t remaining = size;
+    int justRead;
+    char* p = data;
+    do {
+        justRead = read(arduino, p, remaining);
+        if (justRead > 0) {
+            p += justRead;
+            remaining -= justRead;
+        }
+    } while (remaining > 0);
+
+#ifdef PRINTRAWDATA
+    printf("↓");
+    printDataCA(data, size);
+#endif
+}

chef/src/CA.h

@@ -0,0 +1,25 @@
+#ifndef __CA_H_
+#define __CA_H_
+
+#include <sys/ioctl.h>
+#include <pthread.h>
+#include <termios.h> // baudrates
+#include <stdbool.h>
+
+#include "CAsignals.h"
+
+#define ARDUINO_PORTNAME "/dev/ttyACM0"
+#define CA_BAUDRATE B9600
+// #define PRINTRAWDATA
+
+typedef void (*rxHandler)(void);
+
+void registerRxHandlerCA(unsigned char code, rxHandler handler); // À utiliser après configureCA();
+void sendByteCA(unsigned char data); // Privé
+void sendCA(unsigned char code, void* data, size_t size);
+unsigned char readByteCA(); // À utiliser uniquement depuis un rxHandler
+void readCA(void* data, size_t size); // À utiliser uniquement depuis un rxHandler
+void configureCA();
+void deconfigureCA();
+
+#endif

chef/src/CAsignals.h

@@ -0,0 +1,60 @@
+/*
+ * Définition des signaux échagés entre l'Arduino et le chef
+ */
+
+#ifndef __CASIGNALS_H_
+#define __CASIGNALS_H_
+
+#include <stdint.h>
+
+// Structures used everywhere
+
+// D: Direct
+// Sens naturel : Envoi de donnée
+// Sens inverse : Accusé de réception
+
+// I: Indirect
+// Sens inverse : Demande de donnée
+// Sens naturel : Envoi de donnée
+
+// T: Trigger
+// Sens inverse : Paramètrage du trigger
+// Sens naturel : Envoi de trigger
+
+// Raspberry Pi → Arduino
+
+// Pour le debug
+#define C2AD_PING 'P'
+
+#define C2AD_OUVRE_LOQUET 'L'
+
+#define C2AD_FERME_LOQUET 'F'
+
+#define C2AD_POS_BALLE_ATTENTE 'A'
+
+#define C2AD_POS_BALLE_EVACUATION 'V'
+
+#define C2AD_POS_BALLE_EJECTION 'J'
+
+#define C2AD_POUSSER_BALLE 'O'
+
+#define C2AD_BARILLET_SUIVANT 'B'
+
+#define C2AD_BARILLET_SKIP 'H'
+
+#define C2AD_BARILLET_RESET 'R'
+
+#define C2AD_PROPULSION_ON 'T'
+
+#define C2AD_PROPULSION_OFF 'U'
+
+// Arduino → Raspberry Pi
+
+#define A2CD_ERR 'E'
+
+#define A2CD_ERR 'E'
+struct __attribute__ ((packed)) A2CD_ERRs {
+    unsigned char code;
+};
+
+#endif

chef/src/CF.c

@@ -6,7 +6,15 @@
 #include <strings.h> // bzero
 #include <unistd.h>  // read(), write()...
 
-void printData(void* data, size_t size)
+int fpga;
+pthread_mutex_t sSendCF;
+pthread_t tReaderAF;
+
+rxHandler rxHandlersAF[256];
+bool pret;
+
+
+void printDataCF(void* data, size_t size)
 {
     printf(" ");
     unsigned char* p = data;
@@ -82,12 +90,12 @@ void deconfigureFpga()
     printf("Déconnecté\n");
 }
 
-void registerRxHandler(unsigned char code, rxHandler handler)
+void registerRxHandlerCF(unsigned char code, rxHandler handler)
 {
-    rxHandlersAC[code] = handler;
+    rxHandlersAF[code] = handler;
 }
 
-void* TaskReaderAC(void* pdata)
+void* TaskReaderAF(void* pdata)
 {
     (void)pdata;
     unsigned char code;
@@ -96,9 +104,9 @@ void* TaskReaderAC(void* pdata)
 
 #ifdef PRINTRAWDATA
         printf("↓");
-        printData(&code, sizeof(code));
+        printDataCF(&code, sizeof(code));
 #endif
-        rxHandler handler = rxHandlersAC[code];
+        rxHandler handler = rxHandlersAF[code];
         if (handler != NULL) {
             handler();
         } else {
@@ -115,12 +123,12 @@ void onF2CD_ERR()
     printf("Erreur reçue : %c (%2x)\n", s.code, s.code);
 }
 
-void setPret()
+void setPretCF()
 {
     pret = true;
 }
 
-void doNothing()
+void doNothingCF()
 {
 }
 
@@ -128,27 +136,27 @@ void configureCF()
 {
     configureFpga();
     for (int i = 0; i < 256; i++) {
-        rxHandlersAC[i] = NULL;
+        rxHandlersAF[i] = NULL;
     }
 
     pthread_mutex_init(&sSendCF, NULL);
-    pthread_create(&tReaderAC, NULL, TaskReaderAC, NULL);
+    pthread_create(&tReaderAF, NULL, TaskReaderAF, NULL);
 
     printf("Attente de réponse du Fpga... ");
     fflush(stdout);
     // Dans le cas où on aurait laissé l'Fpga en attente de donnée,
     // on envoie des pings en boucle jusqu'à ce qu'il nous réponde.
     pret = false;
-    registerRxHandler(C2FD_PING, setPret);
+    registerRxHandlerCF(C2FD_PING, setPretCF);
     while (!pret) {
         sendCF(C2FD_PING, NULL, 0);
         usleep(100 * 1000);
     }
-    registerRxHandler(C2FD_PING, doNothing); // TODO
-    registerRxHandler(C2FD_PING, NULL);
+    registerRxHandlerCF(C2FD_PING, doNothingCF); // TODO
+    registerRxHandlerCF(C2FD_PING, NULL);
     printf("OK !\n");
 
-    registerRxHandler(F2CD_ERR, onF2CD_ERR);
+    registerRxHandlerCF(F2CD_ERR, onF2CD_ERR);
 }
 
 void deconfigureCF()
@@ -162,7 +170,7 @@ void sendByteCF(unsigned char data)
 
 #ifdef PRINTRAWDATA
     printf("↑");
-    printData(&data, sizeof(data));
+    printDataCF(&data, sizeof(data));
 #endif
 }
 
@@ -184,7 +192,7 @@ void sendCF(unsigned char code, void* data, size_t size)
 #ifdef PRINTRAWDATA
     if (size > 0) {
         printf("↑");
-        printData(data, size);
+        printDataCF(data, size);
     }
 #endif
 }
@@ -200,7 +208,7 @@ unsigned char readByteCF()
 
 #ifdef PRINTRAWDATA
     printf("↓");
-    printData(&c, sizeof(c));
+    printDataCF(&c, sizeof(c));
 #endif
 }
 
@@ -219,6 +227,6 @@ void readCF(void* data, size_t size)
 
 #ifdef PRINTRAWDATA
     printf("↓");
-    printData(data, size);
+    printDataCF(data, size);
 #endif
 }

chef/src/CF.h

@@ -1,5 +1,5 @@
-#ifndef __AC_H_
-#define __AC_H_
+#ifndef __CF_H_
+#define __CF_H_
 
 #include <sys/ioctl.h>
 #include <pthread.h>
@@ -11,16 +11,10 @@
 #define FPGA_PORTNAME "/dev/ttyUSB0"
 #define CF_BAUDRATE B115200
 // #define PRINTRAWDATA
-
-int fpga;
-pthread_mutex_t sSendCF;
-pthread_t tReaderAC;
-
+//
 typedef void (*rxHandler)(void);
-rxHandler rxHandlersAC[256];
-bool pret;
 
-void registerRxHandler(unsigned char code, rxHandler handler); // À utiliser après configureCF();
+void registerRxHandlerCF(unsigned char code, rxHandler handler); // À utiliser après configureCF();
 void sendByteCF(unsigned char data); // Privé
 void sendCF(unsigned char code, void* data, size_t size);
 unsigned char readByteCF(); // À utiliser uniquement depuis un rxHandler

chef/src/actionneurs.c

@@ -0,0 +1,97 @@
+// Robotech Lille 2017-2018
+
+#include "actionneurs.h"
+#include "CA.h"
+
+pthread_mutex_t receptionActionMutex;
+pthread_cond_t receptionActionCond;
+
+// On fait un truc de malpropre : étant donné que l'Arduino
+// peut faire qu'une seule tache à la fois on suppose que le
+// caractère reçu répond à celui qu'on attendait
+
+void configureActionneurs()
+{
+    pthread_mutex_init(&receptionActionMutex, NULL);
+    pthread_cond_init(&receptionActionCond, NULL);
+}
+
+void setLoquet(bool state)
+{
+    attendAction(state ? C2AD_OUVRE_LOQUET : C2AD_FERME_LOQUET);
+}
+
+void setPositionBalle(enum positionBalle pos)
+{
+    unsigned char code;
+    switch (pos) {
+        case attente:
+            code = C2AD_POS_BALLE_ATTENTE;
+            break;
+        case ejection:
+            code = C2AD_POS_BALLE_EJECTION;
+            break;
+        case evacuation:
+            code = C2AD_POS_BALLE_EVACUATION;
+            break;
+    }
+    attendAction(code);
+}
+
+void pousserBalle()
+{
+    attendAction(C2AD_POUSSER_BALLE);
+}
+
+void barilletSuivant()
+{
+    attendAction(C2AD_BARILLET_SUIVANT);
+}
+
+void barilletSkip()
+{
+    attendAction(C2AD_BARILLET_SKIP);
+}
+
+void barilletReset()
+{
+    attendAction(C2AD_BARILLET_RESET);
+}
+
+void setPropulsion(bool state)
+{
+    attendAction(state ? C2AD_PROPULSION_ON : C2AD_PROPULSION_OFF);
+}
+
+
+void resetActionneurs()
+{
+    setPropulsion(false);
+    barilletReset();
+}
+
+void stopActionneurs()
+{
+    setPropulsion(false);
+}
+
+void receptionAction()
+{
+    pthread_mutex_lock(&receptionActionMutex);
+    pthread_cond_signal(&receptionActionCond);
+    pthread_mutex_unlock(&receptionActionMutex);
+}
+
+void attendAction(unsigned char code)
+{
+    pthread_mutex_lock(&receptionActionMutex);
+    registerRxHandlerCA(code, receptionAction);
+    sendCA(code, NULL, 0);
+    pthread_cond_wait(&receptionActionCond, &receptionActionMutex);
+    pthread_mutex_unlock(&receptionActionMutex);
+}
+
+void deconfigureActionneurs()
+{
+    stopActionneurs();
+}

chef/src/actionneurs.h

@@ -0,0 +1,38 @@
+// Robotech Lille 2017-2018
+
+#ifndef __ACTIONNEURS_H_
+#define __ACTIONNEURS_H_
+
+#include <stdbool.h>
+
+enum positionBalle {
+    attente,
+    evacuation,
+    ejection
+};
+
+// Public
+
+// Specific
+
+// True: Ouvert, False: fermé
+void setLoquet(bool state);
+void barilletReset();
+void barilletSuivant();
+void barilletSkip();
+void pousserBalle();
+void setPositionBalle(enum positionBalle pos);
+void setPropulsion(bool state);
+
+// Common
+
+void configureActionneurs();
+void resetActionneurs();
+void stopActionneurs();
+void deconfigureActionneurs();
+
+// Private
+
+void attendAction(unsigned char code);
+
+#endif

chef/src/diagnostics.c

@@ -1,3 +1,4 @@
+#include <stdbool.h>
 #include <stdio.h>
 #include <unistd.h>
 
@@ -5,7 +6,9 @@
 #include "diagnostics.h"
 #include "lcd.h"
 
+#include "CA.h"
 #include "CF.h"
+#include "actionneurs.h"
 #include "imu.h"
 #include "motor.h"
 #include "position.h"
@@ -38,7 +41,19 @@ bool diagFPGA(void* arg)
 bool diagArduino(void* arg)
 {
     (void)arg;
-    return false;
+
+    recu = false;
+    registerRxHandlerCA(C2AD_PING, setRecu);
+    sendCA(C2AD_PING, NULL, 0);
+
+    for (int i = 0; i <= DIAGNOSTIC_SERIAL_TIMEOUT; i += DIAGNOSTIC_POLL_INTERVAL) {
+        if (recu) {
+            break;
+        }
+        usleep(DIAGNOSTIC_POLL_INTERVAL * 1000);
+    }
+    registerRxHandlerCA(C2AD_PING, NULL);
+    return recu;
 }
 
 bool diagCodeuse(void* arg)
@@ -78,12 +93,48 @@ bool diagIMU(void* arg)
     return connectedIMU();
 }
 
-void execDiagnostic(char* name, bool (*diagnostic)(void* arg), void* arg)
+bool diagJustRun(void* arg)
+{
+    void (*fonction)(void) = arg;
+    fonction();
+    return true;
+}
+
+void diagSetLoquetOuvert()
+{
+    setLoquet(true);
+}
+void diagSetLoquetFerme()
+{
+    setLoquet(false);
+}
+void diagSetPositionBalleAttente()
+{
+    setPositionBalle(attente);
+}
+void diagSetPositionBalleEjection()
+{
+    setPositionBalle(ejection);
+}
+void diagSetPositionBalleEvacuation()
+{
+    setPositionBalle(evacuation);
+}
+void diagSetPropulsionOn()
+{
+    setPropulsion(true);
+}
+void diagSetPropulsionOff()
+{
+    setPropulsion(false);
+}
+
+void execDiagnostic(char* name, diagnosticFunc diag, void* arg)
 {
     clearLCD();
     printToLCD(LCD_LINE_1, name);
     printToLCD(LCD_LINE_2, "...");
-    bool res = diagnostic(arg);
+    bool res = diag(arg);
     if (res) {
         printToLCD(LCD_LINE_2, "Ok!");
         usleep(DIAGNOSTIC_INTERVAL * 1000);
@@ -96,7 +147,7 @@ void execDiagnostic(char* name, bool (*diagnostic)(void* arg), void* arg)
 void runDiagnostics()
 {
     execDiagnostic("Lien FPGA", diagFPGA, NULL);
-    /* execDiagnostic("Lien Arduino", diagArduino); */
+    execDiagnostic("Lien Arduino", diagArduino, NULL);
     execDiagnostic("Lien IMU", diagIMU, NULL);
     int i;
     i = 0;
@@ -107,4 +158,16 @@ void runDiagnostics()
     execDiagnostic("Mot+Cod R AV", diagCodeuse, &i);
     i = 3;
     execDiagnostic("Mot+Cod R AR", diagCodeuse, &i);
+
+    execDiagnostic("Ouverture loquet", diagJustRun, &diagSetLoquetOuvert);
+    execDiagnostic("Fermeture loquet", diagJustRun, &diagSetLoquetFerme);
+    execDiagnostic("Reset barillet", diagJustRun, &barilletReset);
+    execDiagnostic("T+1 barillet", diagJustRun, &barilletSuivant);
+    execDiagnostic("T+2 barillet", diagJustRun, &barilletSkip);
+    execDiagnostic("Pousser balle", diagJustRun, &pousserBalle);
+    execDiagnostic("Pos. attente", diagJustRun, &diagSetPositionBalleAttente);
+    execDiagnostic("Pos. ejection", diagJustRun, &diagSetPositionBalleEjection);
+    execDiagnostic("Pos. evacuation", diagJustRun, &diagSetPositionBalleEvacuation);
+    execDiagnostic("Propulsion off", diagJustRun, &diagSetPropulsionOn);
+    execDiagnostic("Propulsion on", diagJustRun, &diagSetPropulsionOff);
 }

chef/src/diagnostics.h

@@ -12,10 +12,12 @@
 #define DIAGNOSTIC_CODEUSES_DIFF_MIN 100
 #define DIAGNOSTIC_TEMPS_ROTATION 250
 
+typedef bool (*diagnosticFunc)(void* arg);
+
 // Public
 void runDiagnostics();
 
 // Private
-void execDiagnostic(char *name, bool (*diagnostic)(void* arg), void* arg);
+void execDiagnostic(char* name, diagnosticFunc diag, void* arg);
 
 #endif

chef/src/motor.c

@@ -1,4 +1,5 @@
 #include "motor.h"
+#include "actionneurs.h"
 
 uint8_t tensionToPWM(float V)
 {
@@ -96,5 +97,5 @@ int freewheel()
 int stop()
 {
     brake();
-    // TODO Actionneurs
+    stopActionneurs();
 }

chef/src/premier.c

@@ -6,6 +6,7 @@
 #include <unistd.h> // sleep
 #include <wiringPi.h>
 
+#include "CA.h"
 #include "CF.h"
 #include "debug.h"
 #include "i2c.h"
@@ -35,9 +36,11 @@ int main()
     configureDebug();
     configureIHM();
     configureCF();
+    configureCA();
     configureIMU();
     configurePosition();
     configureMovement();
+    configureActionneurs();
     startDebug();
     startIHM();
 
@@ -49,9 +52,11 @@ int main()
     pthread_mutex_lock(&sRunning);
     pthread_mutex_lock(&sRunning);
 
+    deconfigureActionneurs();
     deconfigureMovement();
     deconfigurePosition();
     deconfigureIMU();
+    deconfigureCA();
     deconfigureCF();
     deconfigureIHM();
     deconfigureDebug();