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 +// #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 + +#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)