From 5f6e9b3ee5e05028b8735887a6b669c1b0f15b6e Mon Sep 17 00:00:00 2001
From: Geoffrey Frogeye <geoffrey@frogeye.fr>
Date: Wed, 16 May 2018 07:58:55 +0200
Subject: [PATCH] Essai de calibrage en position

---
 chef/src/calibrage.c     | 42 +++++++++++++++++++++++++++++++++++
 chef/src/calibrage.h     | 16 ++++++++++++++
 chef/src/testCalibrage.c | 47 ++++++++++++++++++++++++++++++++++++++++
 3 files changed, 105 insertions(+)
 create mode 100644 chef/src/calibrage.c
 create mode 100644 chef/src/calibrage.h
 create mode 100644 chef/src/testCalibrage.c

diff --git a/chef/src/calibrage.c b/chef/src/calibrage.c
new file mode 100644
index 0000000..84d1d24
--- /dev/null
+++ b/chef/src/calibrage.c
@@ -0,0 +1,42 @@
+#include <stdbool.h>
+#include <stdio.h>
+#include <unistd.h>
+
+#include "calibrage.h"
+
+#include "movement.h"
+#include "dimensions.h"
+#include "securite.h"
+#include "motor.h"
+#include "common.h"
+
+void calibrer(bool orange)
+{
+    struct position pos = {0, 0, 0};
+    setPosition(&pos);
+    return;
+
+    // Calibrage contre mur court
+    /* pos.o = orange ? M_PI_2 : -M_PI_2; */
+    setDestination(&pos);
+    waitDestination();
+    disableAsservissement();
+
+    float frontL, frontR, backL, backR;
+    struct movAvg diffL, diffR;
+    initMovAvg(&diffL, CALIBRAGE_MOVAVG_SIZE);
+    initMovAvg(&diffR, CALIBRAGE_MOVAVG_SIZE);
+    for (int i = 0; i < CALIBRAGE_TEMPS_ACQUISITION; i += SENSOR_SAMPLING_INTERVAL) {
+        getAllDistance(&frontL, &frontR, &backL, &backR);
+        addMovAvg(&diffL, frontL - CALIBRAGE_DISTANCE_Y);
+        addMovAvg(&diffR, frontR - CALIBRAGE_DISTANCE_Y);
+        setMoteurTension(diffL.current * D_KP, diffR.current * D_KP);
+        usleep(SENSOR_SAMPLING_INTERVAL * 1000);
+    }
+    // Les moteurs n'auront peut-être pas atteint le point voulu exactement,
+    // on enregistre alors la différence
+    float diff = (diffL.current + diffR.current) / 2;
+    pos.y = diff;
+
+
+}
diff --git a/chef/src/calibrage.h b/chef/src/calibrage.h
new file mode 100644
index 0000000..65c8071
--- /dev/null
+++ b/chef/src/calibrage.h
@@ -0,0 +1,16 @@
+#ifndef __CALIBRAGE_H_
+#define __CALIBRAGE_H_
+
+#include <stdbool.h>
+
+// Constantes
+
+#define CALIBRAGE_TEMPS_ACQUISITION 30000
+#define CALIBRAGE_DISTANCE_X 300
+#define CALIBRAGE_DISTANCE_Y 300
+#define CALIBRAGE_MOVAVG_SIZE 4
+
+// Public
+void calibrer(bool orange);
+
+#endif
diff --git a/chef/src/testCalibrage.c b/chef/src/testCalibrage.c
new file mode 100644
index 0000000..70bb7ad
--- /dev/null
+++ b/chef/src/testCalibrage.c
@@ -0,0 +1,47 @@
+#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 "actionneurs.h"
+#include "calibrage.h"
+#include "debug.h"
+#include "i2c.h"
+#include "motor.h"
+#include "movement.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();
+    configurePosition();
+    configureMovement();
+    debugSetActive(true);
+    startDebug();
+
+    calibrer(false);
+
+    deconfigureMovement();
+    deconfigurePosition();
+    deconfigureDebug();
+    return EXIT_SUCCESS;
+}