diff --git a/.gitmodules b/.gitmodules index 44d5887..d23d172 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,12 +1,6 @@ [submodule "root/buildroot"] path = root/buildroot url = git://git.buildroot.net/buildroot -[submodule "ardPince/Arduino-Makefile"] - path = ardPince/Arduino-Makefile - url = https://github.com/sudar/Arduino-Makefile -[submodule "ardMoteur/Arduino-Makefile"] - path = ardMoteur/Arduino-Makefile - url = https://github.com/sudar/Arduino-Makefile [submodule "fpga/uart"] path = fpga/uart url = https://github.com/pabennett/uart.git diff --git a/chef/Makefile b/chef/Makefile index 12c05e2..af9aa89 100644 --- a/chef/Makefile +++ b/chef/Makefile @@ -5,7 +5,7 @@ CC=gcc # Bibliothèques LIBS= ## Drapeaux pour le linker -LDFLAGS=-lpthread +LDFLAGS_CUSTOM += -lpthread -lwiringPi ## Drapeaux pour le compilateur CFLAGS= ## Générateurs de drapeaux pour les bibliothèques @@ -13,7 +13,7 @@ PKG_CONFIG=pkg-config # VARIABLES AUTOMATIQUES ifdef LIBS - LDFLAGS += $(shell $(PKG_CONFIG) --libs $(LIBS)) + LDFLAGS_CUSTOM += $(shell $(PKG_CONFIG) --libs $(LIBS)) CFLAGS += $(shell $(PKG_CONFIG) --cflags $(LIBS)) endif @@ -25,7 +25,7 @@ CFLAGS += -Wall -Wextra -pedantic -g -DDEBUG # Génération des fichiers éxecutables bin/%: obj/%.o - $(CC) $(LDFLAGS) $^ -o $@ + $(CC) $(LDFLAGS_CUSTOM) $^ -o $@ # On enlève les symboles inutiles pour gagner en temps de chargement de l'éxecutable ifeq ($(DEBUG),no) strip $@ @@ -34,12 +34,14 @@ endif # RÈGLES DE COMPILATION # Règle éxecutée par défaut (quand on fait juste `make`) -default: bin/testpin bin/premier +default: bin/testpin bin/premier bin/local # Binaires (dont il faut spécifier les objets explicitement) -bin/premier: obj/CA.o obj/debug.o obj/movement.o +bin/premier: obj/CF.o obj/movement.o obj/debug.o obj/position.o bin/testPin: obj/testPin.o - $(CC) $(LDFLAGS) $^ -lwiringPi -o $@ + +bin/local: obj/local.o obj/CF.o obj/position.o + $(CC) -lpthread $^ -o $@ # Génération des fichiers objets obj/%.o: src/%.c src/%.h diff --git a/chef/log/.gitignore b/chef/log/.gitignore new file mode 100644 index 0000000..d6b7ef3 --- /dev/null +++ b/chef/log/.gitignore @@ -0,0 +1,2 @@ +* +!.gitignore diff --git a/chef/src/ACsignals.h b/chef/src/ACsignals.h deleted file mode 120000 index 3156691..0000000 --- a/chef/src/ACsignals.h +++ /dev/null @@ -1 +0,0 @@ -../../arduino/ACsignals.h \ No newline at end of file diff --git a/chef/src/AFsignals.h b/chef/src/AFsignals.h deleted file mode 120000 index d3658b4..0000000 --- a/chef/src/AFsignals.h +++ /dev/null @@ -1 +0,0 @@ -../../arduino/AFsignals.h \ No newline at end of file diff --git a/chef/src/CA.h b/chef/src/CA.h deleted file mode 100644 index 56b45d5..0000000 --- a/chef/src/CA.h +++ /dev/null @@ -1,31 +0,0 @@ -#ifndef __AC_H_ -#define __AC_H_ - -#include -#include -#include // baudrates -#include - -#include "ACsignals.h" - -#define ARDUINO_PORTNAME "/dev/ttyACM0" -#define CA_BAUDRATE B9600 -// #define PRINTRAWDATA - -int arduino; -pthread_mutex_t sSendCA; -pthread_t tReaderAC; - -typedef void (*rxHandler)(void); -rxHandler rxHandlersAC[256]; -bool pret; - -void registerRxHandler(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 diff --git a/chef/src/CA.c b/chef/src/CF.c similarity index 62% rename from chef/src/CA.c rename to chef/src/CF.c index 6538cf7..e2945cb 100644 --- a/chef/src/CA.c +++ b/chef/src/CF.c @@ -1,4 +1,4 @@ -#include "CA.h" +#include "CF.h" #include // O_* #include // printf... #include // stuff @@ -21,13 +21,13 @@ void printData(void* data, size_t size) printf("\n"); } -void configureArduino() +void configureFpga() { // Connection au port série - printf("Connexion à %s... ", ARDUINO_PORTNAME); + printf("Connexion à %s... ", FPGA_PORTNAME); fflush(stdout); - arduino = open(ARDUINO_PORTNAME, O_RDWR | O_NOCTTY | O_NDELAY); - if (arduino < 0) { + fpga = open(FPGA_PORTNAME, O_RDWR | O_NOCTTY | O_NDELAY); + if (fpga < 0) { printf("Échec !\n"); exit(1); } @@ -35,13 +35,13 @@ void configureArduino() // Configuration du port série struct termios cfg; bzero(&cfg, sizeof(cfg)); - cfg.c_cflag = CLOCAL | CREAD | CA_BAUDRATE | CS8; + cfg.c_cflag = CLOCAL | CREAD | CF_BAUDRATE | CS8; cfg.c_iflag = 0; cfg.c_oflag = 0; cfg.c_lflag = 0; /* set input mode (non-canonical, no echo,...) */ cfg.c_cc[VTIME] = 0; /* inter-character timer unused */ cfg.c_cc[VMIN] = 1; /* blocking read until 1 char received */ - if (tcsetattr(arduino, TCSANOW, &cfg) < 0) { + if (tcsetattr(fpga, TCSANOW, &cfg) < 0) { perror("serialConfig.tcsetattr"); exit(1); } @@ -50,14 +50,14 @@ void configureArduino() // Flush unsigned char trash[1024]; - read(arduino, &trash, sizeof(trash)); + read(fpga, &trash, sizeof(trash)); printf("OK!\n"); } -void deconfigureArduino() +void deconfigureFpga() { - close(arduino); + close(fpga); printf("Déconnecté\n"); } @@ -71,7 +71,7 @@ void* TaskReaderAC(void* pdata) (void)pdata; unsigned char code; for (;;) { - code = readByteCA(); + code = readByteCF(); #ifdef PRINTRAWDATA printf("↓"); @@ -87,10 +87,10 @@ void* TaskReaderAC(void* pdata) return NULL; } -void onA2CD_ERR() +void onF2CD_ERR() { - struct A2CD_ERRs s; - readCA(&s, sizeof(struct A2CD_ERRs)); + struct F2CD_ERRs s; + readCF(&s, sizeof(struct F2CD_ERRs)); printf("Erreur reçue : %c (%2x)\n", s.code, s.code); } @@ -104,56 +104,44 @@ void doNothing() } -void configureCA() +void configureCF() { - configureArduino(); + configureFpga(); for (int i = 0; i < 256; i++) { rxHandlersAC[i] = NULL; } - pthread_mutex_init(&sSendCA, NULL); + pthread_mutex_init(&sSendCF, NULL); pthread_create(&tReaderAC, NULL, TaskReaderAC, NULL); - printf("Attente de réponse de l'Arduino... "); + printf("Attente de réponse de l'Fpga... "); fflush(stdout); struct timespec tim; tim.tv_sec = 0; tim.tv_nsec = 100000000L; - // Dans le cas où on aurait laissé l'Arduino en attente de donnée, + // 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(C2AD_PING, setPret); + registerRxHandler(C2FD_PING, setPret); while (!pret) { - sendCA(C2AD_PING, NULL, 0); + sendCF(C2FD_PING, NULL, 0); nanosleep(&tim, NULL); } - registerRxHandler(C2AD_PING, doNothing); - - // Dans le cas où les données de ping complèteraient une commande de déplacement, - // on envoie un STOP en préventif. Ça permet aussi d'attendre que les PING - // en trop aient été absorbés - pret = false; - registerRxHandler(C2AD_STOP, setPret); - sendCA(C2AD_STOP, NULL, 0); - while (!pret) { - nanosleep(&tim, NULL); - } - - registerRxHandler(C2AD_STOP, NULL); - registerRxHandler(C2AD_PING, NULL); + registerRxHandler(C2FD_PING, doNothing); // TODO + registerRxHandler(C2FD_PING, NULL); printf("OK !\n"); - registerRxHandler(A2CD_ERR, onA2CD_ERR); + registerRxHandler(F2CD_ERR, onF2CD_ERR); } -void deconfigureCA() +void deconfigureCF() { - deconfigureArduino(); + deconfigureFpga(); } -void sendByteCA(unsigned char data) +void sendByteCF(unsigned char data) { - write(arduino, &data, sizeof(data)); + write(fpga, &data, sizeof(data)); #ifdef PRINTRAWDATA printf("↑"); @@ -161,20 +149,20 @@ void sendByteCA(unsigned char data) #endif } -void sendCA(unsigned char code, void* data, size_t size) +void sendCF(unsigned char code, void* data, size_t size) { - pthread_mutex_lock(&sSendCA); + pthread_mutex_lock(&sSendCF); - sendByteCA(code); + sendByteCF(code); if (size > 0) { unsigned char* p = data; for (size_t i = 0; i < size; i++) { - write(arduino, p, sizeof(unsigned char)); + write(fpga, p, sizeof(unsigned char)); p++; } // Envoyer plus d'un octet d'un coup curieusement il aime pas ça du tout } - pthread_mutex_unlock(&sSendCA); + pthread_mutex_unlock(&sSendCF); #ifdef PRINTRAWDATA if (size > 0) { @@ -184,11 +172,11 @@ void sendCA(unsigned char code, void* data, size_t size) #endif } -unsigned char readByteCA() +unsigned char readByteCF() { unsigned char c; - while (read(arduino, &c, sizeof(c)) < 1) { + while (read(fpga, &c, sizeof(c)) < 1) { sleep(0); } return c; @@ -199,13 +187,13 @@ unsigned char readByteCA() #endif } -void readCA(void* data, size_t size) +void readCF(void* data, size_t size) { size_t remaining = size; int justRead; char* p = data; do { - justRead = read(arduino, p, remaining); + justRead = read(fpga, p, remaining); if (justRead > 0) { p += justRead; remaining -= justRead; diff --git a/chef/src/CF.h b/chef/src/CF.h new file mode 100644 index 0000000..bc10e8a --- /dev/null +++ b/chef/src/CF.h @@ -0,0 +1,31 @@ +#ifndef __AC_H_ +#define __AC_H_ + +#include +#include +#include // baudrates +#include + +#include "CFsignals.h" + +#define FPGA_PORTNAME "/dev/ttyUSB0" +#define CF_BAUDRATE B9600 +// #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 sendByteCF(unsigned char data); // Privé +void sendCF(unsigned char code, void* data, size_t size); +unsigned char readByteCF(); // À utiliser uniquement depuis un rxHandler +void readCF(void* data, size_t size); // À utiliser uniquement depuis un rxHandler +void configureCF(); +void deconfigureCF(); + +#endif diff --git a/chef/src/CFsignals.h b/chef/src/CFsignals.h new file mode 100644 index 0000000..ee04968 --- /dev/null +++ b/chef/src/CFsignals.h @@ -0,0 +1,62 @@ +/* + * Définition des signaux échagés entre l'Arduino et le chef + */ + +#ifndef __CFSIGNALS_H_ +#define __CFSIGNALS_H_ + +#include + +// 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 + +// Arduino → FPGA + +// Pour le debug +#define C2FD_PING 'P' + +// FPGA → Arduino + +// Erreur quelconque +#define F2CD_ERR 'E' +struct __attribute__ ((packed)) F2CD_ERRs { + unsigned char code; +}; + +#define ERR_UNKNOWN_CODE 'C' + +// Récupère les valeur des encodeurs +#define F2CI_CODER 'D' +struct __attribute__ ((packed)) F2CI_CODERs { + int16_t dL; + int16_t dR; +}; + +// Récupère les valeur des capteurs de distance +#define F2CI_CAPT 'C' +struct __attribute__ ((packed)) F2CI_CAPTs { + uint16_t front; + uint16_t back; +}; + +// Récupère les valeur des capteurs de distance (trigger au supérieur) +#define F2CT_CAPT 'c' +// /!\ Structure de la requête de trigger (A→F). Les données seront envoyées avec F2CI_CAPT +struct __attribute__ ((packed)) F2CT_CAPTs { + uint16_t front; + uint16_t back; +}; + + +#endif diff --git a/chef/src/debug.c b/chef/src/debug.c index 4431d4f..6c4e501 100644 --- a/chef/src/debug.c +++ b/chef/src/debug.c @@ -1,60 +1,69 @@ #include "debug.h" #include #include // sleep +#include +#include +#include +#include + +#include "position.h" + void* TaskDebug(void* pdata) { (void)pdata; - struct timespec tim; - /* tim.tv_sec = 0; */ - /* tim.tv_nsec = 100000000L; */ - tim.tv_sec = 1; - tim.tv_nsec = 0; + struct timespec tim; // 100 ms + tim.tv_sec = 0; + tim.tv_nsec = 100000000L; + /* tim.tv_sec = 1; */ + /* tim.tv_nsec = 0; */ + + char line[1024]; + clock_t t; for (;;) { + + // Calculating time index + t = clock() - debugStart; + + // Generating line + sprintf(line, "%ld,%d,%ld,%ld\n", t, nbCalcPos, lCodTot, rCodTot); + + // Writing + write(debugFd, line, strlen(line)); + + // Sleeping nanosleep(&tim, NULL); - sendCA(A2CI_DBG, NULL, 0); } return NULL; } -void printDebugInfos(struct A2CI_DBGs* debug) -{ - // Position - printf("← + % .6g; % .6g % .6g°", debug->actuel.x, debug->actuel.y, debug->actuel.o); - // Frequence de calcul de la position - printf(" % 5d☼", debug->nbCalcPos); - // Delta codeuses - printf(", %5d↿↾%-5d", debug->deltaCoder.dL, debug->deltaCoder.dR); - // Destination - printf(", "); - if (debug->movement == C2AD_BRAKE) { - printf("⇞ \n"); - } else if (debug->movement == C2AD_FREE) { - printf("↕ \n"); - } else { - printf("↑ % .6g; % .6g % .6g°\n", debug->destination.x, debug->destination.y, debug->destination.o); - } -} - -void onA2CI_DBG() -{ - readCA(&debug, sizeof(struct A2CI_DBGs)); - printDebugInfos(&debug); -} - void configureDebug() { - registerRxHandler(A2CI_DBG, onA2CI_DBG); -#ifdef REGULARREPORTS + debugStart = clock(); + + // Génération du nom de fichier + char path[256]; + time_t startTime; + time(&startTime); + sprintf(path, "log/%ld.csv", startTime); + + // Open file + debugFd = open(path, O_WRONLY | O_APPEND | O_CREAT, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH); + if (debugFd < 0) { + fprintf(stderr, "Impossible d'ouvrir le fichier '%s', debug désactivé.\n", path); + return; + } + + char header[] = "time,nbCalcPos,lCodTot,rCodTot\n"; + write(debugFd, header, strlen(header)); + pthread_create(&tDebug, NULL, TaskDebug, NULL); -#endif } void deconfigureDebug() { -#ifdef REGULARREPORTS pthread_cancel(tDebug); -#endif + close(debugFd); } diff --git a/chef/src/debug.h b/chef/src/debug.h index aac10a7..b5c26bb 100644 --- a/chef/src/debug.h +++ b/chef/src/debug.h @@ -2,14 +2,14 @@ #define __DEBUG_H_ #include -#include "CA.h" -#define REGULARREPORTS +#include + +clock_t debugStart; +int debugFd; -struct A2CI_DBGs debug; pthread_t tDebug; void* TaskDebug(void *pdata); -void onA2CI_DBG(); void configureDebug(); void deconfigureDebug(); diff --git a/chef/src/local.c b/chef/src/local.c new file mode 100644 index 0000000..f85ca66 --- /dev/null +++ b/chef/src/local.c @@ -0,0 +1,29 @@ +#include +#include +#include +#include // sleep + +#include "CF.h" +#include "position.h" + +#define TEMPSMAX 10 + +int main() +{ + + printf("Démarrage...\n"); + configureCF(); + configurePosition(); + srand(time(NULL)); + + printf("C'est parti !\n"); + + sleep(TEMPSMAX); + + printf("Fin des %d secondes\n", TEMPSMAX); + + + deconfigureCF(); + return EXIT_SUCCESS; +} + diff --git a/chef/src/movement.c b/chef/src/movement.c index d248d53..a2166f6 100644 --- a/chef/src/movement.c +++ b/chef/src/movement.c @@ -1,70 +1,92 @@ #include "movement.h" -#include "CA.h" #include #include #include +#include -void onC2AD_STOP() +void configureMovement() { - // On considère que l'arrêt se fait très rapidement pour ne pas - // avoir à attendre le signal de retour de C2AD_STOP - registerRxHandler(C2AD_STOP, NULL); -} - -void stop() -{ - printf("→ Arrêt\n"); - registerRxHandler(C2AD_STOP, onC2AD_STOP); - sendCA(C2AD_STOP, NULL, 0); -} - -void onC2AD_BRAKE() -{ - // On considère que l'arrêt se fait très rapidement pour ne pas - // avoir à attendre le signal de retour de C2AD_BRAKE - registerRxHandler(C2AD_BRAKE, NULL); -} - -void brake() -{ - printf("→ Frein\n"); - registerRxHandler(C2AD_BRAKE, onC2AD_BRAKE); - sendCA(C2AD_BRAKE, NULL, 0); -} - -// Inspiré de https://stackoverflow.com/a/1760819 -pthread_mutex_t reponseMutex = PTHREAD_MUTEX_INITIALIZER; -pthread_cond_t reponseCond = PTHREAD_COND_INITIALIZER; -bool attenteReponse = false; - -void onC2AD_GOTO() -{ - pthread_mutex_lock(&reponseMutex); - attenteReponse = false; - pthread_cond_signal(&reponseCond); - pthread_mutex_unlock(&reponseMutex); -} - -void aller(struct position* pos) -{ - printf("→ Déplacement vers (%f; %f) (%f°)\n", pos->x, pos->y, pos->o); - - if (attenteReponse) { - printf("Déjà en déplacement !\n"); + if (wiringPiSetup() == -1) { + fprintf(stderr, "Impossible d'initialiser WiringPi\n"); exit(EXIT_FAILURE); } - attenteReponse = true; - - registerRxHandler(C2AD_GOTO, onC2AD_GOTO); - sendCA(C2AD_GOTO, (struct C2AD_GOTOs*)pos, sizeof(struct C2AD_GOTOs)); - - pthread_mutex_lock(&reponseMutex); - while (attenteReponse) { - pthread_cond_wait(&reponseCond, &reponseMutex); - } - pthread_mutex_unlock(&reponseMutex); - - registerRxHandler(C2AD_GOTO, NULL); - - printf("← Arrivé à destination\n"); + pinMode(ENA, PWM_OUTPUT); + pinMode(ENB, PWM_OUTPUT); +} + +void aller(struct position* pos); + +int dbg = 0; + +int changerMoteurs(float lVit, float rVit) +{ + // Gauche + bool lFor = lVit < 0; + float lVolt = fabs(lVit); // TODO Utiliser les vitesses + if (lVolt > MOT_MAX_V) { + lVolt = MOT_MAX_V; + } + if (lVolt < MOT_MIN_V) { + digitalWrite(IN1, LOW); + digitalWrite(IN2, LOW); + printf("x"); + + } else if (lFor) { + digitalWrite(IN1, HIGH); + digitalWrite(IN2, LOW); + printf("↑"); + } else { + digitalWrite(IN1, LOW); + digitalWrite(IN2, HIGH); + printf("↓"); + } + int lAbs = lVolt * PWM_MAX / PWM_MAX_V; + pwmWrite(ENA, lAbs); + printf("%6d", lAbs); + + // Droite + bool rFor = rVit < 0; + float rVolt = fabs(rVit); // TODO Utiriser res vitesses + if (rVolt > MOT_MAX_V) { + rVolt = MOT_MAX_V; + } + if (rVolt < MOT_MIN_V) { + digitalWrite(IN3, LOW); + digitalWrite(IN4, LOW); + printf("x"); + + } else if (rFor) { + digitalWrite(IN3, HIGH); + digitalWrite(IN4, LOW); + printf("↑"); + } else { + digitalWrite(IN3, LOW); + digitalWrite(IN4, HIGH); + printf("↓"); + } + int rAbs = rVolt * PWM_MAX / PWM_MAX_V; + pwmWrite(ENB, rAbs); + printf("%6d", rAbs); + printf("\n"); +} + +int brake() +{ + digitalWrite(IN1, LOW); + digitalWrite(IN2, LOW); + digitalWrite(IN3, LOW); + digitalWrite(IN4, LOW); +} + +int freewheel() +{ + digitalWrite(IN1, HIGH); + digitalWrite(IN2, HIGH); + digitalWrite(IN3, HIGH); + digitalWrite(IN4, HIGH); +} + +void deconfigureMovement() +{ + } diff --git a/chef/src/movement.h b/chef/src/movement.h index e54ac11..f5fbe63 100644 --- a/chef/src/movement.h +++ b/chef/src/movement.h @@ -1,8 +1,40 @@ +/* + * Fonctions de déplacment du robot + */ + +#ifndef __MOVEMENT_H_ +#define __MOVEMENT_H_ + +#include "position.h" + +#include + +#define PWM_MAX 1023 +#define PWM_MAX_V 3.3 +#define MOT_MIN_V 0.1 +#define MOT_MAX_V 2.0 + +// Pins definition +// Physical 32 +#define ENA 26 +#define IN1 2 +#define IN2 3 + +// Physical 33 +#define ENB 23 +#define IN3 4 +#define IN4 5 -int bouger(float vitL, float vitR); +void configureMovement(); +void aller(struct position* pos); +int changerMoteurs(float vitL, float vitR); // Vitesse en mm/s // Vitesse < 0 ⇒ sens inverse // Si vitesse < seuil ⇒ brake int brake(); +int stop(); int freewheel(); +void deconfigureMovement(); + +#endif diff --git a/chef/src/position.c b/chef/src/position.c new file mode 100644 index 0000000..2392fd0 --- /dev/null +++ b/chef/src/position.c @@ -0,0 +1,53 @@ +/* + * Fonctions de calcul de la position du robot + */ + +#include "position.h" +#include + +void* TaskPosition(void* pData) +{ + (void)pData; + + nbCalcPos = 0; + lCodTot = 0; + rCodTot = 0; + + pthread_mutex_init(&posPolling, NULL); + + for (;;) { + + // Sending + pthread_mutex_lock(&posPolling); + sendCF(F2CI_CODER, NULL, 0); + + // Waiting for reception + pthread_mutex_lock(&posPolling); + pthread_mutex_unlock(&posPolling); + + // Calculation + nbCalcPos++; + lCodTot += deltaCoders.dL; + rCodTot += deltaCoders.dR; + + } + + return NULL; +} + +void onF2CI_CODER() +{ + readCF(&deltaCoders, sizeof(struct F2CI_CODERs)); + pthread_mutex_unlock(&posPolling); +} + +void configurePosition() +{ + pthread_create(&tPosition, NULL, TaskPosition, NULL); + registerRxHandler(F2CI_CODER, onF2CI_CODER); +} + +void deconfigurePosition() +{ + pthread_cancel(tPosition); +} diff --git a/chef/src/position.h b/chef/src/position.h new file mode 100644 index 0000000..9966cab --- /dev/null +++ b/chef/src/position.h @@ -0,0 +1,34 @@ +/* + * Fonctions de calcul de la position du robot + */ + +#ifndef __POSITION_H_ +#define __POSITION_H_ + +#include "CF.h" +#include + + +struct __attribute__ ((packed)) position { + float x; + float y; + float o; +}; + +struct position actuel; +struct F2CI_CODERs deltaCoders; + +// Debug +unsigned int nbCalcPos; +long lCodTot, rCodTot; + +// +pthread_mutex_t posPolling; +pthread_t tPosition; + +// Fonctions +void configurePosition(); +void deconfigurePosition(); + +#endif + diff --git a/chef/src/premier.c b/chef/src/premier.c index 417b7d6..52d349f 100644 --- a/chef/src/premier.c +++ b/chef/src/premier.c @@ -4,8 +4,9 @@ #include #include // sleep -#include "CA.h" +#include "CF.h" #include "movement.h" +#include "position.h" #include "debug.h" #define TEMPSMAX 60 @@ -14,15 +15,42 @@ void* TaskParcours(void *pdata) { (void) pdata; - struct position pos; + /* struct position pos; */ + /* for (;;) { */ + /* pos.x = (int) (rand()*200.0/RAND_MAX); */ + /* pos.y = (int) (rand()*100.0/RAND_MAX); */ + /* pos.o = (int) (rand()*360.0/RAND_MAX); */ + /* aller(&pos); */ + /* sleep(1); */ + /* brake(); */ + /* sleep(2); */ + /* } */ + + struct timespec tim; // 10 ms + tim.tv_sec = 0; + tim.tv_nsec = 10000000L; + +#define RAMP_TIME 100 +#define MAX_VIT MOT_MAX_V + for (;;) { - pos.x = (int) (rand()*200.0/RAND_MAX); - pos.y = (int) (rand()*100.0/RAND_MAX); - pos.o = (int) (rand()*360.0/RAND_MAX); - aller(&pos); - sleep(1); - brake(); + // ↗ + for (int i = 0; i < RAMP_TIME; i++) { + float p = (float) i / (float) RAMP_TIME; + changerMoteurs(p * MOT_MAX_V, p * MOT_MAX_V); + nanosleep(&tim, NULL); + } + changerMoteurs(MOT_MAX_V, MOT_MAX_V); + // ↑ sleep(2); + // ↘ + for (int i = 0; i < RAMP_TIME; i++) { + float p = (float) i / (float) RAMP_TIME; + p = 1 - p; + changerMoteurs(p * MOT_MAX_V, p * MOT_MAX_V); + nanosleep(&tim, NULL); + } + sleep(5); } printf("Fin du parcours\n"); @@ -33,17 +61,17 @@ int main() { printf("Démarrage...\n"); - configureCA(); + configureCF(); configureDebug(); + configureMovement(); + configurePosition(); srand(time(NULL)); - /* printf("Synchronisation avec le Raspberry Pi\n"); // TODO */ - /* printf("En attente de la tirette...\n"); // TODO */ printf("C'est parti !\n"); - /* pthread_t tParcours; */ - /* pthread_create(&tParcours, NULL, TaskParcours, NULL); */ + pthread_t tParcours; + pthread_create(&tParcours, NULL, TaskParcours, NULL); sleep(TEMPSMAX); @@ -51,9 +79,11 @@ int main() /* pthread_cancel(tParcours); */ - stop(); + /* stop(); */ + deconfigureMovement(); + deconfigurePosition(); deconfigureDebug(); - deconfigureCA(); + deconfigureCF(); return EXIT_SUCCESS; } diff --git a/raspberrypi/Makefile b/raspberrypi/Makefile index 2b2a3a7..180bab1 100644 --- a/raspberrypi/Makefile +++ b/raspberrypi/Makefile @@ -92,7 +92,6 @@ upgrade-fpga: # CHEF -prog: chef chef: make -C buildroot chef-rebuild diff --git a/raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/etc/init.d/S30hardware b/raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/etc/init.d/S30hardware new file mode 100755 index 0000000..6999dc0 --- /dev/null +++ b/raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/etc/init.d/S30hardware @@ -0,0 +1,34 @@ +#!/bin/sh +# +# Loads required hardware modules +# + +start() { + printf "Starting hardware handling: " + modprobe pl2303 # USB↔Serial cable + echo "OK" +} + +stop() { + printf "Stopping hardware handling: " + rmmod pl2303 # USB↔Serial cable + echo "OK" +} + +case "$1" in + start) + start + ;; + stop) + stop + ;; + restart|reload) + stop + start + ;; + *) + echo "Usage: $0 {start|stop|restart}" + exit 1 +esac + +exit $? diff --git a/raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/etc/init.d/S90extra b/raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/etc/init.d/S90extra index ef037ce..e0ec7cd 100755 --- a/raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/etc/init.d/S90extra +++ b/raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/etc/init.d/S90extra @@ -5,10 +5,10 @@ start() { printf "Starting extra services: " - if ! /opt/cdf/bin/testpin 0 1 - then + # if ! /opt/cdf/bin/testpin 0 1 + # then /etc/extra.d/rcS - fi + # fi echo "OK" } diff --git a/raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/root/.profile b/raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/root/.profile index dfb5415..a7c2a1e 100644 --- a/raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/root/.profile +++ b/raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/root/.profile @@ -7,7 +7,7 @@ alias mv="mv -i" alias free='free -m' alias df='df -h' -export PS1="[\u@\h \$] " +export PS1="[\u@\h \W] " export PS2="> " export PS3="+ " export PS4="- " diff --git a/raspberrypi/package/robotech/chef/chef.mk b/raspberrypi/package/robotech/chef/chef.mk index 8b2a3bb..8f8404e 100644 --- a/raspberrypi/package/robotech/chef/chef.mk +++ b/raspberrypi/package/robotech/chef/chef.mk @@ -16,6 +16,7 @@ endef define CHEF_INSTALL_TARGET_CMDS $(INSTALL) -d -m 0755 $(TARGET_DIR)/opt/chef/bin $(INSTALL) -d -m 0755 $(TARGET_DIR)/opt/chef/com + $(INSTALL) -d -m 0755 $(TARGET_DIR)/opt/chef/log $(INSTALL) -D -m 0755 $(@D)/bin/* $(TARGET_DIR)/opt/chef/bin $(INSTALL) -D -m 0755 $(@D)/run.sh $(TARGET_DIR)/opt/chef endef