mirror of
https://github.com/RobotechLille/cdf2018-principal
synced 2024-11-21 07:36:03 +01:00
Gestion correcte de l'asservissement
This commit is contained in:
parent
fe3ae8efe9
commit
aa519e33bf
|
|
@ -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
|
||||
|
|
|
|||
46
chef/src/common.c
Normal file
46
chef/src/common.c
Normal file
|
|
@ -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;
|
||||
}
|
||||
|
||||
21
chef/src/common.h
Normal file
21
chef/src/common.h
Normal file
|
|
@ -0,0 +1,21 @@
|
|||
#ifndef __COMMON_H_
|
||||
#define __COMMON_H_
|
||||
|
||||
#include <time.h>
|
||||
|
||||
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
|
||||
|
|
@ -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);
|
||||
|
|
|
|||
0
chef/src/dimensions.c
Normal file
0
chef/src/dimensions.c
Normal file
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -1,8 +1,8 @@
|
|||
#include <pthread.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <stdbool.h>
|
||||
#include <wiringPiI2C.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -3,6 +3,8 @@
|
|||
|
||||
#include <stdint.h>
|
||||
|
||||
#define I2C_DRIVEN_HIGH_RETRIES 3
|
||||
|
||||
void initI2C();
|
||||
int openI2C(uint8_t address);
|
||||
uint8_t readI2C(int fd, uint8_t reg);
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -5,7 +5,9 @@
|
|||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#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();
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -8,6 +8,8 @@
|
|||
// #define INVERSE_L_CODER
|
||||
#define INVERSE_R_CODER
|
||||
|
||||
#define POSITION_INTERVAL 10
|
||||
|
||||
// Structures
|
||||
struct __attribute__ ((packed)) position {
|
||||
float x;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -1,67 +0,0 @@
|
|||
#include <pthread.h>
|
||||
#include <signal.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <time.h> // random seed
|
||||
#include <unistd.h> // sleep
|
||||
#include <wiringPi.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
|
@ -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");
|
||||
|
|
|
|||
71
chef/src/testRetour.c
Normal file
71
chef/src/testRetour.c
Normal file
|
|
@ -0,0 +1,71 @@
|
|||
#include <pthread.h>
|
||||
#include <signal.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <time.h> // random seed
|
||||
#include <unistd.h> // sleep
|
||||
#include <wiringPi.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
|
@ -7,10 +7,12 @@
|
|||
#include <wiringPi.h>
|
||||
#include <wiringPiI2C.h>
|
||||
|
||||
#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);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
component I2CSLAVE
|
||||
generic(
|
||||
DEVICE: std_logic_vector(7 downto 0) := x"42"
|
||||
);
|
||||
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
|
||||
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,7 +181,7 @@ begin
|
|||
chA => RIGHTCHA,
|
||||
chB => RIGHTCHB,
|
||||
reset => reset,
|
||||
zero => zerocoder,
|
||||
zero => '0',
|
||||
counts => right
|
||||
);
|
||||
|
||||
|
|
@ -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
|
||||
);
|
||||
IN4 <= in4d;
|
||||
IN3 <= ind(2);
|
||||
IN4 <= ind(3);
|
||||
|
||||
|
||||
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
|
||||
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
|
||||
);
|
||||
|
||||
frontMin <= frontLRaw when frontLRaw < frontRRaw else frontRRaw;
|
||||
backMin <= backLRaw when backLRaw < backRRaw else backRRaw;
|
||||
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;
|
||||
|
||||
|
||||
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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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';
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
283
fpga/i2c.vhd
Normal file
283
fpga/i2c.vhd
Normal file
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
|
||||
|
||||
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;
|
||||
|
|
@ -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 ;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)" > "$@"
|
||||
|
|
|
|||
108
simu/simu.m
108
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
|
||||
|
||||
|
|
@ -89,6 +109,9 @@ else
|
|||
T.y = T.yConnu;
|
||||
T.o = T.oConnu;
|
||||
|
||||
T.secBackL = -T.secBackL;
|
||||
T.secBackR = -T.secBackR;
|
||||
|
||||
disp("Enregistrement des données");
|
||||
s = containers.Map;
|
||||
for name=T.Properties.VariableNames
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in a new issue