mirror of
https://github.com/RobotechLille/cdf2018-principal
synced 2024-11-14 04:16:05 +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
|
||||
PKG_CONFIG=pkg-config
|
||||
## 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)))
|
||||
|
||||
# VARIABLES AUTOMATIQUES
|
||||
|
|
|
@ -27,7 +27,6 @@ void configureFpga()
|
|||
printf("Connexion à %s... ", FPGA_PORTNAME);
|
||||
fflush(stdout);
|
||||
fpga = open(FPGA_PORTNAME, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
|
||||
fpga = serialOpen(FPGA_PORTNAME, 9600);
|
||||
if (fpga < 0) {
|
||||
printf("Échec !\n");
|
||||
exit(1);
|
||||
|
|
|
@ -51,9 +51,6 @@ void* TaskDebug(void* pdata)
|
|||
{
|
||||
(void)pdata;
|
||||
|
||||
if (DEBUG_INTERVAL <= 0) {
|
||||
return NULL;
|
||||
}
|
||||
clock_gettime(CLOCK_REALTIME, &debugStart);
|
||||
|
||||
fprintf(debugFd, "\n");
|
||||
|
|
|
@ -1,4 +1,5 @@
|
|||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include "diagnostics.h"
|
||||
#include "buttons.h"
|
||||
|
@ -6,6 +7,7 @@
|
|||
|
||||
#include "CF.h"
|
||||
#include "movement.h"
|
||||
#include "imu.h"
|
||||
|
||||
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)
|
||||
{
|
||||
clearLCD();
|
||||
|
@ -90,6 +98,7 @@ void runDiagnostics()
|
|||
{
|
||||
execDiagnostic("Lien FPGA", diagFPGA, NULL);
|
||||
/* 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);
|
||||
|
|
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 "i2c.h"
|
||||
#include "ihm.h"
|
||||
#include "imu.h"
|
||||
#include "movement.h"
|
||||
#include "position.h"
|
||||
|
||||
|
@ -18,7 +19,6 @@ pthread_mutex_t sRunning;
|
|||
void endRunning(int signal)
|
||||
{
|
||||
(void)signal;
|
||||
printf("21\n");
|
||||
pthread_mutex_unlock(&sRunning);
|
||||
}
|
||||
|
||||
|
@ -32,9 +32,10 @@ int main()
|
|||
initI2C();
|
||||
srand(time(NULL));
|
||||
|
||||
configureIHM();
|
||||
configureDebug();
|
||||
configureIHM();
|
||||
configureCF();
|
||||
configureIMU();
|
||||
configureMovement();
|
||||
configurePosition();
|
||||
startDebug();
|
||||
|
@ -48,11 +49,11 @@ int main()
|
|||
pthread_mutex_lock(&sRunning);
|
||||
pthread_mutex_lock(&sRunning);
|
||||
|
||||
deconfigureMovement();
|
||||
deconfigureMovement();
|
||||
deconfigurePosition();
|
||||
deconfigureMovement();
|
||||
deconfigureIMU();
|
||||
deconfigureCF();
|
||||
deconfigureDebug();
|
||||
deconfigureIHM();
|
||||
deconfigureDebug();
|
||||
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
|
||||
|
||||
upgrade-chef: chef
|
||||
make -C buildroot chef-reinstall
|
||||
ssh -F sshconf principal true
|
||||
rsync --rsh 'ssh -F sshconf' --archive --chown root:root buildroot/output/target/opt/chef principal:/opt/
|
||||
|
||||
|
|
|
@ -6,17 +6,16 @@
|
|||
start() {
|
||||
printf "Starting hardware handling: "
|
||||
modprobe pl2303 # USB↔Serial cable
|
||||
modprobe i2c-bcm2708 # I2C
|
||||
modprobe i2c-dev # I2C
|
||||
# I2C
|
||||
modprobe i2c-bcm2708 # RPi2
|
||||
modprobe i2c-bcm2835 # RPi3
|
||||
modprobe i2c-dev # Both
|
||||
/opt/chef/lcdOff.sh
|
||||
echo "OK"
|
||||
}
|
||||
|
||||
stop() {
|
||||
printf "Stopping hardware handling: "
|
||||
rmmod i2c-dev # I2C
|
||||
rmmod i2c-bcm2708 # I2C
|
||||
rmmod pl2303 # USB↔Serial cable
|
||||
echo "OK"
|
||||
}
|
||||
|
||||
|
|
|
@ -70,7 +70,6 @@ BR2_PACKAGE_WPA_SUPPLICANT=y
|
|||
# Pour upgrader à chaud
|
||||
BR2_PACKAGE_DROPBEAR=y
|
||||
BR2_PACKAGE_RSYNC=y
|
||||
BR2_PACKAGE_PPPD=y
|
||||
|
||||
# Pour faire plaisir à Geoffrey
|
||||
BR2_PACKAGE_HTOP=y
|
||||
|
|
Loading…
Reference in a new issue