mirror of
https://github.com/RobotechLille/cdf2018-principal
synced 2024-11-21 15:46:06 +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)
|
||||
|
||||
# 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 $@
|
||||
|
||||
# Génération des fichiers objets
|
||||
|
|
|
@ -10,7 +10,7 @@
|
|||
|
||||
#define FPGA_PORTNAME "/dev/ttyUSB0"
|
||||
#define CF_BAUDRATE B115200
|
||||
#define PRINTRAWDATA
|
||||
// #define PRINTRAWDATA
|
||||
|
||||
int fpga;
|
||||
pthread_mutex_t sSendCF;
|
||||
|
|
|
@ -6,8 +6,8 @@
|
|||
#include <stdbool.h>
|
||||
|
||||
// Constantes
|
||||
#define DEBUG_INTERVAL_IDLE 50
|
||||
#define DEBUG_INTERVAL_ACTIVE 1000
|
||||
#define DEBUG_INTERVAL_IDLE 1000
|
||||
#define DEBUG_INTERVAL_ACTIVE 50
|
||||
|
||||
// Structures
|
||||
enum debugArgTypes {
|
||||
|
|
|
@ -1,13 +1,13 @@
|
|||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "diagnostics.h"
|
||||
#include "buttons.h"
|
||||
#include "diagnostics.h"
|
||||
#include "lcd.h"
|
||||
|
||||
#include "CF.h"
|
||||
#include "motor.h"
|
||||
#include "imu.h"
|
||||
#include "motor.h"
|
||||
#include "position.h"
|
||||
|
||||
bool recu;
|
||||
|
@ -19,7 +19,7 @@ void setRecu()
|
|||
|
||||
bool diagFPGA(void* arg)
|
||||
{
|
||||
(void) arg;
|
||||
(void)arg;
|
||||
|
||||
recu = false;
|
||||
registerRxHandler(C2FD_PING, setRecu);
|
||||
|
@ -37,37 +37,34 @@ bool diagFPGA(void* arg)
|
|||
|
||||
bool diagArduino(void* arg)
|
||||
{
|
||||
(void) arg;
|
||||
(void)arg;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool diagCodeuse(void* arg)
|
||||
{
|
||||
int i = *((int*) arg);
|
||||
int i = *((int*)arg);
|
||||
long lCod, rCod;
|
||||
getCoders(&lCod, &rCod);
|
||||
float tension = DIAGNOSTIC_TENSION_TEST;
|
||||
if (i % 2 == 1) { // Arrière
|
||||
tension = - tension;
|
||||
tension = -tension;
|
||||
}
|
||||
printf("49 %f\n", tension);
|
||||
if (i < 2) {
|
||||
setPWMTension(tension, 0);
|
||||
setMoteurTension(tension, 0);
|
||||
} else {
|
||||
setPWMTension(0, tension);
|
||||
setMoteurTension(0, tension);
|
||||
}
|
||||
usleep(500*1000);
|
||||
usleep(DIAGNOSTIC_TEMPS_ROTATION * 1000);
|
||||
brake();
|
||||
long lCodn, rCodn;
|
||||
getCoders(&lCodn, &rCodn);
|
||||
long diff;
|
||||
printf("60 %ld %ld %ld %ld\n", lCod, lCodn, rCod, rCodn);
|
||||
if (i < 2) {
|
||||
diff = lCodn - lCod;
|
||||
} else {
|
||||
diff = rCodn - rCod;
|
||||
}
|
||||
printf("65 %ld\n", diff);
|
||||
if (i % 2 == 0) { // Avant
|
||||
return (diff > DIAGNOSTIC_CODEUSES_DIFF_MIN);
|
||||
} else { // Arrière
|
||||
|
@ -77,11 +74,11 @@ bool diagCodeuse(void* arg)
|
|||
|
||||
bool diagIMU(void* arg)
|
||||
{
|
||||
(void) arg;
|
||||
(void)arg;
|
||||
return connectedIMU();
|
||||
}
|
||||
|
||||
void execDiagnostic(char *name, bool (*diagnostic)(void* arg), void* arg)
|
||||
void execDiagnostic(char* name, bool (*diagnostic)(void* arg), void* arg)
|
||||
{
|
||||
clearLCD();
|
||||
printToLCD(LCD_LINE_1, name);
|
||||
|
@ -102,9 +99,12 @@ void runDiagnostics()
|
|||
/* execDiagnostic("Lien Arduino", diagArduino); */
|
||||
execDiagnostic("Lien IMU", diagIMU, NULL);
|
||||
int i;
|
||||
i = 0; execDiagnostic("Mot+Cod L AV", diagCodeuse, &i);
|
||||
i = 1; 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);
|
||||
i = 0;
|
||||
execDiagnostic("Mot+Cod L AV", diagCodeuse, &i);
|
||||
i = 1;
|
||||
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>
|
||||
|
||||
// Constantes
|
||||
#define DIAGNOSTIC_INTERVAL 500
|
||||
#define DIAGNOSTIC_INTERVAL 100
|
||||
#define DIAGNOSTIC_POLL_INTERVAL 100
|
||||
#define DIAGNOSTIC_SERIAL_TIMEOUT 10000
|
||||
|
||||
#define DIAGNOSTIC_TENSION_TEST 1
|
||||
#define DIAGNOSTIC_TENSION_TEST 3
|
||||
#define DIAGNOSTIC_CODEUSES_DIFF_MIN 1000
|
||||
#define DIAGNOSTIC_TEMPS_ROTATION 250
|
||||
|
||||
// Public
|
||||
void runDiagnostics();
|
||||
|
|
|
@ -20,9 +20,9 @@
|
|||
#define MOTOR_NOMINAL_TENSION 24.0 // V (from datasheet)
|
||||
#define MOTOR_CONTROLLER_ALIMENTATION 24.0 // V (from elec)
|
||||
#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 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_DATA_FACTOR 4.0 // increments/motor cycles
|
||||
#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
|
||||
|
||||
// Constantes asservissement
|
||||
#define D_DIR_ECART_MIN 1 // mm
|
||||
#define D_DIR_ECART_MAX 5 // mm
|
||||
#define O_DIR_ECART_MIN 1 / 360 * 2 * M_PI // rad
|
||||
#define O_DIR_ECART_MAX 3 / 360 * 2 * M_PI // rad
|
||||
#define D_DIR_ECART_MIN 1.0 // mm
|
||||
#define D_DIR_ECART_MAX 5.0 // mm
|
||||
#define O_DIR_ECART_MIN (2.5 / 360.0 * 2.0 * 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 I 0
|
||||
#define D 0
|
||||
|
|
|
@ -2,6 +2,7 @@
|
|||
#include <signal.h>
|
||||
#include <time.h>
|
||||
|
||||
#include "position.h"
|
||||
#include "buttons.h"
|
||||
#include "diagnostics.h"
|
||||
#include "ihm.h"
|
||||
|
@ -115,7 +116,7 @@ void* TaskIHM(void* pdata)
|
|||
if (bout == rouge) {
|
||||
clearLCD();
|
||||
printToLCD(LCD_LINE_1, "Calibrage...");
|
||||
delay(3000); // TODO
|
||||
resetPosition();
|
||||
clock_gettime(CLOCK_REALTIME, &calibrageLast);
|
||||
} else if (bout == jaune) {
|
||||
break;
|
||||
|
|
|
@ -5,31 +5,48 @@
|
|||
#include <time.h> // random seed
|
||||
#include <unistd.h> // sleep
|
||||
|
||||
#include "CF.h"
|
||||
#include "debug.h"
|
||||
#include "movement.h"
|
||||
pthread_mutex_t posConnu;
|
||||
pthread_cond_t newPos;
|
||||
pthread_t tPosition;
|
||||
pthread_t tMovement;
|
||||
|
||||
pthread_mutex_t sRunning;
|
||||
|
||||
void endRunning(int signal)
|
||||
void* TaskPosition(void* pData)
|
||||
{
|
||||
(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()
|
||||
{
|
||||
|
||||
configureDebug();
|
||||
configureCF();
|
||||
configurePosition();
|
||||
|
||||
/* long lCod, rCod; */
|
||||
for (;;) {
|
||||
sleep(1);
|
||||
/* getCoders(&lCod, &rCod); */
|
||||
/* printf("%ld %ld\n", lCod, rCod); */
|
||||
/* usleep(100*1000); */
|
||||
}
|
||||
|
||||
pthread_mutex_init(&posConnu, NULL);
|
||||
pthread_cond_init(&newPos, NULL);
|
||||
pthread_create(&tPosition, NULL, TaskPosition, NULL);
|
||||
pthread_create(&tMovement, NULL, TaskMovement, NULL);
|
||||
sleep(300);
|
||||
}
|
||||
|
|
|
@ -2,20 +2,17 @@
|
|||
|
||||
uint8_t tensionToPWM(float V)
|
||||
{
|
||||
printf("PWM tension %f\n", V);
|
||||
if (V >= PWM_MAX) {
|
||||
return UINT8_MAX;
|
||||
} else if (V <= 0) {
|
||||
return 0;
|
||||
} else {
|
||||
printf("PWM value %d\n", (uint8_t) (V * UINT8_MAX / PWM_MAX));
|
||||
return V * UINT8_MAX / PWM_MAX;
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t moteurTensionToPWM(float V)
|
||||
{
|
||||
printf("Moteur tension %f\n", V);
|
||||
if (V >= MOTOR_CONTROLLER_ALIMENTATION) {
|
||||
V = MOTOR_CONTROLLER_ALIMENTATION;
|
||||
} else if (V <= 0) {
|
||||
|
@ -44,7 +41,7 @@ void setMoteurTensionRaw(float lVolt, float rVolt, bool lFor, bool rFor)
|
|||
#endif
|
||||
if (rVolt > 0) {
|
||||
msg.in |= 1 << (rFor ? IN3 : IN4);
|
||||
msg.enb = moteurTensionToPWM(lVolt);
|
||||
msg.enb = moteurTensionToPWM(rVolt);
|
||||
} else {
|
||||
// Nothing needs to be changed for this motor controller
|
||||
}
|
||||
|
|
|
@ -1,21 +1,160 @@
|
|||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdio.h>
|
||||
#include <pthread.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "CF.h"
|
||||
#include "dimensions.h"
|
||||
#include "debug.h"
|
||||
#include "motor.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()
|
||||
{
|
||||
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()
|
||||
{
|
||||
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 deconfigureMovement();
|
||||
void setDestination(struct position* pos);
|
||||
void* TaskMovement(void* pData);
|
||||
void enableConsigne();
|
||||
void disableConsigne();
|
||||
|
||||
#endif
|
||||
|
|
|
@ -32,6 +32,7 @@ void prepareParcours(bool orange)
|
|||
resetPoints();
|
||||
showPoints();
|
||||
printRightLCD(LCD_LINE_2, isOrange ? "Org" : "Vrt");
|
||||
enableConsigne();
|
||||
}
|
||||
|
||||
void startParcours()
|
||||
|
@ -71,6 +72,7 @@ void stopParcours()
|
|||
{
|
||||
pthread_cancel(tParcours);
|
||||
stop();
|
||||
disableConsigne();
|
||||
|
||||
updateTimeDisplay();
|
||||
printRightLCD(LCD_LINE_1, "FIN");
|
||||
|
@ -78,42 +80,24 @@ void stopParcours()
|
|||
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)pdata;
|
||||
|
||||
for (;;) {
|
||||
setPWMTension(MAX_VIT, MAX_VIT);
|
||||
}
|
||||
struct position dest1 = {0, 0, 0};
|
||||
setDestination(&dest1);
|
||||
|
||||
for (;;) {
|
||||
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);
|
||||
sleep(1);
|
||||
|
||||
addPoints(1);
|
||||
for (int i = 0; i < DOWN_TIME; i++) {
|
||||
float p = (float)i / (float)DOWN_TIME;
|
||||
p = 1 - p;
|
||||
setPWMTension(p * MAX_VIT, p * MAX_VIT);
|
||||
usleep(1000 * 1);
|
||||
}
|
||||
addPoints(1);
|
||||
setPWMTension(0, 0);
|
||||
usleep(1000 * LOW_TIME);
|
||||
}
|
||||
struct position dest2 = {300, -300, 0};
|
||||
setDestination(&dest2);
|
||||
|
||||
sleep(5);
|
||||
/* */
|
||||
/* struct position dest3 = {1000, 1000, 0}; */
|
||||
/* setDestination(&dest3); */
|
||||
/* */
|
||||
/* sleep(5); */
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
|
|
@ -2,17 +2,21 @@
|
|||
* Fonctions de calcul de la position du robot
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "debug.h"
|
||||
#include "position.h"
|
||||
#include "dimensions.h"
|
||||
#include "position.h"
|
||||
|
||||
// Globales
|
||||
struct position connu;
|
||||
struct F2CI_CODERs deltaCoders;
|
||||
pthread_mutex_t posPolling;
|
||||
pthread_mutex_t posConnu;
|
||||
pthread_cond_t newPos;
|
||||
pthread_t tPosition;
|
||||
|
||||
// Globales
|
||||
|
@ -23,12 +27,11 @@ void* TaskPosition(void* pData)
|
|||
{
|
||||
(void)pData;
|
||||
|
||||
resetPosition();
|
||||
nbCalcPos = 0;
|
||||
lCodTot = 0;
|
||||
rCodTot = 0;
|
||||
|
||||
pthread_mutex_init(&posPolling, NULL);
|
||||
|
||||
for (;;) {
|
||||
|
||||
// Sending
|
||||
|
@ -40,7 +43,6 @@ void* TaskPosition(void* pData)
|
|||
pthread_mutex_unlock(&posPolling);
|
||||
|
||||
// Calculation
|
||||
nbCalcPos++;
|
||||
#ifdef INVERSE_L_CODER
|
||||
deltaCoders.dL = -deltaCoders.dL;
|
||||
#endif
|
||||
|
@ -56,11 +58,17 @@ void* TaskPosition(void* pData)
|
|||
float deltaO = atan2(dR - dL, DISTANCE_BETWEEN_WHEELS);
|
||||
float deltaD = (dL + dR) / 2;
|
||||
|
||||
pthread_mutex_lock(&posConnu);
|
||||
connu.o += deltaO;
|
||||
float deltaX = deltaD * cos(connu.o);
|
||||
float deltaY = deltaD * sin(connu.o);
|
||||
connu.x += deltaX;
|
||||
connu.y += deltaY;
|
||||
nbCalcPos++;
|
||||
pthread_cond_signal(&newPos);
|
||||
pthread_mutex_unlock(&posConnu);
|
||||
|
||||
|
||||
}
|
||||
|
||||
return NULL;
|
||||
|
@ -74,6 +82,9 @@ void onF2CI_CODER()
|
|||
|
||||
void configurePosition()
|
||||
{
|
||||
pthread_mutex_init(&posPolling, NULL);
|
||||
pthread_mutex_init(&posConnu, NULL);
|
||||
pthread_cond_init(&newPos, NULL);
|
||||
registerRxHandler(F2CI_CODER, onF2CI_CODER);
|
||||
registerDebugVar("lCodTot", ld, &lCodTot);
|
||||
registerDebugVar("rCodTot", ld, &rCodTot);
|
||||
|
@ -96,5 +107,36 @@ void getCoders(long* l, long* r)
|
|||
{
|
||||
*l = lCodTot;
|
||||
*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 deconfigurePosition();
|
||||
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
|
||||
|
||||
|
|
|
@ -36,8 +36,8 @@ int main()
|
|||
configureIHM();
|
||||
configureCF();
|
||||
configureIMU();
|
||||
configureMovement();
|
||||
configurePosition();
|
||||
configureMovement();
|
||||
startDebug();
|
||||
startIHM();
|
||||
|
||||
|
@ -49,8 +49,8 @@ int main()
|
|||
pthread_mutex_lock(&sRunning);
|
||||
pthread_mutex_lock(&sRunning);
|
||||
|
||||
deconfigurePosition();
|
||||
deconfigureMovement();
|
||||
deconfigurePosition();
|
||||
deconfigureIMU();
|
||||
deconfigureCF();
|
||||
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 VITL VIT
|
||||
#define VITR 0
|
||||
#define VITR VIT
|
||||
|
||||
|
||||
void setPWMTensionWrapper(float l, float r) {
|
||||
|
|
|
@ -21,6 +21,12 @@ s()
|
|||
/opt/chef/bin/testStop
|
||||
}
|
||||
|
||||
f()
|
||||
{
|
||||
/etc/init.d/S50chef stop
|
||||
/opt/chef/bin/testFree
|
||||
}
|
||||
|
||||
l()
|
||||
{
|
||||
tail -f $(find /opt/chef/log | sort | tail -1)
|
||||
|
|
Loading…
Reference in a new issue