geoffrey/cdf2018-principal
geoffrey/cdf2018-principal
1
0
Fork 0
mirror of https://github.com/RobotechLille/cdf2018-principal synced 2024-05-02 04:06:43 +00:00
Code Issues Releases Wiki Activity

Serial → I2C

Browse source
This commit is contained in:
Geoffrey Frogeye 2018-05-11 05:34:06 +02:00
parent f730d7b2b1
commit fe3ae8efe9
34 changed files with 207 additions and 872 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
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 CF debug diagnostics i2c imu ihm lcd motor movement parcours points position securite
OBJS=actionneurs buttons CA debug diagnostics fpga i2c imu ihm lcd motor movement parcours points position securite
OBJS_O=$(addprefix obj/,$(addsuffix .o,$(OBJS)))
# VARIABLES AUTOMATIQUES
@ -32,7 +32,7 @@ default: bin/premier bin/local $(subst src,bin,$(subst .c,,$(wildcard src/test*.
# Génération des fichiers éxecutables
bin/%: obj/%.o $(OBJS_O)
$(CC) $(LDFLAGS) $(LDFLAGS_CUSTOM) $^ -o $@
$(OBJCOPY) --only-keep-debug $@ $@.debug
# $(OBJCOPY) --only-keep-debug $@ $@.debug
$(STRIP) --strip-debug --strip-unneeded $@
# Binaires (dont il faut spécifier les objets explicitement)

232
chef/src/CF.c
View file

@ -1,232 +0,0 @@
#include "CF.h"
#include <fcntl.h> // O_*
#include <stdio.h> // printf...
#include <stdlib.h> // stuff
#include <string.h> // memcpy
#include <strings.h> // bzero
#include <unistd.h> // read(), write()...
int fpga;
pthread_mutex_t sSendCF;
pthread_t tReaderAF;
rxHandler rxHandlersAF[256];
bool pret;
void printDataCF(void* data, size_t size)
{
printf(" ");
unsigned char* p = data;
for (size_t i = 0; i < size; i++) {
if (*p >= ' ' && *p <= '~') {
printf(" %c", *p);
} else {
printf(" %02x", *p);
}
p++;
}
printf("\n");
}
void configureFpga()
{
// Connection au port série
printf("Connexion à %s... ", FPGA_PORTNAME);
fflush(stdout);
fpga = open(FPGA_PORTNAME, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
if (fpga < 0) {
printf("Échec !\n");
exit(1);
}
// Configuration du port série
fcntl(fpga, F_SETFL, O_RDWR);
struct termios cfg;
tcgetattr(fpga, &cfg);
cfmakeraw(&cfg);
cfsetispeed(&cfg, CF_BAUDRATE);
cfsetospeed(&cfg, CF_BAUDRATE);
cfg.c_cflag |= (CLOCAL | CREAD);
cfg.c_cflag &= ~PARENB;
cfg.c_cflag &= ~CSTOPB;
cfg.c_cflag &= ~CSIZE;
cfg.c_cflag |= CS8;
cfg.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
cfg.c_oflag &= ~OPOST;
cfg.c_cc[VMIN] = 0;
cfg.c_cc[VTIME] = 10;
if (tcsetattr(fpga, TCSANOW, &cfg) < 0) {
perror("serialConfig.tcsetattr");
exit(1);
}
int status;
ioctl(fpga, TIOCMGET, &status);
status |= TIOCM_DTR;
status |= TIOCM_RTS;
ioctl(fpga, TIOCMSET, &status);
usleep(10 * 1000);
// Flush
unsigned char trash[1024];
read(fpga, &trash, sizeof(trash));
printf("OK!\n");
}
void deconfigureFpga()
{
close(fpga);
printf("Déconnecté\n");
}
void registerRxHandlerCF(unsigned char code, rxHandler handler)
{
rxHandlersAF[code] = handler;
}
void* TaskReaderAF(void* pdata)
{
(void)pdata;
unsigned char code;
for (;;) {
code = readByteCF();
#ifdef PRINTRAWDATA
printf("↓");
printDataCF(&code, sizeof(code));
#endif
rxHandler handler = rxHandlersAF[code];
if (handler != NULL) {
handler();
} else {
printf("Code inconnu: %x (%c)\n", code, code);
}
}
return NULL;
}
void onF2CD_ERR()
{
struct F2CD_ERRs s;
readCF(&s, sizeof(struct F2CD_ERRs));
printf("Erreur reçue : %c (%2x)\n", s.code, s.code);
}
void setPretCF()
{
pret = true;
}
void doNothingCF()
{
}
void configureCF()
{
configureFpga();
for (int i = 0; i < 256; i++) {
rxHandlersAF[i] = NULL;
}
pthread_mutex_init(&sSendCF, NULL);
pthread_create(&tReaderAF, NULL, TaskReaderAF, NULL);
printf("Attente de réponse du Fpga... ");
fflush(stdout);
// Dans le cas où on aurait laissé l'Fpga en attente de donnée,
// on envoie des pings en boucle jusqu'à ce qu'il nous réponde.
pret = false;
registerRxHandlerCF(C2FD_PING, setPretCF);
while (!pret) {
sendCF(C2FD_PING, NULL, 0);
usleep(100 * 1000);
}
registerRxHandlerCF(C2FD_PING, doNothingCF); // TODO
registerRxHandlerCF(C2FD_PING, NULL);
printf("OK !\n");
registerRxHandlerCF(F2CD_ERR, onF2CD_ERR);
}
void deconfigureCF()
{
deconfigureFpga();
}
void sendByteCF(unsigned char data)
{
write(fpga, &data, sizeof(data));
#ifdef PRINTRAWDATA
printf("↑");
printDataCF(&data, sizeof(data));
#endif
}
void sendCF(unsigned char code, void* data, size_t size)
{
pthread_mutex_lock(&sSendCF);
sendByteCF(code);
if (size > 0) {
unsigned char* p = data;
for (size_t i = 0; i < size; i++) {
write(fpga, p, sizeof(unsigned char));
p++;
}
// Envoyer plus d'un octet d'un coup curieusement il aime pas ça du tout
}
pthread_mutex_unlock(&sSendCF);
#ifdef PRINTRAWDATA
if (size > 0) {
printf("↑");
printDataCF(data, size);
}
#endif
}
unsigned char readByteCF()
{
unsigned char c;
while (read(fpga, &c, sizeof(c)) < 1) {
sleep(0);
}
return c;
#ifdef PRINTRAWDATA
printf("↓");
printDataCF(&c, sizeof(c));
#endif
}
void readCF(void* data, size_t size)
{
size_t remaining = size;
int justRead;
char* p = data;
do {
justRead = read(fpga, p, remaining);
if (justRead > 0) {
p += justRead;
remaining -= justRead;
}
} while (remaining > 0);
#ifdef PRINTRAWDATA
printf("↓");
printDataCF(data, size);
#endif
}

25
chef/src/CF.h
View file

@ -1,25 +0,0 @@
#ifndef __CF_H_
#define __CF_H_
#include <sys/ioctl.h>
#include <pthread.h>
#include <termios.h> // baudrates
#include <stdbool.h>
#include "CFsignals.h"
#define FPGA_PORTNAME "/dev/ttyUSB0"
#define CF_BAUDRATE B115200
// #define PRINTRAWDATA
typedef void (*rxHandler)(void);
void registerRxHandlerCF(unsigned char code, rxHandler handler); // À utiliser après configureCF();
void sendByteCF(unsigned char data); // Privé
void sendCF(unsigned char code, void* data, size_t size);
unsigned char readByteCF(); // À utiliser uniquement depuis un rxHandler
void readCF(void* data, size_t size); // À utiliser uniquement depuis un rxHandler
void configureCF();
void deconfigureCF();
#endif

78
chef/src/CFsignals.h
View file

@ -1,78 +0,0 @@
/*
* Définition des signaux échagés entre l'Arduino et le chef
*/
#ifndef __CFSIGNALS_H_
#define __CFSIGNALS_H_
#include <stdint.h>
// Structures used everywhere
// D: Direct
// Sens naturel : Envoi de donnée
// Sens inverse : Accusé de réception
// I: Indirect
// Sens inverse : Demande de donnée
// Sens naturel : Envoi de donnée
// T: Trigger
// Sens inverse : Paramètrage du trigger
// Sens naturel : Envoi de trigger
// Arduino → FPGA
// Pour le debug
#define C2FD_PING 'P'
// FPGA → Arduino
// Erreur quelconque
#define F2CD_ERR 'E'
struct __attribute__ ((packed)) F2CD_ERRs {
unsigned char code;
};
#define ERR_UNKNOWN_CODE 'C'
// Récupère les valeur des encodeurs
#define F2CI_CODER 'D'
struct __attribute__ ((packed)) F2CI_CODERs {
int16_t dL;
int16_t dR;
};
// Récupère les valeur des capteurs de distance
#define F2CI_CAPT 'C'
struct __attribute__ ((packed)) F2CI_CAPTs {
uint16_t front;
uint16_t back;
};
// Récupère les valeur des capteurs de distance (trigger au supérieur)
#define F2CT_CAPT 'c'
// /!\ Structure de la requête de trigger (A→F). Les données seront envoyées avec F2CI_CAPT
struct __attribute__ ((packed)) F2CT_CAPTs {
uint16_t front;
uint16_t back;
};
// Met à jour la PWM (pour ctrl moteur (EN, IN × 2) × 2)
#define C2FD_PWM 'W'
struct __attribute__ ((packed)) C2FD_PWMs {
uint8_t ena;
uint8_t enb;
uint8_t in;
};
// Met à jour la PWM (pour ctrl moteur (EN × 2) × 2)
#define C2FD_PWM2 'w'
struct __attribute__ ((packed)) C2FD_PWM2s {
uint8_t ena;
uint8_t enc;
uint8_t enb;
uint8_t end;
};
#endif

26
chef/src/diagnostics.c
View file

@ -7,7 +7,7 @@
#include "lcd.h"
#include "CA.h"
#include "CF.h"
#include "fpga.h"
#include "actionneurs.h"
#include "imu.h"
#include "motor.h"
@ -20,24 +20,6 @@ void setRecu()
recu = true;
}
bool diagFPGA(void* arg)
{
(void)arg;
recu = false;
registerRxHandlerCF(C2FD_PING, setRecu);
sendCF(C2FD_PING, NULL, 0);
for (int i = 0; i <= DIAGNOSTIC_SERIAL_TIMEOUT; i += DIAGNOSTIC_POLL_INTERVAL) {
if (recu) {
break;
}
usleep(DIAGNOSTIC_POLL_INTERVAL * 1000);
}
registerRxHandlerCF(C2FD_PING, NULL);
return recu;
}
bool diagArduino(void* arg)
{
(void)arg;
@ -87,6 +69,12 @@ bool diagCodeuse(void* arg)
}
}
bool diagFPGA(void* arg)
{
(void)arg;
return connectedFPGA();
}
bool diagIMU(void* arg)
{
(void)arg;

5
chef/src/dimensions.h
View file

@ -19,8 +19,8 @@
#define MOTOR_SPEED_GAIN (MOTOR_SPEED_GAIN_RPMP_V / 60.0) // motor rev/s/V
#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 0 // V (from random)
#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 PWM_MAX 3.3 // V (from FPGA datasheet)
#define CODER_RESOLUTION 370.0 // cycles/motor rev
@ -42,5 +42,6 @@
#define P 5.0
#define I 0.0
#define D 0.0
#define M 0.0
#endif

21
chef/src/fpga.c Normal file
View file

@ -0,0 +1,21 @@
#include <stdlib.h>
#include <stdio.h>
#include "fpga.h"
int fpgaFd = -1;
int fdFPGA()
{
if (fpgaFd < 0) {
fpgaFd = openI2C(FPGA_ADDRESS);
if (fpgaFd < 0) {
perror("fdFPGA.openI2C");
}
}
return fpgaFd;
}
bool connectedFPGA()
{
return readI2C(fdFPGA(), FPGA_WHO_AM_I) == FPGA_IDENTITY;
}

43
chef/src/fpga.h Normal file
View file

@ -0,0 +1,43 @@
#ifndef __FPGA_H__
#define __FPGA_H__
#include <stdbool.h>
#include "i2c.h"
#define FPGA_ADDRESS 0x42
#define FPGA_WHO_AM_I 0x00
#define FPGA_IDENTITY 0x50
// Read
#define CODER_LEFT_H 0x10
#define CODER_LEFT_L 0x11
#define CODER_RIGHT_H 0x12
#define CODER_RIGHT_L 0x13
#define CAPT_FRONT_LEFT_H_RAW 0x20
#define CAPT_FRONT_LEFT_L_RAW 0x21
#define CAPT_FRONT_RIGHT_H_RAW 0x22
#define CAPT_FRONT_RIGHT_L_RAW 0x23
#define CAPT_BACK_LEFT_H_RAW 0x24
#define CAPT_BACK_LEFT_L_RAW 0x25
#define CAPT_BACK_RIGHT_H_RAW 0x26
#define CAPT_BACK_RIGHT_L_RAW 0x27
#define CAPT_FRONT_LEFT_H 0x30
#define CAPT_FRONT_LEFT_L 0x31
#define CAPT_FRONT_RIGHT_H 0x32
#define CAPT_FRONT_RIGHT_L 0x33
#define CAPT_BACK_LEFT_H 0x34
#define CAPT_BACK_LEFT_L 0x35
#define CAPT_BACK_RIGHT_H 0x36
#define CAPT_BACK_RIGHT_L 0x37
// Write
#define MOTOR_IN 0x60
#define MOTOR_ENA 0x61
#define MOTOR_ENB 0x62
int fdFPGA();
bool connectedFPGA();
#endif

27
chef/src/i2c.c
View file

@ -1,18 +1,25 @@
#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <stdbool.h>
#include <wiringPiI2C.h>
#include "i2c.h"
pthread_mutex_t sI2C;
bool i2cInited = false;
void initI2C()
{
pthread_mutex_init(&sI2C, NULL);
if (!i2cInited) {
pthread_mutex_init(&sI2C, NULL);
i2cInited = true;
}
}
int openI2C(int8_t address)
int openI2C(uint8_t address)
{
lockI2C();
int fd = wiringPiI2CSetup(address);
@ -24,24 +31,24 @@ int openI2C(int8_t address)
return fd;
}
int8_t readI2C(int fd, int8_t reg)
uint8_t readI2C(int fd, uint8_t reg)
{
lockI2C();
int8_t res = wiringPiI2CReadReg8(fd, reg);
uint8_t res = wiringPiI2CReadReg8(fd, reg);
unlockI2C();
return res;
}
void writeI2C(int fd, int8_t reg, int8_t data)
void writeI2C(int fd, uint8_t reg, uint8_t data)
{
lockI2C();
int res = wiringPiI2CWriteReg8(fd, reg, data);
unlockI2C();
if (res < 0) {
int delay = 1;
while (wiringPiI2CWriteReg8(fd, reg, data) < 0) {
perror("wiringPiI2CWriteReg8");
exit(EXIT_FAILURE);
usleep(delay);
delay *= 2;
}
unlockI2C();
}
void lockI2C()

6
chef/src/i2c.h
View file

@ -4,9 +4,9 @@
#include <stdint.h>
void initI2C();
int openI2C(int8_t address);
int8_t readI2C(int fd, int8_t reg);
void writeI2C(int fd, int8_t reg, int8_t data);
int openI2C(uint8_t address);
uint8_t readI2C(int fd, uint8_t reg);
void writeI2C(int fd, uint8_t reg, uint8_t data);
// Get full control over the I2C communication
// Note: Don't use methods from this library

6
chef/src/lcd.c
View file

@ -37,7 +37,7 @@ void eraseLCD()
void onLCD()
{
digitalWrite(LCD_ON_PIN, LOW);
delay(100);
delay(50);
// TODO More details
sendLCD(0x33, LCD_MODE_CMD); // Initialise
@ -47,13 +47,13 @@ void onLCD()
sendLCD(0x28, LCD_MODE_CMD); // Data length, number of lines, font size
eraseLCD();
delay(50);
delay(25);
}
void offLCD()
{
digitalWrite(LCD_ON_PIN, HIGH);
delay(100);
delay(50);
}
void resetLCD()

35
chef/src/motor.c
View file

@ -3,6 +3,7 @@
#include "actionneurs.h"
#include "debug.h"
#include "motor.h"
#include "fpga.h"
float lVolt;
float rVolt;
@ -15,9 +16,6 @@ pthread_mutex_t motCons;
pthread_t tMotor;
enum motorState motState = braking;
static struct C2FD_PWMs msgBrake = { 0, 0, UINT8_MAX };
static struct C2FD_PWMs msgFree = { 0, 0, (1 << IN1) | (1 << IN2) | (1 << IN3) | (1 << IN4) };
void configureMotor()
{
registerDebugVar("lVolt", f, &lVolt);
@ -57,29 +55,33 @@ void setMoteurTensionRaw(float l, float r, bool lFor, bool rFor)
lVolt = l;
rVolt = r;
uint8_t enA = 0;
uint8_t enB = 0;
uint8_t in = 0;
#ifdef INVERSE_L_MOTOR
lFor = !lFor;
#endif
static struct C2FD_PWMs msg;
msg.in = 0x00;
if (l > 0) {
msg.in |= 1 << (lFor ? IN1 : IN2);
msg.ena = moteurTensionToPWM(l);
in |= 1 << (lFor ? IN1 : IN2);
enA = moteurTensionToPWM(l);
} else {
// Nothing needs to be changed for this motor controller
// Stay at 0 : brake mode
}
#ifdef INVERSE_R_MOTOR
rFor = !rFor;
#endif
if (r > 0) {
msg.in |= 1 << (rFor ? IN3 : IN4);
msg.enb = moteurTensionToPWM(r);
in |= 1 << (rFor ? IN3 : IN4);
enB = moteurTensionToPWM(r);
} else {
// Nothing needs to be changed for this motor controller
// Stay at 0 : brake mode
}
sendCF(C2FD_PWM, &msg, sizeof(struct C2FD_PWMs));
writeI2C(fdFPGA(), MOTOR_IN, in);
writeI2C(fdFPGA(), MOTOR_ENA, enA);
writeI2C(fdFPGA(), MOTOR_ENB, enB);
}
void* TaskMotor(void* pData)
@ -151,7 +153,6 @@ void* TaskMotor(void* pData)
break;
case freewheeling:
rawFreewheel();
sendCF(C2FD_PWM, &msgFree, sizeof(struct C2FD_PWMs));
break;
}
@ -165,13 +166,17 @@ void rawBrake()
{
lVolt = 0;
rVolt = 0;
sendCF(C2FD_PWM, &msgBrake, sizeof(struct C2FD_PWMs));
writeI2C(fdFPGA(), MOTOR_IN, 0);
writeI2C(fdFPGA(), MOTOR_ENA, UINT8_MAX);
writeI2C(fdFPGA(), MOTOR_ENB, UINT8_MAX);
}
void rawFreewheel()
{
lVolt = 0;
rVolt = 0;
sendCF(C2FD_PWM, &msgFree, sizeof(struct C2FD_PWMs));
writeI2C(fdFPGA(), MOTOR_IN, (1 << IN1) | (1 << IN2) | (1 << IN3) | (1 << IN4));
writeI2C(fdFPGA(), MOTOR_ENA, 0);
writeI2C(fdFPGA(), MOTOR_ENA, 0);
}
void setMoteurTension(float l, float r)

2
chef/src/motor.h
View file

@ -3,8 +3,8 @@
#include <math.h>
#include <stdint.h>
#include <stdbool.h>
#include "CF.h"
#include "dimensions.h"
#define INVERSE_L_MOTOR

14
chef/src/movement.c
View file

@ -38,7 +38,9 @@ void configureMovement()
stop();
configureMotor();
#ifdef ENABLE_SECURITE
configureSecurite();
#endif
nbCalcCons = 0;
@ -123,8 +125,8 @@ void* TaskMovement(void* pData)
yDiff = cons.y - connu.y;
oDirEcart = angleGap(atan2(yDiff, xDiff), connu.o);
float oErrRev = oDirEcart * DISTANCE_BETWEEN_WHEELS / WHEEL_PERIMETER;
float lVolt = -oErrRev * P;
float rVolt = oErrRev * P;
float lVolt = -oErrRev * P + (oErrRev > 0 ? -M : M);
float rVolt = oErrRev * P + (oErrRev > 0 ? M : -M);
setMoteurTension(lVolt, rVolt);
nbCalcCons++;
@ -158,8 +160,8 @@ void* TaskMovement(void* pData)
lErr = dErrRev - oErrRev;
rErr = dErrRev + oErrRev;
float lVolt = lErr * P;
float rVolt = rErr * P;
float lVolt = lErr * P + (lErr > 0 ? M : -M);
float rVolt = rErr * P + (rErr > 0 ? M : -M);
setMoteurTension(lVolt, rVolt);
nbCalcCons++;
@ -175,8 +177,8 @@ void* TaskMovement(void* pData)
lastPosCalc = getPositionNewer(&connu, lastPosCalc);
oDirEcart = angleGap(cons.o, connu.o);
float oErrRev = oDirEcart * DISTANCE_BETWEEN_WHEELS / WHEEL_PERIMETER;
float lVolt = -oErrRev * P;
float rVolt = oErrRev * P;
float lVolt = -oErrRev * P + (oErrRev > 0 ? -M : M);
float rVolt = oErrRev * P + (oErrRev > 0 ? M : -M);
setMoteurTension(lVolt, rVolt);
nbCalcCons++;

25
chef/src/parcours.c
View file

@ -112,28 +112,9 @@ void* TaskParcours(void* pdata)
{
(void)pdata;
float vitBase = 1.5;
float secuBase = 500;
for (int i = 0; i <= 3000; i++) {
float av, ar;
getDistance(&av, &ar);
if (av < secuBase || ar < secuBase) {
brake();
} else {
setMoteurTension(vitBase, vitBase);
}
usleep(1000);
}
return NULL;
}
void* TaskParcoursLoquet(void* pdata)
{
(void)pdata;
gotoPoint(350, 0, 1.05*M_PI/3.0);
/* gotoPoint(350, 0, 1.05*M_PI/3.0); */
gotoPoint(500, 0, 0);
waitDestination();
for (int i = 0; i < 5; i++) {
setLoquet(false);
setLoquet(true);

49
chef/src/position.c
View file

@ -4,18 +4,18 @@
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <unistd.h>
#include "debug.h"
#include "dimensions.h"
#include "fpga.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;
@ -24,27 +24,18 @@ pthread_t tPosition;
unsigned int nbCalcPos;
long lCodTot, rCodTot;
void onF2CI_CODER()
{
readCF(&deltaCoders, sizeof(struct F2CI_CODERs));
pthread_mutex_unlock(&posPolling);
}
struct timespec maxDelayDelta = { 0, 10000000 };
int16_t oldL, oldR;
int16_t newL, newR;
int16_t deltaL, deltaR;
void updateDelta()
{
int ret = -1;
pthread_mutex_lock(&posPolling);
while (ret != 0) {
// Sending
sendCF(F2CI_CODER, NULL, 0);
// Waiting for reception
ret = pthread_mutex_timedlock(&posPolling, &maxDelayDelta);
}
pthread_mutex_unlock(&posPolling);
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;
oldL = newL;
oldR = newR;
}
void* TaskPosition(void* pData)
@ -55,6 +46,8 @@ void* TaskPosition(void* pData)
nbCalcPos = 0;
lCodTot = 0;
rCodTot = 0;
oldL = 0;
oldR = 0;
updateDelta();
@ -64,16 +57,16 @@ void* TaskPosition(void* pData)
// Calculation
#ifdef INVERSE_L_CODER
deltaCoders.dL = -deltaCoders.dL;
deltaL = -deltaL;
#endif
#ifdef INVERSE_R_CODER
deltaCoders.dR = -deltaCoders.dR;
deltaR = -deltaR;
#endif
lCodTot += deltaCoders.dL;
rCodTot += deltaCoders.dR;
lCodTot += deltaL;
rCodTot += deltaR;
float dR = deltaCoders.dR * AV_PER_CYCLE;
float dL = deltaCoders.dL * AV_PER_CYCLE;
float dR = deltaR * AV_PER_CYCLE;
float dL = deltaL * AV_PER_CYCLE;
float deltaO = atan2(dR - dL, DISTANCE_BETWEEN_WHEELS);
float deltaD = (dL + dR) / 2;
@ -94,12 +87,12 @@ void* TaskPosition(void* pData)
void configurePosition()
{
pthread_mutex_init(&posPolling, NULL);
pthread_mutex_init(&posConnu, NULL);
pthread_cond_init(&newPos, NULL);
registerRxHandlerCF(F2CI_CODER, onF2CI_CODER);
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;

3
chef/src/position.h
View file

@ -5,9 +5,6 @@
#ifndef __POSITION_H_
#define __POSITION_H_
#include "CF.h"
#include <pthread.h>
// #define INVERSE_L_CODER
#define INVERSE_R_CODER

3
chef/src/premier.c
View file

@ -6,7 +6,6 @@
#include <unistd.h> // sleep
#include <wiringPi.h>
#include "CF.h"
#include "actionneurs.h"
#include "debug.h"
#include "i2c.h"
@ -35,7 +34,6 @@ int main()
configureDebug();
configureIHM();
configureCF();
configureIMU();
configureActionneurs();
configurePosition();
@ -55,7 +53,6 @@ int main()
deconfigurePosition();
deconfigureActionneurs();
deconfigureIMU();
deconfigureCF();
deconfigureIHM();
deconfigureDebug();
return EXIT_SUCCESS;

52
chef/src/securite.c
View file

@ -4,42 +4,32 @@
#include <unistd.h>
#include "debug.h"
#include "fpga.h"
#include "securite.h"
// Globales
pthread_t tSecurite;
pthread_mutex_t secPolling;
pthread_mutex_t secData;
struct F2CI_CAPTs secRaw;
float secFrontL, secFrontR, secBackL, secBackR;
float secFront, secBack;
void onF2CI_CAPT()
{
readCF(&secRaw, sizeof(struct F2CI_CAPTs));
pthread_mutex_unlock(&secPolling);
}
struct timespec maxDelaySecu = { 0, 10000000 };
void* TaskSecurite(void* pData)
{
(void)pData;
for (;;) {
int ret = -1;
pthread_mutex_lock(&secPolling);
while (ret != 0) {
// Sending
sendCF(F2CI_CAPT, NULL, 0);
// Waiting for reception
ret = pthread_mutex_timedlock(&secPolling, &maxDelaySecu);
}
pthread_mutex_unlock(&secPolling);
pthread_mutex_lock(&secData);
secFront = (float)secRaw.front * SOUND_MM_P_MS;
secBack = (float)secRaw.back * SOUND_MM_P_MS;
uint16_t secFrontLB = (readI2C(fdFPGA(), CAPT_FRONT_LEFT_H) << 8 | readI2C(fdFPGA(), CAPT_FRONT_LEFT_L));
uint16_t secFrontRB = (readI2C(fdFPGA(), CAPT_FRONT_RIGHT_H) << 8 | readI2C(fdFPGA(), CAPT_FRONT_RIGHT_L));
uint16_t secBackLB = (readI2C(fdFPGA(), CAPT_BACK_LEFT_H) << 8 | readI2C(fdFPGA(), CAPT_BACK_LEFT_L));
uint16_t secBackRB = (readI2C(fdFPGA(), CAPT_BACK_RIGHT_H) << 8 | readI2C(fdFPGA(), CAPT_BACK_RIGHT_L));
secFrontL = secFrontLB * SOUND_MM_P_MS;
secFrontR = secFrontRB * SOUND_MM_P_MS;
secBackL = secBackLB * SOUND_MM_P_MS;
secBackR = secBackRB * SOUND_MM_P_MS;
secFront = fmin(secFrontL, secFrontR);
secBack = fmin(secBackL, secBackR);
pthread_mutex_unlock(&secData);
usleep(SENSOR_SAMPLING_INTERVAL * 1000);
@ -48,6 +38,16 @@ void* TaskSecurite(void* pData)
return NULL;
}
void getAllDistance(float* frontL, float* frontR, float* backL, float* backR)
{
pthread_mutex_lock(&secData);
*frontR = secFrontR;
*frontL = secFrontL;
*backL = secBackL;
*backR = secBackR;
pthread_mutex_unlock(&secData);
}
void getDistance(float* front, float* back)
{
pthread_mutex_lock(&secData);
@ -58,11 +58,13 @@ void getDistance(float* front, float* back)
void configureSecurite()
{
pthread_mutex_init(&secPolling, NULL);
pthread_mutex_init(&secData, NULL);
registerRxHandlerCF(F2CI_CAPT, onF2CI_CAPT);
registerDebugVar("secFront", f, &secFront);
registerDebugVar("secBack", f, &secBack);
registerDebugVar("secFrontL", f, &secFrontL);
registerDebugVar("secBackL", f, &secBackL);
registerDebugVar("secFrontR", f, &secFrontR);
registerDebugVar("secBackR", f, &secBackR);
pthread_mutex_init(&secData, NULL);
pthread_create(&tSecurite, NULL, TaskSecurite, NULL);
}

2
chef/src/securite.h
View file

@ -1,7 +1,6 @@
#ifndef __SECURITE_H_
#define __SECURITE_H_
#include "CF.h"
#include <pthread.h>
#define SOUND_MM_P_MS 0.3312
@ -11,6 +10,7 @@
void configureSecurite();
void deconfigureSecurite();
void getDistance(float* avant, float* arriere);
void getAllDistance(float* frontL, float* frontR, float* backL, float* backR);
#endif

7
chef/src/test1m.c
View file

@ -6,7 +6,6 @@
#include <unistd.h> // sleep
#include <wiringPi.h>
#include "CF.h"
#include "actionneurs.h"
#include "debug.h"
#include "i2c.h"
@ -35,7 +34,6 @@ int main()
srand(time(NULL));
configureDebug();
configureCF();
configureIMU();
configureActionneurs();
configurePosition();
@ -43,9 +41,7 @@ int main()
startDebug();
debugSetActive(true);
sleep(1);
/* struct position pos = {350, 0, -0.95*M_PI/3.0 }; */
struct position pos = {100000, 0, 0 };
struct position pos = {1000, 0, 0 };
setDestination(&pos);
waitDestination();
for (;;) {
@ -66,7 +62,6 @@ int main()
deconfigurePosition();
deconfigureActionneurs();
deconfigureIMU();
deconfigureCF();
deconfigureDebug();
return EXIT_SUCCESS;
}

13
chef/src/testAvance.c
View file

@ -6,7 +6,6 @@
#include <unistd.h> // sleep
#include <wiringPi.h>
#include "CF.h"
#include "actionneurs.h"
#include "debug.h"
#include "i2c.h"
@ -35,23 +34,16 @@ int main()
srand(time(NULL));
configureDebug();
configureCF();
configureIMU();
configureActionneurs();
configurePosition();
configureMovement();
startDebug();
debugSetActive(true);
sleep(1);
/* struct position pos = {350, 0, -0.95*M_PI/3.0 }; */
struct position pos = {100000, 0, 0 };
setDestination(&pos);
waitDestination();
for (;;) {
setLoquet(false);
setLoquet(true);
}
brake();
printf("Done\n");
// Bloque jusqu'à l'arrivée d'un signal
@ -64,9 +56,6 @@ int main()
deconfigureMovement();
deconfigurePosition();
deconfigureActionneurs();
deconfigureIMU();
deconfigureCF();
deconfigureDebug();
return EXIT_SUCCESS;
}

11
chef/src/testCodeuse.c
View file

@ -6,17 +6,14 @@
#include <unistd.h> // sleep
#include <wiringPi.h>
#include "CF.h"
#include "debug.h"
#include "i2c.h"
#include "ihm.h"
#include "imu.h"
#include "motor.h"
#include "position.h"
pthread_mutex_t sRunning;
#define VIT 0
#define VIT 1
void endRunning(int signal)
{
@ -28,11 +25,13 @@ int main()
{
configureDebug();
configureCF();
configurePosition();
configureMotor();
startDebug();
debugSetActive(true);
setPWMTension(VIT, VIT);
setMoteurTension(VIT, VIT);
/* freewheel(); */
long lCod, rCod;
for (;;) {

17
chef/src/testForward.c
View file

@ -8,45 +8,46 @@
#include <wiringPiI2C.h>
#include "lcd.h"
#include "CF.h"
#include "motor.h"
#include "buttons.h"
#define PATATE 2
int main(int argc, char* argv[])
{
(void)argc;
(void)argv;
float patate = 2;
if (argc == 2) {
sscanf(argv[1], "%f", &patate);
}
wiringPiSetup();
initI2C();
initLCD();
configureCF();
configureButtons();
configureMotor();
for (;;) {
clearLCD();
printToLCD(LCD_LINE_1, "Forward");
setMoteurTension(PATATE, PATATE);
setMoteurTension(patate, patate);
pressedButton(BUT_BLOCK);
clearLCD();
printToLCD(LCD_LINE_1, "Right");
setMoteurTension(-PATATE, PATATE);
setMoteurTension(-patate, patate);
pressedButton(BUT_BLOCK);
clearLCD();
printToLCD(LCD_LINE_1, "Left");
setMoteurTension(PATATE, -PATATE);
setMoteurTension(patate, -patate);
pressedButton(BUT_BLOCK);
clearLCD();
printToLCD(LCD_LINE_1, "Backward");
setMoteurTension(-PATATE, -PATATE);
setMoteurTension(-patate, -patate);
pressedButton(BUT_BLOCK);
clearLCD();

4
chef/src/testFree.c
View file

@ -2,7 +2,6 @@
#include <stdlib.h>
#include "CF.h"
#include "motor.h"
int main(int argc, char* argv[])
@ -11,7 +10,8 @@ int main(int argc, char* argv[])
(void)argc;
(void)argv;
configureCF();
configureMotor();
freewheel();
}

71
chef/src/testOrange.c
View file

@ -1,71 +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 "CF.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();
configureCF();
configureIMU();
configureActionneurs();
configurePosition();
configureMovement();
startDebug();
debugSetActive(true);
sleep(1);
struct position pos = {350, 0, -0.95*M_PI/3.0 };
setDestination(&pos);
sleep(3);
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();
deconfigureCF();
deconfigureDebug();
return EXIT_SUCCESS;
}

55
chef/src/testParcours.c
View file

@ -1,55 +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 "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;
}

26
chef/src/testPuissance.c
View file

@ -1,26 +0,0 @@
#include <stdlib.h>
#include <unistd.h>
#include "CF.h"
#include "motor.h"
#include "debug.h"
int main(int argc, char* argv[])
{
(void)argc;
(void)argv;
configureDebug();
configureCF();
configureMotor();
setMoteurTension(24, 24);
sleep(10);
setMoteurTension(0, 0);
sleep(3);
deconfigureMotor();
deconfigureCF();
deconfigureDebug();
}

72
chef/src/testRecule.c
View file

@ -1,72 +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 "CF.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();
configureCF();
configureIMU();
configureActionneurs();
configurePosition();
configureMovement();
startDebug();
debugSetActive(true);
sleep(1);
/* struct position pos = {350, 0, -0.95*M_PI/3.0 }; */
struct position pos = {-100000, 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();
deconfigureCF();
deconfigureDebug();
return EXIT_SUCCESS;
}

77
chef/src/testRot.c
View file

@ -1,77 +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 "CF.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();
configureCF();
configureIMU();
printf("40\n");
configureActionneurs();
printf("41\n");
configurePosition();
printf("44\n");
configureMovement();
printf("46\n");
startDebug();
printf("48\n");
debugSetActive(true);
sleep(1);
printf("46\n");
struct position pos = { 0, 0, M_PI_2 };
setDestination(&pos);
printf("50\n");
waitDestination();
printf("52\n");
printf("54\n");
brake();
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();
deconfigureCF();
deconfigureDebug();
return EXIT_SUCCESS;
}

10
chef/src/testSecu.c
View file

@ -3,7 +3,7 @@
#include <unistd.h>
#include <unistd.h>
#include "CF.h"
#include "i2c.h"
#include "securite.h"
int main(int argc, char* argv[])
@ -12,14 +12,14 @@ int main(int argc, char* argv[])
(void)argc;
(void)argv;
configureCF();
initI2C();
configureSecurite();
float f, b;
float fl, fr, bl, br;
for (;;) {
getDistance(&f, &b);
printf("Av: %6f Ar: %6f\n", f, b);
getAllDistance(&fl, &fr, &bl, &br);
printf("FL: %9f FR: %9f BL: %9f BR: %9f\n", fl, fr, bl, br);
usleep(60 * 1000);
}
}

48
chef/src/testSlow.c
View file

@ -1,48 +0,0 @@
/* Teste si une broche est connecté à une autre */
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <wiringPi.h>
#include <wiringPiI2C.h>
#include "lcd.h"
#include "CF.h"
#include "motor.h"
#include "buttons.h"
#define VIT 1
#define VITL VIT
#define VITR VIT
void setPWMTensionWrapper(float l, float r) {
/* clearLCD(); */
printfToLCD(LCD_LINE_1, "L: %f", l);
printfToLCD(LCD_LINE_2, "R: %f", r);
setPWMTension(l, r);
}
int main(int argc, char* argv[])
{
(void)argc;
(void)argv;
wiringPiSetup();
initI2C();
initLCD();
configureCF();
configureButtons();
configureMotor();
for (;;) {
setPWMTensionWrapper(VITL, VITR);
pressedButton(BUT_BLOCK);
brake();
pressedButton(BUT_BLOCK);
}
}

5
chef/src/testStop.c
View file

@ -2,7 +2,6 @@
#include <stdlib.h>
#include "CF.h"
#include "motor.h"
int main(int argc, char* argv[])
@ -11,8 +10,8 @@ int main(int argc, char* argv[])
(void)argc;
(void)argv;
configureCF();
configureMotor();
stop();
brake();
}

5
chef/src/testUpDown.c
View file

@ -8,15 +8,15 @@
#include <wiringPiI2C.h>
#include "lcd.h"
#include "CF.h"
#include "motor.h"
#include "buttons.h"
#include "dimensions.h"
#define UP_TIME 1000
#define HIGH_TIME 3000
#define DOWN_TIME 1000
#define LOW_TIME 2000
#define MAXI 12
#define MAXI MOTOR_SATURATION_MAX
#define INTERVAL 10
void changerMoteursWrapper(float l, float r) {
@ -36,7 +36,6 @@ int main(int argc, char* argv[])
initI2C();
initLCD();
configureCF();
configureButtons();
configureMotor();