From 739c818bf0774ffc9035cf46898dd7010bd7d466 Mon Sep 17 00:00:00 2001 From: Geoffrey Frogeye Date: Sun, 6 May 2018 08:14:51 +0200 Subject: [PATCH] Refined motors --- chef/Makefile | 2 +- chef/src/diagnostics.c | 6 +-- chef/src/dimensions.h | 37 +++++++++++-------- chef/src/motor.c | 83 ++++++++++++++++++++++++++++++++++++++++++ chef/src/motor.h | 29 +++++++++++++++ chef/src/movement.c | 70 ++--------------------------------- chef/src/movement.h | 29 --------------- chef/src/parcours.c | 11 +++--- chef/src/testCodeuse.c | 4 +- chef/src/testForward.c | 11 +++--- chef/src/testSlow.c | 10 ++--- chef/src/testUpDown.c | 11 +++--- 12 files changed, 165 insertions(+), 138 deletions(-) create mode 100644 chef/src/motor.c create mode 100644 chef/src/motor.h diff --git a/chef/Makefile b/chef/Makefile index 62fe483..1a48696 100644 --- a/chef/Makefile +++ b/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 diff --git a/chef/src/diagnostics.c b/chef/src/diagnostics.c index 64d65e8..3fa1ad7 100644 --- a/chef/src/diagnostics.c +++ b/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(); diff --git a/chef/src/dimensions.h b/chef/src/dimensions.h index d820d50..546d0e2 100644 --- a/chef/src/dimensions.h +++ b/chef/src/dimensions.h @@ -4,26 +4,33 @@ #include // Dimensions pistes -#define M_PISTE_WIDTH 3000 -#define M_PISTE_HEIGHT 2000 -#define M_PISTE_ORIG_X 0 -#define M_PISTE_ORIG_Y 0 +#define M_PISTE_WIDTH 3000.0 +#define M_PISTE_HEIGHT 2000.0 +#define M_PISTE_ORIG_X 0.0 +#define M_PISTE_ORIG_Y 0.0 // Dimensions robot -#define WIDTH 250 // mm (from meca) -#define HEIGHT 100 // mm (from random); +#define WIDTH 250.0 // mm (from meca) +#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 MOTOR_SPEED_GAIN_RPMP_V / 60 // rev/s/V -#define MOTOR_NOMINAL_TENSION 24 // V (from datasheet) -#define CODER_RESOLUTION 100 // cycles/rev -#define CODER_DATA_FACTOR 4 // increments/cycles -#define CODER_DATA_RESOLUTION CODER_RESOLUTION * CODER_DATA_FACTOR // cycles/rev -#define CRAN_REDUC_OUT 48 // nb crans (from meca) -#define CRAN_REDUC_IN 12 // nb crans (from meca) +#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_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 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 diff --git a/chef/src/motor.c b/chef/src/motor.c new file mode 100644 index 0000000..3b31a8d --- /dev/null +++ b/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 +} diff --git a/chef/src/motor.h b/chef/src/motor.h new file mode 100644 index 0000000..09969f8 --- /dev/null +++ b/chef/src/motor.h @@ -0,0 +1,29 @@ +#ifndef __MOTOR_H__ +#define __MOTOR_H__ + +#include +#include + +#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 diff --git a/chef/src/movement.c b/chef/src/movement.c index c6ae718..48d2a03 100644 --- a/chef/src/movement.c +++ b/chef/src/movement.c @@ -1,10 +1,12 @@ +#include #include #include #include -#include -#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(); } - diff --git a/chef/src/movement.h b/chef/src/movement.h index 5400af2..98ddc11 100644 --- a/chef/src/movement.h +++ b/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 diff --git a/chef/src/parcours.c b/chef/src/parcours.c index fb3c81b..1c1d358 100644 --- a/chef/src/parcours.c +++ b/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); } diff --git a/chef/src/testCodeuse.c b/chef/src/testCodeuse.c index c63c897..60a9dba 100644 --- a/chef/src/testCodeuse.c +++ b/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 (;;) { diff --git a/chef/src/testForward.c b/chef/src/testForward.c index 9487688..b900312 100644 --- a/chef/src/testForward.c +++ b/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(); diff --git a/chef/src/testSlow.c b/chef/src/testSlow.c index 66365bd..0ae20db 100644 --- a/chef/src/testSlow.c +++ b/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); diff --git a/chef/src/testUpDown.c b/chef/src/testUpDown.c index 2d884ef..12e636e 100644 --- a/chef/src/testUpDown.c +++ b/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);