mirror of
https://github.com/RobotechLille/cdf2018-principal
synced 2024-11-24 00:56:04 +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 CF_BAUDRATE B115200
|
||||
// #define PRINTRAWDATA
|
||||
#define PRINTRAWDATA
|
||||
|
||||
int fpga;
|
||||
pthread_mutex_t sSendCF;
|
||||
|
|
|
@ -114,6 +114,8 @@ void configureDebug()
|
|||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
printf("Writing log in file: %s\n", path);
|
||||
|
||||
fprintf(debugFd, "time");
|
||||
}
|
||||
|
||||
|
|
|
@ -8,6 +8,7 @@
|
|||
#include "CF.h"
|
||||
#include "motor.h"
|
||||
#include "imu.h"
|
||||
#include "position.h"
|
||||
|
||||
bool recu;
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -7,6 +7,9 @@
|
|||
#include "dimensions.h"
|
||||
#include "CF.h"
|
||||
|
||||
#define INVERSE_L_MOTOR
|
||||
// #define INVERSE_R_MOTOR
|
||||
|
||||
#define TESTINATOR
|
||||
// #define TLE5206
|
||||
|
||||
|
|
|
@ -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++) {
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -8,6 +8,8 @@
|
|||
#include "CF.h"
|
||||
#include <pthread.h>
|
||||
|
||||
// #define INVERSE_L_CODER
|
||||
#define INVERSE_R_CODER
|
||||
|
||||
// Structures
|
||||
struct __attribute__ ((packed)) position {
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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
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 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[])
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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)
|
||||
|
|
Loading…
Reference in a new issue