geoffrey/cdf2018-principal
geoffrey/cdf2018-principal
1
0
Fork 0
mirror of https://github.com/RobotechLille/cdf2018-principal synced 2024-11-24 09:06:03 +01:00
Code Issues Releases Wiki Activity

Refined motors

Browse source
This commit is contained in:
Geoffrey Frogeye 2018-05-06 08:14:51 +02:00
parent e758218dca
commit 739c818bf0
12 changed files with 165 additions and 138 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
chef/Makefile
View file

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

6
chef/src/diagnostics.c
View file

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

37
chef/src/dimensions.h
View file

@ -4,26 +4,33 @@
#include <math.h>
// 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

83
chef/src/motor.c Normal file
View file

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

29
chef/src/motor.h Normal file
View file

@ -0,0 +1,29 @@
#ifndef __MOTOR_H__
#define __MOTOR_H__
#include <math.h>
#include <stdint.h>
#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

70
chef/src/movement.c
View file

@ -1,10 +1,12 @@
#include <math.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#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();
}

29
chef/src/movement.h
View file

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

11
chef/src/parcours.c
View file

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

4
chef/src/testCodeuse.c
View file

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

11
chef/src/testForward.c
View file

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

10
chef/src/testSlow.c
View file

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

11
chef/src/testUpDown.c
View file

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