mirror of
https://github.com/RobotechLille/cdf2018-principal
synced 2024-11-24 09:06:03 +01:00
Movement
This commit is contained in:
parent
c9a7bee3b1
commit
4c44af3e4c
|
@ -40,7 +40,7 @@ bin/premier: obj/premier.o $(OBJS_O)
|
||||||
bin/test%: obj/test%.o $(OBJS_O)
|
bin/test%: obj/test%.o $(OBJS_O)
|
||||||
|
|
||||||
# Programme de test sur PC, n'embarquant pas wiringPi
|
# Programme de test sur PC, n'embarquant pas wiringPi
|
||||||
bin/local: obj/local.o obj/CF.o obj/debug.o obj/position.o
|
bin/local: obj/local.o
|
||||||
$(CC) $(CFLAGS) $(CFLAGS_CUSTOM) -lpthread -lm $^ -o $@
|
$(CC) $(CFLAGS) $(CFLAGS_CUSTOM) -lpthread -lm $^ -o $@
|
||||||
|
|
||||||
# Génération des fichiers objets
|
# Génération des fichiers objets
|
||||||
|
|
|
@ -10,7 +10,7 @@
|
||||||
|
|
||||||
#define FPGA_PORTNAME "/dev/ttyUSB0"
|
#define FPGA_PORTNAME "/dev/ttyUSB0"
|
||||||
#define CF_BAUDRATE B115200
|
#define CF_BAUDRATE B115200
|
||||||
#define PRINTRAWDATA
|
// #define PRINTRAWDATA
|
||||||
|
|
||||||
int fpga;
|
int fpga;
|
||||||
pthread_mutex_t sSendCF;
|
pthread_mutex_t sSendCF;
|
||||||
|
|
|
@ -6,8 +6,8 @@
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
|
|
||||||
// Constantes
|
// Constantes
|
||||||
#define DEBUG_INTERVAL_IDLE 50
|
#define DEBUG_INTERVAL_IDLE 1000
|
||||||
#define DEBUG_INTERVAL_ACTIVE 1000
|
#define DEBUG_INTERVAL_ACTIVE 50
|
||||||
|
|
||||||
// Structures
|
// Structures
|
||||||
enum debugArgTypes {
|
enum debugArgTypes {
|
||||||
|
|
|
@ -1,13 +1,13 @@
|
||||||
#include <unistd.h>
|
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
|
||||||
#include "diagnostics.h"
|
|
||||||
#include "buttons.h"
|
#include "buttons.h"
|
||||||
|
#include "diagnostics.h"
|
||||||
#include "lcd.h"
|
#include "lcd.h"
|
||||||
|
|
||||||
#include "CF.h"
|
#include "CF.h"
|
||||||
#include "motor.h"
|
|
||||||
#include "imu.h"
|
#include "imu.h"
|
||||||
|
#include "motor.h"
|
||||||
#include "position.h"
|
#include "position.h"
|
||||||
|
|
||||||
bool recu;
|
bool recu;
|
||||||
|
@ -19,7 +19,7 @@ void setRecu()
|
||||||
|
|
||||||
bool diagFPGA(void* arg)
|
bool diagFPGA(void* arg)
|
||||||
{
|
{
|
||||||
(void) arg;
|
(void)arg;
|
||||||
|
|
||||||
recu = false;
|
recu = false;
|
||||||
registerRxHandler(C2FD_PING, setRecu);
|
registerRxHandler(C2FD_PING, setRecu);
|
||||||
|
@ -37,37 +37,34 @@ bool diagFPGA(void* arg)
|
||||||
|
|
||||||
bool diagArduino(void* arg)
|
bool diagArduino(void* arg)
|
||||||
{
|
{
|
||||||
(void) arg;
|
(void)arg;
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool diagCodeuse(void* arg)
|
bool diagCodeuse(void* arg)
|
||||||
{
|
{
|
||||||
int i = *((int*) arg);
|
int i = *((int*)arg);
|
||||||
long lCod, rCod;
|
long lCod, rCod;
|
||||||
getCoders(&lCod, &rCod);
|
getCoders(&lCod, &rCod);
|
||||||
float tension = DIAGNOSTIC_TENSION_TEST;
|
float tension = DIAGNOSTIC_TENSION_TEST;
|
||||||
if (i % 2 == 1) { // Arrière
|
if (i % 2 == 1) { // Arrière
|
||||||
tension = - tension;
|
tension = -tension;
|
||||||
}
|
}
|
||||||
printf("49 %f\n", tension);
|
|
||||||
if (i < 2) {
|
if (i < 2) {
|
||||||
setPWMTension(tension, 0);
|
setMoteurTension(tension, 0);
|
||||||
} else {
|
} else {
|
||||||
setPWMTension(0, tension);
|
setMoteurTension(0, tension);
|
||||||
}
|
}
|
||||||
usleep(500*1000);
|
usleep(DIAGNOSTIC_TEMPS_ROTATION * 1000);
|
||||||
brake();
|
brake();
|
||||||
long lCodn, rCodn;
|
long lCodn, rCodn;
|
||||||
getCoders(&lCodn, &rCodn);
|
getCoders(&lCodn, &rCodn);
|
||||||
long diff;
|
long diff;
|
||||||
printf("60 %ld %ld %ld %ld\n", lCod, lCodn, rCod, rCodn);
|
|
||||||
if (i < 2) {
|
if (i < 2) {
|
||||||
diff = lCodn - lCod;
|
diff = lCodn - lCod;
|
||||||
} else {
|
} else {
|
||||||
diff = rCodn - rCod;
|
diff = rCodn - rCod;
|
||||||
}
|
}
|
||||||
printf("65 %ld\n", diff);
|
|
||||||
if (i % 2 == 0) { // Avant
|
if (i % 2 == 0) { // Avant
|
||||||
return (diff > DIAGNOSTIC_CODEUSES_DIFF_MIN);
|
return (diff > DIAGNOSTIC_CODEUSES_DIFF_MIN);
|
||||||
} else { // Arrière
|
} else { // Arrière
|
||||||
|
@ -77,11 +74,11 @@ bool diagCodeuse(void* arg)
|
||||||
|
|
||||||
bool diagIMU(void* arg)
|
bool diagIMU(void* arg)
|
||||||
{
|
{
|
||||||
(void) arg;
|
(void)arg;
|
||||||
return connectedIMU();
|
return connectedIMU();
|
||||||
}
|
}
|
||||||
|
|
||||||
void execDiagnostic(char *name, bool (*diagnostic)(void* arg), void* arg)
|
void execDiagnostic(char* name, bool (*diagnostic)(void* arg), void* arg)
|
||||||
{
|
{
|
||||||
clearLCD();
|
clearLCD();
|
||||||
printToLCD(LCD_LINE_1, name);
|
printToLCD(LCD_LINE_1, name);
|
||||||
|
@ -102,9 +99,12 @@ void runDiagnostics()
|
||||||
/* execDiagnostic("Lien Arduino", diagArduino); */
|
/* execDiagnostic("Lien Arduino", diagArduino); */
|
||||||
execDiagnostic("Lien IMU", diagIMU, NULL);
|
execDiagnostic("Lien IMU", diagIMU, NULL);
|
||||||
int i;
|
int i;
|
||||||
i = 0; execDiagnostic("Mot+Cod L AV", diagCodeuse, &i);
|
i = 0;
|
||||||
i = 1; execDiagnostic("Mot+Cod L AR", diagCodeuse, &i);
|
execDiagnostic("Mot+Cod L AV", diagCodeuse, &i);
|
||||||
i = 2; execDiagnostic("Mot+Cod R AV", diagCodeuse, &i);
|
i = 1;
|
||||||
i = 3; execDiagnostic("Mot+Cod R AR", diagCodeuse, &i);
|
execDiagnostic("Mot+Cod L AR", diagCodeuse, &i);
|
||||||
|
i = 2;
|
||||||
|
execDiagnostic("Mot+Cod R AV", diagCodeuse, &i);
|
||||||
|
i = 3;
|
||||||
|
execDiagnostic("Mot+Cod R AR", diagCodeuse, &i);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -4,12 +4,13 @@
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
|
|
||||||
// Constantes
|
// Constantes
|
||||||
#define DIAGNOSTIC_INTERVAL 500
|
#define DIAGNOSTIC_INTERVAL 100
|
||||||
#define DIAGNOSTIC_POLL_INTERVAL 100
|
#define DIAGNOSTIC_POLL_INTERVAL 100
|
||||||
#define DIAGNOSTIC_SERIAL_TIMEOUT 10000
|
#define DIAGNOSTIC_SERIAL_TIMEOUT 10000
|
||||||
|
|
||||||
#define DIAGNOSTIC_TENSION_TEST 1
|
#define DIAGNOSTIC_TENSION_TEST 3
|
||||||
#define DIAGNOSTIC_CODEUSES_DIFF_MIN 1000
|
#define DIAGNOSTIC_CODEUSES_DIFF_MIN 1000
|
||||||
|
#define DIAGNOSTIC_TEMPS_ROTATION 250
|
||||||
|
|
||||||
// Public
|
// Public
|
||||||
void runDiagnostics();
|
void runDiagnostics();
|
||||||
|
|
|
@ -20,9 +20,9 @@
|
||||||
#define MOTOR_NOMINAL_TENSION 24.0 // V (from datasheet)
|
#define MOTOR_NOMINAL_TENSION 24.0 // V (from datasheet)
|
||||||
#define MOTOR_CONTROLLER_ALIMENTATION 24.0 // V (from elec)
|
#define MOTOR_CONTROLLER_ALIMENTATION 24.0 // V (from elec)
|
||||||
#define MOTOR_CONTROLLER_REFERENCE 5 // V (from wiring)
|
#define MOTOR_CONTROLLER_REFERENCE 5 // V (from wiring)
|
||||||
#define MOTOR_SATURATION_MIN 1.0 // V (from random)
|
#define MOTOR_SATURATION_MIN 0.1 // V (from random)
|
||||||
#define MOTOR_SATURATION_MAX 12.0 // V (from testing)
|
#define MOTOR_SATURATION_MAX 12.0 // V (from testing)
|
||||||
#define PWM_MAX 3.5 // V (from FPGA datasheet)
|
#define PWM_MAX 3.3 // V (from FPGA datasheet)
|
||||||
#define CODER_RESOLUTION 370.0 // cycles/motor rev
|
#define CODER_RESOLUTION 370.0 // cycles/motor rev
|
||||||
#define CODER_DATA_FACTOR 4.0 // increments/motor cycles
|
#define CODER_DATA_FACTOR 4.0 // increments/motor cycles
|
||||||
#define CODER_DATA_RESOLUTION (CODER_RESOLUTION * CODER_DATA_FACTOR) // cycles/motor rev
|
#define CODER_DATA_RESOLUTION (CODER_RESOLUTION * CODER_DATA_FACTOR) // cycles/motor rev
|
||||||
|
@ -33,10 +33,11 @@
|
||||||
#define AV_PER_CYCLE (WHEEL_PERIMETER / CODER_FULL_RESOLUTION) // mm
|
#define AV_PER_CYCLE (WHEEL_PERIMETER / CODER_FULL_RESOLUTION) // mm
|
||||||
|
|
||||||
// Constantes asservissement
|
// Constantes asservissement
|
||||||
#define D_DIR_ECART_MIN 1 // mm
|
#define D_DIR_ECART_MIN 1.0 // mm
|
||||||
#define D_DIR_ECART_MAX 5 // mm
|
#define D_DIR_ECART_MAX 5.0 // mm
|
||||||
#define O_DIR_ECART_MIN 1 / 360 * 2 * M_PI // rad
|
#define O_DIR_ECART_MIN (2.5 / 360.0 * 2.0 * M_PI) // rad
|
||||||
#define O_DIR_ECART_MAX 3 / 360 * 2 * M_PI // rad
|
#define O_DIR_ECART_MAX (7.5 / 360.0 * 2.0 * M_PI) // rad
|
||||||
|
#define O_GAIN 1
|
||||||
#define P 2
|
#define P 2
|
||||||
#define I 0
|
#define I 0
|
||||||
#define D 0
|
#define D 0
|
||||||
|
|
|
@ -2,6 +2,7 @@
|
||||||
#include <signal.h>
|
#include <signal.h>
|
||||||
#include <time.h>
|
#include <time.h>
|
||||||
|
|
||||||
|
#include "position.h"
|
||||||
#include "buttons.h"
|
#include "buttons.h"
|
||||||
#include "diagnostics.h"
|
#include "diagnostics.h"
|
||||||
#include "ihm.h"
|
#include "ihm.h"
|
||||||
|
@ -115,7 +116,7 @@ void* TaskIHM(void* pdata)
|
||||||
if (bout == rouge) {
|
if (bout == rouge) {
|
||||||
clearLCD();
|
clearLCD();
|
||||||
printToLCD(LCD_LINE_1, "Calibrage...");
|
printToLCD(LCD_LINE_1, "Calibrage...");
|
||||||
delay(3000); // TODO
|
resetPosition();
|
||||||
clock_gettime(CLOCK_REALTIME, &calibrageLast);
|
clock_gettime(CLOCK_REALTIME, &calibrageLast);
|
||||||
} else if (bout == jaune) {
|
} else if (bout == jaune) {
|
||||||
break;
|
break;
|
||||||
|
|
|
@ -5,31 +5,48 @@
|
||||||
#include <time.h> // random seed
|
#include <time.h> // random seed
|
||||||
#include <unistd.h> // sleep
|
#include <unistd.h> // sleep
|
||||||
|
|
||||||
#include "CF.h"
|
pthread_mutex_t posConnu;
|
||||||
#include "debug.h"
|
pthread_cond_t newPos;
|
||||||
#include "movement.h"
|
pthread_t tPosition;
|
||||||
|
pthread_t tMovement;
|
||||||
|
|
||||||
pthread_mutex_t sRunning;
|
void* TaskPosition(void* pData)
|
||||||
|
|
||||||
void endRunning(int signal)
|
|
||||||
{
|
{
|
||||||
(void)signal;
|
|
||||||
pthread_mutex_unlock(&sRunning);
|
(void)pData;
|
||||||
|
for (;;) {
|
||||||
|
printf("Begin position\n");
|
||||||
|
sleep(1);
|
||||||
|
|
||||||
|
pthread_mutex_lock(&posConnu);
|
||||||
|
printf("Sending position\n");
|
||||||
|
pthread_cond_signal(&newPos);
|
||||||
|
pthread_mutex_unlock(&posConnu);
|
||||||
|
}
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
void* TaskMovement(void* pData)
|
||||||
|
{
|
||||||
|
(void)pData;
|
||||||
|
for (;;) {
|
||||||
|
printf("Begin Movement\n");
|
||||||
|
sleep(3);
|
||||||
|
|
||||||
|
pthread_mutex_lock(&posConnu);
|
||||||
|
printf("Waiting movement\n");
|
||||||
|
pthread_cond_wait(&newPos, &posConnu);
|
||||||
|
pthread_mutex_unlock(&posConnu);
|
||||||
|
|
||||||
|
}
|
||||||
|
return NULL;
|
||||||
}
|
}
|
||||||
|
|
||||||
int main()
|
int main()
|
||||||
{
|
{
|
||||||
|
pthread_mutex_init(&posConnu, NULL);
|
||||||
configureDebug();
|
pthread_cond_init(&newPos, NULL);
|
||||||
configureCF();
|
pthread_create(&tPosition, NULL, TaskPosition, NULL);
|
||||||
configurePosition();
|
pthread_create(&tMovement, NULL, TaskMovement, NULL);
|
||||||
|
sleep(300);
|
||||||
/* long lCod, rCod; */
|
|
||||||
for (;;) {
|
|
||||||
sleep(1);
|
|
||||||
/* getCoders(&lCod, &rCod); */
|
|
||||||
/* printf("%ld %ld\n", lCod, rCod); */
|
|
||||||
/* usleep(100*1000); */
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -2,20 +2,17 @@
|
||||||
|
|
||||||
uint8_t tensionToPWM(float V)
|
uint8_t tensionToPWM(float V)
|
||||||
{
|
{
|
||||||
printf("PWM tension %f\n", V);
|
|
||||||
if (V >= PWM_MAX) {
|
if (V >= PWM_MAX) {
|
||||||
return UINT8_MAX;
|
return UINT8_MAX;
|
||||||
} else if (V <= 0) {
|
} else if (V <= 0) {
|
||||||
return 0;
|
return 0;
|
||||||
} else {
|
} else {
|
||||||
printf("PWM value %d\n", (uint8_t) (V * UINT8_MAX / PWM_MAX));
|
|
||||||
return V * UINT8_MAX / PWM_MAX;
|
return V * UINT8_MAX / PWM_MAX;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t moteurTensionToPWM(float V)
|
uint8_t moteurTensionToPWM(float V)
|
||||||
{
|
{
|
||||||
printf("Moteur tension %f\n", V);
|
|
||||||
if (V >= MOTOR_CONTROLLER_ALIMENTATION) {
|
if (V >= MOTOR_CONTROLLER_ALIMENTATION) {
|
||||||
V = MOTOR_CONTROLLER_ALIMENTATION;
|
V = MOTOR_CONTROLLER_ALIMENTATION;
|
||||||
} else if (V <= 0) {
|
} else if (V <= 0) {
|
||||||
|
@ -44,7 +41,7 @@ void setMoteurTensionRaw(float lVolt, float rVolt, bool lFor, bool rFor)
|
||||||
#endif
|
#endif
|
||||||
if (rVolt > 0) {
|
if (rVolt > 0) {
|
||||||
msg.in |= 1 << (rFor ? IN3 : IN4);
|
msg.in |= 1 << (rFor ? IN3 : IN4);
|
||||||
msg.enb = moteurTensionToPWM(lVolt);
|
msg.enb = moteurTensionToPWM(rVolt);
|
||||||
} else {
|
} else {
|
||||||
// Nothing needs to be changed for this motor controller
|
// Nothing needs to be changed for this motor controller
|
||||||
}
|
}
|
||||||
|
|
|
@ -1,21 +1,160 @@
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
#include <stdbool.h>
|
#include <pthread.h>
|
||||||
#include <stdio.h>
|
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
|
||||||
#include "CF.h"
|
#include "debug.h"
|
||||||
#include "dimensions.h"
|
|
||||||
#include "motor.h"
|
#include "motor.h"
|
||||||
#include "movement.h"
|
#include "movement.h"
|
||||||
|
|
||||||
|
pthread_t tMovement;
|
||||||
|
struct position cons;
|
||||||
|
pthread_mutex_t movCons;
|
||||||
|
pthread_mutex_t movEnableMutex;
|
||||||
|
pthread_cond_t movEnableCond;
|
||||||
|
bool movEnableBool;
|
||||||
|
|
||||||
|
float xDiff;
|
||||||
|
float yDiff;
|
||||||
|
float oEcart;
|
||||||
|
float dDirEcart;
|
||||||
|
float oDirEcart;
|
||||||
|
float dErr;
|
||||||
|
float oErr;
|
||||||
|
bool oRetenu;
|
||||||
|
bool dRetenu;
|
||||||
|
float lErr;
|
||||||
|
float rErr;
|
||||||
|
unsigned int nbCalcCons;
|
||||||
|
|
||||||
void configureMovement()
|
void configureMovement()
|
||||||
{
|
{
|
||||||
stop();
|
stop();
|
||||||
|
|
||||||
|
nbCalcCons = 0;
|
||||||
|
pthread_mutex_init(&movCons, NULL);
|
||||||
|
|
||||||
|
pthread_mutex_init(&movEnableMutex, NULL);
|
||||||
|
pthread_cond_init(&movEnableCond, NULL);
|
||||||
|
movEnableBool = false;
|
||||||
|
|
||||||
|
pthread_create(&tMovement, NULL, TaskMovement, NULL);
|
||||||
|
|
||||||
|
registerDebugVar("xCons", f, &cons.x);
|
||||||
|
registerDebugVar("yCons", f, &cons.y);
|
||||||
|
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("dDirEcart", f, &dDirEcart);
|
||||||
|
registerDebugVar("oDirEcart", f, &oDirEcart);
|
||||||
|
registerDebugVar("dRetenu", d, &dRetenu);
|
||||||
|
registerDebugVar("oRetenu", d, &oRetenu);
|
||||||
|
registerDebugVar("lErr", f, &lErr);
|
||||||
|
registerDebugVar("rErr", f, &rErr);
|
||||||
|
registerDebugVar("nbCalcCons", d, &nbCalcCons);
|
||||||
|
|
||||||
|
disableConsigne();
|
||||||
}
|
}
|
||||||
|
|
||||||
void aller(struct position* pos);
|
void setDestination(struct position* pos)
|
||||||
|
{
|
||||||
|
pthread_mutex_lock(&movCons);
|
||||||
|
memcpy(&cons, pos, sizeof(struct position));
|
||||||
|
pthread_mutex_unlock(&movCons);
|
||||||
|
}
|
||||||
|
|
||||||
|
float angleGap(float target, float actual)
|
||||||
|
{
|
||||||
|
return fmod(target - actual + M_PI, 2 * M_PI) - M_PI;
|
||||||
|
}
|
||||||
|
|
||||||
|
void* TaskMovement(void* pData)
|
||||||
|
{
|
||||||
|
(void)pData;
|
||||||
|
|
||||||
|
unsigned int lastPosCalc = 0;
|
||||||
|
struct position connu;
|
||||||
|
|
||||||
|
oRetenu = true;
|
||||||
|
dRetenu = true;
|
||||||
|
|
||||||
|
for (;;) {
|
||||||
|
|
||||||
|
// Test if enabled
|
||||||
|
pthread_mutex_lock(&movEnableMutex);
|
||||||
|
while (!movEnableBool) {
|
||||||
|
pthread_cond_wait(&movEnableCond, &movEnableMutex);
|
||||||
|
}
|
||||||
|
pthread_mutex_unlock(&movEnableMutex);
|
||||||
|
|
||||||
|
lastPosCalc = getPositionNewer(&connu, lastPosCalc);
|
||||||
|
// Destination → ordre
|
||||||
|
pthread_mutex_lock(&movCons);
|
||||||
|
xDiff = cons.x - connu.x;
|
||||||
|
yDiff = cons.y - connu.y;
|
||||||
|
oEcart = angleGap(cons.o, connu.o);
|
||||||
|
|
||||||
|
dDirEcart = hypotf(xDiff, yDiff);
|
||||||
|
oDirEcart = angleGap(atan2(yDiff, xDiff), connu.o);
|
||||||
|
pthread_mutex_unlock(&movCons);
|
||||||
|
|
||||||
|
if (dDirEcart > D_DIR_ECART_MAX) {
|
||||||
|
oRetenu = true;
|
||||||
|
} else if (dDirEcart < D_DIR_ECART_MIN) {
|
||||||
|
oRetenu = false;
|
||||||
|
}
|
||||||
|
oErr = oRetenu ? oDirEcart : oEcart;
|
||||||
|
|
||||||
|
float oDirEcartAbs = fabs(oDirEcart);
|
||||||
|
if (oDirEcartAbs > O_DIR_ECART_MAX) {
|
||||||
|
dRetenu = true;
|
||||||
|
} else if (oDirEcartAbs < O_DIR_ECART_MIN) {
|
||||||
|
dRetenu = false;
|
||||||
|
}
|
||||||
|
dErr = dRetenu ? 0 : dDirEcart;
|
||||||
|
|
||||||
|
// Ordre → Volt
|
||||||
|
float dErrRev = dErr / WHEEL_PERIMETER;
|
||||||
|
float oErrRev = O_GAIN * oErr * DISTANCE_BETWEEN_WHEELS / WHEEL_PERIMETER;
|
||||||
|
|
||||||
|
lErr = dErrRev - oErrRev;
|
||||||
|
rErr = dErrRev + oErrRev;
|
||||||
|
|
||||||
|
// PID
|
||||||
|
float lVoltCons = P * lErr;
|
||||||
|
float rVoltCons = P * rErr;
|
||||||
|
|
||||||
|
setMoteurTension(lVoltCons, rVoltCons);
|
||||||
|
|
||||||
|
nbCalcCons++;
|
||||||
|
}
|
||||||
|
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
void deconfigureMovement()
|
void deconfigureMovement()
|
||||||
{
|
{
|
||||||
stop();
|
stop();
|
||||||
|
pthread_cancel(tMovement);
|
||||||
|
}
|
||||||
|
|
||||||
|
void enableConsigne()
|
||||||
|
{
|
||||||
|
pthread_mutex_lock(&movEnableMutex);
|
||||||
|
movEnableBool = true;
|
||||||
|
pthread_cond_signal(&movEnableCond);
|
||||||
|
pthread_mutex_unlock(&movEnableMutex);
|
||||||
|
}
|
||||||
|
|
||||||
|
void disableConsigne()
|
||||||
|
{
|
||||||
|
pthread_mutex_lock(&movEnableMutex);
|
||||||
|
movEnableBool = false;
|
||||||
|
// No signal here, will be disabled on next run
|
||||||
|
pthread_mutex_unlock(&movEnableMutex);
|
||||||
}
|
}
|
||||||
|
|
|
@ -9,5 +9,9 @@
|
||||||
|
|
||||||
void configureMovement();
|
void configureMovement();
|
||||||
void deconfigureMovement();
|
void deconfigureMovement();
|
||||||
|
void setDestination(struct position* pos);
|
||||||
|
void* TaskMovement(void* pData);
|
||||||
|
void enableConsigne();
|
||||||
|
void disableConsigne();
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -32,6 +32,7 @@ void prepareParcours(bool orange)
|
||||||
resetPoints();
|
resetPoints();
|
||||||
showPoints();
|
showPoints();
|
||||||
printRightLCD(LCD_LINE_2, isOrange ? "Org" : "Vrt");
|
printRightLCD(LCD_LINE_2, isOrange ? "Org" : "Vrt");
|
||||||
|
enableConsigne();
|
||||||
}
|
}
|
||||||
|
|
||||||
void startParcours()
|
void startParcours()
|
||||||
|
@ -71,6 +72,7 @@ void stopParcours()
|
||||||
{
|
{
|
||||||
pthread_cancel(tParcours);
|
pthread_cancel(tParcours);
|
||||||
stop();
|
stop();
|
||||||
|
disableConsigne();
|
||||||
|
|
||||||
updateTimeDisplay();
|
updateTimeDisplay();
|
||||||
printRightLCD(LCD_LINE_1, "FIN");
|
printRightLCD(LCD_LINE_1, "FIN");
|
||||||
|
@ -78,42 +80,24 @@ void stopParcours()
|
||||||
debugSetActive(false);
|
debugSetActive(false);
|
||||||
}
|
}
|
||||||
|
|
||||||
#define UP_TIME 1000
|
|
||||||
#define HIGH_TIME 3000
|
|
||||||
#define DOWN_TIME 1000
|
|
||||||
#define LOW_TIME 2000
|
|
||||||
#define MAX_VIT 1
|
|
||||||
|
|
||||||
void* TaskParcours(void* pdata)
|
void* TaskParcours(void* pdata)
|
||||||
{
|
{
|
||||||
(void)pdata;
|
(void)pdata;
|
||||||
|
|
||||||
for (;;) {
|
struct position dest1 = {0, 0, 0};
|
||||||
setPWMTension(MAX_VIT, MAX_VIT);
|
setDestination(&dest1);
|
||||||
}
|
|
||||||
|
|
||||||
for (;;) {
|
sleep(1);
|
||||||
addPoints(1);
|
|
||||||
for (int i = 0; i < UP_TIME; i++) {
|
|
||||||
float p = (float)i / (float)UP_TIME;
|
|
||||||
setPWMTension(p * MAX_VIT, p * MAX_VIT);
|
|
||||||
usleep(1000 * 1);
|
|
||||||
}
|
|
||||||
addPoints(1);
|
|
||||||
setPWMTension(MAX_VIT, MAX_VIT);
|
|
||||||
usleep(1000 * HIGH_TIME);
|
|
||||||
|
|
||||||
addPoints(1);
|
struct position dest2 = {300, -300, 0};
|
||||||
for (int i = 0; i < DOWN_TIME; i++) {
|
setDestination(&dest2);
|
||||||
float p = (float)i / (float)DOWN_TIME;
|
|
||||||
p = 1 - p;
|
sleep(5);
|
||||||
setPWMTension(p * MAX_VIT, p * MAX_VIT);
|
/* */
|
||||||
usleep(1000 * 1);
|
/* struct position dest3 = {1000, 1000, 0}; */
|
||||||
}
|
/* setDestination(&dest3); */
|
||||||
addPoints(1);
|
/* */
|
||||||
setPWMTension(0, 0);
|
/* sleep(5); */
|
||||||
usleep(1000 * LOW_TIME);
|
|
||||||
}
|
|
||||||
|
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
|
|
@ -2,17 +2,21 @@
|
||||||
* Fonctions de calcul de la position du robot
|
* Fonctions de calcul de la position du robot
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
|
||||||
#include "debug.h"
|
#include "debug.h"
|
||||||
#include "position.h"
|
|
||||||
#include "dimensions.h"
|
#include "dimensions.h"
|
||||||
|
#include "position.h"
|
||||||
|
|
||||||
// Globales
|
// Globales
|
||||||
struct position connu;
|
struct position connu;
|
||||||
struct F2CI_CODERs deltaCoders;
|
struct F2CI_CODERs deltaCoders;
|
||||||
pthread_mutex_t posPolling;
|
pthread_mutex_t posPolling;
|
||||||
|
pthread_mutex_t posConnu;
|
||||||
|
pthread_cond_t newPos;
|
||||||
pthread_t tPosition;
|
pthread_t tPosition;
|
||||||
|
|
||||||
// Globales
|
// Globales
|
||||||
|
@ -23,12 +27,11 @@ void* TaskPosition(void* pData)
|
||||||
{
|
{
|
||||||
(void)pData;
|
(void)pData;
|
||||||
|
|
||||||
|
resetPosition();
|
||||||
nbCalcPos = 0;
|
nbCalcPos = 0;
|
||||||
lCodTot = 0;
|
lCodTot = 0;
|
||||||
rCodTot = 0;
|
rCodTot = 0;
|
||||||
|
|
||||||
pthread_mutex_init(&posPolling, NULL);
|
|
||||||
|
|
||||||
for (;;) {
|
for (;;) {
|
||||||
|
|
||||||
// Sending
|
// Sending
|
||||||
|
@ -40,7 +43,6 @@ void* TaskPosition(void* pData)
|
||||||
pthread_mutex_unlock(&posPolling);
|
pthread_mutex_unlock(&posPolling);
|
||||||
|
|
||||||
// Calculation
|
// Calculation
|
||||||
nbCalcPos++;
|
|
||||||
#ifdef INVERSE_L_CODER
|
#ifdef INVERSE_L_CODER
|
||||||
deltaCoders.dL = -deltaCoders.dL;
|
deltaCoders.dL = -deltaCoders.dL;
|
||||||
#endif
|
#endif
|
||||||
|
@ -56,11 +58,17 @@ void* TaskPosition(void* pData)
|
||||||
float deltaO = atan2(dR - dL, DISTANCE_BETWEEN_WHEELS);
|
float deltaO = atan2(dR - dL, DISTANCE_BETWEEN_WHEELS);
|
||||||
float deltaD = (dL + dR) / 2;
|
float deltaD = (dL + dR) / 2;
|
||||||
|
|
||||||
|
pthread_mutex_lock(&posConnu);
|
||||||
connu.o += deltaO;
|
connu.o += deltaO;
|
||||||
float deltaX = deltaD * cos(connu.o);
|
float deltaX = deltaD * cos(connu.o);
|
||||||
float deltaY = deltaD * sin(connu.o);
|
float deltaY = deltaD * sin(connu.o);
|
||||||
connu.x += deltaX;
|
connu.x += deltaX;
|
||||||
connu.y += deltaY;
|
connu.y += deltaY;
|
||||||
|
nbCalcPos++;
|
||||||
|
pthread_cond_signal(&newPos);
|
||||||
|
pthread_mutex_unlock(&posConnu);
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return NULL;
|
return NULL;
|
||||||
|
@ -74,6 +82,9 @@ void onF2CI_CODER()
|
||||||
|
|
||||||
void configurePosition()
|
void configurePosition()
|
||||||
{
|
{
|
||||||
|
pthread_mutex_init(&posPolling, NULL);
|
||||||
|
pthread_mutex_init(&posConnu, NULL);
|
||||||
|
pthread_cond_init(&newPos, NULL);
|
||||||
registerRxHandler(F2CI_CODER, onF2CI_CODER);
|
registerRxHandler(F2CI_CODER, onF2CI_CODER);
|
||||||
registerDebugVar("lCodTot", ld, &lCodTot);
|
registerDebugVar("lCodTot", ld, &lCodTot);
|
||||||
registerDebugVar("rCodTot", ld, &rCodTot);
|
registerDebugVar("rCodTot", ld, &rCodTot);
|
||||||
|
@ -96,5 +107,36 @@ void getCoders(long* l, long* r)
|
||||||
{
|
{
|
||||||
*l = lCodTot;
|
*l = lCodTot;
|
||||||
*r = rCodTot;
|
*r = rCodTot;
|
||||||
|
}
|
||||||
|
|
||||||
|
unsigned int getPosition(struct position* pos)
|
||||||
|
{
|
||||||
|
pthread_mutex_lock(&posConnu);
|
||||||
|
unsigned int nb = nbCalcPos;
|
||||||
|
memcpy(pos, &connu, sizeof(struct position));
|
||||||
|
pthread_mutex_unlock(&posConnu);
|
||||||
|
return nb;
|
||||||
|
}
|
||||||
|
|
||||||
|
unsigned int getPositionNewer(struct position* pos, unsigned int lastCalc)
|
||||||
|
{
|
||||||
|
pthread_mutex_lock(&posConnu);
|
||||||
|
while (nbCalcPos <= lastCalc) {
|
||||||
|
pthread_cond_wait(&newPos, &posConnu);
|
||||||
|
}
|
||||||
|
pthread_mutex_unlock(&posConnu);
|
||||||
|
return getPosition(pos);
|
||||||
|
}
|
||||||
|
|
||||||
|
void setPosition(struct position* pos)
|
||||||
|
{
|
||||||
|
pthread_mutex_lock(&posConnu);
|
||||||
|
memcpy(&connu, pos, sizeof(struct position));
|
||||||
|
pthread_mutex_unlock(&posConnu);
|
||||||
|
}
|
||||||
|
|
||||||
|
void resetPosition()
|
||||||
|
{
|
||||||
|
struct position pos = {0, 0, 0};
|
||||||
|
setPosition(&pos);
|
||||||
}
|
}
|
||||||
|
|
|
@ -22,6 +22,10 @@ struct __attribute__ ((packed)) position {
|
||||||
void configurePosition();
|
void configurePosition();
|
||||||
void deconfigurePosition();
|
void deconfigurePosition();
|
||||||
void getCoders(long* l, long* r);
|
void getCoders(long* l, long* r);
|
||||||
|
unsigned int getPositionNewer(struct position* pos, unsigned int lastCalc);
|
||||||
|
unsigned int getPosition(struct position* pos);
|
||||||
|
void setPosition(struct position* pos);
|
||||||
|
void resetPosition();
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -36,8 +36,8 @@ int main()
|
||||||
configureIHM();
|
configureIHM();
|
||||||
configureCF();
|
configureCF();
|
||||||
configureIMU();
|
configureIMU();
|
||||||
configureMovement();
|
|
||||||
configurePosition();
|
configurePosition();
|
||||||
|
configureMovement();
|
||||||
startDebug();
|
startDebug();
|
||||||
startIHM();
|
startIHM();
|
||||||
|
|
||||||
|
@ -49,8 +49,8 @@ int main()
|
||||||
pthread_mutex_lock(&sRunning);
|
pthread_mutex_lock(&sRunning);
|
||||||
pthread_mutex_lock(&sRunning);
|
pthread_mutex_lock(&sRunning);
|
||||||
|
|
||||||
deconfigurePosition();
|
|
||||||
deconfigureMovement();
|
deconfigureMovement();
|
||||||
|
deconfigurePosition();
|
||||||
deconfigureIMU();
|
deconfigureIMU();
|
||||||
deconfigureCF();
|
deconfigureCF();
|
||||||
deconfigureIHM();
|
deconfigureIHM();
|
||||||
|
|
16
chef/src/testFree.c
Normal file
16
chef/src/testFree.c
Normal file
|
@ -0,0 +1,16 @@
|
||||||
|
/* Teste si une broche est connecté à une autre */
|
||||||
|
|
||||||
|
#include <stdlib.h>
|
||||||
|
|
||||||
|
#include "CF.h"
|
||||||
|
#include "motor.h"
|
||||||
|
|
||||||
|
int main(int argc, char* argv[])
|
||||||
|
{
|
||||||
|
|
||||||
|
(void)argc;
|
||||||
|
(void)argv;
|
||||||
|
|
||||||
|
configureCF();
|
||||||
|
freewheel();
|
||||||
|
}
|
55
chef/src/testParcours.c
Normal file
55
chef/src/testParcours.c
Normal file
|
@ -0,0 +1,55 @@
|
||||||
|
#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 "CF.h"
|
||||||
|
#include "debug.h"
|
||||||
|
#include "i2c.h"
|
||||||
|
#include "imu.h"
|
||||||
|
#include "movement.h"
|
||||||
|
#include "buttons.h"
|
||||||
|
#include "parcours.h"
|
||||||
|
#include "position.h"
|
||||||
|
|
||||||
|
pthread_mutex_t sRunning;
|
||||||
|
|
||||||
|
int main()
|
||||||
|
{
|
||||||
|
|
||||||
|
if (wiringPiSetup() < 0) {
|
||||||
|
fprintf(stderr, "Impossible d'initialiser WiringPi\n");
|
||||||
|
exit(EXIT_FAILURE);
|
||||||
|
}
|
||||||
|
initI2C();
|
||||||
|
srand(time(NULL));
|
||||||
|
|
||||||
|
configureDebug();
|
||||||
|
configureCF();
|
||||||
|
configureIMU();
|
||||||
|
configurePosition();
|
||||||
|
configureMovement();
|
||||||
|
startDebug();
|
||||||
|
|
||||||
|
configureParcours();
|
||||||
|
prepareParcours(false);
|
||||||
|
startParcours();
|
||||||
|
|
||||||
|
int toWait;
|
||||||
|
while ((toWait = updateParcours()) >= 0) {
|
||||||
|
if (pressedButton(toWait) != none) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
stopParcours();
|
||||||
|
|
||||||
|
deconfigureMovement();
|
||||||
|
deconfigurePosition();
|
||||||
|
deconfigureIMU();
|
||||||
|
deconfigureCF();
|
||||||
|
deconfigureDebug();
|
||||||
|
return EXIT_SUCCESS;
|
||||||
|
}
|
|
@ -14,7 +14,7 @@
|
||||||
|
|
||||||
#define VIT 1
|
#define VIT 1
|
||||||
#define VITL VIT
|
#define VITL VIT
|
||||||
#define VITR 0
|
#define VITR VIT
|
||||||
|
|
||||||
|
|
||||||
void setPWMTensionWrapper(float l, float r) {
|
void setPWMTensionWrapper(float l, float r) {
|
||||||
|
|
|
@ -21,6 +21,12 @@ s()
|
||||||
/opt/chef/bin/testStop
|
/opt/chef/bin/testStop
|
||||||
}
|
}
|
||||||
|
|
||||||
|
f()
|
||||||
|
{
|
||||||
|
/etc/init.d/S50chef stop
|
||||||
|
/opt/chef/bin/testFree
|
||||||
|
}
|
||||||
|
|
||||||
l()
|
l()
|
||||||
{
|
{
|
||||||
tail -f $(find /opt/chef/log | sort | tail -1)
|
tail -f $(find /opt/chef/log | sort | tail -1)
|
||||||
|
|
Loading…
Reference in a new issue