1
0
Fork 0
mirror of https://github.com/RobotechLille/cdf2018-principal synced 2024-12-22 05:30:37 +01:00
This commit is contained in:
Geoffrey Frogeye 2018-05-06 18:35:26 +02:00
parent c9a7bee3b1
commit 4c44af3e4c
19 changed files with 369 additions and 102 deletions

View file

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

View file

@ -10,7 +10,7 @@
#define FPGA_PORTNAME "/dev/ttyUSB0"
#define CF_BAUDRATE B115200
#define PRINTRAWDATA
// #define PRINTRAWDATA
int fpga;
pthread_mutex_t sSendCF;

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

@ -9,5 +9,9 @@
void configureMovement();
void deconfigureMovement();
void setDestination(struct position* pos);
void* TaskMovement(void* pData);
void enableConsigne();
void disableConsigne();
#endif

View file

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

View file

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

View file

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

View file

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

View file

@ -14,7 +14,7 @@
#define VIT 1
#define VITL VIT
#define VITR 0
#define VITR VIT
void setPWMTensionWrapper(float l, float r) {

View file

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