Hard reset LCD

Connexions.md

@@ -16,6 +16,6 @@ Ce document a pour but de recenser les connexions entre les différents composan
 
 - BCM 2 (SDA), BCM 3 (SCL) : liaison I2C (LCD, IMU, SRF08)
 - BCM 14 (TXD), BCM 15 (RXD) : liaison série (PC debug)
-- BCM 12 (PWM0), BCM 13 (PWM1) : contrôleur moteur
 - BCM 17, BCM 27 : bouton rouge, bouton jaune
 - BCM 22 : tirette
+- BCM 23 : LCD on/off

chef/src/imu.c

@@ -1,34 +1,116 @@
-#include "imu.h"
+#include <math.h>
+#include <pthread.h>
+#include <stdlib.h>
+#include <unistd.h>
+
 #include "i2c.h"
+#include "imu.h"
 
 int gyroFd;
-int accelFd;
+int acelFd;
+pthread_t tIMU;
 
 void configureIMU()
 {
     gyroFd = openI2C(GYRO_ADDR);
-    accelFd = openI2C(ACCEL_ADDR);
+    acelFd = openI2C(ACEL_ADDR);
 
-    writeI2C(gyroFd, GYRO_LOW_ODR, 0x00); // Low_ODR = 0 (low speed ODR disabled)
+    writeI2C(gyroFd, GYRO_CTRL_REG1, 0x0F); // PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
-    writeI2C(gyroFd, GYRO_CTRL_REG4, 0x00); // FS = 00 (+/- 250 dps full scale)
+    writeI2C(gyroFd, GYRO_CTRL_REG4, 0xB0); // Block data update, 2000 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)
+
+    writeI2C(acelFd, ACEL_CTRL1, 0x6F); // 50 Hz Acceleration data-rate; block data update; Zen = Yen = Xen = 1 (all axes enabled)
+    writeI2C(acelFd, ACEL_CTRL2, 0x20); // +/- 16G full scale
+
+    pthread_create(&tIMU, NULL, TaskIMU, NULL);
 }
 
 bool connectedIMU()
 {
-    return (uint8_t) readI2C(gyroFd, GYRO_WHO_AM_I) == GYRO_IDENTITY;
+    return (uint8_t)readI2C(gyroFd, GYRO_WHO_AM_I) == GYRO_IDENTITY && (uint8_t)readI2C(acelFd, ACEL_WHO_AM_I) == ACEL_IDENTITY;
 }
 
-struct gyroRaw readGyroRaw()
+struct axesRaw readGyroRaw()
 {
-    struct gyroRaw res;
+    struct axesRaw 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;
 }
 
+struct axes readGyro()
+{
+    struct axesRaw raw = readGyroRaw();
+    struct axes computed;
+    computed.x = (float)raw.x * GYRO_GAIN;
+    computed.y = (float)raw.y * GYRO_GAIN;
+    computed.z = (float)raw.z * GYRO_GAIN;
+    return computed;
+}
+
+struct axesRaw readAcelRaw()
+{
+    struct axesRaw res;
+    res.x = (readI2C(acelFd, ACEL_OUT_X_H_A) << 8 | readI2C(acelFd, ACEL_OUT_X_L_A));
+    res.y = (readI2C(acelFd, ACEL_OUT_Y_H_A) << 8 | readI2C(acelFd, ACEL_OUT_Y_L_A));
+    res.z = (readI2C(acelFd, ACEL_OUT_Z_H_A) << 8 | readI2C(acelFd, ACEL_OUT_Z_L_A));
+    return res;
+}
+
+struct axes readAcel()
+{
+    struct axesRaw raw = readAcelRaw();
+    struct axes computed;
+    computed.x = atan2(raw.y, raw.z) + M_PI;
+    computed.y = atan2(raw.z, raw.x) + M_PI;
+    computed.z = atan2(raw.x, raw.y) + M_PI;
+    return computed;
+}
+
+struct timespec imuStart;
+struct timespec imuNow;
+struct timespec imuEcoule;
+
+void* TaskIMU(void* pData)
+{
+    (void)pData;
+
+    struct axes gyroNew;
+    struct axes gyro;
+    struct axes acel;
+    struct axes complemFilter;
+
+    clock_gettime(CLOCK_REALTIME, &imuStart);
+
+    for (;;) {
+        clock_gettime(CLOCK_REALTIME, &imuNow);
+        if ((imuNow.tv_nsec - imuStart.tv_nsec) < 0) {
+            imuEcoule.tv_sec = imuNow.tv_sec - imuStart.tv_sec - 1;
+            imuEcoule.tv_nsec = imuNow.tv_nsec - imuStart.tv_nsec + 1000000000UL;
+        } else {
+            imuEcoule.tv_sec = imuNow.tv_sec - imuStart.tv_sec;
+            imuEcoule.tv_nsec = imuNow.tv_nsec - imuStart.tv_nsec;
+        }
+        imuStart.tv_sec = imuNow.tv_sec;
+        imuStart.tv_nsec = imuNow.tv_nsec;
+        float dt = imuEcoule.tv_sec + imuStart.tv_nsec * 1E-9;
+
+        gyroNew = readGyro();
+        gyro.x += gyroNew.x * dt;
+        gyro.y += gyroNew.y * dt;
+        gyro.z += gyroNew.z * dt;
+
+        acel = readAcel();
+
+        /* complemFilter.x = c * (complemFilter.x + gyroNew.x * dt) + (1 - AA) * acel.x; */
+
+        usleep(IMU_INTERVAL * 1000);
+    }
+
+    return NULL;
+}
+
 void deconfigureIMU()
 {
-
+    pthread_cancel(tIMU);
 }

chef/src/imu.h

@@ -5,10 +5,14 @@
 #include <stdint.h>
 
 #define GYRO_ADDR 0x6b
-#define ACCEL_ADDR 0x1d
+#define ACEL_ADDR 0x1d
+#define IMU_INTERVAL 10
+#define COMPLEM_FILTER_RATE 0.98
+// ms (100 Hz)
 
 // Gyro
 
+#define GYRO_GAIN 0.07
 #define GYRO_WHO_AM_I 0x0F
 #define GYRO_IDENTITY 0xD7
 
@@ -60,17 +64,135 @@
 
 #define GYRO_LOW_ODR 0x39 // D20H
 
+// Aceletomètre
+
+#define ACEL_IDENTITY 0x49
+
+#define ACEL_TEMP_OUT_L 0x05 // D
+#define ACEL_TEMP_OUT_H 0x06 // D
+
+#define ACEL_STATUS_M 0x07 // D
+
+#define ACEL_INT_CTRL_M 0x12  // D
+#define ACEL_INT_SRC_M 0x13   // D
+#define ACEL_INT_THS_L_M 0x14 // D
+#define ACEL_INT_THS_H_M 0x15 // D
+
+#define ACEL_OFFSET_X_L_M 0x16 // D
+#define ACEL_OFFSET_X_H_M 0x17 // D
+#define ACEL_OFFSET_Y_L_M 0x18 // D
+#define ACEL_OFFSET_Y_H_M 0x19 // D
+#define ACEL_OFFSET_Z_L_M 0x1A // D
+#define ACEL_OFFSET_Z_H_M 0x1B // D
+#define ACEL_REFERENCE_X 0x1C  // D
+#define ACEL_REFERENCE_Y 0x1D  // D
+#define ACEL_REFERENCE_Z 0x1E  // D
+
+#define ACEL_CTRL0 0x1F             // D
+#define ACEL_CTRL1 0x20             // D
+#define ACEL_CTRL_REG1_A 0x20       // DLH DLM DLHC
+#define ACEL_CTRL2 0x21             // D
+#define ACEL_CTRL_REG2_A 0x21       // DLH DLM DLHC
+#define ACEL_CTRL3 0x22             // D
+#define ACEL_CTRL_REG3_A 0x22       // DLH DLM DLHC
+#define ACEL_CTRL4 0x23             // D
+#define ACEL_CTRL_REG4_A 0x23       // DLH DLM DLHC
+#define ACEL_CTRL5 0x24             // D
+#define ACEL_CTRL_REG5_A 0x24       // DLH DLM DLHC
+#define ACEL_CTRL6 0x25             // D
+#define ACEL_CTRL_REG6_A 0x25       // DLHC
+#define ACEL_HP_FILTER_RESET_A 0x25 // DLH DLM
+#define ACEL_CTRL7 0x26             // D
+#define ACEL_REFERENCE_A 0x26       // DLH DLM DLHC
+#define ACEL_STATUS_A 0x27          // D
+#define ACEL_STATUS_REG_A 0x27      // DLH DLM DLHC
+
+#define ACEL_OUT_X_L_A 0x28
+#define ACEL_OUT_X_H_A 0x29
+#define ACEL_OUT_Y_L_A 0x2A
+#define ACEL_OUT_Y_H_A 0x2B
+#define ACEL_OUT_Z_L_A 0x2C
+#define ACEL_OUT_Z_H_A 0x2D
+
+#define ACEL_FIFO_CTRL 0x2E       // D
+#define ACEL_FIFO_CTRL_REG_A 0x2E // DLHC
+#define ACEL_FIFO_SRC 0x2F        // D
+#define ACEL_FIFO_SRC_REG_A 0x2F  // DLHC
+
+#define ACEL_IG_CFG1 0x30         // D
+#define ACEL_INT1_CFG_A 0x30      // DLH DLM DLHC
+#define ACEL_IG_SRC1 0x31         // D
+#define ACEL_INT1_SRC_A 0x31      // DLH DLM DLHC
+#define ACEL_IG_THS1 0x32         // D
+#define ACEL_INT1_THS_A 0x32      // DLH DLM DLHC
+#define ACEL_IG_DUR1 0x33         // D
+#define ACEL_INT1_DURATION_A 0x33 // DLH DLM DLHC
+#define ACEL_IG_CFG2 0x34         // D
+#define ACEL_INT2_CFG_A 0x34      // DLH DLM DLHC
+#define ACEL_IG_SRC2 0x35         // D
+#define ACEL_INT2_SRC_A 0x35      // DLH DLM DLHC
+#define ACEL_IG_THS2 0x36         // D
+#define ACEL_INT2_THS_A 0x36      // DLH DLM DLHC
+#define ACEL_IG_DUR2 0x37         // D
+#define ACEL_INT2_DURATION_A 0x37 // DLH DLM DLHC
+
+#define ACEL_CLICK_CFG 0x38      // D
+#define ACEL_CLICK_CFG_A 0x38    // DLHC
+#define ACEL_CLICK_SRC 0x39      // D
+#define ACEL_CLICK_SRC_A 0x39    // DLHC
+#define ACEL_CLICK_THS 0x3A      // D
+#define ACEL_CLICK_THS_A 0x3A    // DLHC
+#define ACEL_TIME_LIMIT 0x3B     // D
+#define ACEL_TIME_LIMIT_A 0x3B   // DLHC
+#define ACEL_TIME_LATENCY 0x3C   // D
+#define ACEL_TIME_LATENCY_A 0x3C // DLHC
+#define ACEL_TIME_WINDOW 0x3D    // D
+#define ACEL_TIME_WINDOW_A 0x3D  // DLHC
+
+#define ACEL_Act_THS 0x3E // D
+#define ACEL_Act_DUR 0x3F // D
+
+#define ACEL_CRA_REG_M 0x00 // DLH DLM DLHC
+#define ACEL_CRB_REG_M 0x01 // DLH DLM DLHC
+#define ACEL_MR_REG_M 0x02  // DLH DLM DLHC
+
+#define ACEL_SR_REG_M 0x09  // DLH DLM DLHC
+#define ACEL_IRA_REG_M 0x0A // DLH DLM DLHC
+#define ACEL_IRB_REG_M 0x0B // DLH DLM DLHC
+#define ACEL_IRC_REG_M 0x0C // DLH DLM DLHC
+
+#define ACEL_WHO_AM_I 0x0F   // D
+#define ACEL_WHO_AM_I_M 0x0F // DLM
+
+#define ACEL_TEMP_OUT_H_M 0x31 // DLHC
+#define ACEL_TEMP_OUT_L_M 0x32 // DLHC
+
+#define ACEL_D_OUT_X_L_M 0x08
+#define ACEL_D_OUT_X_H_M 0x09
+#define ACEL_D_OUT_Y_L_M 0x0A
+#define ACEL_D_OUT_Y_H_M 0x0B
+#define ACEL_D_OUT_Z_L_M 0x0C
+#define ACEL_D_OUT_Z_H_M 0x0D
+
 // Structures
 
-struct gyroRaw {
+struct axesRaw {
     int16_t x;
     int16_t y;
     int16_t z;
 };
 
+struct axes {
+    float x;
+    float y;
+    float z;
+};
+
 void configureIMU();
 bool connectedIMU();
 void deconfigureIMU();
-struct gyroRaw readGyroRaw();
+struct axesRaw readGyroRaw();
+struct axesRaw readAcelRaw();
+void* TaskIMU(void* pData);
 
 #endif

chef/src/lcd.c

@@ -16,6 +16,15 @@ uint8_t pos;
 void initLCD()
 {
     lcdFd = openI2C(LCD_ADDR);
+    offLCD();
+    onLCD();
+    pinMode(LCD_ON_PIN, OUTPUT);
+}
+
+void onLCD()
+{
+    digitalWrite(LCD_ON_PIN, LOW);
+    delay(100);
 
     // TODO More details
     sendLCD(0x33, LCD_MODE_CMD); // Initialise
@@ -25,7 +34,25 @@ void initLCD()
     sendLCD(0x28, LCD_MODE_CMD); // Data length, number of lines, font size
     clearLCD();
 
-    delayMicroseconds(500);
+    delay(50);
+}
+
+void offLCD()
+{
+    digitalWrite(LCD_ON_PIN, HIGH);
+    delay(100);
+}
+
+void resetLCD()
+{
+    char line1bkp[LCD_NB_TOTAL];
+    char line2bkp[LCD_NB_TOTAL];
+    memcpy(line1bkp, virtual, LCD_NB_CHARS * sizeof(char));
+    memcpy(line2bkp, virtual + LCD_NB_CHARS, LCD_NB_CHARS * sizeof(char));
+    offLCD();
+    onLCD();
+    printToLCD(LCD_LINE_1, line1bkp);
+    printToLCD(LCD_LINE_2, line2bkp);
 }
 
 void clearLCD()

chef/src/lcd.h

@@ -23,6 +23,8 @@
 
 #define LCD_MASK_ENABLE 0x04 // Enable bit
 
+#define LCD_ON_PIN 4
+
 // Public
 void initLCD();
 void clearLCD();
@@ -39,5 +41,8 @@ void printLCD(char* s);
 void displayLCD();
 void lockLCD();
 void unlockLCD();
+void onLCD();
+void offLCD();
+void resetLCD();
 
 #endif

chef/src/parcours.c

@@ -74,6 +74,7 @@ void stopParcours()
     stop();
     disableConsigne();
 
+    resetLCD();
     updateTimeDisplay();
     printRightLCD(LCD_LINE_1, "FIN");
     showPoints();

chef/src/premier.c

@@ -6,8 +6,8 @@
 #include <unistd.h> // sleep
 #include <wiringPi.h>
 
-#include "CA.h"
 #include "CF.h"
+#include "actionneurs.h"
 #include "debug.h"
 #include "i2c.h"
 #include "ihm.h"
@@ -36,11 +36,10 @@ int main()
     configureDebug();
     configureIHM();
     configureCF();
-    configureCA();
     configureIMU();
+    configureActionneurs();
     configurePosition();
     configureMovement();
-    configureActionneurs();
     startDebug();
     startIHM();
 
@@ -52,11 +51,10 @@ int main()
     pthread_mutex_lock(&sRunning);
     pthread_mutex_lock(&sRunning);
 
-    deconfigureActionneurs();
     deconfigureMovement();
     deconfigurePosition();
+    deconfigureActionneurs();
     deconfigureIMU();
-    deconfigureCA();
     deconfigureCF();
     deconfigureIHM();
     deconfigureDebug();

chef/src/testIMU.c

@@ -22,15 +22,15 @@ int main(int argc, char* argv[])
         exit(1);
     }
 
-    struct gyroRaw a;
+    struct axesRaw a;
-    struct gyroRaw t;
+    struct axesRaw g;
     for (;;) {
-        a = readGyroRaw();
+        a = readAcelRaw();
-        t.x += a.x;
+        printf("X:%5d Y:%5d Z:%5d\n", a.x, a.y, a.z);
-        t.y += a.y;
+        g = readGyroRaw();
-        t.z += a.z;
+        printf("X:%5d Y:%5d Z:%5d\n", g.x, g.y, g.z);
+        printf("\n");
         usleep(100*1000);
-        printf("X:%5d Y:%5d Z:%5d\n", t.x, t.y, t.z);
     }
 
     exit(0);

chef/src/testLCD.c

@@ -0,0 +1,30 @@
+/* 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 "i2c.h"
+#include "lcd.h"
+
+int main(int argc, char* argv[])
+{
+
+    (void)argc;
+    (void)argv;
+
+    wiringPiSetup();
+    initI2C();
+    initLCD();
+
+    for (;;) {
+        printToLCD(LCD_LINE_1, "Buongiorno");
+        printToLCD(LCD_LINE_2, "Come stai?");
+        sleep(1);
+        resetLCD();
+        sleep(1);
+    }
+}