From 6817aaf7799719bb71ff56e5f7bf076e519d7295 Mon Sep 17 00:00:00 2001
From: Geoffrey Frogeye <geoffrey@frogeye.fr>
Date: Sun, 6 May 2018 12:50:03 +0200
Subject: [PATCH] Bon sens des moteurs

---
 chef/src/CF.h                                 |  2 +-
 chef/src/debug.c                              |  2 ++
 chef/src/diagnostics.c                        |  1 +
 chef/src/dimensions.h                         | 22 ++++++++--------
 chef/src/motor.c                              | 26 ++++++++++++++++---
 chef/src/motor.h                              |  3 +++
 chef/src/parcours.c                           |  6 ++++-
 chef/src/position.c                           | 14 +++++++---
 chef/src/position.h                           |  2 ++
 chef/src/testForward.c                        | 10 +++----
 chef/src/testSlow.c                           |  9 +++----
 chef/src/testStop.c                           | 17 ++++++++++++
 chef/src/testUpDown.c                         |  4 +--
 fpga/Principal.vhd                            |  2 +-
 .../cdfprincipal/rootfs_overlay/root/.profile |  8 +++++-
 15 files changed, 95 insertions(+), 33 deletions(-)
 create mode 100644 chef/src/testStop.c

diff --git a/chef/src/CF.h b/chef/src/CF.h
index af20ca1..d43d455 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 3fe2d3a..ccb5677 100644
--- a/chef/src/debug.c
+++ b/chef/src/debug.c
@@ -114,6 +114,8 @@ void configureDebug()
         exit(EXIT_FAILURE);
     }
 
+    printf("Writing log in file: %s\n", path);
+
     fprintf(debugFd, "time");
 }
 
diff --git a/chef/src/diagnostics.c b/chef/src/diagnostics.c
index 3fa1ad7..beafd32 100644
--- a/chef/src/diagnostics.c
+++ b/chef/src/diagnostics.c
@@ -8,6 +8,7 @@
 #include "CF.h"
 #include "motor.h"
 #include "imu.h"
+#include "position.h"
 
 bool recu;
 
diff --git a/chef/src/dimensions.h b/chef/src/dimensions.h
index 546d0e2..50c4c78 100644
--- a/chef/src/dimensions.h
+++ b/chef/src/dimensions.h
@@ -11,26 +11,26 @@
 
 // Dimensions robot
 #define WIDTH 250.0 // mm (from meca)
-#define HEIGHT 100.0 // mm (from random);
+#define HEIGHT 100.0 // mm (from random)
 #define DISTANCE_BETWEEN_WHEELS WIDTH // mm (from meca)
 #define WHEEL_DIAMETER 80.0 // mm (from meca)
-#define WHEEL_PERIMETER WHEEL_DIAMETER * M_PI // mm
+#define WHEEL_PERIMETER (WHEEL_DIAMETER * M_PI) // mm
 #define MOTOR_SPEED_GAIN_RPMP_V 233.0 // rpm/V (from datasheet)
-#define MOTOR_SPEED_GAIN MOTOR_SPEED_GAIN_RPMP_V / 60.0 // motor rev/s/V
+#define MOTOR_SPEED_GAIN (MOTOR_SPEED_GAIN_RPMP_V / 60.0) // motor rev/s/V
 #define MOTOR_NOMINAL_TENSION 24.0 // V (from datasheet)
 #define MOTOR_CONTROLLER_ALIMENTATION 24.0 // V (from elec)
-#define MOTOR_CONTROLLER_REFERENCE 3.3 // V (from wiring)
-#define MOTOR_SATURATION_MIN 1.0 //V (from random)
-#define MOTOR_SATURATION_MAX 12.0 //V (from testing)
-#define PWM_MAX 3.3 // V (from FPGA datasheet)
+#define MOTOR_CONTROLLER_REFERENCE 5 // V (from wiring)
+#define MOTOR_SATURATION_MIN 1.0 // V (from random)
+#define MOTOR_SATURATION_MAX 12.0 // V (from testing)
+#define PWM_MAX 3.5 // V (from FPGA datasheet)
 #define CODER_RESOLUTION 370.0 // cycles/motor rev
 #define CODER_DATA_FACTOR 4.0 // increments/motor cycles
-#define CODER_DATA_RESOLUTION CODER_RESOLUTION * CODER_DATA_FACTOR // cycles/motor rev
+#define CODER_DATA_RESOLUTION (CODER_RESOLUTION * CODER_DATA_FACTOR) // cycles/motor rev
 #define CRAN_REDUC_OUT 48.0 // nb crans (from meca)
 #define CRAN_REDUC_IN 12.0 // nb crans (from meca)
-#define REDUC_RATIO CRAN_REDUC_IN / CRAN_REDUC_OUT // reduction ratio
-#define CODER_FULL_RESOLUTION CODER_DATA_RESOLUTION / REDUC_RATIO // cycles / wheel rev
-#define AV_PER_CYCLE WHEEL_PERIMETER / CODER_FULL_RESOLUTION // mm
+#define REDUC_RATIO (CRAN_REDUC_IN / CRAN_REDUC_OUT) // reduction ratio
+#define CODER_FULL_RESOLUTION (CODER_DATA_RESOLUTION / REDUC_RATIO) // cycles / wheel rev
+#define AV_PER_CYCLE (WHEEL_PERIMETER / CODER_FULL_RESOLUTION) // mm
 
 // Constantes asservissement
 #define D_DIR_ECART_MIN 1 // mm
diff --git a/chef/src/motor.c b/chef/src/motor.c
index 3b31a8d..ceeb535 100644
--- a/chef/src/motor.c
+++ b/chef/src/motor.c
@@ -1,18 +1,35 @@
 #include "motor.h"
 
-uint8_t moteurTensionToPWM(float V)
+uint8_t tensionToPWM(float V)
 {
-    if (V >= MOTOR_CONTROLLER_ALIMENTATION) {
+    printf("PWM tension %f\n", V);
+    if (V >= PWM_MAX) {
         return UINT8_MAX;
     } else if (V <= 0) {
         return 0;
     } else {
-        return V * UINT8_MAX / MOTOR_CONTROLLER_ALIMENTATION;
+        printf("PWM value %d\n", (uint8_t) (V * UINT8_MAX / PWM_MAX));
+        return V * UINT8_MAX / PWM_MAX;
     }
 }
 
+uint8_t moteurTensionToPWM(float V)
+{
+    printf("Moteur tension %f\n", V);
+    if (V >= MOTOR_CONTROLLER_ALIMENTATION) {
+        V = MOTOR_CONTROLLER_ALIMENTATION;
+    } else if (V <= 0) {
+        V = 0;
+    }
+    return tensionToPWM(V * MOTOR_CONTROLLER_REFERENCE / MOTOR_CONTROLLER_ALIMENTATION);
+}
+
 void setMoteurTensionRaw(float lVolt, float rVolt, bool lFor, bool rFor)
 {
+
+#ifdef INVERSE_L_MOTOR
+    lFor = !lFor;
+#endif
     static struct C2FD_PWMs msg;
     msg.in = 0x00;
     if (lVolt > 0) {
@@ -22,6 +39,9 @@ void setMoteurTensionRaw(float lVolt, float rVolt, bool lFor, bool rFor)
         // Nothing needs to be changed for this motor controller
     }
 
+#ifdef INVERSE_R_MOTOR
+    rFor = !rFor;
+#endif
     if (rVolt > 0) {
         msg.in |= 1 << (rFor ? IN3 : IN4);
         msg.enb = moteurTensionToPWM(lVolt);
diff --git a/chef/src/motor.h b/chef/src/motor.h
index 09969f8..fe07247 100644
--- a/chef/src/motor.h
+++ b/chef/src/motor.h
@@ -7,6 +7,9 @@
 #include "dimensions.h"
 #include "CF.h"
 
+#define INVERSE_L_MOTOR
+// #define INVERSE_R_MOTOR
+
 #define TESTINATOR
 // #define TLE5206
 
diff --git a/chef/src/parcours.c b/chef/src/parcours.c
index 1c1d358..8f9500c 100644
--- a/chef/src/parcours.c
+++ b/chef/src/parcours.c
@@ -82,12 +82,16 @@ void stopParcours()
 #define HIGH_TIME 3000
 #define DOWN_TIME 1000
 #define LOW_TIME 2000
-#define MAX_VIT 2
+#define MAX_VIT 1
 
 void* TaskParcours(void* pdata)
 {
     (void)pdata;
 
+    for (;;) {
+        setPWMTension(MAX_VIT, MAX_VIT);
+    }
+
     for (;;) {
         addPoints(1);
         for (int i = 0; i < UP_TIME; i++) {
diff --git a/chef/src/position.c b/chef/src/position.c
index ec3c5f5..3e0d425 100644
--- a/chef/src/position.c
+++ b/chef/src/position.c
@@ -19,7 +19,6 @@ pthread_t tPosition;
 unsigned int nbCalcPos;
 long lCodTot, rCodTot;
 
-
 void* TaskPosition(void* pData)
 {
     (void)pData;
@@ -42,11 +41,20 @@ void* TaskPosition(void* pData)
 
         // Calculation
         nbCalcPos++;
+#ifdef INVERSE_L_CODER
+        deltaCoders.dL = -deltaCoders.dL;
+#endif
+#ifdef INVERSE_R_CODER
+        deltaCoders.dR = -deltaCoders.dR;
+#endif
         lCodTot += deltaCoders.dL;
         rCodTot += deltaCoders.dR;
 
-        float deltaO = atan2(deltaCoders.dR - deltaCoders.dL, DISTANCE_BETWEEN_WHEELS);
-        float deltaD = (deltaCoders.dL + deltaCoders.dR) / 2;
+        float dR = deltaCoders.dR * AV_PER_CYCLE;
+        float dL = deltaCoders.dL * AV_PER_CYCLE;
+
+        float deltaO = atan2(dR - dL, DISTANCE_BETWEEN_WHEELS);
+        float deltaD = (dL + dR) / 2;
 
         connu.o += deltaO;
         float deltaX = deltaD * cos(connu.o);
diff --git a/chef/src/position.h b/chef/src/position.h
index 661d098..7c79c3a 100644
--- a/chef/src/position.h
+++ b/chef/src/position.h
@@ -8,6 +8,8 @@
 #include "CF.h"
 #include <pthread.h>
 
+// #define INVERSE_L_CODER
+#define INVERSE_R_CODER
 
 // Structures
 struct __attribute__ ((packed)) position {
diff --git a/chef/src/testForward.c b/chef/src/testForward.c
index b900312..dd72b99 100644
--- a/chef/src/testForward.c
+++ b/chef/src/testForward.c
@@ -12,7 +12,7 @@
 #include "motor.h"
 #include "buttons.h"
 
-#define PATATE 3.3/2
+#define PATATE 2
 
 int main(int argc, char* argv[])
 {
@@ -30,22 +30,22 @@ int main(int argc, char* argv[])
     for (;;) {
         clearLCD();
         printToLCD(LCD_LINE_1, "Forward");
-        setPWMTension(PATATE, PATATE);
+        setMoteurTension(PATATE, PATATE);
         pressedButton(BUT_BLOCK);
 
         clearLCD();
         printToLCD(LCD_LINE_1, "Right");
-        setPWMTension(-PATATE, PATATE);
+        setMoteurTension(-PATATE, PATATE);
         pressedButton(BUT_BLOCK);
 
         clearLCD();
         printToLCD(LCD_LINE_1, "Left");
-        setPWMTension(-PATATE, -PATATE);
+        setMoteurTension(PATATE, -PATATE);
         pressedButton(BUT_BLOCK);
 
         clearLCD();
         printToLCD(LCD_LINE_1, "Backward");
-        setPWMTension(PATATE, -PATATE);
+        setMoteurTension(-PATATE, -PATATE);
         pressedButton(BUT_BLOCK);
 
         clearLCD();
diff --git a/chef/src/testSlow.c b/chef/src/testSlow.c
index 0ae20db..0816741 100644
--- a/chef/src/testSlow.c
+++ b/chef/src/testSlow.c
@@ -12,7 +12,9 @@
 #include "motor.h"
 #include "buttons.h"
 
-#define VIT 0.40
+#define VIT 1
+#define VITL VIT
+#define VITR 0
 
 
 void setPWMTensionWrapper(float l, float r) {
@@ -34,12 +36,9 @@ int main(int argc, char* argv[])
     initLCD();
     configureCF();
     configureButtons();
-    configureMovement();
-
-    setPWMTensionWrapper(VIT, VIT);
 
     for (;;) {
-        setPWMTensionWrapper(VIT, VIT);
+        setPWMTensionWrapper(VITL, VITR);
         pressedButton(BUT_BLOCK);
         brake();
         pressedButton(BUT_BLOCK);
diff --git a/chef/src/testStop.c b/chef/src/testStop.c
new file mode 100644
index 0000000..2d2a878
--- /dev/null
+++ b/chef/src/testStop.c
@@ -0,0 +1,17 @@
+/* Teste si une broche est connecté à une autre */
+
+#include <stdlib.h>
+
+#include "CF.h"
+#include "motor.h"
+
+int main(int argc, char* argv[])
+{
+
+    (void)argc;
+    (void)argv;
+
+    configureCF();
+    stop();
+
+}
diff --git a/chef/src/testUpDown.c b/chef/src/testUpDown.c
index 12e636e..5e60580 100644
--- a/chef/src/testUpDown.c
+++ b/chef/src/testUpDown.c
@@ -16,14 +16,14 @@
 #define HIGH_TIME 3000
 #define DOWN_TIME 1000
 #define LOW_TIME 2000
-#define MAXI 3.3
+#define MAXI 12
 #define INTERVAL 10
 
 void changerMoteursWrapper(float l, float r) {
     /* clearLCD(); */
     printfToLCD(LCD_LINE_1, "L: %f", l);
     printfToLCD(LCD_LINE_2, "R: %f", r);
-    setPWMTension(l, r);
+    setMoteurTension(l, r);
 }
 
 int main(int argc, char* argv[])
diff --git a/fpga/Principal.vhd b/fpga/Principal.vhd
index 74baa08..35b0a5d 100644
--- a/fpga/Principal.vhd
+++ b/fpga/Principal.vhd
@@ -85,7 +85,7 @@ architecture Behavioral of Principal is
     -- PWM clock
     signal pwmClk : std_logic := '0';
     signal pwmCounter : integer := 0;
-    constant PWM_DIVIDER : integer := 1024;
+    constant PWM_DIVIDER : integer := 4096;
 
     -- Motor controller
     signal enAd : std_logic_vector(7 downto 0);
diff --git a/raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/root/.profile b/raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/root/.profile
index e626399..9c46d4a 100644
--- a/raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/root/.profile
+++ b/raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/root/.profile
@@ -13,8 +13,14 @@ export PS3="+ "
 export PS4="- "
 
 alias r="/etc/init.d/S50chef restart"
-alias s="/etc/init.d/S50chef stop"
 alias c="cd /opt/chef/"
+
+s()
+{
+    /etc/init.d/S50chef stop
+    /opt/chef/bin/testStop
+}
+
 l()
 {
     tail -f $(find /opt/chef/log | sort | tail -1)