mirror of
https://github.com/RobotechLille/cdf2018-principal
synced 2024-11-24 09:06:03 +01:00
WIP IMU
This commit is contained in:
parent
98c2f27358
commit
678b7e939b
|
@ -11,7 +11,7 @@ CFLAGS_CUSTOM += -g
|
||||||
## Générateurs de drapeaux pour les bibliothèques
|
## Générateurs de drapeaux pour les bibliothèques
|
||||||
PKG_CONFIG=pkg-config
|
PKG_CONFIG=pkg-config
|
||||||
## Nom des objets communs
|
## Nom des objets communs
|
||||||
OBJS=buttons CF debug diagnostics i2c ihm lcd movement parcours points position
|
OBJS=buttons CF debug diagnostics i2c imu ihm lcd movement parcours points position
|
||||||
OBJS_O=$(addprefix obj/,$(addsuffix .o,$(OBJS)))
|
OBJS_O=$(addprefix obj/,$(addsuffix .o,$(OBJS)))
|
||||||
|
|
||||||
# VARIABLES AUTOMATIQUES
|
# VARIABLES AUTOMATIQUES
|
||||||
|
|
|
@ -27,7 +27,6 @@ void configureFpga()
|
||||||
printf("Connexion à %s... ", FPGA_PORTNAME);
|
printf("Connexion à %s... ", FPGA_PORTNAME);
|
||||||
fflush(stdout);
|
fflush(stdout);
|
||||||
fpga = open(FPGA_PORTNAME, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
|
fpga = open(FPGA_PORTNAME, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
|
||||||
fpga = serialOpen(FPGA_PORTNAME, 9600);
|
|
||||||
if (fpga < 0) {
|
if (fpga < 0) {
|
||||||
printf("Échec !\n");
|
printf("Échec !\n");
|
||||||
exit(1);
|
exit(1);
|
||||||
|
|
|
@ -51,9 +51,6 @@ void* TaskDebug(void* pdata)
|
||||||
{
|
{
|
||||||
(void)pdata;
|
(void)pdata;
|
||||||
|
|
||||||
if (DEBUG_INTERVAL <= 0) {
|
|
||||||
return NULL;
|
|
||||||
}
|
|
||||||
clock_gettime(CLOCK_REALTIME, &debugStart);
|
clock_gettime(CLOCK_REALTIME, &debugStart);
|
||||||
|
|
||||||
fprintf(debugFd, "\n");
|
fprintf(debugFd, "\n");
|
||||||
|
|
|
@ -1,4 +1,5 @@
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
#include "diagnostics.h"
|
#include "diagnostics.h"
|
||||||
#include "buttons.h"
|
#include "buttons.h"
|
||||||
|
@ -6,6 +7,7 @@
|
||||||
|
|
||||||
#include "CF.h"
|
#include "CF.h"
|
||||||
#include "movement.h"
|
#include "movement.h"
|
||||||
|
#include "imu.h"
|
||||||
|
|
||||||
bool recu;
|
bool recu;
|
||||||
|
|
||||||
|
@ -71,6 +73,12 @@ bool diagCodeuse(void* arg)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool diagIMU(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();
|
clearLCD();
|
||||||
|
@ -90,6 +98,7 @@ void runDiagnostics()
|
||||||
{
|
{
|
||||||
execDiagnostic("Lien FPGA", diagFPGA, NULL);
|
execDiagnostic("Lien FPGA", diagFPGA, NULL);
|
||||||
/* execDiagnostic("Lien Arduino", diagArduino); */
|
/* execDiagnostic("Lien Arduino", diagArduino); */
|
||||||
|
execDiagnostic("Lien IMU", diagIMU, NULL);
|
||||||
int i;
|
int i;
|
||||||
i = 0; execDiagnostic("Mot+Cod L AV", diagCodeuse, &i);
|
i = 0; execDiagnostic("Mot+Cod L AV", diagCodeuse, &i);
|
||||||
i = 1; execDiagnostic("Mot+Cod L AR", diagCodeuse, &i);
|
i = 1; execDiagnostic("Mot+Cod L AR", diagCodeuse, &i);
|
||||||
|
|
34
chef/src/imu.c
Normal file
34
chef/src/imu.c
Normal file
|
@ -0,0 +1,34 @@
|
||||||
|
#include "imu.h"
|
||||||
|
#include "i2c.h"
|
||||||
|
|
||||||
|
int gyroFd;
|
||||||
|
int accelFd;
|
||||||
|
|
||||||
|
void configureIMU()
|
||||||
|
{
|
||||||
|
gyroFd = openI2C(GYRO_ADDR);
|
||||||
|
accelFd = openI2C(ACCEL_ADDR);
|
||||||
|
|
||||||
|
writeI2C(gyroFd, GYRO_LOW_ODR, 0x00); // Low_ODR = 0 (low speed ODR disabled)
|
||||||
|
writeI2C(gyroFd, GYRO_CTRL_REG4, 0x00); // FS = 00 (+/- 250 dps full scale)
|
||||||
|
writeI2C(gyroFd, GYRO_CTRL_REG1, 0x6F); // DR = 01 (200 Hz ODR); BW = 10 (50 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
|
||||||
|
}
|
||||||
|
|
||||||
|
bool connectedIMU()
|
||||||
|
{
|
||||||
|
return (uint8_t) readI2C(gyroFd, GYRO_WHO_AM_I) == GYRO_IDENTITY;
|
||||||
|
}
|
||||||
|
|
||||||
|
struct gyroRaw readGyroRaw()
|
||||||
|
{
|
||||||
|
struct gyroRaw res;
|
||||||
|
res.x = (readI2C(gyroFd, GYRO_OUT_X_H) << 8 | readI2C(gyroFd, GYRO_OUT_X_L));
|
||||||
|
res.y = (readI2C(gyroFd, GYRO_OUT_Y_H) << 8 | readI2C(gyroFd, GYRO_OUT_Y_L));
|
||||||
|
res.z = (readI2C(gyroFd, GYRO_OUT_Z_H) << 8 | readI2C(gyroFd, GYRO_OUT_Z_L));
|
||||||
|
return res;
|
||||||
|
}
|
||||||
|
|
||||||
|
void deconfigureIMU()
|
||||||
|
{
|
||||||
|
|
||||||
|
}
|
76
chef/src/imu.h
Normal file
76
chef/src/imu.h
Normal file
|
@ -0,0 +1,76 @@
|
||||||
|
#ifndef __IMU_H_
|
||||||
|
#define __IMU_H_
|
||||||
|
|
||||||
|
#include <stdbool.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
#define GYRO_ADDR 0x6b
|
||||||
|
#define ACCEL_ADDR 0x1d
|
||||||
|
|
||||||
|
// Gyro
|
||||||
|
|
||||||
|
#define GYRO_WHO_AM_I 0x0F
|
||||||
|
#define GYRO_IDENTITY 0xD7
|
||||||
|
|
||||||
|
#define GYRO_CTRL1 0x20 // D20H
|
||||||
|
#define GYRO_CTRL_REG1 0x20 // D20, 4200D
|
||||||
|
#define GYRO_CTRL2 0x21 // D20H
|
||||||
|
#define GYRO_CTRL_REG2 0x21 // D20, 4200D
|
||||||
|
#define GYRO_CTRL3 0x22 // D20H
|
||||||
|
#define GYRO_CTRL_REG3 0x22 // D20, 4200D
|
||||||
|
#define GYRO_CTRL4 0x23 // D20H
|
||||||
|
#define GYRO_CTRL_REG4 0x23 // D20, 4200D
|
||||||
|
#define GYRO_CTRL5 0x24 // D20H
|
||||||
|
#define GYRO_CTRL_REG5 0x24 // D20, 4200D
|
||||||
|
#define GYRO_REFERENCE 0x25
|
||||||
|
#define GYRO_OUT_TEMP 0x26
|
||||||
|
#define GYRO_STATUS 0x27 // D20H
|
||||||
|
#define GYRO_STATUS_REG 0x27 // D20, 4200D
|
||||||
|
|
||||||
|
#define GYRO_OUT_X_L 0x28
|
||||||
|
#define GYRO_OUT_X_H 0x29
|
||||||
|
#define GYRO_OUT_Y_L 0x2A
|
||||||
|
#define GYRO_OUT_Y_H 0x2B
|
||||||
|
#define GYRO_OUT_Z_L 0x2C
|
||||||
|
#define GYRO_OUT_Z_H 0x2D
|
||||||
|
|
||||||
|
#define GYRO_FIFO_CTRL 0x2E // D20H
|
||||||
|
#define GYRO_FIFO_CTRL_REG 0x2E // D20, 4200D
|
||||||
|
#define GYRO_FIFO_SRC 0x2F // D20H
|
||||||
|
#define GYRO_FIFO_SRC_REG 0x2F // D20, 4200D
|
||||||
|
|
||||||
|
#define GYRO_IG_CFG 0x30 // D20H
|
||||||
|
#define GYRO_INT1_CFG 0x30 // D20, 4200D
|
||||||
|
#define GYRO_IG_SRC 0x31 // D20H
|
||||||
|
#define GYRO_INT1_SRC 0x31 // D20, 4200D
|
||||||
|
#define GYRO_IG_THS_XH 0x32 // D20H
|
||||||
|
#define GYRO_INT1_THS_XH 0x32 // D20, 4200D
|
||||||
|
#define GYRO_IG_THS_XL 0x33 // D20H
|
||||||
|
#define GYRO_INT1_THS_XL 0x33 // D20, 4200D
|
||||||
|
#define GYRO_IG_THS_YH 0x34 // D20H
|
||||||
|
#define GYRO_INT1_THS_YH 0x34 // D20, 4200D
|
||||||
|
#define GYRO_IG_THS_YL 0x35 // D20H
|
||||||
|
#define GYRO_INT1_THS_YL 0x35 // D20, 4200D
|
||||||
|
#define GYRO_IG_THS_ZH 0x36 // D20H
|
||||||
|
#define GYRO_INT1_THS_ZH 0x36 // D20, 4200D
|
||||||
|
#define GYRO_IG_THS_ZL 0x37 // D20H
|
||||||
|
#define GYRO_INT1_THS_ZL 0x37 // D20, 4200D
|
||||||
|
#define GYRO_IG_DURATION 0x38 // D20H
|
||||||
|
#define GYRO_INT1_DURATION 0x38 // D20, 4200D
|
||||||
|
|
||||||
|
#define GYRO_LOW_ODR 0x39 // D20H
|
||||||
|
|
||||||
|
// Structures
|
||||||
|
|
||||||
|
struct gyroRaw {
|
||||||
|
int16_t x;
|
||||||
|
int16_t y;
|
||||||
|
int16_t z;
|
||||||
|
};
|
||||||
|
|
||||||
|
void configureIMU();
|
||||||
|
bool connectedIMU();
|
||||||
|
void deconfigureIMU();
|
||||||
|
struct gyroRaw readGyroRaw();
|
||||||
|
|
||||||
|
#endif
|
|
@ -10,6 +10,7 @@
|
||||||
#include "debug.h"
|
#include "debug.h"
|
||||||
#include "i2c.h"
|
#include "i2c.h"
|
||||||
#include "ihm.h"
|
#include "ihm.h"
|
||||||
|
#include "imu.h"
|
||||||
#include "movement.h"
|
#include "movement.h"
|
||||||
#include "position.h"
|
#include "position.h"
|
||||||
|
|
||||||
|
@ -18,7 +19,6 @@ pthread_mutex_t sRunning;
|
||||||
void endRunning(int signal)
|
void endRunning(int signal)
|
||||||
{
|
{
|
||||||
(void)signal;
|
(void)signal;
|
||||||
printf("21\n");
|
|
||||||
pthread_mutex_unlock(&sRunning);
|
pthread_mutex_unlock(&sRunning);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -32,9 +32,10 @@ int main()
|
||||||
initI2C();
|
initI2C();
|
||||||
srand(time(NULL));
|
srand(time(NULL));
|
||||||
|
|
||||||
configureIHM();
|
|
||||||
configureDebug();
|
configureDebug();
|
||||||
|
configureIHM();
|
||||||
configureCF();
|
configureCF();
|
||||||
|
configureIMU();
|
||||||
configureMovement();
|
configureMovement();
|
||||||
configurePosition();
|
configurePosition();
|
||||||
startDebug();
|
startDebug();
|
||||||
|
@ -48,11 +49,11 @@ int main()
|
||||||
pthread_mutex_lock(&sRunning);
|
pthread_mutex_lock(&sRunning);
|
||||||
pthread_mutex_lock(&sRunning);
|
pthread_mutex_lock(&sRunning);
|
||||||
|
|
||||||
deconfigureMovement();
|
|
||||||
deconfigureMovement();
|
|
||||||
deconfigurePosition();
|
deconfigurePosition();
|
||||||
|
deconfigureMovement();
|
||||||
|
deconfigureIMU();
|
||||||
deconfigureCF();
|
deconfigureCF();
|
||||||
deconfigureDebug();
|
|
||||||
deconfigureIHM();
|
deconfigureIHM();
|
||||||
|
deconfigureDebug();
|
||||||
return EXIT_SUCCESS;
|
return EXIT_SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
37
chef/src/testIMU.c
Normal file
37
chef/src/testIMU.c
Normal file
|
@ -0,0 +1,37 @@
|
||||||
|
/* Teste l'IMU */
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <wiringPiI2C.h>
|
||||||
|
|
||||||
|
#include "i2c.h"
|
||||||
|
#include "imu.h"
|
||||||
|
|
||||||
|
int main(int argc, char* argv[])
|
||||||
|
{
|
||||||
|
|
||||||
|
(void)argc;
|
||||||
|
(void)argv;
|
||||||
|
|
||||||
|
initI2C();
|
||||||
|
configureIMU();
|
||||||
|
if (!connectedIMU()) {
|
||||||
|
printf("IMU not connected\n");
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
struct gyroRaw a;
|
||||||
|
struct gyroRaw t;
|
||||||
|
for (;;) {
|
||||||
|
a = readGyroRaw();
|
||||||
|
t.x += a.x;
|
||||||
|
t.y += a.y;
|
||||||
|
t.z += a.z;
|
||||||
|
usleep(100*1000);
|
||||||
|
printf("X:%5d Y:%5d Z:%5d\n", t.x, t.y, t.z);
|
||||||
|
}
|
||||||
|
|
||||||
|
exit(0);
|
||||||
|
}
|
|
@ -1,32 +0,0 @@
|
||||||
/* Teste si une broche est connecté à une autre */
|
|
||||||
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <wiringPi.h>
|
|
||||||
|
|
||||||
char testPin(char input, char output) {
|
|
||||||
pinMode(output, OUTPUT);
|
|
||||||
pinMode(input, INPUT);
|
|
||||||
digitalWrite(output, 1);
|
|
||||||
delay(0);
|
|
||||||
return digitalRead(input);
|
|
||||||
}
|
|
||||||
|
|
||||||
int main(int argc, char *argv[]) {
|
|
||||||
|
|
||||||
if (argc != 3) {
|
|
||||||
printf("Usage: %s INPUT_PIN OUTPUT_PIN\n", argv[0]);
|
|
||||||
return 2;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (wiringPiSetup() == -1) {
|
|
||||||
return 3;
|
|
||||||
}
|
|
||||||
|
|
||||||
char input = atoi(argv[1]);
|
|
||||||
char output = atoi(argv[2]);
|
|
||||||
|
|
||||||
char rep = testPin(input, output);
|
|
||||||
|
|
||||||
return rep;
|
|
||||||
}
|
|
|
@ -112,7 +112,6 @@ chef:
|
||||||
make -C buildroot chef-rebuild
|
make -C buildroot chef-rebuild
|
||||||
|
|
||||||
upgrade-chef: chef
|
upgrade-chef: chef
|
||||||
make -C buildroot chef-reinstall
|
|
||||||
ssh -F sshconf principal true
|
ssh -F sshconf principal true
|
||||||
rsync --rsh 'ssh -F sshconf' --archive --chown root:root buildroot/output/target/opt/chef principal:/opt/
|
rsync --rsh 'ssh -F sshconf' --archive --chown root:root buildroot/output/target/opt/chef principal:/opt/
|
||||||
|
|
||||||
|
|
|
@ -6,17 +6,16 @@
|
||||||
start() {
|
start() {
|
||||||
printf "Starting hardware handling: "
|
printf "Starting hardware handling: "
|
||||||
modprobe pl2303 # USB↔Serial cable
|
modprobe pl2303 # USB↔Serial cable
|
||||||
modprobe i2c-bcm2708 # I2C
|
# I2C
|
||||||
modprobe i2c-dev # I2C
|
modprobe i2c-bcm2708 # RPi2
|
||||||
|
modprobe i2c-bcm2835 # RPi3
|
||||||
|
modprobe i2c-dev # Both
|
||||||
/opt/chef/lcdOff.sh
|
/opt/chef/lcdOff.sh
|
||||||
echo "OK"
|
echo "OK"
|
||||||
}
|
}
|
||||||
|
|
||||||
stop() {
|
stop() {
|
||||||
printf "Stopping hardware handling: "
|
printf "Stopping hardware handling: "
|
||||||
rmmod i2c-dev # I2C
|
|
||||||
rmmod i2c-bcm2708 # I2C
|
|
||||||
rmmod pl2303 # USB↔Serial cable
|
|
||||||
echo "OK"
|
echo "OK"
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -70,7 +70,6 @@ BR2_PACKAGE_WPA_SUPPLICANT=y
|
||||||
# Pour upgrader à chaud
|
# Pour upgrader à chaud
|
||||||
BR2_PACKAGE_DROPBEAR=y
|
BR2_PACKAGE_DROPBEAR=y
|
||||||
BR2_PACKAGE_RSYNC=y
|
BR2_PACKAGE_RSYNC=y
|
||||||
BR2_PACKAGE_PPPD=y
|
|
||||||
|
|
||||||
# Pour faire plaisir à Geoffrey
|
# Pour faire plaisir à Geoffrey
|
||||||
BR2_PACKAGE_HTOP=y
|
BR2_PACKAGE_HTOP=y
|
||||||
|
|
Loading…
Reference in a new issue