geoffrey/cdf2018-principal
geoffrey/cdf2018-principal
1
0
Fork 0
mirror of https://github.com/RobotechLille/cdf2018-principal synced 2025-09-04 01:05:56 +02:00
Code Issues Releases Wiki Activity

Gestion correcte de l'asservissement

Browse source
This commit is contained in:
Geoffrey Frogeye 2018-05-11 15:58:18 +02:00
parent fe3ae8efe9
commit aa519e33bf
27 changed files with 972 additions and 449 deletions
Download patch file Download diff file Expand all files Collapse all files

2
chef/Makefile
View file

@ -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
View 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
View 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

2
chef/src/diagnostics.c
View file

@ -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
View file

41
chef/src/dimensions.h
View 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

21
chef/src/i2c.c
View file

@ -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;
}

2
chef/src/i2c.h
View file

@ -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);

56
chef/src/motor.c
View file

@ -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)

7
chef/src/motor.h
View file

@ -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

239
chef/src/movement.c
View file

@ -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();

12
chef/src/movement.h
View file

@ -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

47
chef/src/position.c
View file

@ -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);
}

2
chef/src/position.h
View file

@ -8,6 +8,8 @@
// #define INVERSE_L_CODER
#define INVERSE_R_CODER
#define POSITION_INTERVAL 10
// Structures
struct __attribute__ ((packed)) position {
float x;

3
chef/src/premier.c
View file

@ -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;

67
chef/src/test1m.c
View file

@ -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;
}

25
chef/src/testAvance.c
View file

@ -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
View 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;
}

14
chef/src/testUpDown.c
View file

@ -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);
}
}