mirror of
https://github.com/RobotechLille/cdf2018-principal
synced 2024-11-14 12:26:06 +01:00
Bon sens des moteurs
This commit is contained in:
parent
739c818bf0
commit
6817aaf779
|
@ -10,7 +10,7 @@
|
||||||
|
|
||||||
#define FPGA_PORTNAME "/dev/ttyUSB0"
|
#define FPGA_PORTNAME "/dev/ttyUSB0"
|
||||||
#define CF_BAUDRATE B115200
|
#define CF_BAUDRATE B115200
|
||||||
// #define PRINTRAWDATA
|
#define PRINTRAWDATA
|
||||||
|
|
||||||
int fpga;
|
int fpga;
|
||||||
pthread_mutex_t sSendCF;
|
pthread_mutex_t sSendCF;
|
||||||
|
|
|
@ -114,6 +114,8 @@ void configureDebug()
|
||||||
exit(EXIT_FAILURE);
|
exit(EXIT_FAILURE);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
printf("Writing log in file: %s\n", path);
|
||||||
|
|
||||||
fprintf(debugFd, "time");
|
fprintf(debugFd, "time");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -8,6 +8,7 @@
|
||||||
#include "CF.h"
|
#include "CF.h"
|
||||||
#include "motor.h"
|
#include "motor.h"
|
||||||
#include "imu.h"
|
#include "imu.h"
|
||||||
|
#include "position.h"
|
||||||
|
|
||||||
bool recu;
|
bool recu;
|
||||||
|
|
||||||
|
|
|
@ -11,26 +11,26 @@
|
||||||
|
|
||||||
// Dimensions robot
|
// Dimensions robot
|
||||||
#define WIDTH 250.0 // mm (from meca)
|
#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 DISTANCE_BETWEEN_WHEELS WIDTH // mm (from meca)
|
||||||
#define WHEEL_DIAMETER 80.0 // 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_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_NOMINAL_TENSION 24.0 // V (from datasheet)
|
||||||
#define MOTOR_CONTROLLER_ALIMENTATION 24.0 // V (from elec)
|
#define MOTOR_CONTROLLER_ALIMENTATION 24.0 // V (from elec)
|
||||||
#define MOTOR_CONTROLLER_REFERENCE 3.3 // V (from wiring)
|
#define MOTOR_CONTROLLER_REFERENCE 5 // V (from wiring)
|
||||||
#define MOTOR_SATURATION_MIN 1.0 //V (from random)
|
#define MOTOR_SATURATION_MIN 1.0 // V (from random)
|
||||||
#define MOTOR_SATURATION_MAX 12.0 //V (from testing)
|
#define MOTOR_SATURATION_MAX 12.0 // V (from testing)
|
||||||
#define PWM_MAX 3.3 // V (from FPGA datasheet)
|
#define PWM_MAX 3.5 // V (from FPGA datasheet)
|
||||||
#define CODER_RESOLUTION 370.0 // cycles/motor rev
|
#define CODER_RESOLUTION 370.0 // cycles/motor rev
|
||||||
#define CODER_DATA_FACTOR 4.0 // increments/motor cycles
|
#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_OUT 48.0 // nb crans (from meca)
|
||||||
#define CRAN_REDUC_IN 12.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 REDUC_RATIO (CRAN_REDUC_IN / CRAN_REDUC_OUT) // reduction ratio
|
||||||
#define CODER_FULL_RESOLUTION CODER_DATA_RESOLUTION / REDUC_RATIO // cycles / wheel rev
|
#define CODER_FULL_RESOLUTION (CODER_DATA_RESOLUTION / REDUC_RATIO) // cycles / wheel rev
|
||||||
#define AV_PER_CYCLE WHEEL_PERIMETER / CODER_FULL_RESOLUTION // mm
|
#define AV_PER_CYCLE (WHEEL_PERIMETER / CODER_FULL_RESOLUTION) // mm
|
||||||
|
|
||||||
// Constantes asservissement
|
// Constantes asservissement
|
||||||
#define D_DIR_ECART_MIN 1 // mm
|
#define D_DIR_ECART_MIN 1 // mm
|
||||||
|
|
|
@ -1,18 +1,35 @@
|
||||||
#include "motor.h"
|
#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;
|
return UINT8_MAX;
|
||||||
} else if (V <= 0) {
|
} else if (V <= 0) {
|
||||||
return 0;
|
return 0;
|
||||||
} else {
|
} 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)
|
void setMoteurTensionRaw(float lVolt, float rVolt, bool lFor, bool rFor)
|
||||||
{
|
{
|
||||||
|
|
||||||
|
#ifdef INVERSE_L_MOTOR
|
||||||
|
lFor = !lFor;
|
||||||
|
#endif
|
||||||
static struct C2FD_PWMs msg;
|
static struct C2FD_PWMs msg;
|
||||||
msg.in = 0x00;
|
msg.in = 0x00;
|
||||||
if (lVolt > 0) {
|
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
|
// Nothing needs to be changed for this motor controller
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#ifdef INVERSE_R_MOTOR
|
||||||
|
rFor = !rFor;
|
||||||
|
#endif
|
||||||
if (rVolt > 0) {
|
if (rVolt > 0) {
|
||||||
msg.in |= 1 << (rFor ? IN3 : IN4);
|
msg.in |= 1 << (rFor ? IN3 : IN4);
|
||||||
msg.enb = moteurTensionToPWM(lVolt);
|
msg.enb = moteurTensionToPWM(lVolt);
|
||||||
|
|
|
@ -7,6 +7,9 @@
|
||||||
#include "dimensions.h"
|
#include "dimensions.h"
|
||||||
#include "CF.h"
|
#include "CF.h"
|
||||||
|
|
||||||
|
#define INVERSE_L_MOTOR
|
||||||
|
// #define INVERSE_R_MOTOR
|
||||||
|
|
||||||
#define TESTINATOR
|
#define TESTINATOR
|
||||||
// #define TLE5206
|
// #define TLE5206
|
||||||
|
|
||||||
|
|
|
@ -82,12 +82,16 @@ void stopParcours()
|
||||||
#define HIGH_TIME 3000
|
#define HIGH_TIME 3000
|
||||||
#define DOWN_TIME 1000
|
#define DOWN_TIME 1000
|
||||||
#define LOW_TIME 2000
|
#define LOW_TIME 2000
|
||||||
#define MAX_VIT 2
|
#define MAX_VIT 1
|
||||||
|
|
||||||
void* TaskParcours(void* pdata)
|
void* TaskParcours(void* pdata)
|
||||||
{
|
{
|
||||||
(void)pdata;
|
(void)pdata;
|
||||||
|
|
||||||
|
for (;;) {
|
||||||
|
setPWMTension(MAX_VIT, MAX_VIT);
|
||||||
|
}
|
||||||
|
|
||||||
for (;;) {
|
for (;;) {
|
||||||
addPoints(1);
|
addPoints(1);
|
||||||
for (int i = 0; i < UP_TIME; i++) {
|
for (int i = 0; i < UP_TIME; i++) {
|
||||||
|
|
|
@ -19,7 +19,6 @@ pthread_t tPosition;
|
||||||
unsigned int nbCalcPos;
|
unsigned int nbCalcPos;
|
||||||
long lCodTot, rCodTot;
|
long lCodTot, rCodTot;
|
||||||
|
|
||||||
|
|
||||||
void* TaskPosition(void* pData)
|
void* TaskPosition(void* pData)
|
||||||
{
|
{
|
||||||
(void)pData;
|
(void)pData;
|
||||||
|
@ -42,11 +41,20 @@ void* TaskPosition(void* pData)
|
||||||
|
|
||||||
// Calculation
|
// Calculation
|
||||||
nbCalcPos++;
|
nbCalcPos++;
|
||||||
|
#ifdef INVERSE_L_CODER
|
||||||
|
deltaCoders.dL = -deltaCoders.dL;
|
||||||
|
#endif
|
||||||
|
#ifdef INVERSE_R_CODER
|
||||||
|
deltaCoders.dR = -deltaCoders.dR;
|
||||||
|
#endif
|
||||||
lCodTot += deltaCoders.dL;
|
lCodTot += deltaCoders.dL;
|
||||||
rCodTot += deltaCoders.dR;
|
rCodTot += deltaCoders.dR;
|
||||||
|
|
||||||
float deltaO = atan2(deltaCoders.dR - deltaCoders.dL, DISTANCE_BETWEEN_WHEELS);
|
float dR = deltaCoders.dR * AV_PER_CYCLE;
|
||||||
float deltaD = (deltaCoders.dL + deltaCoders.dR) / 2;
|
float dL = deltaCoders.dL * AV_PER_CYCLE;
|
||||||
|
|
||||||
|
float deltaO = atan2(dR - dL, DISTANCE_BETWEEN_WHEELS);
|
||||||
|
float deltaD = (dL + dR) / 2;
|
||||||
|
|
||||||
connu.o += deltaO;
|
connu.o += deltaO;
|
||||||
float deltaX = deltaD * cos(connu.o);
|
float deltaX = deltaD * cos(connu.o);
|
||||||
|
|
|
@ -8,6 +8,8 @@
|
||||||
#include "CF.h"
|
#include "CF.h"
|
||||||
#include <pthread.h>
|
#include <pthread.h>
|
||||||
|
|
||||||
|
// #define INVERSE_L_CODER
|
||||||
|
#define INVERSE_R_CODER
|
||||||
|
|
||||||
// Structures
|
// Structures
|
||||||
struct __attribute__ ((packed)) position {
|
struct __attribute__ ((packed)) position {
|
||||||
|
|
|
@ -12,7 +12,7 @@
|
||||||
#include "motor.h"
|
#include "motor.h"
|
||||||
#include "buttons.h"
|
#include "buttons.h"
|
||||||
|
|
||||||
#define PATATE 3.3/2
|
#define PATATE 2
|
||||||
|
|
||||||
int main(int argc, char* argv[])
|
int main(int argc, char* argv[])
|
||||||
{
|
{
|
||||||
|
@ -30,22 +30,22 @@ int main(int argc, char* argv[])
|
||||||
for (;;) {
|
for (;;) {
|
||||||
clearLCD();
|
clearLCD();
|
||||||
printToLCD(LCD_LINE_1, "Forward");
|
printToLCD(LCD_LINE_1, "Forward");
|
||||||
setPWMTension(PATATE, PATATE);
|
setMoteurTension(PATATE, PATATE);
|
||||||
pressedButton(BUT_BLOCK);
|
pressedButton(BUT_BLOCK);
|
||||||
|
|
||||||
clearLCD();
|
clearLCD();
|
||||||
printToLCD(LCD_LINE_1, "Right");
|
printToLCD(LCD_LINE_1, "Right");
|
||||||
setPWMTension(-PATATE, PATATE);
|
setMoteurTension(-PATATE, PATATE);
|
||||||
pressedButton(BUT_BLOCK);
|
pressedButton(BUT_BLOCK);
|
||||||
|
|
||||||
clearLCD();
|
clearLCD();
|
||||||
printToLCD(LCD_LINE_1, "Left");
|
printToLCD(LCD_LINE_1, "Left");
|
||||||
setPWMTension(-PATATE, -PATATE);
|
setMoteurTension(PATATE, -PATATE);
|
||||||
pressedButton(BUT_BLOCK);
|
pressedButton(BUT_BLOCK);
|
||||||
|
|
||||||
clearLCD();
|
clearLCD();
|
||||||
printToLCD(LCD_LINE_1, "Backward");
|
printToLCD(LCD_LINE_1, "Backward");
|
||||||
setPWMTension(PATATE, -PATATE);
|
setMoteurTension(-PATATE, -PATATE);
|
||||||
pressedButton(BUT_BLOCK);
|
pressedButton(BUT_BLOCK);
|
||||||
|
|
||||||
clearLCD();
|
clearLCD();
|
||||||
|
|
|
@ -12,7 +12,9 @@
|
||||||
#include "motor.h"
|
#include "motor.h"
|
||||||
#include "buttons.h"
|
#include "buttons.h"
|
||||||
|
|
||||||
#define VIT 0.40
|
#define VIT 1
|
||||||
|
#define VITL VIT
|
||||||
|
#define VITR 0
|
||||||
|
|
||||||
|
|
||||||
void setPWMTensionWrapper(float l, float r) {
|
void setPWMTensionWrapper(float l, float r) {
|
||||||
|
@ -34,12 +36,9 @@ int main(int argc, char* argv[])
|
||||||
initLCD();
|
initLCD();
|
||||||
configureCF();
|
configureCF();
|
||||||
configureButtons();
|
configureButtons();
|
||||||
configureMovement();
|
|
||||||
|
|
||||||
setPWMTensionWrapper(VIT, VIT);
|
|
||||||
|
|
||||||
for (;;) {
|
for (;;) {
|
||||||
setPWMTensionWrapper(VIT, VIT);
|
setPWMTensionWrapper(VITL, VITR);
|
||||||
pressedButton(BUT_BLOCK);
|
pressedButton(BUT_BLOCK);
|
||||||
brake();
|
brake();
|
||||||
pressedButton(BUT_BLOCK);
|
pressedButton(BUT_BLOCK);
|
||||||
|
|
17
chef/src/testStop.c
Normal file
17
chef/src/testStop.c
Normal file
|
@ -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();
|
||||||
|
|
||||||
|
}
|
|
@ -16,14 +16,14 @@
|
||||||
#define HIGH_TIME 3000
|
#define HIGH_TIME 3000
|
||||||
#define DOWN_TIME 1000
|
#define DOWN_TIME 1000
|
||||||
#define LOW_TIME 2000
|
#define LOW_TIME 2000
|
||||||
#define MAXI 3.3
|
#define MAXI 12
|
||||||
#define INTERVAL 10
|
#define INTERVAL 10
|
||||||
|
|
||||||
void changerMoteursWrapper(float l, float r) {
|
void changerMoteursWrapper(float l, float r) {
|
||||||
/* clearLCD(); */
|
/* clearLCD(); */
|
||||||
printfToLCD(LCD_LINE_1, "L: %f", l);
|
printfToLCD(LCD_LINE_1, "L: %f", l);
|
||||||
printfToLCD(LCD_LINE_2, "R: %f", r);
|
printfToLCD(LCD_LINE_2, "R: %f", r);
|
||||||
setPWMTension(l, r);
|
setMoteurTension(l, r);
|
||||||
}
|
}
|
||||||
|
|
||||||
int main(int argc, char* argv[])
|
int main(int argc, char* argv[])
|
||||||
|
|
|
@ -85,7 +85,7 @@ architecture Behavioral of Principal is
|
||||||
-- PWM clock
|
-- PWM clock
|
||||||
signal pwmClk : std_logic := '0';
|
signal pwmClk : std_logic := '0';
|
||||||
signal pwmCounter : integer := 0;
|
signal pwmCounter : integer := 0;
|
||||||
constant PWM_DIVIDER : integer := 1024;
|
constant PWM_DIVIDER : integer := 4096;
|
||||||
|
|
||||||
-- Motor controller
|
-- Motor controller
|
||||||
signal enAd : std_logic_vector(7 downto 0);
|
signal enAd : std_logic_vector(7 downto 0);
|
||||||
|
|
|
@ -13,8 +13,14 @@ export PS3="+ "
|
||||||
export PS4="- "
|
export PS4="- "
|
||||||
|
|
||||||
alias r="/etc/init.d/S50chef restart"
|
alias r="/etc/init.d/S50chef restart"
|
||||||
alias s="/etc/init.d/S50chef stop"
|
|
||||||
alias c="cd /opt/chef/"
|
alias c="cd /opt/chef/"
|
||||||
|
|
||||||
|
s()
|
||||||
|
{
|
||||||
|
/etc/init.d/S50chef stop
|
||||||
|
/opt/chef/bin/testStop
|
||||||
|
}
|
||||||
|
|
||||||
l()
|
l()
|
||||||
{
|
{
|
||||||
tail -f $(find /opt/chef/log | sort | tail -1)
|
tail -f $(find /opt/chef/log | sort | tail -1)
|
||||||
|
|
Loading…
Reference in a new issue