Refined motors

chef/Makefile

@@ -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 imu ihm lcd movement parcours points position
+OBJS=buttons CF debug diagnostics i2c imu ihm lcd motor movement parcours points position
 OBJS_O=$(addprefix obj/,$(addsuffix .o,$(OBJS)))
 
 # VARIABLES AUTOMATIQUES

chef/src/diagnostics.c

@@ -6,7 +6,7 @@
 #include "lcd.h"
 
 #include "CF.h"
-#include "movement.h"
+#include "motor.h"
 #include "imu.h"
 
 bool recu;
@@ -51,9 +51,9 @@ bool diagCodeuse(void* arg)
     }
     printf("49 %f\n", tension);
     if (i < 2) {
-        changerMoteurs(tension, 0);
+        setPWMTension(tension, 0);
     } else {
-        changerMoteurs(0, tension);
+        setPWMTension(0, tension);
     }
     usleep(500*1000);
     brake();

chef/src/dimensions.h

@@ -4,26 +4,33 @@
 #include <math.h>
 
 // Dimensions pistes
-#define M_PISTE_WIDTH 3000
+#define M_PISTE_WIDTH 3000.0
-#define M_PISTE_HEIGHT 2000
+#define M_PISTE_HEIGHT 2000.0
-#define M_PISTE_ORIG_X 0
+#define M_PISTE_ORIG_X 0.0
-#define M_PISTE_ORIG_Y 0
+#define M_PISTE_ORIG_Y 0.0
 
 // Dimensions robot
-#define WIDTH 250 // mm (from meca)
+#define WIDTH 250.0 // mm (from meca)
-#define HEIGHT 100 // mm (from random);
+#define HEIGHT 100.0 // mm (from random);
 #define DISTANCE_BETWEEN_WHEELS WIDTH // mm (from meca)
-#define WHEEL_DIAMETER 80 // mm (from meca)
+#define WHEEL_DIAMETER 80.0 // mm (from meca)
 #define WHEEL_PERIMETER WHEEL_DIAMETER * M_PI // mm
-#define MOTOR_SPEED_GAIN_RPMP_V 233 // rpm/V (from datasheet)
+#define MOTOR_SPEED_GAIN_RPMP_V 233.0 // rpm/V (from datasheet)
-#define MOTOR_SPEED_GAIN MOTOR_SPEED_GAIN_RPMP_V / 60 // rev/s/V
+#define MOTOR_SPEED_GAIN MOTOR_SPEED_GAIN_RPMP_V / 60.0 // motor rev/s/V
-#define MOTOR_NOMINAL_TENSION 24 // V (from datasheet)
+#define MOTOR_NOMINAL_TENSION 24.0 // V (from datasheet)
-#define CODER_RESOLUTION 100 // cycles/rev
+#define MOTOR_CONTROLLER_ALIMENTATION 24.0 // V (from elec)
-#define CODER_DATA_FACTOR 4 // increments/cycles
+#define MOTOR_CONTROLLER_REFERENCE 3.3 // V (from wiring)
-#define CODER_DATA_RESOLUTION CODER_RESOLUTION * CODER_DATA_FACTOR // cycles/rev
+#define MOTOR_SATURATION_MIN 1.0 //V (from random)
-#define CRAN_REDUC_OUT 48 // nb crans (from meca)
+#define MOTOR_SATURATION_MAX 12.0 //V (from testing)
-#define CRAN_REDUC_IN 12 // nb crans (from meca)
+#define PWM_MAX 3.3 // 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 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
 
 // Constantes asservissement
 #define D_DIR_ECART_MIN 1 // mm

chef/src/motor.c

@@ -0,0 +1,83 @@
+#include "motor.h"
+
+uint8_t moteurTensionToPWM(float V)
+{
+    if (V >= MOTOR_CONTROLLER_ALIMENTATION) {
+        return UINT8_MAX;
+    } else if (V <= 0) {
+        return 0;
+    } else {
+        return V * UINT8_MAX / MOTOR_CONTROLLER_ALIMENTATION;
+    }
+}
+
+void setMoteurTensionRaw(float lVolt, float rVolt, bool lFor, bool rFor)
+{
+    static struct C2FD_PWMs msg;
+    msg.in = 0x00;
+    if (lVolt > 0) {
+        msg.in |= 1 << (lFor ? IN1 : IN2);
+        msg.ena = moteurTensionToPWM(lVolt);
+    } else {
+        // Nothing needs to be changed for this motor controller
+    }
+
+    if (rVolt > 0) {
+        msg.in |= 1 << (rFor ? IN3 : IN4);
+        msg.enb = moteurTensionToPWM(lVolt);
+    } else {
+        // Nothing needs to be changed for this motor controller
+    }
+
+    sendCF(C2FD_PWM, &msg, sizeof(struct C2FD_PWMs));
+}
+
+void setMoteurTension(float lVolt, float rVolt)
+{
+
+    // Gauche
+    bool lFor = lVolt < 0;
+    lVolt = fabs(lVolt);
+    if (lVolt < MOTOR_SATURATION_MIN) {
+        lVolt = 0;
+    } else if (lVolt > MOTOR_SATURATION_MAX) {
+        lVolt = MOTOR_SATURATION_MAX;
+    }
+
+    // Droite
+    bool rFor = rVolt < 0;
+    rVolt = fabs(rVolt);
+    if (rVolt < MOTOR_SATURATION_MIN) {
+        rVolt = 0;
+    } else if (rVolt > MOTOR_SATURATION_MAX) {
+        rVolt = MOTOR_SATURATION_MAX;
+    }
+
+    setMoteurTensionRaw(lVolt, rVolt, lFor, rFor);
+}
+
+void setPWMTension(float lVolt, float rVolt)
+{
+    setMoteurTension(
+        lVolt * MOTOR_CONTROLLER_ALIMENTATION / MOTOR_CONTROLLER_REFERENCE,
+        rVolt * MOTOR_CONTROLLER_ALIMENTATION / MOTOR_CONTROLLER_REFERENCE);
+}
+
+static struct C2FD_PWMs msgBrake = { 0, 0, 0x00 };
+static struct C2FD_PWMs msgFree = { 0, 0, (1 << IN1) | (1 << IN2) | (1 << IN3) | (1 << IN4) };
+
+int brake()
+{
+    sendCF(C2FD_PWM, &msgBrake, sizeof(struct C2FD_PWMs));
+}
+
+int freewheel()
+{
+    sendCF(C2FD_PWM, &msgFree, sizeof(struct C2FD_PWMs));
+}
+
+int stop()
+{
+    brake();
+    // TODO Actionneurs
+}

chef/src/motor.h

@@ -0,0 +1,29 @@
+#ifndef __MOTOR_H__
+#define __MOTOR_H__
+
+#include <math.h>
+#include <stdint.h>
+
+#include "dimensions.h"
+#include "CF.h"
+
+#define TESTINATOR
+// #define TLE5206
+
+#define IN1 0
+#define IN2 1
+#define IN3 2
+#define IN4 3
+
+// Public
+void setMoteurTension(float lVolt, float rVolt);
+void setPWMTension(float lVolt, float rVolt);
+int brake();
+int freewheel();
+int stop();
+
+// Private
+uint8_t moteurTensionToPWM(float V);
+void setMoteurTensionRaw(float lVolt, float rVolt, bool lFor, bool rFor);
+
+#endif

chef/src/movement.c

@@ -1,10 +1,12 @@
+#include <math.h>
 #include <stdbool.h>
 #include <stdio.h>
 #include <stdlib.h>
-#include <math.h>
 
-#include "movement.h"
 #include "CF.h"
+#include "dimensions.h"
+#include "motor.h"
+#include "movement.h"
 
 void configureMovement()
 {
@@ -13,71 +15,7 @@ void configureMovement()
 
 void aller(struct position* pos);
 
-int dbg = 0;
-
-uint8_t tensionToPWM(float V)
-{
-    if (V >= PWM_MAX_V) {
-        return PWM_MAX;
-    } else if (V <= 0) {
-        return 0;
-    } else {
-        return V * (float) PWM_MAX / (float) PWM_MAX_V;
-    }
-}
-
-// Tension de PWM
-// TODO Changer en tension de moteur
-int setMoteurTension(float lVolt, float rVolt)
-{
-    static struct C2FD_PWMs msg;
-
-    msg.in = 0x00;
-
-    // TODO Protections
-
-    // Gauche
-    bool lFor = lVolt < 0;
-    lVolt = fabs(lVolt);
-    msg.in |= 1 << (lFor ? IN1 : IN2);
-    msg.ena = tensionToPWM(lVolt);
-
-    // Droite
-    bool rFor = rVolt < 0;
-    rVolt = fabs(rVolt);
-    msg.in |= 1 << (rFor ? IN3 : IN4);
-    msg.enb = tensionToPWM(rVolt);
-
-    sendCF(C2FD_PWM, &msg, sizeof(struct C2FD_PWMs));
-}
-
-int changerMoteurs(float lVit, float rVit)
-{
-    // TODO Conversion en vitesse
-    setMoteurTension(lVit, rVit);
-}
-
-static struct C2FD_PWMs msgBrake = {0, 0, 0x00};
-static struct C2FD_PWMs msgFree = {0, 0, (1 << IN1) | (1 << IN2) | (1 << IN3) | (1 << IN4)};
-
-int brake()
-{
-    sendCF(C2FD_PWM, &msgBrake, sizeof(struct C2FD_PWMs));
-}
-
-int freewheel()
-{
-    sendCF(C2FD_PWM, &msgFree, sizeof(struct C2FD_PWMs));
-}
-
-int stop()
-{
-    brake();
-    // TODO Actionneurs
-}
-
 void deconfigureMovement()
 {
     stop();
 }
-

chef/src/movement.h

@@ -7,36 +7,7 @@
 
 #include "position.h"
 
-#define PWM_MAX 255
-#define PWM_MAX_V 3.3
-
-#define TESTINATOR
-// #define TLE5206
-
-#ifdef TESTINATOR
-#define MOT_MIN_V 0.1
-#define MOT_MAX_V 2.0
-#endif
-
-#ifdef TLE5206
-#define MOT_MIN_V 0.1
-#define MOT_MAX_V 2.5
-#endif
-
-#define IN1 0
-#define IN2 1
-#define IN3 2
-#define IN4 3
-
 void configureMovement();
-void aller(struct position* pos);
-int changerMoteurs(float vitL, float vitR);
-// Vitesse en mm/s
-// Vitesse < 0 ⇒ sens inverse
-// Si vitesse < seuil ⇒ brake
-int brake();
-int stop();
-int freewheel();
 void deconfigureMovement();
 
 #endif

chef/src/parcours.c

@@ -5,6 +5,7 @@
 #include "debug.h"
 #include "lcd.h"
 #include "movement.h"
+#include "motor.h"
 #include "parcours.h"
 #include "points.h"
 #include "position.h"
@@ -81,7 +82,7 @@ void stopParcours()
 #define HIGH_TIME 3000
 #define DOWN_TIME 1000
 #define LOW_TIME 2000
-#define MAX_VIT MOT_MAX_V
+#define MAX_VIT 2
 
 void* TaskParcours(void* pdata)
 {
@@ -91,22 +92,22 @@ void* TaskParcours(void* pdata)
         addPoints(1);
         for (int i = 0; i < UP_TIME; i++) {
             float p = (float)i / (float)UP_TIME;
-            changerMoteurs(p * MOT_MAX_V, p * MOT_MAX_V);
+            setPWMTension(p * MAX_VIT, p * MAX_VIT);
             usleep(1000 * 1);
         }
         addPoints(1);
-        changerMoteurs(MOT_MAX_V, MOT_MAX_V);
+        setPWMTension(MAX_VIT, MAX_VIT);
         usleep(1000 * HIGH_TIME);
 
         addPoints(1);
         for (int i = 0; i < DOWN_TIME; i++) {
             float p = (float)i / (float)DOWN_TIME;
             p = 1 - p;
-            changerMoteurs(p * MOT_MAX_V, p * MOT_MAX_V);
+            setPWMTension(p * MAX_VIT, p * MAX_VIT);
             usleep(1000 * 1);
         }
         addPoints(1);
-        changerMoteurs(0, 0);
+        setPWMTension(0, 0);
         usleep(1000 * LOW_TIME);
     }
 

chef/src/testCodeuse.c

@@ -11,7 +11,7 @@
 #include "i2c.h"
 #include "ihm.h"
 #include "imu.h"
-#include "movement.h"
+#include "motor.h"
 #include "position.h"
 
 pthread_mutex_t sRunning;
@@ -31,7 +31,7 @@ int main()
     configureCF();
     configurePosition();
 
-    changerMoteurs(VIT, VIT);
+    setPWMTension(VIT, VIT);
 
     long lCod, rCod;
     for (;;) {

chef/src/testForward.c

@@ -9,7 +9,7 @@
 
 #include "lcd.h"
 #include "CF.h"
-#include "movement.h"
+#include "motor.h"
 #include "buttons.h"
 
 #define PATATE 3.3/2
@@ -26,27 +26,26 @@ int main(int argc, char* argv[])
     initLCD();
     configureCF();
     configureButtons();
-    configureMovement();
 
     for (;;) {
         clearLCD();
         printToLCD(LCD_LINE_1, "Forward");
-        changerMoteurs(PATATE, PATATE);
+        setPWMTension(PATATE, PATATE);
         pressedButton(BUT_BLOCK);
 
         clearLCD();
         printToLCD(LCD_LINE_1, "Right");
-        changerMoteurs(-PATATE, PATATE);
+        setPWMTension(-PATATE, PATATE);
         pressedButton(BUT_BLOCK);
 
         clearLCD();
         printToLCD(LCD_LINE_1, "Left");
-        changerMoteurs(-PATATE, -PATATE);
+        setPWMTension(-PATATE, -PATATE);
         pressedButton(BUT_BLOCK);
 
         clearLCD();
         printToLCD(LCD_LINE_1, "Backward");
-        changerMoteurs(PATATE, -PATATE);
+        setPWMTension(PATATE, -PATATE);
         pressedButton(BUT_BLOCK);
 
         clearLCD();

chef/src/testSlow.c

@@ -9,17 +9,17 @@
 
 #include "lcd.h"
 #include "CF.h"
-#include "movement.h"
+#include "motor.h"
 #include "buttons.h"
 
 #define VIT 0.40
 
 
-void changerMoteursWrapper(float l, float r) {
+void setPWMTensionWrapper(float l, float r) {
     /* clearLCD(); */
     printfToLCD(LCD_LINE_1, "L: %f", l);
     printfToLCD(LCD_LINE_2, "R: %f", r);
-    changerMoteurs(l, r);
+    setPWMTension(l, r);
 }
 
 int main(int argc, char* argv[])
@@ -36,10 +36,10 @@ int main(int argc, char* argv[])
     configureButtons();
     configureMovement();
 
-    changerMoteursWrapper(VIT, VIT);
+    setPWMTensionWrapper(VIT, VIT);
 
     for (;;) {
-        changerMoteursWrapper(VIT, VIT);
+        setPWMTensionWrapper(VIT, VIT);
         pressedButton(BUT_BLOCK);
         brake();
         pressedButton(BUT_BLOCK);

chef/src/testUpDown.c

@@ -9,7 +9,7 @@
 
 #include "lcd.h"
 #include "CF.h"
-#include "movement.h"
+#include "motor.h"
 #include "buttons.h"
 
 #define UP_TIME 1000
@@ -23,7 +23,7 @@ void changerMoteursWrapper(float l, float r) {
     /* clearLCD(); */
     printfToLCD(LCD_LINE_1, "L: %f", l);
     printfToLCD(LCD_LINE_2, "R: %f", r);
-    changerMoteurs(l, r);
+    setPWMTension(l, r);
 }
 
 int main(int argc, char* argv[])
@@ -38,21 +38,20 @@ int main(int argc, char* argv[])
     initLCD();
     configureCF();
     configureButtons();
-    configureMovement();
 
     for (;;) {
         for (int i = 0; i < UP_TIME; i += INTERVAL) {
             float p = (float)i / (float)UP_TIME;
-            changerMoteursWrapper(p * MOT_MAX_V, p * MOT_MAX_V);
+            changerMoteursWrapper(p * MAXI, p * MAXI);
             delay(INTERVAL);
         }
-        changerMoteursWrapper(MOT_MAX_V, MOT_MAX_V);
+        changerMoteursWrapper(MAXI, MAXI);
         delay(HIGH_TIME);
 
         for (int i = 0; i < DOWN_TIME; i += INTERVAL) {
             float p = (float)i / (float)DOWN_TIME;
             p = 1 - p;
-            changerMoteursWrapper(p * MOT_MAX_V, p * MOT_MAX_V);
+            changerMoteursWrapper(p * MAXI, p * MAXI);
             delay(INTERVAL);
         }
         changerMoteursWrapper(0, 0);