From aa519e33bfe69452e0f9a9211910f80dad077113 Mon Sep 17 00:00:00 2001 From: Geoffrey Frogeye Date: Fri, 11 May 2018 15:58:18 +0200 Subject: [PATCH] Gestion correcte de l'asservissement --- chef/Makefile | 2 +- chef/src/common.c | 46 ++++++ chef/src/common.h | 21 +++ chef/src/diagnostics.c | 2 +- chef/src/dimensions.c | 0 chef/src/dimensions.h | 41 +++-- chef/src/i2c.c | 21 ++- chef/src/i2c.h | 2 + chef/src/motor.c | 56 +++++-- chef/src/motor.h | 7 +- chef/src/movement.c | 239 +++++++++++++++-------------- chef/src/movement.h | 12 +- chef/src/position.c | 47 ++++-- chef/src/position.h | 2 + chef/src/premier.c | 3 - chef/src/test1m.c | 67 --------- chef/src/testAvance.c | 25 +++- chef/src/testRetour.c | 71 +++++++++ chef/src/testUpDown.c | 14 +- fpga/Principal.vhd | 333 +++++++++++++++++++++-------------------- fpga/communication.vhd | 6 + fpga/fir.vhd | 2 +- fpga/i2c.vhd | 283 ++++++++++++++++++++++++++++++++++ fpga/principal.ucf | 8 +- fpga/project.cfg | 2 +- raspberrypi/Makefile | 1 + simu/simu.m | 108 +++++++++---- 27 files changed, 972 insertions(+), 449 deletions(-) create mode 100644 chef/src/common.c create mode 100644 chef/src/common.h create mode 100644 chef/src/dimensions.c delete mode 100644 chef/src/test1m.c create mode 100644 chef/src/testRetour.c create mode 100644 fpga/i2c.vhd diff --git a/chef/Makefile b/chef/Makefile index c7a36d5..4a230bd 100644 --- a/chef/Makefile +++ b/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=actionneurs buttons CA debug diagnostics fpga i2c imu ihm lcd motor movement parcours points position securite +OBJS=actionneurs buttons CA common debug diagnostics dimensions fpga i2c imu ihm lcd motor movement parcours points position securite OBJS_O=$(addprefix obj/,$(addsuffix .o,$(OBJS))) # VARIABLES AUTOMATIQUES diff --git a/chef/src/common.c b/chef/src/common.c new file mode 100644 index 0000000..083142e --- /dev/null +++ b/chef/src/common.c @@ -0,0 +1,46 @@ +#include "common.h" + +void diffTime(const struct timespec* debut, const struct timespec* fin, struct timespec* ecoule) +{ + if ((fin->tv_nsec - debut->tv_nsec) < 0) { + ecoule->tv_sec = fin->tv_sec - debut->tv_sec - 1; + ecoule->tv_nsec = fin->tv_nsec - debut->tv_nsec + 1000000000UL; + } else { + ecoule->tv_sec = fin->tv_sec - debut->tv_sec; + ecoule->tv_nsec = fin->tv_nsec - debut->tv_nsec; + } +} + +float diffTimeSec(const struct timespec* debut, const struct timespec* fin) +{ + struct timespec ecoule; + diffTime(debut, fin, &ecoule); + return ecoule.tv_sec + ecoule.tv_nsec * 1E-9; +} + +void resetPID(struct PID *pid) +{ + clock_gettime(CLOCK_REALTIME, &pid->lastCalc); + pid->prevErr = 0; + pid->integErr = 0; +} + +void initPID(struct PID *pid, float KP, float KI, float KD) +{ + pid->KP = KP; + pid->KI = KI; + pid->KD = KD; + resetPID(pid); +} + +float updatePID(struct PID *pid, float err) +{ + struct timespec now; + clock_gettime(CLOCK_REALTIME, &now); + float dt = diffTimeSec(&pid->lastCalc, &now); + pid->integErr += (err + pid->prevErr) / 2 * dt; + float derivErr = (err - pid->prevErr) / dt; + pid->prevErr = err; + return pid->KP * err + pid->KI * pid->integErr + pid->KP * derivErr; +} + diff --git a/chef/src/common.h b/chef/src/common.h new file mode 100644 index 0000000..5986d0f --- /dev/null +++ b/chef/src/common.h @@ -0,0 +1,21 @@ +#ifndef __COMMON_H_ +#define __COMMON_H_ + +#include + +struct PID { + struct timespec lastCalc; + float KP; + float KI; + float KD; + float prevErr; + float integErr; +}; + +void diffTime(const struct timespec* debut, const struct timespec* fin, struct timespec* ecoule); +float diffTimeSec(const struct timespec* debut, const struct timespec* fin); +void resetPID(struct PID *pid); +void initPID(struct PID *pid, float KP, float KI, float KD); +float updatePID(struct PID *pid, float err); + +#endif diff --git a/chef/src/diagnostics.c b/chef/src/diagnostics.c index da9e8fa..c9ff1b4 100644 --- a/chef/src/diagnostics.c +++ b/chef/src/diagnostics.c @@ -137,7 +137,7 @@ void runDiagnostics() { execDiagnostic("Lien FPGA", diagFPGA, NULL); execDiagnostic("Lien Arduino", diagArduino, NULL); - execDiagnostic("Lien IMU", diagIMU, NULL); + /* execDiagnostic("Lien IMU", diagIMU, NULL); */ int i; i = 0; execDiagnostic("Mot+Cod L AV", diagCodeuse, &i); diff --git a/chef/src/dimensions.c b/chef/src/dimensions.c new file mode 100644 index 0000000..e69de29 diff --git a/chef/src/dimensions.h b/chef/src/dimensions.h index cf3905f..c5321eb 100644 --- a/chef/src/dimensions.h +++ b/chef/src/dimensions.h @@ -21,7 +21,7 @@ #define MOTOR_CONTROLLER_ALIMENTATION 24.0 // V (from elec) #define MOTOR_CONTROLLER_REFERENCE 5.0 // V (from wiring) #define MOTOR_SATURATION_MIN 0.0 // V (from random) -#define MOTOR_SATURATION_MAX 3.0 // V (from testing) +#define MOTOR_SATURATION_MAX 4.0 // V (from testing) #define PWM_MAX 3.3 // V (from FPGA datasheet) #define CODER_RESOLUTION 370.0 // cycles/motor rev #define CODER_DATA_FACTOR 4.0 // increments/motor cycles @@ -32,16 +32,35 @@ #define CODER_FULL_RESOLUTION (CODER_DATA_RESOLUTION / REDUC_RATIO) // cycles / wheel rev #define AV_PER_CYCLE (WHEEL_PERIMETER / CODER_FULL_RESOLUTION) // mm +// Pour éviter les pics de codeuse liées à la communication I2C +#define ABSOLUTE_MAX_VITESSE_ROBOT 10.0 // km/h +#define ABSOLUTE_MAX_VITESSE_ROBOT_MMP_S (ABSOLUTE_MAX_VITESSE_ROBOT * 10000.0 / 36.0) // mm/s +#define ABSOLUTE_MAX_VITESSE_ROBOT_REVP_S (ABSOLUTE_MAX_VITESSE_ROBOT_MMP_S / WHEEL_PERIMETER) // rev/s +#define ABSOLUTE_MAX_VITESSE_ROBOT_CYCP_S (ABSOLUTE_MAX_VITESSE_ROBOT_REVP_S * CODER_FULL_RESOLUTION) // cycle/s + // Constantes asservissement -#define D_DIR_ECART_MIN 7.0 // mm -#define D_DIR_ECART_MAX 10.0 // mm -#define O_DIR_ECART_MIN (5.0 / 360.0 * 2.0 * M_PI) // rad -#define O_DIR_ECART_MAX (25.0 / 360.0 * 2.0 * M_PI) // rad -#define O_ECART_MAX (25.0 / 360.0 * 2.0 * M_PI) // rad -#define O_GAIN 5.0 -#define P 5.0 -#define I 0.0 -#define D 0.0 -#define M 0.0 + + +// Asservissement en distance +#define D_DIR_ECART_MIN 30.0 // mm +#define D_DIR_ECART_MAX 50.0 // mm +#define D_KP 0.05 +#define D_KI 0.0 +#define D_KD 0.0 +#define TARGET_TENSION_RATIO 0.75 +#define TARGET_TENSION (TARGET_TENSION_RATIO * MOTOR_SATURATION_MAX) // V +#define CAROTTE_DISTANCE (TARGET_TENSION / D_KP) // mm + +// Asservissement en angle +#define O_DIR_ECART_MIN (25.0 / 360.0 * 2.0 * M_PI) // rad +#define O_DIR_ECART_MAX (45.0 / 360.0 * 2.0 * M_PI) // rad +#define O_ECART_MIN (25.0 / 360.0 * 2.0 * M_PI) // rad +#define O_ECART_MAX (45.0 / 360.0 * 2.0 * M_PI) // rad +#define O_KP (MOTOR_SATURATION_MAX / (WHEEL_PERIMETER * M_PI)) // au max peut dérivier de pi +#define O_KI 0.0 +#define O_KD 0.0 +#define CAROTTE_ANGLE (TARGET_TENSION / O_KP) // mm + +#define MARGE_SECURITE 300.0 // mm #endif diff --git a/chef/src/i2c.c b/chef/src/i2c.c index 289b083..924174c 100644 --- a/chef/src/i2c.c +++ b/chef/src/i2c.c @@ -1,8 +1,8 @@ #include +#include #include #include #include -#include #include #include "i2c.h" @@ -34,7 +34,20 @@ int openI2C(uint8_t address) uint8_t readI2C(int fd, uint8_t reg) { lockI2C(); - uint8_t res = wiringPiI2CReadReg8(fd, reg); + int res; + int delay = 1; + static char errBuffer[1024]; + for (int i = 0; i < I2C_DRIVEN_HIGH_RETRIES; i++) { + while ((res = wiringPiI2CReadReg8(fd, reg)) < 0) { + snprintf(errBuffer, 1024, "wiringPiI2CReadReg8 @%3d %2x %9d", fd, reg, delay); + perror(errBuffer); + usleep(delay); + delay *= 2; + } + if (res != 0xFF) { + break; + } + } unlockI2C(); return res; } @@ -43,8 +56,10 @@ void writeI2C(int fd, uint8_t reg, uint8_t data) { lockI2C(); int delay = 1; + static char errBuffer[1024]; while (wiringPiI2CWriteReg8(fd, reg, data) < 0) { - perror("wiringPiI2CWriteReg8"); + snprintf(errBuffer, 1024, "wiringPiI2CWriteReg8 @%3d %2x←%2x %9d", fd, reg, data, delay); + perror(errBuffer); usleep(delay); delay *= 2; } diff --git a/chef/src/i2c.h b/chef/src/i2c.h index 9181be9..e4a0809 100644 --- a/chef/src/i2c.h +++ b/chef/src/i2c.h @@ -3,6 +3,8 @@ #include +#define I2C_DRIVEN_HIGH_RETRIES 3 + void initI2C(); int openI2C(uint8_t address); uint8_t readI2C(int fd, uint8_t reg); diff --git a/chef/src/motor.c b/chef/src/motor.c index 67ae91c..be69a06 100644 --- a/chef/src/motor.c +++ b/chef/src/motor.c @@ -7,6 +7,8 @@ float lVolt; float rVolt; +float lVoltDbg; +float rVoltDbg; float lVoltCons; float rVoltCons; struct timespec motStart; @@ -18,8 +20,8 @@ enum motorState motState = braking; void configureMotor() { - registerDebugVar("lVolt", f, &lVolt); - registerDebugVar("rVolt", f, &rVolt); + registerDebugVar("lVolt", f, &lVoltDbg); + registerDebugVar("rVolt", f, &rVoltDbg); pthread_mutex_init(&motCons, NULL); pthread_create(&tMotor, NULL, TaskMotor, NULL); } @@ -54,6 +56,8 @@ void setMoteurTensionRaw(float l, float r, bool lFor, bool rFor) { lVolt = l; rVolt = r; + lVoltDbg = (lFor ? -1 : 1) * l; + rVoltDbg = (rFor ? -1 : 1) * r; uint8_t enA = 0; uint8_t enB = 0; @@ -79,9 +83,33 @@ void setMoteurTensionRaw(float l, float r, bool lFor, bool rFor) // Stay at 0 : brake mode } - writeI2C(fdFPGA(), MOTOR_IN, in); - writeI2C(fdFPGA(), MOTOR_ENA, enA); - writeI2C(fdFPGA(), MOTOR_ENB, enB); + setIn(in); + setEnA(enA); + setEnB(enB); +} + +uint8_t enAbuff = 0x80; +void setEnA(uint8_t val) { + if (val != enAbuff) { + writeI2C(fdFPGA(), MOTOR_ENA, val); + enAbuff = val; + } +} + +uint8_t enBbuff = 0x80; +void setEnB(uint8_t val) { + if (val != enBbuff) { + writeI2C(fdFPGA(), MOTOR_ENB, val); + enBbuff = val; + } +} + +uint8_t inbuff = 0xFF; +void setIn(uint8_t val) { + if (val != inbuff) { + writeI2C(fdFPGA(), MOTOR_IN, val); + inbuff = val; + } } void* TaskMotor(void* pData) @@ -156,7 +184,7 @@ void* TaskMotor(void* pData) break; } - usleep(1000); + usleep(MOTOR_INTERVAL * 1000); } return NULL; @@ -166,17 +194,21 @@ void rawBrake() { lVolt = 0; rVolt = 0; - writeI2C(fdFPGA(), MOTOR_IN, 0); - writeI2C(fdFPGA(), MOTOR_ENA, UINT8_MAX); - writeI2C(fdFPGA(), MOTOR_ENB, UINT8_MAX); + lVoltDbg = 0; + rVoltDbg = 0; + setIn(0); + setEnA(UINT8_MAX); + setEnB(UINT8_MAX); } void rawFreewheel() { lVolt = 0; rVolt = 0; - writeI2C(fdFPGA(), MOTOR_IN, (1 << IN1) | (1 << IN2) | (1 << IN3) | (1 << IN4)); - writeI2C(fdFPGA(), MOTOR_ENA, 0); - writeI2C(fdFPGA(), MOTOR_ENA, 0); + lVoltDbg = 0; + rVoltDbg = 0; + setIn((1 << IN1) | (1 << IN2) | (1 << IN3) | (1 << IN4)); + setEnA(0); + setEnB(0); } void setMoteurTension(float l, float r) diff --git a/chef/src/motor.h b/chef/src/motor.h index 2e1541b..abad80b 100644 --- a/chef/src/motor.h +++ b/chef/src/motor.h @@ -10,7 +10,9 @@ #define INVERSE_L_MOTOR // #define INVERSE_R_MOTOR -#define ENABLE_RATE_LIMITER +// #define ENABLE_RATE_LIMITER + +#define MOTOR_INTERVAL 10 // V/s #define RATE_LIMITER_UP 6 @@ -45,5 +47,8 @@ uint8_t moteurTensionToPWM(float V); void setMoteurTensionRaw(float lVolt, float rVolt, bool lFor, bool rFor); void rawFreewheel(); void rawBrake(); +void setEnA(uint8_t val); +void setEnB(uint8_t val); +void setIn(uint8_t val); #endif diff --git a/chef/src/movement.c b/chef/src/movement.c index e2eab9e..86b01f7 100644 --- a/chef/src/movement.c +++ b/chef/src/movement.c @@ -5,7 +5,9 @@ #include #include +#include "common.h" #include "debug.h" +#include "dimensions.h" #include "motor.h" #include "movement.h" #include "securite.h" @@ -20,17 +22,17 @@ bool movInstructionBool; float xDiff; float yDiff; +float dEcart; float oEcart; -float dDirEcart; float oDirEcart; +float dDirEcart; +float oConsEcart; float dErr; float oErr; -bool oRetenu; -bool dRetenu; +float dVolt; +float oVolt; float lErr; float rErr; -float lErrPrev; -float rErrPrev; unsigned int nbCalcCons; void configureMovement() @@ -38,9 +40,7 @@ void configureMovement() stop(); configureMotor(); -#ifdef ENABLE_SECURITE configureSecurite(); -#endif nbCalcCons = 0; @@ -55,18 +55,127 @@ void configureMovement() registerDebugVar("oCons", f, &cons.o); registerDebugVar("xDiff", f, &xDiff); registerDebugVar("yDiff", f, &yDiff); - registerDebugVar("oEcart", f, &oEcart); registerDebugVar("dErr", f, &dErr); registerDebugVar("oErr", f, &oErr); + registerDebugVar("dVolt", f, &dVolt); + registerDebugVar("oVolt", f, &oVolt); registerDebugVar("dDirEcart", f, &dDirEcart); registerDebugVar("oDirEcart", f, &oDirEcart); - registerDebugVar("dRetenu", d, &dRetenu); - registerDebugVar("oRetenu", d, &oRetenu); + registerDebugVar("dEcart", f, &dEcart); + registerDebugVar("oEcart", f, &oEcart); + registerDebugVar("oConsEcart", f, &oConsEcart); registerDebugVar("lErr", f, &lErr); registerDebugVar("rErr", f, &rErr); registerDebugVar("nbCalcCons", d, &nbCalcCons); } +float angleMod(float angle) +{ + return fmodf(angle + M_PI, 2 * M_PI) - M_PI; +} + +float fcap(float value, float cap) +{ + if (value > 0) { + return fmin(value, cap); + } else { + return fmax(value, -cap); + } +} + +void* TaskMovement(void* pData) +{ + (void)pData; + + unsigned int lastPosCalc = 0; + struct position connu; + + struct PID dPid; + struct PID oPid; + + initPID(&dPid, D_KP, D_KI, D_KD); + initPID(&oPid, O_KP, O_KI, O_KD); + + bool orienteDestination = false; + bool procheDestination = false; + bool orienteConsigne = false; + bool reverse; + + for (;;) { + + lastPosCalc = getPositionNewer(&connu, lastPosCalc); + + xDiff = cons.x - connu.x; + yDiff = cons.y - connu.y; + dDirEcart = hypotf(xDiff, yDiff); + oDirEcart = angleMod(atan2(yDiff, xDiff) - connu.o); + oConsEcart = angleMod(cons.o - connu.o); + + if ((reverse = fabsf(oDirEcart) > M_PI_2)) { + dDirEcart = -dDirEcart; + oDirEcart = angleMod(oDirEcart + M_PI); + } + + if (fabsf(oDirEcart) < O_DIR_ECART_MIN) { + orienteDestination = true; + } else if (fabsf(oDirEcart) > O_DIR_ECART_MAX) { + orienteDestination = false; + } + + if (fabsf(dDirEcart) < D_DIR_ECART_MIN) { + procheDestination = true; + } else if (fabsf(dDirEcart) > D_DIR_ECART_MAX) { + procheDestination = false; + } + + if (fabsf(oConsEcart) < O_ECART_MIN) { + orienteConsigne = true; + } else if (fabsf(oConsEcart) > O_ECART_MAX) { + orienteConsigne = false; + } + + // Carotte + dEcart = (orienteDestination && !procheDestination) ? dDirEcart : 0; + dErr = fcap(dEcart, CAROTTE_DISTANCE); + oEcart = procheDestination ? oConsEcart : oDirEcart; + oErr = fcap(oEcart * DISTANCE_BETWEEN_WHEELS, CAROTTE_ANGLE); + + dVolt = updatePID(&dPid, dErr); + oVolt = updatePID(&oPid, oErr); + + float lVolt = dVolt - oVolt; + float rVolt = dVolt + oVolt; + + pthread_mutex_lock(&movInstructionMutex); + if (movInstructionBool) { + if (procheDestination && orienteConsigne) { + brake(); + } else { + setMoteurTension(lVolt, rVolt); + } + } + pthread_mutex_unlock(&movInstructionMutex); + + nbCalcCons++; + } + + return NULL; +} + +void enableAsservissement() +{ + pthread_mutex_lock(&movInstructionMutex); + movInstructionBool = true; + pthread_mutex_unlock(&movInstructionMutex); +} + +void disableAsservissement() +{ + pthread_mutex_lock(&movInstructionMutex); + movInstructionBool = false; + pthread_mutex_unlock(&movInstructionMutex); +} + void setDestination(struct position* pos) { pthread_mutex_lock(&movInstructionMutex); @@ -85,116 +194,6 @@ void waitDestination() pthread_mutex_unlock(&movInstructionMutex); } -float angleGap(float target, float actual) -{ - float ret = fmodf(target - actual + M_PI, 2 * M_PI) - M_PI; - return ret; -} - -float angleGap180(float target, float actual, float* dist) -{ - if (fabs(fmodf(target - actual + M_PI, 2 * M_PI) - M_PI) > M_PI_2) { - *dist = -*dist; - } - return fmodf(target - actual + M_PI_2, M_PI) - M_PI_2; -} - -void* TaskMovement(void* pData) -{ - (void)pData; - - unsigned int lastPosCalc = 0; - struct position connu; - - for (;;) { - // Attend instruction - printf("Wait instruction\n"); - pthread_mutex_lock(&movInstructionMutex); - while (!movInstructionBool) { - pthread_cond_wait(&movInstructionCond, &movInstructionMutex); - } // Début de l'instruction - - printf("Oriente dir\n"); - // Oriente vers direction - // TODO Marche arrière - do { - - lastPosCalc = getPositionNewer(&connu, lastPosCalc); - - xDiff = cons.x - connu.x; - yDiff = cons.y - connu.y; - oDirEcart = angleGap(atan2(yDiff, xDiff), connu.o); - float oErrRev = oDirEcart * DISTANCE_BETWEEN_WHEELS / WHEEL_PERIMETER; - float lVolt = -oErrRev * P + (oErrRev > 0 ? -M : M); - float rVolt = oErrRev * P + (oErrRev > 0 ? M : -M); - setMoteurTension(lVolt, rVolt); - - nbCalcCons++; - } while (fabsf(oDirEcart) > O_DIR_ECART_MIN); - brake(); - usleep(500 * 1000); - - printf("Avance dir\n"); - // Avance vers direction - do { - - lastPosCalc = getPositionNewer(&connu, lastPosCalc); - - xDiff = cons.x - connu.x; - yDiff = cons.y - connu.y; - dDirEcart = hypotf(xDiff, yDiff); - oDirEcart = angleGap(atan2(yDiff, xDiff), connu.o); - -#ifdef ENABLE_SECURITE - float distDevant, distDerriere; - getDistance(&distDevant, &distDerriere); - float maxEcart = fmax(0, distDevant - SECURITE_MARGE); - float minEcart = fmin(0, -distDerriere + SECURITE_MARGE); - dErr = fmin(maxEcart, fmax(minEcart, dDirEcart)); -#else - dErr = dDirEcart; -#endif - - float dErrRev = dErr / WHEEL_PERIMETER; - float oErrRev = O_GAIN * oDirEcart * DISTANCE_BETWEEN_WHEELS / WHEEL_PERIMETER; - - lErr = dErrRev - oErrRev; - rErr = dErrRev + oErrRev; - float lVolt = lErr * P + (lErr > 0 ? M : -M); - float rVolt = rErr * P + (rErr > 0 ? M : -M); - setMoteurTension(lVolt, rVolt); - - nbCalcCons++; - } while (fabsf(dDirEcart) > D_DIR_ECART_MIN); - brake(); - usleep(500 * 1000); - - printf("Orientation finale\n"); - // Orientation finale (si nécessaire) - if (!isnan(cons.o)) { - do { - - lastPosCalc = getPositionNewer(&connu, lastPosCalc); - oDirEcart = angleGap(cons.o, connu.o); - float oErrRev = oDirEcart * DISTANCE_BETWEEN_WHEELS / WHEEL_PERIMETER; - float lVolt = -oErrRev * P + (oErrRev > 0 ? -M : M); - float rVolt = oErrRev * P + (oErrRev > 0 ? M : -M); - setMoteurTension(lVolt, rVolt); - - nbCalcCons++; - } while (fabsf(oDirEcart) > O_DIR_ECART_MIN); - brake(); - usleep(500 * 1000); - } - - movInstructionBool = false; // Fin de l'instruction - pthread_cond_signal(&movInstructionCond); - pthread_mutex_unlock(&movInstructionMutex); - } - - return NULL; -} - void deconfigureMovement() { stop(); diff --git a/chef/src/movement.h b/chef/src/movement.h index 78522cb..85226f9 100644 --- a/chef/src/movement.h +++ b/chef/src/movement.h @@ -7,17 +7,19 @@ #define ANGLE_INSIGNIFIANT NAN -#define ENABLE_SECURITE - -#define SECURITE_MARGE 300 - +// #define ENABLE_SECURITE #include "position.h" +// Public void configureMovement(); void deconfigureMovement(); void setDestination(struct position* pos); -void* TaskMovement(void* pData); void waitDestination(); +void enableAsservissement(); +void disableAsservissement(); + +// Private +void* TaskMovement(void* pData); #endif diff --git a/chef/src/position.c b/chef/src/position.c index 311d217..ddc6840 100644 --- a/chef/src/position.c +++ b/chef/src/position.c @@ -13,6 +13,7 @@ #include "dimensions.h" #include "fpga.h" #include "position.h" +#include "common.h" // Globales struct position connu; @@ -24,16 +25,34 @@ pthread_t tPosition; unsigned int nbCalcPos; long lCodTot, rCodTot; -int16_t oldL, oldR; -int16_t newL, newR; +uint16_t oldL, oldR; +uint16_t newL, newR; int16_t deltaL, deltaR; +int newLdbg, newRdbg; + +struct timespec lastCoderRead; void updateDelta() { - newL = (readI2C(fdFPGA(), CODER_LEFT_H) << 8 | readI2C(fdFPGA(), CODER_LEFT_L)); - newR = (readI2C(fdFPGA(), CODER_RIGHT_H) << 8 | readI2C(fdFPGA(), CODER_RIGHT_L)); - deltaL = (abs(oldL - newL) < UINT16_MAX/2) ? newL - oldL : UINT16_MAX - oldL + newL; - deltaR = (abs(oldR - newR) < UINT16_MAX/2) ? newR - oldR : UINT16_MAX - oldR + newR; + newL = (readI2C(fdFPGA(), CODER_LEFT_H) << 8 | readI2C(fdFPGA(), CODER_LEFT_L)) & 0xFFFF; + newR = (readI2C(fdFPGA(), CODER_RIGHT_H) << 8 | readI2C(fdFPGA(), CODER_RIGHT_L)) & 0xFFFF; + newLdbg = newL; + newRdbg = newR; + deltaL = (abs(oldL - newL) < UINT16_MAX / 2) ? newL - oldL : UINT16_MAX - oldL + newL; + deltaR = (abs(oldR - newR) < UINT16_MAX / 2) ? newR - oldR : UINT16_MAX - oldR + newR; + + // Verification de valeur abbérante + struct timespec now; + clock_gettime(CLOCK_REALTIME, &now); + float maxDelta = diffTimeSec(&lastCoderRead, &now) * ABSOLUTE_MAX_VITESSE_ROBOT_CYCP_S; + if (abs(deltaL) > maxDelta) { + deltaL = 0; + } + if (abs(deltaR) > maxDelta) { + deltaR = 0; + } + lastCoderRead = now; + oldL = newL; oldR = newR; } @@ -46,8 +65,6 @@ void* TaskPosition(void* pData) nbCalcPos = 0; lCodTot = 0; rCodTot = 0; - oldL = 0; - oldR = 0; updateDelta(); @@ -80,6 +97,8 @@ void* TaskPosition(void* pData) nbCalcPos++; pthread_cond_signal(&newPos); pthread_mutex_unlock(&posConnu); + + usleep(POSITION_INTERVAL * 1000); } return NULL; @@ -87,19 +106,17 @@ void* TaskPosition(void* pData) void configurePosition() { - pthread_mutex_init(&posConnu, NULL); - pthread_cond_init(&newPos, NULL); + resetPosition(); registerDebugVar("lCodTot", ld, &lCodTot); registerDebugVar("rCodTot", ld, &rCodTot); - registerDebugVar("newL", ld, &newL); - registerDebugVar("newR", ld, &newR); - connu.x = 0; - connu.y = 0; - connu.o = 0; + registerDebugVar("newL", d, &newLdbg); + registerDebugVar("newR", d, &newRdbg); registerDebugVar("xConnu", f, &connu.x); registerDebugVar("yConnu", f, &connu.y); registerDebugVar("oConnu", f, &connu.o); registerDebugVar("nbCalcPos", d, &nbCalcPos); + pthread_mutex_init(&posConnu, NULL); + pthread_cond_init(&newPos, NULL); pthread_create(&tPosition, NULL, TaskPosition, NULL); } diff --git a/chef/src/position.h b/chef/src/position.h index 544087c..6f91e79 100644 --- a/chef/src/position.h +++ b/chef/src/position.h @@ -8,6 +8,8 @@ // #define INVERSE_L_CODER #define INVERSE_R_CODER +#define POSITION_INTERVAL 10 + // Structures struct __attribute__ ((packed)) position { float x; diff --git a/chef/src/premier.c b/chef/src/premier.c index acac81d..5321039 100644 --- a/chef/src/premier.c +++ b/chef/src/premier.c @@ -10,7 +10,6 @@ #include "debug.h" #include "i2c.h" #include "ihm.h" -#include "imu.h" #include "movement.h" #include "position.h" @@ -34,7 +33,6 @@ int main() configureDebug(); configureIHM(); - configureIMU(); configureActionneurs(); configurePosition(); configureMovement(); @@ -52,7 +50,6 @@ int main() deconfigureMovement(); deconfigurePosition(); deconfigureActionneurs(); - deconfigureIMU(); deconfigureIHM(); deconfigureDebug(); return EXIT_SUCCESS; diff --git a/chef/src/test1m.c b/chef/src/test1m.c deleted file mode 100644 index 6fc8dd4..0000000 --- a/chef/src/test1m.c +++ /dev/null @@ -1,67 +0,0 @@ -#include -#include -#include -#include -#include // random seed -#include // sleep -#include - -#include "actionneurs.h" -#include "debug.h" -#include "i2c.h" -#include "ihm.h" -#include "imu.h" -#include "movement.h" -#include "motor.h" -#include "position.h" - -pthread_mutex_t sRunning; - -void endRunning(int signal) -{ - (void)signal; - pthread_mutex_unlock(&sRunning); -} - -int main() -{ - - if (wiringPiSetup() < 0) { - fprintf(stderr, "Impossible d'initialiser WiringPi\n"); - exit(EXIT_FAILURE); - } - initI2C(); - srand(time(NULL)); - - configureDebug(); - configureIMU(); - configureActionneurs(); - configurePosition(); - configureMovement(); - startDebug(); - - debugSetActive(true); - struct position pos = {1000, 0, 0 }; - setDestination(&pos); - waitDestination(); - for (;;) { - setLoquet(false); - setLoquet(true); - } - printf("Done\n"); - - // Bloque jusqu'à l'arrivée d'un signal - pthread_mutex_init(&sRunning, NULL); - signal(SIGINT, endRunning); - signal(SIGTERM, endRunning); - signal(SIGQUIT, endRunning); - pthread_mutex_lock(&sRunning); - pthread_mutex_lock(&sRunning); - - deconfigureMovement(); - deconfigurePosition(); - deconfigureActionneurs(); - deconfigureIMU(); - deconfigureDebug(); - return EXIT_SUCCESS; -} diff --git a/chef/src/testAvance.c b/chef/src/testAvance.c index e235dec..be0097f 100644 --- a/chef/src/testAvance.c +++ b/chef/src/testAvance.c @@ -10,9 +10,8 @@ #include "debug.h" #include "i2c.h" #include "ihm.h" -#include "imu.h" -#include "movement.h" #include "motor.h" +#include "movement.h" #include "position.h" pthread_mutex_t sRunning; @@ -23,7 +22,7 @@ void endRunning(int signal) pthread_mutex_unlock(&sRunning); } -int main() +int main(int argc, char* argv[]) { if (wiringPiSetup() < 0) { @@ -33,15 +32,29 @@ int main() initI2C(); srand(time(NULL)); + float x = 0; + float y = 0; + float o = 0; + if (argc >= 2) { + sscanf(argv[1], "%f", &x); + if (argc >= 3) { + sscanf(argv[2], "%f", &y); + if (argc >= 4) { + sscanf(argv[3], "%f", &o); + } + } + } + configureDebug(); configurePosition(); configureMovement(); + debugSetActive(true); startDebug(); - debugSetActive(true); - sleep(1); - struct position pos = {100000, 0, 0 }; + struct position pos = { x, y, o }; + printf("Go\n"); setDestination(&pos); + enableAsservissement(); waitDestination(); brake(); printf("Done\n"); diff --git a/chef/src/testRetour.c b/chef/src/testRetour.c new file mode 100644 index 0000000..e5b6a5c --- /dev/null +++ b/chef/src/testRetour.c @@ -0,0 +1,71 @@ +#include +#include +#include +#include +#include // random seed +#include // sleep +#include + +#include "actionneurs.h" +#include "buttons.h" +#include "debug.h" +#include "i2c.h" +#include "lcd.h" +#include "motor.h" +#include "movement.h" +#include "position.h" + +int main() +{ + if (wiringPiSetup() < 0) { + fprintf(stderr, "Impossible d'initialiser WiringPi\n"); + exit(EXIT_FAILURE); + } + initI2C(); + srand(time(NULL)); + + configureDebug(); + configurePosition(); + configureMovement(); + configureButtons(); + initLCD(); + debugSetActive(true); + startDebug(); + + printToLCD(LCD_LINE_1, "RGE: Set origin"); + printToLCD(LCD_LINE_2, "JNE: Toggle free"); + + bool isFree = true; + disableAsservissement(); + freewheel(); + + for (;;) { + switch (pressedButton(BUT_BLOCK)) { + case jaune: + isFree = !isFree; + if (isFree) { + disableAsservissement(); + freewheel(); + } else { + enableAsservissement(); + } + break; + case rouge: + resetPosition(); + break; + default: + break; + } + clearLCD(); + if (isFree) { + printToLCD(LCD_LINE_1, "Freewheel"); + } else { + printToLCD(LCD_LINE_1, "Asservi"); + } + } + + deconfigureMovement(); + deconfigurePosition(); + deconfigureDebug(); + return EXIT_SUCCESS; +} diff --git a/chef/src/testUpDown.c b/chef/src/testUpDown.c index 9bc1734..6418c20 100644 --- a/chef/src/testUpDown.c +++ b/chef/src/testUpDown.c @@ -7,10 +7,12 @@ #include #include +#include "buttons.h" +#include "debug.h" +#include "dimensions.h" #include "lcd.h" #include "motor.h" -#include "buttons.h" -#include "dimensions.h" +#include "position.h" #define UP_TIME 1000 #define HIGH_TIME 3000 @@ -19,7 +21,8 @@ #define MAXI MOTOR_SATURATION_MAX #define INTERVAL 10 -void changerMoteursWrapper(float l, float r) { +void changerMoteursWrapper(float l, float r) +{ /* clearLCD(); */ printfToLCD(LCD_LINE_1, "L: %f", l); printfToLCD(LCD_LINE_2, "R: %f", r); @@ -36,8 +39,12 @@ int main(int argc, char* argv[]) initI2C(); initLCD(); + configureDebug(); configureButtons(); configureMotor(); + configurePosition(); + startDebug(); + debugSetActive(true); for (;;) { for (int i = 0; i < UP_TIME; i += INTERVAL) { @@ -73,5 +80,4 @@ int main(int argc, char* argv[]) changerMoteursWrapper(0, 0); delay(LOW_TIME); } - } diff --git a/fpga/Principal.vhd b/fpga/Principal.vhd index 3f4f35b..0ca8a4d 100644 --- a/fpga/Principal.vhd +++ b/fpga/Principal.vhd @@ -5,14 +5,13 @@ use IEEE.NUMERIC_STD.ALL; entity Principal is Generic( - fFpga : INTEGER := 50_000_000; - fBaud : INTEGER := 115200 + fFpga : INTEGER := 50_000_000 ); Port ( CLK : in std_logic; BTN: in std_logic; - RX: in std_logic; - TX: out std_logic; + SDA: inout std_logic; + SCL: inout std_logic; LEFTCHA: in std_logic; LEFTCHB: in std_logic; RIGHTCHA: in std_logic; @@ -25,11 +24,11 @@ entity Principal is BACKRECHO: in std_logic; ENAREF: out std_logic; ENA: out std_logic; - IN1ENC: out std_logic; + IN1: out std_logic; IN2: out std_logic; ENBREF: out std_logic; ENB: out std_logic; - IN3END: out std_logic; + IN3: out std_logic; IN4: out std_logic ); end Principal; @@ -41,7 +40,6 @@ architecture Behavioral of Principal is -- Encoder signal left : integer; signal right : integer; - signal zerocoder : std_logic; component hedm is Port ( @@ -101,11 +99,9 @@ architecture Behavioral of Principal is -- Motor controller signal enAd : std_logic_vector(7 downto 0); - signal in1enCd : std_logic_vector(7 downto 0); - signal in2d : std_logic; signal enBd : std_logic_vector(7 downto 0); - signal in3enDd : std_logic_vector(7 downto 0); - signal in4d : std_logic; + signal ind : std_logic_vector(7 downto 0); + component PWM is port ( clk : in std_logic; @@ -115,54 +111,46 @@ architecture Behavioral of Principal is end component; -- CF - component uart is - generic ( - baud : positive := fBaud; - clock_frequency : positive := fFpga - ); - port ( - clock : in std_logic; - reset : in std_logic; - data_stream_in : in std_logic_vector(7 downto 0); - data_stream_in_stb : in std_logic; - data_stream_in_ack : out std_logic; - data_stream_out : out std_logic_vector(7 downto 0); - data_stream_out_stb : out std_logic; - tx : out std_logic; - rx : in std_logic - ); + component I2CSLAVE + generic( + DEVICE: std_logic_vector(7 downto 0) := x"42" + ); + port( + MCLK : in std_logic; + nRST : in std_logic; + SDA_IN : in std_logic; + SCL_IN : in std_logic; + SDA_OUT : out std_logic; + SCL_OUT : out std_logic; + ADDRESS : out std_logic_vector(7 downto 0); + DATA_OUT : out std_logic_vector(7 downto 0); + DATA_IN : in std_logic_vector(7 downto 0); + WR : out std_logic; + RD : out std_logic + ); end component; + signal sdaIn : std_logic; + signal sclIn : std_logic; + signal sdaOut : std_logic; + signal sclOut : std_logic; + signal address : std_logic_vector(7 downto 0); + signal dataOut : std_logic_vector(7 downto 0); + signal dataIn : std_logic_vector(7 downto 0); + signal wr : std_logic; + signal rd : std_logic; + signal rdP : std_logic; - signal txData : std_logic_vector(7 downto 0); - signal txStb : std_logic := '0'; - signal txAck : std_logic := '0'; - signal rxData : std_logic_vector(7 downto 0); - signal rxStb : std_logic := '0'; - - -- Handling - component communication is - Port ( - clock : in std_logic; - reset : in std_logic; - left : in integer; - right : in integer; - zerocoder : out std_logic; - front : in integer; - back : in integer; - txData : out std_logic_vector(7 downto 0); - txStb : out std_logic; - txAck : in std_logic; - rxData : in std_logic_vector(7 downto 0); - rxStb : in std_logic; - enA : out std_logic_vector(7 downto 0); - in1enC : out std_logic_vector(7 downto 0); - in2 : out std_logic; - enB : out std_logic_vector(7 downto 0); - in3enD : out std_logic_vector(7 downto 0); - in4 : out std_logic - ); - end component; + signal leftB : std_logic_vector(15 downto 0); + signal rightB : std_logic_vector(15 downto 0); + signal frontLRawB : std_logic_vector(15 downto 0); + signal frontRRawB : std_logic_vector(15 downto 0); + signal backLRawB : std_logic_vector(15 downto 0); + signal backRRawB : std_logic_vector(15 downto 0); + signal frontLB : std_logic_vector(15 downto 0); + signal frontRB : std_logic_vector(15 downto 0); + signal backLB : std_logic_vector(15 downto 0); + signal backRB : std_logic_vector(15 downto 0); begin @@ -184,7 +172,7 @@ begin chA => LEFTCHA, chB => LEFTCHB, reset => reset, - zero => zerocoder, + zero => '0', counts => left ); @@ -193,80 +181,80 @@ begin chA => RIGHTCHA, chB => RIGHTCHB, reset => reset, - zero => zerocoder, + zero => '0', counts => right ); frontLCapt: hcsr04 port map ( - clk => CLK, - reset => reset, - echo => FRONTLECHO, - distance => frontLRaw, - trigger => FRONTTRIGGER, - start => '1', - finished => frontLFinished - ); + clk => CLK, + reset => reset, + echo => FRONTLECHO, + distance => frontLRaw, + trigger => FRONTTRIGGER, + start => '1', + finished => frontLFinished + ); frontLFilter : FIR port map ( - clock => CLK, - reset => reset, - signalIn => frontLRaw, - signalOut => frontL, - start => frontLFinished - -- done => done - ); + clock => CLK, + reset => reset, + signalIn => frontLRaw, + signalOut => frontL, + start => frontLFinished + -- done => done + ); frontRCapt: hcsr04 port map ( - clk => CLK, - reset => reset, - echo => FRONTRECHO, - distance => frontRRaw, - -- trigger => FRONTTRIGGER, - start => '1', - finished => frontRFinished - ); + clk => CLK, + reset => reset, + echo => FRONTRECHO, + distance => frontRRaw, + -- trigger => FRONTTRIGGER, + start => '1', + finished => frontRFinished + ); frontRFilter : FIR port map ( - clock => CLK, - reset => reset, - signalIn => frontRRaw, - signalOut => frontR, - start => frontRFinished - -- done => done - ); + clock => CLK, + reset => reset, + signalIn => frontRRaw, + signalOut => frontR, + start => frontRFinished + -- done => done + ); backLCapt: hcsr04 port map ( - clk => CLK, - reset => reset, - echo => BACKLECHO, - distance => backLRaw, - trigger => BACKTRIGGER, - start => '1', - finished => backLFinished - ); + clk => CLK, + reset => reset, + echo => BACKLECHO, + distance => backLRaw, + trigger => BACKTRIGGER, + start => '1', + finished => backLFinished + ); backLFilter : FIR port map ( - clock => CLK, - reset => reset, - signalIn => backLRaw, - signalOut => backL, - start => backLFinished - -- done => done - ); + clock => CLK, + reset => reset, + signalIn => backLRaw, + signalOut => backL, + start => backLFinished + -- done => done + ); backRCapt: hcsr04 port map ( - clk => CLK, - reset => reset, - echo => BACKRECHO, - distance => backRRaw, - -- trigger => BACKTRIGGER, - start => '1', - finished => backRFinished - ); + clk => CLK, + reset => reset, + echo => BACKRECHO, + distance => backRRaw, + -- trigger => BACKTRIGGER, + start => '1', + finished => backRFinished + ); backRFilter : FIR port map ( - clock => CLK, - reset => reset, - signalIn => backRRaw, - signalOut => backR, - start => backRFinished - -- done => done - ); + clock => CLK, + reset => reset, + signalIn => backRRaw, + signalOut => backR, + start => backRFinished + -- done => done + ); enAp : PWM port map ( clk => pwmClk, data => enAd, @@ -274,12 +262,8 @@ begin ); ENAREF <= '1'; - in1enCp : PWM port map ( - clk => pwmClk, - data => in1enCd, - pulse => IN1ENC - ); - IN2 <= in2d; + IN1 <= ind(0); + IN2 <= ind(1); enBp : PWM port map ( clk => pwmClk, @@ -288,48 +272,69 @@ begin ); ENBREF <= '1'; - in3enDp : PWM port map ( - clk => pwmClk, - data => in3enDd, - pulse => IN3END + IN3 <= ind(2); + IN4 <= ind(3); + + FA : i2cslave port map ( + MCLK => clk, + nRST => not reset, + SDA_IN => sdaIn, + SCL_IN => sclIn, + SDA_OUT => sdaOut, + SCL_OUT => sclOut, + ADDRESS => address, + DATA_OUT => dataOut, + DATA_IN => dataIn, + WR => wr, + RD => rd ); - IN4 <= in4d; + + SCL <= 'Z' when sclOut = '1' else '0'; + sclIn <= to_UX01(SCL); + SDA <= 'Z' when sdaOut = '1' else '0'; + sdaIn <= to_UX01(SDA); + + leftB <= std_logic_vector(to_signed(left, 16)); + rightB <= std_logic_vector(to_signed(right, 16)); + frontLRawB <= std_logic_vector(to_unsigned(frontLRaw, 16)); + frontRRawB <= std_logic_vector(to_unsigned(frontRRaw, 16)); + backLRawB <= std_logic_vector(to_unsigned(backLRaw, 16)); + backRRawB <= std_logic_vector(to_unsigned(backRRaw, 16)); + frontLB <= std_logic_vector(to_unsigned(frontL, 16)); + frontRB <= std_logic_vector(to_unsigned(frontR, 16)); + backLB <= std_logic_vector(to_unsigned(backL, 16)); + backRB <= std_logic_vector(to_unsigned(backR, 16)); + + dataIn <= x"50" when address = x"00" else + leftB(15 downto 8) when address = x"10" else + leftB(7 downto 0) when address = x"11" else + rightB(15 downto 8) when address = x"12" else + rightB(7 downto 0) when address = x"13" else + frontLRawB(15 downto 8) when address = x"20" else + frontLRawB(7 downto 0) when address = x"21" else + frontRRawB(15 downto 8) when address = x"22" else + frontRRawB(7 downto 0) when address = x"23" else + backLRawB(15 downto 8) when address = x"24" else + backLRawB(7 downto 0) when address = x"25" else + backRRawB(15 downto 8) when address = x"26" else + backRRawB(7 downto 0) when address = x"27" else + frontLB(15 downto 8) when address = x"30" else + frontLB(7 downto 0) when address = x"31" else + frontRB(15 downto 8) when address = x"32" else + frontRB(7 downto 0) when address = x"33" else + backLB(15 downto 8) when address = x"34" else + backLB(7 downto 0) when address = x"35" else + backRB(15 downto 8) when address = x"36" else + backRB(7 downto 0) when address = x"37" else + ind when address = x"60" else + enAd when address = x"61" else + enBd when address = x"62" else + (others => '0'); + + ind <= dataOut when (address = x"60" and wr = '1') else ind; + enAd <= dataOut when (address = x"61" and wr = '1') else enAd; + enBd <= dataOut when (address = x"62" and wr = '1') else enBd; - FA: uart port map( - clock => CLK, - reset => reset, - data_stream_in => txData, - data_stream_in_stb => txStb, - data_stream_in_ack => txAck, - data_stream_out => rxData, - data_stream_out_stb => rxStb, - tx => TX, - rx => RX - ); - - frontMin <= frontLRaw when frontLRaw < frontRRaw else frontRRaw; - backMin <= backLRaw when backLRaw < backRRaw else backRRaw; - - com: communication port map( - clock => CLK, - reset => reset, - left => left, - right => right, - zerocoder => zerocoder, - front => frontMin, - back => backMin, - txData => txData, - txStb => txStb, - txAck => txAck, - rxData => rxData, - rxStb => rxStb, - enA => enAd, - in1enC => in1enCd, - in2 => in2d, - enB => enBd, - in3enD => in3enDd, - in4 => in4d - ); end Behavioral; diff --git a/fpga/communication.vhd b/fpga/communication.vhd index 54d333a..63440c7 100644 --- a/fpga/communication.vhd +++ b/fpga/communication.vhd @@ -97,6 +97,12 @@ begin txStb <= '0'; zerocoder <= '0'; txData <= x"00"; + enA <= (others => '0'); + in1enC <= (others => '0'); + enB <= (others => '0'); + in3enD <= (others => '0'); + in2 <= '0'; + in4 <= '0'; else if rising_edge(clock) then zerocoder <= '0'; diff --git a/fpga/fir.vhd b/fpga/fir.vhd index 2955433..9a70e58 100644 --- a/fpga/fir.vhd +++ b/fpga/fir.vhd @@ -23,7 +23,7 @@ architecture Behavioral of fir is constant N : INTEGER := 4; -- Nombre de coefficients constant M : INTEGER := 2**6; -- Facteur multiplicatif type INT_ARRAY is array (N-1 downto 0) of integer; - constant coefficients : INT_ARRAY := (16,16,16,16); + constant coefficients : INT_ARRAY := (32,16,8,8); -- ↑ Coefficients du fir multipliés par M signal echantillons : INT_ARRAY := (others => 0); -- stockage des entrées retardées diff --git a/fpga/i2c.vhd b/fpga/i2c.vhd new file mode 100644 index 0000000..c636f81 --- /dev/null +++ b/fpga/i2c.vhd @@ -0,0 +1,283 @@ +--############################### +--# Project Name : I2C slave +--# File : i2cslave.vhd +--# Project : i2c slave for FPGA +--# Engineer : Philippe THIRION +--# Modification History +--############################### + + +-- copyright Philippe Thirion +-- github.com/tirfil +-- +-- Copyright 2016 Philippe THIRION +-- +-- This program is free software: you can redistribute it and/or modify +-- it under the terms of the GNU General Public License as published by +-- the Free Software Foundation, either version 3 of the License, or +-- (at your option) any later version. + +-- This program is distributed in the hope that it will be useful, +-- but WITHOUT ANY WARRANTY; without even the implied warranty of +-- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +-- GNU General Public License for more details. + +-- You should have received a copy of the GNU General Public License +-- along with this program. If not, see . + + +library IEEE; +use IEEE.std_logic_1164.all; +use IEEE.numeric_std.all; + +entity I2CSLAVE is + generic( + DEVICE : std_logic_vector(7 downto 0) := x"38" + ); + port( + MCLK : in std_logic; + nRST : in std_logic; + SDA_IN : in std_logic; + SCL_IN : in std_logic; + SDA_OUT : out std_logic; + SCL_OUT : out std_logic; + ADDRESS : out std_logic_vector(7 downto 0); + DATA_OUT : out std_logic_vector(7 downto 0); + DATA_IN : in std_logic_vector(7 downto 0); + WR : out std_logic; + RD : out std_logic + ); +end I2CSLAVE; + +architecture rtl of I2CSLAVE is + + type tstate is ( S_IDLE, S_START, S_SHIFTIN, S_RW, S_SENDACK, S_SENDACK2, S_SENDNACK, + S_ADDRESS, S_WRITE, S_SHIFTOUT, S_READ, S_WAITACK + ); + + type toperation is (OP_READ, OP_WRITE); + + signal state : tstate; + signal next_state : tstate; + signal operation : toperation; + + signal rising_scl, falling_scl : std_logic; + signal address_i : std_logic_vector(7 downto 0); + signal next_address : std_logic_vector(7 downto 0); + signal counter : integer range 0 to 7; + signal start_cond : std_logic; + signal stop_cond : std_logic; + signal sda_q, sda_qq, sda_qqq : std_logic; + signal scl_q, scl_qq, scl_qqq : std_logic; + signal shiftreg : std_logic_vector(7 downto 0); + signal sda: std_logic; + signal address_incr : std_logic; + signal rd_d : std_logic; +begin + + ADDRESS <= address_i; + + next_address <= (others=>'0') when (address_i = x"FF") else + std_logic_vector(to_unsigned(to_integer(unsigned( address_i )) + 1, 8)); + + S_RSY: process(MCLK,nRST) + begin + if (nRST = '0') then + sda_q <= '1'; + sda_qq <= '1'; + sda_qqq <= '1'; + scl_q <= '1'; + scl_qq <= '1'; + scl_qqq <= '1'; + elsif (MCLK'event and MCLK='1') then + sda_q <= SDA_IN; + sda_qq <= sda_q; + sda_qqq <= sda_qq; + scl_q <= SCL_IN; + scl_qq <= scl_q; + scl_qqq <= scl_qq; + end if; + end process S_RSY; + + rising_scl <= scl_qq and not scl_qqq; + falling_scl <= not scl_qq and scl_qqq; + + START_BIT: process(MCLK,nRST) + begin + if (nRST = '0') then + start_cond <= '0'; + elsif (MCLK'event and MCLK='1') then + if (sda_qqq = '1' and sda_qq = '0' and scl_qq = '1') then + start_cond <= '1'; + else + start_cond <= '0'; + end if; + end if; + end process START_BIT; + + STOP_BIT: process(MCLK,nRST) + begin + if (nRST = '0') then + stop_cond <= '0'; + elsif (MCLK'event and MCLK='1') then + if (sda_qqq = '0' and sda_qq = '1' and scl_qq = '1') then + stop_cond <= '1'; + else + stop_cond <= '0'; + end if; + end if; + end process STOP_BIT; + + sda <= sda_qq; + + RD_DELAY: process(MCLK, nRST) + begin + if (nRST = '0') then + RD <= '0'; + elsif (MCLK'event and MCLK='1') then + RD <= rd_d; + end if; + end process RD_DELAY; + + OTO: process(MCLK, nRST) + begin + if (nRST = '0') then + state <= S_IDLE; + SDA_OUT <= '1'; + SCL_OUT <= '1'; + WR <= '0'; + rd_d <= '0'; + address_i <= (others=>'0'); + DATA_OUT <= (others=>'0'); + shiftreg <= (others=>'0'); + elsif (MCLK'event and MCLK='1') then + if (stop_cond = '1') then + state <= S_IDLE; + SDA_OUT <= '1'; + SCL_OUT <= '1'; + operation <= OP_READ; + WR <= '0'; + rd_d <= '0'; + address_incr <= '0'; + elsif(start_cond = '1') then + state <= S_START; + SDA_OUT <= '1'; + SCL_OUT <= '1'; + operation <= OP_READ; + WR <= '0'; + rd_d <= '0'; + address_incr <= '0'; + elsif(state = S_IDLE) then + state <= S_IDLE; + SDA_OUT <= '1'; + SCL_OUT <= '1'; + operation <= OP_READ; + WR <= '0'; + rd_d <= '0'; + address_incr <= '0'; + elsif(state = S_START) then + shiftreg <= (others=>'0'); + state <= S_SHIFTIN; + next_state <= S_RW; + counter <= 6; + elsif(state = S_SHIFTIN) then + if (rising_scl = '1') then + shiftreg(7 downto 1) <= shiftreg(6 downto 0); + shiftreg(0) <= sda; + if (counter = 0) then + state <= next_state; + counter <= 7; + else + counter <= counter - 1; + end if; + end if; + elsif(state = S_RW) then + if (rising_scl = '1') then + if (shiftreg = DEVICE) then + state <= S_SENDACK; + if (sda = '1') then + operation <= OP_READ; + -- next_state <= S_READ; -- no needed + rd_d <= '1'; + else + operation <= OP_WRITE; + next_state <= S_ADDRESS; + address_incr <= '0'; + end if; + else + state <= S_SENDNACK; + end if; + end if; + elsif(state = S_SENDACK) then + WR <= '0'; + rd_d <= '0'; + if (falling_scl = '1') then + SDA_OUT <= '0'; + counter <= 7; + if (operation= OP_WRITE) then + state <= S_SENDACK2; + else -- OP_READ + state <= S_SHIFTOUT; + shiftreg <= DATA_IN; + end if; + end if; + elsif(state = S_SENDACK2) then + if (falling_scl = '1') then + SDA_OUT <= '1'; + state <= S_SHIFTIN; + shiftreg <= (others=>'0'); + if (address_incr = '1') then + address_i <= next_address; + end if; + end if; + elsif(state = S_SENDNACK) then + if (falling_scl = '1') then + SDA_OUT <= '1'; + state <= S_IDLE; + end if; + elsif(state = S_ADDRESS) then + address_i <= shiftreg; + next_state <= S_WRITE; + state <= S_SENDACK; + address_incr <= '0'; + elsif(state = S_WRITE) then + DATA_OUT <= shiftreg; + next_state <= S_WRITE; + state <= S_SENDACK; + WR <= '1'; + address_incr <= '1'; + elsif(state = S_SHIFTOUT) then + if (falling_scl = '1') then + SDA_OUT <= shiftreg(7); + shiftreg(7 downto 1) <= shiftreg(6 downto 0); + shiftreg(0) <= '1'; + if (counter = 0) then + state <= S_READ; + address_i <= next_address; + rd_d <= '1'; + else + counter <= counter - 1; + end if; + end if; + elsif(state = S_READ) then + rd_d <= '0'; + if (falling_scl = '1') then + SDA_OUT <= '1'; + state <= S_WAITACK; + end if; + elsif(state = S_WAITACK) then + if (rising_scl = '1') then + if (sda = '0') then + state <= S_SHIFTOUT; + counter <= 7; + shiftreg <= DATA_IN; + else + state <= S_IDLE; + end if; + end if; + end if; + end if; + end process OTO; + + +end rtl; diff --git a/fpga/principal.ucf b/fpga/principal.ucf index f1d6fc9..87720a1 100644 --- a/fpga/principal.ucf +++ b/fpga/principal.ucf @@ -8,10 +8,10 @@ TIMESPEC "TS_CLK" = PERIOD "CLK" 20 ns HIGH 50 %; NET "BTN" LOC = "P41" | IOSTANDARD = LVTTL ; # IO<10> -NET "RX" LOC = "P85" | IOSTANDARD = LVTTL ; +NET "SCL" LOC = "P85" | IOSTANDARD = LVTTL ; # IO<11> -NET "TX" LOC = "P84" | IOSTANDARD = LVTTL ; +NET "SDA" LOC = "P84" | IOSTANDARD = LVTTL ; # IO<12> NET "LEFTCHA" LOC = "P83" | IOSTANDARD = LVTTL ; @@ -44,7 +44,7 @@ NET "ENAREF" LOC = "P5" | IOSTANDARD = LVTTL ; NET "ENA" LOC = "P4" | IOSTANDARD = LVTTL ; # IO<22> -NET "IN1ENC" LOC = "P6" | IOSTANDARD = LVTTL ; +NET "IN1" LOC = "P6" | IOSTANDARD = LVTTL ; # IO<23> NET "IN2" LOC = "P98" | IOSTANDARD = LVTTL ; @@ -56,7 +56,7 @@ NET "ENBREF" LOC = "P94" | IOSTANDARD = LVTTL ; NET "ENB" LOC = "P93" | IOSTANDARD = LVTTL ; # IO<26> -NET "IN3END" LOC = "P90" | IOSTANDARD = LVTTL ; +NET "IN3" LOC = "P90" | IOSTANDARD = LVTTL ; # IO<27> NET "IN4" LOC = "P89" | IOSTANDARD = LVTTL ; diff --git a/fpga/project.cfg b/fpga/project.cfg index f233cf4..6063e63 100644 --- a/fpga/project.cfg +++ b/fpga/project.cfg @@ -5,5 +5,5 @@ PROGRAMMER = mercpcl TOPLEVEL = Principal # Prod -VHDSOURCE = $(TOPLEVEL).vhd communication.vhd uart.vhd hedm.vhd hcsr04.vhd fir.vhd pwm.vhd +VHDSOURCE = $(TOPLEVEL).vhd i2c.vhd hedm.vhd hcsr04.vhd fir.vhd pwm.vhd CONSTRAINTS = principal.ucf diff --git a/raspberrypi/Makefile b/raspberrypi/Makefile index 60b6187..b6109f4 100644 --- a/raspberrypi/Makefile +++ b/raspberrypi/Makefile @@ -130,6 +130,7 @@ restart: getlogs: ssh -F sshconf principal true rsync --rsh 'ssh -F sshconf' --archive principal:/opt/chef/log/* ../log/ + rm -f ../log/last.csv; ln -s $$(ls ../log/ | tail -1) ../log/last.csv gdbcommands: echo "set sysroot $(TARGET_DIR)" > "$@" diff --git a/simu/simu.m b/simu/simu.m index fdbbd4e..b9017db 100644 --- a/simu/simu.m +++ b/simu/simu.m @@ -3,7 +3,7 @@ SIMULATION = 0; % Paramètres de lecture DIRNAME = "/home/geoffrey/CdF/cdf2018-principal/log/"; -FILENAME = "000655.csv"; +FILENAME = "last.csv"; PATH = DIRNAME + FILENAME; % Paramètres de simulation @@ -31,9 +31,9 @@ motorSpeedGainRPMpV = 233.0; % rpm/V (from datasheet) motorSpeedGain = (motorSpeedGainRPMpV / 60.0); % motor rev/s/V motorNominalTension = 24.0; % V (from datasheet) motorControllerAlimentation = 24.0; % V (from elec) -motorControllerReference = 5; % V (from wiring) -motorSaturationMin = 0; % V (from random) -motorSaturationMax = 3.0; % V (from testing) +motorControllerReference = 5.0; % V (from wiring) +motorSaturationMin = 0.0; % V (from random) +motorSaturationMax = 4.0; % V (from testing) pwmMax = 3.3; % V (from FPGA datasheet) coderResolution = 370.0; % cycles/motor rev coderDataFactor = 4.0; % increments/motor cycles @@ -44,17 +44,37 @@ reducRatio = (cranReducIn / cranReducOut); % reduction ratio coderFullResolution = (coderDataResolution / reducRatio); % cycles / wheel rev avPerCycle = (wheelPerimeter / coderFullResolution); % mm +% Pour éviter les pics de codeuse liées à la communication I2C +absoluteMaxVitesseRobot = 10.0; % km/h +absoluteMaxVitesseRobotMMpS = (absoluteMaxVitesseRobot * 10000.0 / 36.0); % mm/s +absoluteMaxVitesseRobotRevpS = (absoluteMaxVitesseRobotMMpS / wheelPerimeter); % rev/s +absoluteMaxVitesseRobotCycpS = (absoluteMaxVitesseRobotRevpS * coderFullResolution); % cycle/s + % Constantes asservissement -global dDirEcartMin dDirEcartMax oDirEcartMin oDirEcartMax oGain oEcartMax; -dDirEcartMin = 7.0; % mm -dDirEcartMax = 10.0; % mm -oDirEcartMin = (5.0 / 360.0 * 2.0 * pi); % rad -oDirEcartMax = (25.0 / 360.0 * 2.0 * pi); % rad -oEcartMax = (25.0 / 360.0 * 2.0 * pi); % rad -oGain = 5.0; -P = 5.0; -I = 0.0; -D = 0.0; +global dDirEcartMin dDirEcartMax oDirEcartMin oDirEcartMax oEcartMin oEcartMax targetTensionRatio targetTension carotteDistance dKP dKI dKD oKP oKI oKD margeSecurite; + + +% Asservissement en distance +dDirEcartMin = 30.0; % mm +dDirEcartMax = 50.0; % mm +dKP = 0.05; +dKI = 0.0; +dKD = 0.0; +targetTensionRatio = 0.75; +targetTension = (targetTensionRatio * motorSaturationMax); % V +carotteDistance = (targetTension / dKP); % mm + +% Asservissement en angle +oDirEcartMin = (25.0 / 360.0 * 2.0 * pi); % rad +oDirEcartMax = (45.0 / 360.0 * 2.0 * pi); % rad +oEcartMin = (25.0 / 360.0 * 2.0 * pi); % rad +oEcartMax = (45.0 / 360.0 * 2.0 * pi); % rad +oKP = (motorSaturationMax / (wheelPerimeter * pi)); % au max peut dérivier de pi +oKI = 0.0; +oKD = 0.0; +carotteAngle = (targetTension / oKP); % mm + +margeSecurite = 300.0; % mm %END DIMENSIONS @@ -88,6 +108,9 @@ else T.x = T.xConnu; T.y = T.yConnu; T.o = T.oConnu; + + T.secBackL = -T.secBackL; + T.secBackR = -T.secBackR; disp("Enregistrement des données"); s = containers.Map; @@ -109,14 +132,26 @@ clf global p; % Évolution spatiale -subplot(2, 2, 1); +subplot(2, 3, 1); initGraph updateToTime(SIMULATION_DT); -% Roues -p = subplot(2, 2, 2); +% Codeuses +p = subplot(2, 3, 3); hold on; -timeGraph(["lVolt", "rVolt", "lErr", "rErr"]); +timeGraph(["lCodTot", "rCodTot", "newL", "newR"]); +addLimitline(p, 2^16-1); +addLimitline(p, 0); +title("Codeuses"); +xlabel("Temps (s)"); +ylabel("Crans"); +legend("Total gauche", "Total droite", "Brut gauche", "Brut droite"); + + +% Roues +p = subplot(2, 3, 2); +hold on; +timeGraph(["lVolt", "rVolt", "dVolt", "oVolt"]); addLimitline(p, -motorSaturationMin); addLimitline(p, -motorSaturationMax); addLimitline(p, motorSaturationMin); @@ -125,25 +160,26 @@ addLimitline(p, 0); title("Roues"); xlabel("Temps (s)"); ylabel("Tension (V)"); -legend("Gauche", "Droite", "Err. gauche", "Err. droite"); +legend("Tension gauche", "Tension droite", "Dont distance", "Dont direction"); % Distance -p = subplot(2, 2, 3); +p = subplot(2, 3, 4); hold on; -%timeGraph(["dDirEcart", "dErr"]); -timeGraph(["dDirEcart", "dErr", "secFront", "secBack"]); +timeGraph(["dDirEcart", "dEcart", "oErr"]); +addLimitline(p, 0); addLimitline(p, dDirEcartMin); addLimitline(p, dDirEcartMax); +addLimitline(p, -dDirEcartMin); +addLimitline(p, -dDirEcartMax); title("Distance"); xlabel("Temps (s)"); ylabel("Distance (mm)"); -%legend("Ecart direction", "Err. retenue"); -legend("Ecart direction", "Err. retenue", "Distance avant", "Distance arrière"); +legend("Err. distance", "Err. retenue", "Err rotation"); % Rotation -p = subplot(2, 2, 4); +p = subplot(2, 3, 5); hold on; -timeGraph(["oDirEcart", "oEcart", "oErr"]); +timeGraph(["oDirEcart", "oConsEcart", "oEcart"]); addLimitline(p, oDirEcartMax); addLimitline(p, oDirEcartMin); addLimitline(p, -oDirEcartMax); @@ -151,7 +187,19 @@ addLimitline(p, -oDirEcartMin); title("Rotation"); xlabel("Temps (s)"); ylabel("Angle (rad)"); -legend("Ecart direction", "Ecart orientation", "Err. retenue"); +legend("Err. direction", "Err. consigne", "Err. retenue"); + +% Securité +p = subplot(2, 3, 6); +hold on; +timeGraph(["dDirEcart", "secFrontL", "secFrontR", "secBackL", "secBackR", "dErr"]); +addLimitline(p, 0); +addLimitline(p, margeSecurite); +addLimitline(p, -margeSecurite); +title("Distances de sécurité"); +xlabel("Temps (s)"); +ylabel("Distance (mm)"); +legend("Err. distance", "Avant gauche", "Avant droite", "Arrière gauche", "Arrière droite", "Err. retenue"); % Fonctions @@ -202,7 +250,7 @@ function timeGraph(series) end function addLimitline(p, x) - line(p.XLim, [x x], 'Color', [0.8 0.8 0.8]); + line(p.XLim, [x x], 'Color', [0.8 0.8 0.8], 'LineStyle', '--'); end function addTimeline(p) @@ -287,9 +335,9 @@ function initGraph() global robotPath; robotPath = plot(0, 0, 'b'); global dirQuiver; - dirQuiver = quiver(0, 0, 0, 0, 'Color', 'Red', 'MaxHeadSize', height/4); + dirQuiver = quiver(0, 0, 0, 0, 'Color', 'Red', 'MaxHeadSize', height/2); global consQuiver; - consQuiver = quiver(0, 0, 0, 0, 'Color', 'Green', 'MaxHeadSize', height/4); + consQuiver = quiver(0, 0, 0, 0, 'Color', 'Green', 'MaxHeadSize', height/2); % Draw track global pisteWidth;