From fc1c7786da8dac0697f91122282aaae2037523ce Mon Sep 17 00:00:00 2001
From: Geoffrey Frogeye <geoffrey@frogeye.fr>
Date: Wed, 2 May 2018 08:26:35 +0200
Subject: [PATCH] Misc 2

---
 chef/src/CF.h                                 |  2 +-
 chef/src/debug.c                              |  9 +++-
 chef/src/debug.h                              | 17 ++++--
 chef/src/diagnostics.c                        | 53 +++++++++++++++++--
 chef/src/diagnostics.h                        |  5 +-
 chef/src/ihm.c                                | 42 +++++++--------
 chef/src/parcours.c                           | 11 +++-
 chef/src/parcours.h                           |  1 +
 chef/src/points.c                             |  6 +++
 chef/src/points.h                             |  1 +
 chef/src/position.c                           |  7 +++
 chef/src/position.h                           |  1 +
 .../cdfprincipal/rootfs_overlay/root/.profile |  5 ++
 13 files changed, 124 insertions(+), 36 deletions(-)

diff --git a/chef/src/CF.h b/chef/src/CF.h
index d43d455..af20ca1 100644
--- a/chef/src/CF.h
+++ b/chef/src/CF.h
@@ -10,7 +10,7 @@
 
 #define FPGA_PORTNAME "/dev/ttyUSB0"
 #define CF_BAUDRATE B115200
-#define PRINTRAWDATA
+// #define PRINTRAWDATA
 
 int fpga;
 pthread_mutex_t sSendCF;
diff --git a/chef/src/debug.c b/chef/src/debug.c
index c8e0795..e2f55a2 100644
--- a/chef/src/debug.c
+++ b/chef/src/debug.c
@@ -40,6 +40,13 @@ struct timespec debugStart;
 struct timespec debugNow;
 struct timespec debugEcoule;
 
+int debugInterval = DEBUG_INTERVAL_IDLE;
+
+void debugSetActive(bool active)
+{
+    debugInterval = active ? DEBUG_INTERVAL_ACTIVE : DEBUG_INTERVAL_IDLE;
+}
+
 void* TaskDebug(void* pdata)
 {
     (void)pdata;
@@ -90,7 +97,7 @@ void* TaskDebug(void* pdata)
         fprintf(debugFd, "\n");
         fflush(debugFd);
 
-        usleep(DEBUG_INTERVAL * 1000);
+        usleep(debugInterval * 1000);
     }
 
     return NULL;
diff --git a/chef/src/debug.h b/chef/src/debug.h
index 12c2e14..83bd800 100644
--- a/chef/src/debug.h
+++ b/chef/src/debug.h
@@ -3,13 +3,20 @@
 
 #include <pthread.h>
 #include <time.h>
+#include <stdbool.h>
 
 // Constantes
-#define DEBUG_INTERVAL 100
-
+#define DEBUG_INTERVAL_IDLE 50
+#define DEBUG_INTERVAL_ACTIVE 1000
 
 // Structures
-enum debugArgTypes {d, f, ld, lf, s};
+enum debugArgTypes {
+    d,
+    f,
+    ld,
+    lf,
+    s
+};
 
 struct debugArg {
     enum debugArgTypes type;
@@ -17,14 +24,14 @@ struct debugArg {
     struct debugArg* next;
 };
 
-
 // Public
 void configureDebug(); // Avant tous les configure
 void registerDebugVar(char* name, enum debugArgTypes type, void* var);
 void startDebug(); // Après tous les configure
 void deconfigureDebug();
+void debugSetActive(bool active);
 
 // Private
-void* TaskDebug(void *pdata);
+void* TaskDebug(void* pdata);
 
 #endif
diff --git a/chef/src/diagnostics.c b/chef/src/diagnostics.c
index 48b26e3..ddf1f28 100644
--- a/chef/src/diagnostics.c
+++ b/chef/src/diagnostics.c
@@ -5,6 +5,7 @@
 #include "lcd.h"
 
 #include "CF.h"
+#include "movement.h"
 
 bool recu;
 
@@ -13,8 +14,10 @@ void setRecu()
     recu = true;
 }
 
-bool diagFPGA()
+bool diagFPGA(void* arg)
 {
+    (void) arg;
+
     recu = false;
     registerRxHandler(C2FD_PING, setRecu);
     sendCF(C2FD_PING, NULL, 0);
@@ -28,17 +31,52 @@ bool diagFPGA()
     return recu;
 }
 
-bool diagArduino()
+bool diagArduino(void* arg)
 {
+    (void) arg;
     return false;
 }
 
-void execDiagnostic(char *name, bool (*diagnostic)(void))
+bool diagCodeuse(void* arg)
+{
+    int i = *((int*) arg);
+    long lCod, rCod;
+    getCoders(&lCod, &rCod);
+    float tension = DIAGNOSTIC_TENSION_TEST;
+    if (i % 2 == 1) { // Arrière
+        tension = - tension;
+    }
+    printf("49 %f\n", tension);
+    if (i < 2) {
+        changerMoteurs(tension, 0);
+    } else {
+        changerMoteurs(0, tension);
+    }
+    usleep(500*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
+        return (-diff > DIAGNOSTIC_CODEUSES_DIFF_MIN);
+    }
+}
+
+void execDiagnostic(char *name, bool (*diagnostic)(void* arg), void* arg)
 {
     clearLCD();
     printToLCD(LCD_LINE_1, name);
     printToLCD(LCD_LINE_2, "...");
-    bool res = diagnostic();
+    bool res = diagnostic(arg);
     if (res) {
         printToLCD(LCD_LINE_2, "Ok!");
         usleep(DIAGNOSTIC_INTERVAL * 1000);
@@ -50,7 +88,12 @@ void execDiagnostic(char *name, bool (*diagnostic)(void))
 
 void runDiagnostics()
 {
-    execDiagnostic("Lien FPGA", diagFPGA);
+    execDiagnostic("Lien FPGA", diagFPGA, NULL);
     /* execDiagnostic("Lien Arduino", diagArduino); */
+    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);
 }
 
diff --git a/chef/src/diagnostics.h b/chef/src/diagnostics.h
index 07610c4..7b525b5 100644
--- a/chef/src/diagnostics.h
+++ b/chef/src/diagnostics.h
@@ -8,10 +8,13 @@
 #define DIAGNOSTIC_POLL_INTERVAL 100
 #define DIAGNOSTIC_SERIAL_TIMEOUT 10000
 
+#define DIAGNOSTIC_TENSION_TEST 1
+#define DIAGNOSTIC_CODEUSES_DIFF_MIN 1000
+
 // Public
 void runDiagnostics();
 
 // Private
-void execDiagnostic(char *name, bool (*diagnostic)(void));
+void execDiagnostic(char *name, bool (*diagnostic)(void* arg), void* arg);
 
 #endif
diff --git a/chef/src/ihm.c b/chef/src/ihm.c
index bb6981c..642b595 100644
--- a/chef/src/ihm.c
+++ b/chef/src/ihm.c
@@ -2,13 +2,11 @@
 #include <signal.h>
 #include <time.h>
 
-#include "ihm.h"
-#include "movement.h"
-#include "parcours.h"
-#include "points.h"
-#include "lcd.h"
 #include "buttons.h"
 #include "diagnostics.h"
+#include "ihm.h"
+#include "lcd.h"
+#include "parcours.h"
 
 // Globales
 pthread_t tIHM;
@@ -18,6 +16,7 @@ void configureIHM()
 {
     initLCD();
     printToLCD(LCD_LINE_1, "Demarrage");
+    configureParcours();
 }
 
 void startIHM()
@@ -39,7 +38,7 @@ char* getCouleur()
     return isOrange ? orangeStr : vertStr;
 }
 
-struct timespec calibrageLast = {0, 0};
+struct timespec calibrageLast = { 0, 0 };
 struct timespec calibrageNow;
 struct timespec calibrageEcoule;
 
@@ -66,6 +65,20 @@ void* TaskIHM(void* pdata)
         /*     } */
         /* } */
 
+        // Diagnostics
+        for (;;) {
+            clearLCD();
+            printToLCD(LCD_LINE_1, "Diagnostiquer");
+            bout = pressedButton(BUT_BLOCK);
+
+            if (bout == rouge) {
+                clearLCD();
+                runDiagnostics();
+            } else if (bout == jaune) {
+                break;
+            }
+        }
+
         // Couleur
         for (;;) {
             clearLCD();
@@ -82,7 +95,6 @@ void* TaskIHM(void* pdata)
         // Calibrage
         for (;;) {
             clearLCD();
-            printf("84 %ld\n", calibrageLast.tv_sec);
             if (calibrageLast.tv_sec > 0) {
                 clock_gettime(CLOCK_REALTIME, &calibrageNow);
                 if ((calibrageNow.tv_nsec - calibrageLast.tv_nsec) > 0) {
@@ -103,27 +115,13 @@ void* TaskIHM(void* pdata)
             if (bout == rouge) {
                 clearLCD();
                 printToLCD(LCD_LINE_1, "Calibrage...");
-                delay(3000);             // TODO
+                delay(3000); // TODO
                 clock_gettime(CLOCK_REALTIME, &calibrageLast);
             } else if (bout == jaune) {
                 break;
             }
         }
 
-        // Diagnostics
-        for (;;) {
-            clearLCD();
-            printToLCD(LCD_LINE_1, "Diagnostiquer");
-            bout = pressedButton(BUT_BLOCK);
-
-            if (bout == rouge) {
-                clearLCD();
-                runDiagnostics();
-            } else if (bout == jaune) {
-                break;
-            }
-        }
-
         // Parcours
         for (;;) {
             clearLCD();
diff --git a/chef/src/parcours.c b/chef/src/parcours.c
index 4f79de3..b73b537 100644
--- a/chef/src/parcours.c
+++ b/chef/src/parcours.c
@@ -8,12 +8,19 @@
 #include "parcours.h"
 #include "points.h"
 #include "position.h"
+#include "debug.h"
 
 pthread_t tParcours;
 bool isOrange;
 struct timespec tempsStart;
 struct timespec tempsNow;
-struct timespec tempsEcoule;
+struct timespec tempsEcoule = {0, 0};
+
+void configureParcours()
+{
+    registerDebugVar("temps", ld, &tempsEcoule.tv_sec);
+    configurePoints();
+}
 
 void prepareParcours(bool orange)
 {
@@ -32,6 +39,7 @@ void startParcours()
     clock_gettime(CLOCK_REALTIME, &tempsStart);
     pthread_create(&tParcours, NULL, TaskParcours, NULL); // TODO Start on mutex unlock
     printRightLCD(LCD_LINE_1, "   ");
+    debugSetActive(true);
 }
 
 void updateTimeDisplay()
@@ -67,6 +75,7 @@ void stopParcours()
     updateTimeDisplay();
     printRightLCD(LCD_LINE_1, "FIN");
     showPoints();
+    debugSetActive(false);
 }
 
 #define UP_TIME 1000
diff --git a/chef/src/parcours.h b/chef/src/parcours.h
index 0573836..e4bdff7 100644
--- a/chef/src/parcours.h
+++ b/chef/src/parcours.h
@@ -5,6 +5,7 @@
 
 #define TEMPS_PARCOURS 100
 
+void configureParcours();
 void prepareParcours(bool orange);
 void startParcours();
 // Returns : -1 if parcours ended, N ms for the next time it should be checked
diff --git a/chef/src/points.c b/chef/src/points.c
index 903d0d1..0f69863 100644
--- a/chef/src/points.c
+++ b/chef/src/points.c
@@ -1,9 +1,15 @@
 #include <stdio.h>
 
 #include "lcd.h"
+#include "debug.h"
 
 int points;
 
+void configurePoints()
+{
+    registerDebugVar("points", d, &points);
+}
+
 void resetPoints()
 {
     points = 0;
diff --git a/chef/src/points.h b/chef/src/points.h
index 43bf40d..88c873d 100644
--- a/chef/src/points.h
+++ b/chef/src/points.h
@@ -2,6 +2,7 @@
 #define __POINTS_H__
 
 // Public
+void configurePoints();
 void resetPoints();
 int getPoints();
 void addPoints(int points);
diff --git a/chef/src/position.c b/chef/src/position.c
index fe6886f..ec3c5f5 100644
--- a/chef/src/position.c
+++ b/chef/src/position.c
@@ -83,3 +83,10 @@ void deconfigurePosition()
 {
     pthread_cancel(tPosition);
 }
+
+void getCoders(long* l, long* r)
+{
+    *l = lCodTot;
+    *r = rCodTot;
+
+}
diff --git a/chef/src/position.h b/chef/src/position.h
index 55b8045..661d098 100644
--- a/chef/src/position.h
+++ b/chef/src/position.h
@@ -19,6 +19,7 @@ struct __attribute__ ((packed)) position {
 // Fonctions
 void configurePosition();
 void deconfigurePosition();
+void getCoders(long* l, long* r);
 
 #endif
 
diff --git a/raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/root/.profile b/raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/root/.profile
index a7c2a1e..32442d3 100644
--- a/raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/root/.profile
+++ b/raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/root/.profile
@@ -11,3 +11,8 @@ export PS1="[\u@\h \W] "
 export PS2="> "
 export PS3="+ "
 export PS4="- "
+
+alias r="/etc/init.d/S50chef restart"
+alias s="/etc/init.d/S50chef stop"
+alias c="cd /opt/chef/"
+alias l="tail -f $(find /opt/chef/log | sort | tail -1)"