Motor controller via FPGA

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

chef/src/CFsignals.h

@@ -58,5 +58,21 @@ struct __attribute__ ((packed)) F2CT_CAPTs {
     uint16_t back;
 };
 
+// Met à jour la PWM (pour ctrl moteur (EN, IN × 2) × 2)
+#define C2FD_PWM 'W'
+struct __attribute__ ((packed)) C2FD_PWMs { 
+    uint8_t ena;
+    uint8_t enb;
+    uint8_t in;
+};
+
+// Met à jour la PWM (pour ctrl moteur (EN × 2) × 2)
+#define C2FD_PWM2 'w'
+struct __attribute__ ((packed)) C2FD_PWM2s { 
+    uint8_t ena;
+    uint8_t enc;
+    uint8_t enb;
+    uint8_t end;
+};
 
 #endif

chef/src/buttons.c

@@ -0,0 +1,99 @@
+#include <pthread.h>
+#include <stdio.h>
+
+#include "buttons.h"
+
+// Globales
+pthread_t tStdinButton;
+enum boutons stdinbutton = none;
+
+// Fonctions
+void configureButtons()
+{
+    pinMode(BUT_PIN_ROUGE, INPUT);
+    pullUpDnControl(BUT_PIN_ROUGE, PUD_UP);
+    pinMode(BUT_PIN_JAUNE, INPUT);
+    pullUpDnControl(BUT_PIN_JAUNE, PUD_UP);
+    pinMode(BUT_PIN_TIRETTE, INPUT);
+    pullUpDnControl(BUT_PIN_TIRETTE, PUD_UP);
+
+    pthread_create(&tStdinButton, NULL, TaskStdinButton, NULL);
+}
+
+bool debunkButton(int pin)
+{
+    int t;
+    // Press (cancel if wrong)
+    for (t = BUT_DEBUNK_TIME / 2; t > 0; t--) {
+        if (digitalRead(pin) != LOW) {
+            return false;
+        }
+        delay(1);
+    }
+    // Release (re-wait if wrong)
+    for (t = BUT_DEBUNK_TIME / 2; t > 0; t--) {
+        if (digitalRead(pin) != HIGH) {
+            t = BUT_DEBUNK_TIME / 2;
+        }
+        delay(1);
+    }
+    return true;
+}
+
+
+void* TaskStdinButton(void* pdata)
+
+{
+    (void)pdata;
+
+    for (;;) {
+        char c = getchar();
+        if (c == '1') {
+            stdinbutton = jaune;
+        } else if (c == '2') {
+            stdinbutton = rouge;
+        }
+    }
+
+    return NULL;
+}
+
+enum boutons pressedButton(int timeout)
+{
+    bool block = timeout < 0;
+    while (timeout > 0 || block) {
+
+        if (stdinbutton != none) {
+            enum boutons bout = stdinbutton;
+            stdinbutton = none;
+            return bout;
+        }
+
+        if (debunkButton(BUT_PIN_JAUNE)) {
+            return jaune;
+        }
+
+        if (debunkButton(BUT_PIN_ROUGE)) {
+            return rouge;
+        }
+
+        delay(BUT_POLLING_INTERVAL);
+        timeout -= BUT_POLLING_INTERVAL;
+    }
+    return none;
+}
+
+bool tirettePresente()
+{
+    int etat, newEtat;
+    int t;
+    for (t = 0; t < BUT_DEBUNK_TIME; t++) {
+        newEtat = digitalRead(BUT_PIN_TIRETTE);
+        if (etat != newEtat) {
+            t = 0;
+            etat = newEtat;
+        }
+        delay(1);
+    }
+    return etat == LOW;
+}

chef/src/buttons.h

@@ -0,0 +1,30 @@
+#ifndef __BUTTONS_H_
+#define __BUTTONS_H_
+
+#include <stdbool.h>
+#include <wiringPi.h>
+
+// Constants
+#define BUT_PIN_ROUGE 0
+#define BUT_PIN_JAUNE 2
+#define BUT_PIN_TIRETTE 3
+
+// ms
+#define BUT_BLOCK -1
+#define BUT_POLLING_INTERVAL 50
+#define BUT_DEBUNK_TIME 50
+#define BUT_REFRESH_INTERVAL 1000
+
+// Structures
+enum boutons {none, jaune, rouge};
+
+// Public
+void configureButtons();
+enum boutons pressedButton(int timeout); // timeout: ms or -1
+bool tirettePresente();
+
+// Private
+bool debunkButton(int pin);
+void* TaskStdinButton(void *pdata);
+
+#endif

chef/src/ihm.c

@@ -1,17 +1,16 @@
 #include <pthread.h>
 #include <signal.h>
-#include <stdio.h>
 #include <time.h>
-#include <wiringPi.h>
 
 #include "ihm.h"
 #include "movement.h"
 #include "parcours.h"
 #include "points.h"
+#include "lcd.h"
+#include "buttons.h"
 
 // Globales
 pthread_t tIHM;
-pthread_t tStdinIHM;
 
 // Fonctions
 void configureIHM()
@@ -22,15 +21,8 @@ void configureIHM()
 
 void startIHM()
 {
-    pinMode(IHM_PIN_ROUGE, INPUT);
-    pullUpDnControl(IHM_PIN_ROUGE, PUD_UP);
-    pinMode(IHM_PIN_JAUNE, INPUT);
-    pullUpDnControl(IHM_PIN_JAUNE, PUD_UP);
-    pinMode(IHM_PIN_TIRETTE, INPUT);
-    pullUpDnControl(IHM_PIN_TIRETTE, PUD_UP);
-
+    configureButtons();
     pthread_create(&tIHM, NULL, TaskIHM, NULL);
-    pthread_create(&tStdinIHM, NULL, TaskStdinIHM, NULL);
     pthread_join(tIHM, NULL);
 }
 
@@ -46,84 +38,6 @@ void diffTimespec(const struct timespec* t1, const struct timespec* t2, struct t
     }
 }
 
-bool debunkButtonIHM(int pin)
-{
-    int t;
-    // Press (cancel if wrong)
-    for (t = IHM_DEBUNK_TIME / 2; t > 0; t--) {
-        if (digitalRead(pin) != LOW) {
-            return false;
-        }
-        delay(1);
-    }
-    // Release (re-wait if wrong)
-    for (t = IHM_DEBUNK_TIME / 2; t > 0; t--) {
-        if (digitalRead(pin) != HIGH) {
-            t = IHM_DEBUNK_TIME / 2;
-        }
-        delay(1);
-    }
-    return true;
-}
-
-enum boutons stdinbutton = none;
-
-void* TaskStdinIHM(void* pdata)
-{
-    (void)pdata;
-
-    for (;;) {
-        char c = getchar();
-        if (c == '1') {
-            stdinbutton = jaune;
-        } else if (c == '2') {
-            stdinbutton = rouge;
-        }
-    }
-
-    return NULL;
-}
-
-enum boutons pressedIHM(int timeout)
-{
-    bool block = timeout < 0;
-    while (timeout > 0 || block) {
-
-        if (stdinbutton != none) {
-            enum boutons bout = stdinbutton;
-            stdinbutton = none;
-            return bout;
-        }
-
-        if (debunkButtonIHM(IHM_PIN_JAUNE)) {
-            return jaune;
-        }
-
-        if (debunkButtonIHM(IHM_PIN_ROUGE)) {
-            return rouge;
-        }
-
-        delay(IHM_POLLING_INTERVAL);
-        timeout -= IHM_POLLING_INTERVAL;
-    }
-    return none;
-}
-
-bool tirettePresente()
-{
-    int etat, newEtat;
-    int t;
-    for (t = 0; t < IHM_DEBUNK_TIME; t++) {
-        newEtat = digitalRead(IHM_PIN_TIRETTE);
-        if (etat != newEtat) {
-            t = 0;
-            etat = newEtat;
-        }
-        delay(1);
-    }
-    return etat == LOW;
-}
-
 bool isDebug = false;
 bool isOrange = true;
 bool annuler = false;
@@ -152,7 +66,7 @@ void* TaskIHM(void* pdata)
             if (isDebug) {
                 printToLCD(LCD_LINE_2, "192.168.0.0 TODO");
             }
-            bout = pressedIHM(IHM_REFRESH_INTERVAL);
+            bout = pressedButton(BUT_REFRESH_INTERVAL);
 
             if (bout == rouge) {
                 isDebug = !isDebug;
@@ -165,7 +79,7 @@ void* TaskIHM(void* pdata)
         for (;;) {
             clearLCD();
             printfToLCD(LCD_LINE_1, "Couleur : %s", getCouleur());
-            bout = pressedIHM(IHM_BLOCK);
+            bout = pressedButton(BUT_BLOCK);
 
             if (bout == rouge) {
                 isOrange = !isOrange;
@@ -184,7 +98,7 @@ void* TaskIHM(void* pdata)
                 printToLCD(LCD_LINE_1, "Calibrer");
                 printfToLCD(LCD_LINE_2, "(%s)", getCouleur());
             }
-            bout = pressedIHM(IHM_REFRESH_INTERVAL);
+            bout = pressedButton(BUT_REFRESH_INTERVAL);
 
             if (bout == rouge) {
                 clearLCD();
@@ -200,7 +114,7 @@ void* TaskIHM(void* pdata)
         for (;;) {
             clearLCD();
             printToLCD(LCD_LINE_1, "Diagnostiquer");
-            bout = pressedIHM(IHM_BLOCK);
+            bout = pressedButton(BUT_BLOCK);
 
             if (bout == rouge) {
                 clearLCD();
@@ -216,7 +130,7 @@ void* TaskIHM(void* pdata)
             clearLCD();
             printToLCD(LCD_LINE_1, "Lancer parcours");
             printfToLCD(LCD_LINE_2, "(%s)", getCouleur());
-            bout = pressedIHM(IHM_BLOCK);
+            bout = pressedButton(BUT_BLOCK);
             if (bout == rouge) {
                 // No tirette
                 annuler = false;
@@ -224,7 +138,7 @@ void* TaskIHM(void* pdata)
                     clearLCD();
                     printToLCD(LCD_LINE_1, "Inserez tirette");
                     printToLCD(LCD_LINE_2, "(ROUGE: ignorer)");
-                    bout = pressedIHM(IHM_REFRESH_INTERVAL);
+                    bout = pressedButton(BUT_REFRESH_INTERVAL);
                     if (bout == rouge) {
                         break;
                     } else if (bout == jaune) {
@@ -241,7 +155,7 @@ void* TaskIHM(void* pdata)
 
                 prepareParcours(isOrange);
                 while (tirettePresente()) {
-                    bout = pressedIHM(IHM_POLLING_INTERVAL);
+                    bout = pressedButton(BUT_POLLING_INTERVAL);
                     if (bout == jaune) {
                         annuler = true;
                         break;
@@ -256,15 +170,15 @@ void* TaskIHM(void* pdata)
 
                 int toWait;
                 while ((toWait = updateParcours()) >= 0) {
-                    if (pressedIHM(toWait) != none) {
+                    if (pressedButton(toWait) != none) {
                         break;
                     }
                 }
                 stopParcours();
 
-                pressedIHM(IHM_BLOCK); // Nécessite 3 appuis pour éviter d'enlever le score par inadvertance
-                pressedIHM(IHM_BLOCK);
-                pressedIHM(IHM_BLOCK);
+                pressedButton(BUT_BLOCK); // Nécessite 3 appuis pour éviter d'enlever le score par inadvertance
+                pressedButton(BUT_BLOCK);
+                pressedButton(BUT_BLOCK);
             } else if (bout == jaune) {
                 break;
             }
@@ -274,7 +188,7 @@ void* TaskIHM(void* pdata)
         for (;;) {
             clearLCD();
             printToLCD(LCD_LINE_1, "Remettre a zero");
-            bout = pressedIHM(IHM_BLOCK);
+            bout = pressedButton(BUT_BLOCK);
 
             if (bout == rouge) {
                 clearLCD();

chef/src/ihm.h

@@ -3,20 +3,6 @@
 
 #include <stdbool.h>
 
-#include "lcd.h"
-
-#define IHM_PIN_ROUGE 0
-#define IHM_PIN_JAUNE 2
-#define IHM_PIN_TIRETTE 3
-
-// ms
-#define IHM_BLOCK -1
-#define IHM_POLLING_INTERVAL 50
-#define IHM_DEBUNK_TIME 50
-#define IHM_REFRESH_INTERVAL 1000
-
-enum boutons {none, jaune, rouge};
-
 // Public
 void configureIHM();
 void startIHM();
@@ -24,8 +10,5 @@ void deconfigureIHM();
 
 // Private
 void* TaskIHM(void *pdata);
-void* TaskStdinIHM(void *pdata);
-enum boutons pressedIHM(int timeout); // timeout: ms or -1
-bool debunkButtonIHM(int pin);
 
 #endif

chef/src/movement.c

@@ -1,85 +1,74 @@
-#include "movement.h"
 #include <stdbool.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <math.h>
 
+#include "movement.h"
+#include "CF.h"
+
 void configureMovement()
 {
-    pinMode(ENA, PWM_OUTPUT);
-    pinMode(ENB, PWM_OUTPUT);
+    stop();
 }
 
 void aller(struct position* pos);
 
 int dbg = 0;
 
-int changerMoteurs(float lVit, float rVit)
+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;
+
     // Gauche
-    bool lFor = lVit < 0;
-    float lVolt = fabs(lVit); // TODO Utiliser les vitesses
+    bool lFor = lVolt < 0;
+    lVolt = fabs(lVolt);
     if (lVolt > MOT_MAX_V) {
         lVolt = MOT_MAX_V;
     }
-    if (lVolt < MOT_MIN_V) {
-        digitalWrite(IN1, LOW);
-        digitalWrite(IN2, LOW);
-        printf("x");
-
-    } else if (lFor) {
-        digitalWrite(IN1, HIGH);
-        digitalWrite(IN2, LOW);
-        printf("↑");
-    } else {
-        digitalWrite(IN1, LOW);
-        digitalWrite(IN2, HIGH);
-        printf("↓");
-    }
-    int lAbs = lVolt * PWM_MAX / PWM_MAX_V;
-    pwmWrite(ENA, lAbs);
-    printf("%6d", lAbs);
+    msg.in |= 1 << (lFor ? IN1 : IN2);
+    msg.ena = tensionToPWM(lVolt);
 
     // Droite
-    bool rFor = rVit < 0;
-    float rVolt = fabs(rVit); // TODO Utiriser res vitesses
-    if (rVolt > MOT_MAX_V) {
-        rVolt = MOT_MAX_V;
-    }
-    if (rVolt < MOT_MIN_V) {
-        digitalWrite(IN3, LOW);
-        digitalWrite(IN4, LOW);
-        printf("x");
+    bool rFor = rVolt < 0;
+    rVolt = fabs(rVolt);
+    msg.in |= 1 << (rFor ? IN3 : IN4);
+    msg.enb = tensionToPWM(rVolt);
 
-    } else if (rFor) {
-        digitalWrite(IN3, HIGH);
-        digitalWrite(IN4, LOW);
-        printf("↑");
-    } else {
-        digitalWrite(IN3, LOW);
-        digitalWrite(IN4, HIGH);
-        printf("↓");
-    }
-    int rAbs = rVolt * PWM_MAX / PWM_MAX_V;
-    pwmWrite(ENB, rAbs);
-    printf("%6d", rAbs);
-    printf("\n");
+    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()
 {
-    digitalWrite(IN1, LOW);
-    digitalWrite(IN2, LOW);
-    digitalWrite(IN3, LOW);
-    digitalWrite(IN4, LOW);
+    sendCF(C2FD_PWM, &msgBrake, sizeof(struct C2FD_PWMs));
 }
 
 int freewheel()
 {
-    digitalWrite(IN1, HIGH);
-    digitalWrite(IN2, HIGH);
-    digitalWrite(IN3, HIGH);
-    digitalWrite(IN4, HIGH);
+    sendCF(C2FD_PWM, &msgFree, sizeof(struct C2FD_PWMs));
 }
 
 void deconfigureMovement()
@@ -90,5 +79,5 @@ void deconfigureMovement()
 int stop()
 {
     brake();
-    // TODO
+    // TODO Actionneurs
 }

chef/src/movement.h

@@ -7,8 +7,7 @@
 
 #include "position.h"
 
-#include <wiringPi.h>
-#define PWM_MAX 1023
+#define PWM_MAX 255
 #define PWM_MAX_V 3.3
 
 #define TESTINATOR
@@ -24,17 +23,10 @@
 #define MOT_MAX_V 2.5
 #endif
 
-// Pins definition
-// Left
-#define ENA 26
-#define IN1 21
-#define IN2 22
-
-// Right
-#define ENB 23
-#define IN3 24
-#define IN4 25
-
+#define IN1 0
+#define IN2 1
+#define IN3 2
+#define IN4 3
 
 void configureMovement();
 void aller(struct position* pos);

chef/src/parcours.c

@@ -1,6 +1,7 @@
 #include <pthread.h>
 #include <stdio.h>
 #include <unistd.h>
+#include <wiringPi.h>
 
 #include "lcd.h"
 #include "movement.h"
@@ -69,7 +70,7 @@ void stopParcours()
 }
 
 #define UP_TIME 1000
-#define HIGH_TIME 30000
+#define HIGH_TIME 3000
 #define DOWN_TIME 1000
 #define LOW_TIME 2000
 #define MAX_VIT MOT_MAX_V

chef/src/premier.c

@@ -24,16 +24,16 @@ int main()
 
     srand(time(NULL));
     configureDebug();
-    /* configureCF(); */
-    /* configureMovement(); */
-    /* configurePosition(); */
+    configureCF();
+    configureMovement();
+    configurePosition();
     startDebug();
 
     startIHM();
 
-    /* deconfigureMovement(); */
-    /* deconfigurePosition(); */
-    /* deconfigureCF(); */
+    deconfigureMovement();
+    deconfigurePosition();
+    deconfigureCF();
     deconfigureIHM();
     deconfigureDebug();
     return EXIT_SUCCESS;

chef/src/testForward.c

@@ -4,10 +4,13 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <unistd.h>
+#include <wiringPi.h>
 #include <wiringPiI2C.h>
 
 #include "lcd.h"
+#include "CF.h"
 #include "movement.h"
+#include "buttons.h"
 
 int main(int argc, char* argv[])
 {
@@ -15,17 +18,44 @@ int main(int argc, char* argv[])
     (void)argc;
     (void)argv;
 
+    wiringPiSetup();
+
     initI2C();
-
     initLCD();
-    clearLCD();
-    printToLCD(LCD_LINE_1, "Forward");
-
+    configureCF();
+    configureButtons();
     configureMovement();
 
     for (;;) {
-        changerMoteurs(1.5, 2);
-        sleep(1);
+        clearLCD();
+        printToLCD(LCD_LINE_1, "Forward");
+        changerMoteurs(3.3, 3.3);
+        pressedButton(BUT_BLOCK);
+
+        clearLCD();
+        printToLCD(LCD_LINE_1, "Right");
+        changerMoteurs(-3.3, 3.3);
+        pressedButton(BUT_BLOCK);
+
+        clearLCD();
+        printToLCD(LCD_LINE_1, "Left");
+        changerMoteurs(-3.3, -3.3);
+        pressedButton(BUT_BLOCK);
+
+        clearLCD();
+        printToLCD(LCD_LINE_1, "Backward");
+        changerMoteurs(3.3, -3.3);
+        pressedButton(BUT_BLOCK);
+
+        clearLCD();
+        printToLCD(LCD_LINE_1, "Brake");
+        brake();
+        pressedButton(BUT_BLOCK);
+
+        clearLCD();
+        printToLCD(LCD_LINE_1, "Free");
+        freewheel();
+        pressedButton(BUT_BLOCK);
     }
 
 }