1
0
Fork 0
mirror of https://github.com/RobotechLille/cdf2018-principal synced 2024-12-24 06:30:37 +01:00

Bon sens des moteurs

This commit is contained in:
Geoffrey Frogeye 2018-05-06 12:50:03 +02:00
parent 739c818bf0
commit 6817aaf779
15 changed files with 95 additions and 33 deletions

View file

@ -10,7 +10,7 @@
#define FPGA_PORTNAME "/dev/ttyUSB0"
#define CF_BAUDRATE B115200
// #define PRINTRAWDATA
#define PRINTRAWDATA
int fpga;
pthread_mutex_t sSendCF;

View file

@ -114,6 +114,8 @@ void configureDebug()
exit(EXIT_FAILURE);
}
printf("Writing log in file: %s\n", path);
fprintf(debugFd, "time");
}

View file

@ -8,6 +8,7 @@
#include "CF.h"
#include "motor.h"
#include "imu.h"
#include "position.h"
bool recu;

View file

@ -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

View file

@ -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);

View file

@ -7,6 +7,9 @@
#include "dimensions.h"
#include "CF.h"
#define INVERSE_L_MOTOR
// #define INVERSE_R_MOTOR
#define TESTINATOR
// #define TLE5206

View file

@ -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++) {

View file

@ -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);

View file

@ -8,6 +8,8 @@
#include "CF.h"
#include <pthread.h>
// #define INVERSE_L_CODER
#define INVERSE_R_CODER
// Structures
struct __attribute__ ((packed)) position {

View file

@ -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();

View file

@ -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);

17
chef/src/testStop.c Normal file
View 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();
}

View file

@ -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[])

View file

@ -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);

View file

@ -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)