mirror of
https://github.com/RobotechLille/cdf2018-principal
synced 2024-11-13 03:46:12 +01:00
Rate limiter
This commit is contained in:
parent
9cdebaa915
commit
4195842e85
|
@ -11,6 +11,7 @@
|
|||
|
||||
// Variables globales
|
||||
pthread_t tDebug;
|
||||
bool debugConfigured = false;
|
||||
|
||||
struct debugArg* listeDebugArgs = NULL;
|
||||
|
||||
|
@ -117,24 +118,28 @@ void configureDebug()
|
|||
printf("Writing log in file: %s\n", path);
|
||||
|
||||
fprintf(debugFd, "time");
|
||||
|
||||
debugConfigured = true;
|
||||
}
|
||||
|
||||
void registerDebugVar(char* name, enum debugArgTypes type, void* var)
|
||||
{
|
||||
fprintf(debugFd, ",%s", name);
|
||||
if (debugConfigured) {
|
||||
fprintf(debugFd, ",%s", name);
|
||||
|
||||
struct debugArg* arg = NULL;
|
||||
struct debugArg** addrArg = &listeDebugArgs;
|
||||
while (*addrArg != NULL) {
|
||||
addrArg = &((*addrArg)->next);
|
||||
struct debugArg* arg = NULL;
|
||||
struct debugArg** addrArg = &listeDebugArgs;
|
||||
while (*addrArg != NULL) {
|
||||
addrArg = &((*addrArg)->next);
|
||||
}
|
||||
|
||||
arg = malloc(sizeof(struct debugArg));
|
||||
arg->type = type;
|
||||
arg->var = var;
|
||||
arg->next = NULL;
|
||||
|
||||
*addrArg = arg;
|
||||
}
|
||||
|
||||
arg = malloc(sizeof(struct debugArg));
|
||||
arg->type = type;
|
||||
arg->var = var;
|
||||
arg->next = NULL;
|
||||
|
||||
*addrArg = arg;
|
||||
}
|
||||
|
||||
void startDebug()
|
||||
|
@ -144,7 +149,10 @@ void startDebug()
|
|||
|
||||
void deconfigureDebug()
|
||||
{
|
||||
pthread_cancel(tDebug);
|
||||
fclose(debugFd);
|
||||
// TODO Vider la liste des arguments
|
||||
if (debugConfigured) {
|
||||
pthread_cancel(tDebug);
|
||||
fclose(debugFd);
|
||||
debugConfigured = false;
|
||||
// TODO Vider la liste des arguments
|
||||
}
|
||||
}
|
||||
|
|
|
@ -97,6 +97,7 @@ bool diagJustRun(void* arg)
|
|||
{
|
||||
void (*fonction)(void) = arg;
|
||||
fonction();
|
||||
usleep(1000*1000);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
@ -40,7 +40,7 @@
|
|||
#define D_CONS_THRESOLD 1.0 // mm
|
||||
#define O_CONS_THRESOLD (6.0 / 360.0 * 2.0 * M_PI) // rad
|
||||
#define O_GAIN 3.0
|
||||
#define P 3.0
|
||||
#define P 5.0
|
||||
#define I 0.0
|
||||
#define D 0.0
|
||||
|
||||
|
|
|
@ -93,7 +93,7 @@ void* TaskIMU(void* pData)
|
|||
}
|
||||
imuStart.tv_sec = imuNow.tv_sec;
|
||||
imuStart.tv_nsec = imuNow.tv_nsec;
|
||||
float dt = imuEcoule.tv_sec + imuStart.tv_nsec * 1E-9;
|
||||
float dt = imuEcoule.tv_sec + imuEcoule.tv_nsec * 1E-9;
|
||||
|
||||
gyroNew = readGyro();
|
||||
gyro.x += gyroNew.x * dt;
|
||||
|
|
162
chef/src/motor.c
162
chef/src/motor.c
|
@ -1,5 +1,35 @@
|
|||
#include "motor.h"
|
||||
#include <unistd.h>
|
||||
|
||||
#include "actionneurs.h"
|
||||
#include "debug.h"
|
||||
#include "motor.h"
|
||||
|
||||
float lVolt;
|
||||
float rVolt;
|
||||
float lVoltCons;
|
||||
float rVoltCons;
|
||||
struct timespec motStart;
|
||||
struct timespec motNow;
|
||||
struct timespec motEcoule;
|
||||
pthread_mutex_t motCons;
|
||||
pthread_t tMotor;
|
||||
enum motorState motState = braking;
|
||||
|
||||
static struct C2FD_PWMs msgBrake = { 0, 0, UINT8_MAX };
|
||||
static struct C2FD_PWMs msgFree = { 0, 0, (1 << IN1) | (1 << IN2) | (1 << IN3) | (1 << IN4) };
|
||||
|
||||
void configureMotor()
|
||||
{
|
||||
registerDebugVar("lVolt", f, &lVolt);
|
||||
registerDebugVar("rVolt", f, &rVolt);
|
||||
pthread_mutex_init(&motCons, NULL);
|
||||
pthread_create(&tMotor, NULL, TaskMotor, NULL);
|
||||
}
|
||||
|
||||
void deconfigureMotor()
|
||||
{
|
||||
pthread_cancel(tMotor);
|
||||
}
|
||||
|
||||
uint8_t tensionToPWM(float V)
|
||||
{
|
||||
|
@ -22,17 +52,19 @@ uint8_t moteurTensionToPWM(float V)
|
|||
return tensionToPWM(V * MOTOR_CONTROLLER_REFERENCE / MOTOR_CONTROLLER_ALIMENTATION);
|
||||
}
|
||||
|
||||
void setMoteurTensionRaw(float lVolt, float rVolt, bool lFor, bool rFor)
|
||||
void setMoteurTensionRaw(float l, float r, bool lFor, bool rFor)
|
||||
{
|
||||
lVolt = l;
|
||||
rVolt = r;
|
||||
|
||||
#ifdef INVERSE_L_MOTOR
|
||||
lFor = !lFor;
|
||||
#endif
|
||||
static struct C2FD_PWMs msg;
|
||||
msg.in = 0x00;
|
||||
if (lVolt > 0) {
|
||||
if (l > 0) {
|
||||
msg.in |= 1 << (lFor ? IN1 : IN2);
|
||||
msg.ena = moteurTensionToPWM(lVolt);
|
||||
msg.ena = moteurTensionToPWM(l);
|
||||
} else {
|
||||
// Nothing needs to be changed for this motor controller
|
||||
}
|
||||
|
@ -40,9 +72,9 @@ void setMoteurTensionRaw(float lVolt, float rVolt, bool lFor, bool rFor)
|
|||
#ifdef INVERSE_R_MOTOR
|
||||
rFor = !rFor;
|
||||
#endif
|
||||
if (rVolt > 0) {
|
||||
if (r > 0) {
|
||||
msg.in |= 1 << (rFor ? IN3 : IN4);
|
||||
msg.enb = moteurTensionToPWM(rVolt);
|
||||
msg.enb = moteurTensionToPWM(r);
|
||||
} else {
|
||||
// Nothing needs to be changed for this motor controller
|
||||
}
|
||||
|
@ -50,28 +82,99 @@ void setMoteurTensionRaw(float lVolt, float rVolt, bool lFor, bool rFor)
|
|||
sendCF(C2FD_PWM, &msg, sizeof(struct C2FD_PWMs));
|
||||
}
|
||||
|
||||
void setMoteurTension(float lVolt, float rVolt)
|
||||
void* TaskMotor(void* pData)
|
||||
{
|
||||
(void)pData;
|
||||
|
||||
// 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;
|
||||
float debug = 0;
|
||||
|
||||
clock_gettime(CLOCK_REALTIME, &motStart);
|
||||
for (;;) {
|
||||
// Calcul du temps écoulé depuis la dernière mise à jour des moteurs
|
||||
clock_gettime(CLOCK_REALTIME, &motNow);
|
||||
if ((motNow.tv_nsec - motStart.tv_nsec) < 0) {
|
||||
motEcoule.tv_sec = motNow.tv_sec - motStart.tv_sec - 1;
|
||||
motEcoule.tv_nsec = motNow.tv_nsec - motStart.tv_nsec + 1000000000L;
|
||||
} else {
|
||||
motEcoule.tv_sec = motNow.tv_sec - motStart.tv_sec;
|
||||
motEcoule.tv_nsec = motNow.tv_nsec - motStart.tv_nsec;
|
||||
}
|
||||
motStart.tv_sec = motNow.tv_sec;
|
||||
motStart.tv_nsec = motNow.tv_nsec;
|
||||
float dt = motEcoule.tv_sec + (float) motEcoule.tv_nsec * 1E-9;
|
||||
debug += dt;
|
||||
|
||||
pthread_mutex_lock(&motCons);
|
||||
bool lFor = lVoltCons < 0;
|
||||
float l = fabs(lVoltCons);
|
||||
bool rFor = rVoltCons < 0;
|
||||
float r = fabs(rVoltCons);
|
||||
enum motorState state = motState;
|
||||
pthread_mutex_unlock(&motCons);
|
||||
|
||||
switch (state) {
|
||||
case running:
|
||||
|
||||
if (l < MOTOR_SATURATION_MIN) {
|
||||
l = 0;
|
||||
} else if (l > MOTOR_SATURATION_MAX) {
|
||||
l = MOTOR_SATURATION_MAX;
|
||||
}
|
||||
|
||||
if (r < MOTOR_SATURATION_MIN) {
|
||||
r = 0;
|
||||
} else if (r > MOTOR_SATURATION_MAX) {
|
||||
r = MOTOR_SATURATION_MAX;
|
||||
}
|
||||
|
||||
// Rate limiter pour éviter que l'accélération soit trop brusque
|
||||
float maxUp = RATE_LIMITER_UP * dt;
|
||||
float maxDown = RATE_LIMITER_UP * dt;
|
||||
|
||||
if (l - lVolt > maxUp) {
|
||||
l = lVolt + maxUp;
|
||||
} else if (lVolt - l > maxDown) {
|
||||
l = lVolt - maxDown;
|
||||
}
|
||||
if (r - rVolt > maxUp) {
|
||||
r = rVolt + maxUp;
|
||||
} else if (rVolt - r > maxDown) {
|
||||
r = rVolt - maxDown;
|
||||
}
|
||||
|
||||
setMoteurTensionRaw(l, r, lFor, rFor);
|
||||
break;
|
||||
case braking:
|
||||
rawBrake();
|
||||
break;
|
||||
case freewheeling:
|
||||
rawFreewheel();
|
||||
sendCF(C2FD_PWM, &msgFree, sizeof(struct C2FD_PWMs));
|
||||
break;
|
||||
}
|
||||
|
||||
usleep(1000);
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
setMoteurTensionRaw(lVolt, rVolt, lFor, rFor);
|
||||
void rawBrake()
|
||||
{
|
||||
sendCF(C2FD_PWM, &msgBrake, sizeof(struct C2FD_PWMs));
|
||||
}
|
||||
void rawFreewheel()
|
||||
{
|
||||
sendCF(C2FD_PWM, &msgFree, sizeof(struct C2FD_PWMs));
|
||||
}
|
||||
|
||||
void setMoteurTension(float l, float r)
|
||||
{
|
||||
pthread_mutex_lock(&motCons);
|
||||
motState = running;
|
||||
lVoltCons = l;
|
||||
rVoltCons = r;
|
||||
pthread_mutex_unlock(&motCons);
|
||||
}
|
||||
|
||||
void setPWMTension(float lVolt, float rVolt)
|
||||
|
@ -81,17 +184,20 @@ void setPWMTension(float lVolt, float rVolt)
|
|||
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));
|
||||
pthread_mutex_lock(&motCons);
|
||||
motState = braking;
|
||||
pthread_mutex_unlock(&motCons);
|
||||
rawBrake();
|
||||
}
|
||||
|
||||
int freewheel()
|
||||
{
|
||||
sendCF(C2FD_PWM, &msgFree, sizeof(struct C2FD_PWMs));
|
||||
pthread_mutex_lock(&motCons);
|
||||
motState = freewheeling;
|
||||
pthread_mutex_unlock(&motCons);
|
||||
rawFreewheel();
|
||||
}
|
||||
|
||||
int stop()
|
||||
|
|
|
@ -4,12 +4,16 @@
|
|||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "dimensions.h"
|
||||
#include "CF.h"
|
||||
#include "dimensions.h"
|
||||
|
||||
#define INVERSE_L_MOTOR
|
||||
// #define INVERSE_R_MOTOR
|
||||
|
||||
// V/s
|
||||
#define RATE_LIMITER_UP 12
|
||||
#define RATE_LIMITER_DOWN 1
|
||||
|
||||
#define TESTINATOR
|
||||
// #define TLE5206
|
||||
|
||||
|
@ -18,7 +22,15 @@
|
|||
#define IN3 2
|
||||
#define IN4 3
|
||||
|
||||
enum motorState {
|
||||
running,
|
||||
braking,
|
||||
freewheeling
|
||||
};
|
||||
|
||||
// Public
|
||||
void configureMotor();
|
||||
void deconfigureMotor();
|
||||
void setMoteurTension(float lVolt, float rVolt);
|
||||
void setPWMTension(float lVolt, float rVolt);
|
||||
int brake();
|
||||
|
@ -26,7 +38,10 @@ int freewheel();
|
|||
int stop();
|
||||
|
||||
// Private
|
||||
void* TaskMotor(void* pData);
|
||||
uint8_t moteurTensionToPWM(float V);
|
||||
void setMoteurTensionRaw(float lVolt, float rVolt, bool lFor, bool rFor);
|
||||
void rawFreewheel();
|
||||
void rawBrake();
|
||||
|
||||
#endif
|
||||
|
|
|
@ -33,8 +33,6 @@ float lErr;
|
|||
float rErr;
|
||||
float lErrPrev;
|
||||
float rErrPrev;
|
||||
float lVolt;
|
||||
float rVolt;
|
||||
unsigned int nbCalcCons;
|
||||
|
||||
struct timespec pidStart;
|
||||
|
@ -45,6 +43,7 @@ void configureMovement()
|
|||
{
|
||||
stop();
|
||||
|
||||
configureMotor();
|
||||
configureSecurite();
|
||||
|
||||
nbCalcCons = 0;
|
||||
|
@ -73,8 +72,6 @@ void configureMovement()
|
|||
registerDebugVar("oRetenu", d, &oRetenu);
|
||||
registerDebugVar("lErr", f, &lErr);
|
||||
registerDebugVar("rErr", f, &rErr);
|
||||
registerDebugVar("lVolt", f, &lVolt);
|
||||
registerDebugVar("rVolt", f, &rVolt);
|
||||
registerDebugVar("nbCalcCons", d, &nbCalcCons);
|
||||
|
||||
disableConsigne();
|
||||
|
@ -173,9 +170,11 @@ void* TaskMovement(void* pData)
|
|||
dErr = dRetenu ? 0 : dDirEcart;
|
||||
|
||||
// Limitation par les valeurs des capteurs
|
||||
#ifdef ENABLE_SECURITE
|
||||
float avant, arriere;
|
||||
getDistance(&avant, &arriere);
|
||||
dErr = fmaxf(-arriere, fminf(avant, dErr));
|
||||
#endif
|
||||
|
||||
// Calcul si on est arrivé
|
||||
pthread_mutex_lock(&movDoneMutex);
|
||||
|
@ -213,7 +212,7 @@ void* TaskMovement(void* pData)
|
|||
// Enregistrement de cette mesure comme la dernière mesure
|
||||
pidStart.tv_sec = pidNow.tv_sec;
|
||||
pidStart.tv_nsec = pidNow.tv_nsec;
|
||||
float timeStep = pidEcoule.tv_sec + pidStart.tv_nsec * 1E-9;
|
||||
float timeStep = pidEcoule.tv_sec + pidEcoule.tv_nsec * 1E-9;
|
||||
|
||||
// Calcul des facteurs dérivé et intégrale
|
||||
lErrInteg += (lErr + lErrPrev) / 2 * timeStep;
|
||||
|
@ -225,8 +224,8 @@ void* TaskMovement(void* pData)
|
|||
rErrPrev = rErr;
|
||||
|
||||
// Calcul de la commande
|
||||
lVolt = P * lErr + I * lErrInteg + D * lErrDeriv;
|
||||
rVolt = P * rErr + I * rErrInteg + D * rErrDeriv;
|
||||
float lVolt = P * lErr + I * lErrInteg + D * lErrDeriv;
|
||||
float rVolt = P * rErr + I * rErrInteg + D * rErrDeriv;
|
||||
|
||||
// Envoi de la commande
|
||||
setMoteurTension(lVolt, rVolt);
|
||||
|
@ -242,6 +241,7 @@ void deconfigureMovement()
|
|||
stop();
|
||||
pthread_cancel(tMovement);
|
||||
deconfigureSecurite();
|
||||
deconfigureMotor();
|
||||
}
|
||||
|
||||
void enableConsigne()
|
||||
|
|
|
@ -7,6 +7,8 @@
|
|||
|
||||
#define ANGLE_INSIGNIFIANT NAN
|
||||
|
||||
// #define ENABLE_SECURITE
|
||||
|
||||
#include "position.h"
|
||||
|
||||
void configureMovement();
|
||||
|
|
|
@ -168,3 +168,4 @@ void* TaskParcours(void* pdata)
|
|||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
|
|
@ -45,5 +45,6 @@ void startParcours();
|
|||
int updateParcours();
|
||||
void stopParcours();
|
||||
void* TaskParcours(void* pdata);
|
||||
void gotoPoint(float x, float y, float o);
|
||||
|
||||
#endif
|
||||
|
|
|
@ -53,7 +53,7 @@ void configureSecurite()
|
|||
{
|
||||
pthread_mutex_init(&secPolling, NULL);
|
||||
pthread_mutex_init(&secData, NULL);
|
||||
registerRxHandlerCF(F2CI_CODER, onF2CI_CAPT);
|
||||
registerRxHandlerCF(F2CI_CAPT, onF2CI_CAPT);
|
||||
registerDebugVar("secFront", ld, &secFront);
|
||||
registerDebugVar("secBack", ld, &secBack);
|
||||
pthread_create(&tSecurite, NULL, TaskSecurite, NULL);
|
||||
|
|
68
chef/src/test1m.c
Normal file
68
chef/src/test1m.c
Normal file
|
@ -0,0 +1,68 @@
|
|||
#include <pthread.h>
|
||||
#include <signal.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <time.h> // random seed
|
||||
#include <unistd.h> // sleep
|
||||
#include <wiringPi.h>
|
||||
|
||||
#include "CF.h"
|
||||
#include "actionneurs.h"
|
||||
#include "debug.h"
|
||||
#include "i2c.h"
|
||||
#include "ihm.h"
|
||||
#include "imu.h"
|
||||
#include "movement.h"
|
||||
#include "motor.h"
|
||||
#include "position.h"
|
||||
|
||||
pthread_mutex_t sRunning;
|
||||
|
||||
void endRunning(int signal)
|
||||
{
|
||||
(void)signal;
|
||||
pthread_mutex_unlock(&sRunning);
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
|
||||
if (wiringPiSetup() < 0) {
|
||||
fprintf(stderr, "Impossible d'initialiser WiringPi\n");
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
initI2C();
|
||||
srand(time(NULL));
|
||||
|
||||
configureDebug();
|
||||
configureCF();
|
||||
configureIMU();
|
||||
configureActionneurs();
|
||||
configurePosition();
|
||||
configureMovement();
|
||||
startDebug();
|
||||
|
||||
debugSetActive(true);
|
||||
enableConsigne();
|
||||
struct position pos = { 100, 0, 0 };
|
||||
setDestination(&pos);
|
||||
waitDestination();
|
||||
brake();
|
||||
disableConsigne();
|
||||
|
||||
// Bloque jusqu'à l'arrivée d'un signal
|
||||
pthread_mutex_init(&sRunning, NULL);
|
||||
signal(SIGINT, endRunning);
|
||||
signal(SIGTERM, endRunning);
|
||||
signal(SIGQUIT, endRunning);
|
||||
pthread_mutex_lock(&sRunning);
|
||||
pthread_mutex_lock(&sRunning);
|
||||
|
||||
deconfigureMovement();
|
||||
deconfigurePosition();
|
||||
deconfigureActionneurs();
|
||||
deconfigureIMU();
|
||||
deconfigureCF();
|
||||
deconfigureDebug();
|
||||
return EXIT_SUCCESS;
|
||||
}
|
|
@ -16,7 +16,7 @@
|
|||
|
||||
pthread_mutex_t sRunning;
|
||||
|
||||
#define VIT 1
|
||||
#define VIT 0
|
||||
|
||||
void endRunning(int signal)
|
||||
{
|
||||
|
@ -30,6 +30,7 @@ int main()
|
|||
configureDebug();
|
||||
configureCF();
|
||||
configurePosition();
|
||||
configureMotor();
|
||||
|
||||
setPWMTension(VIT, VIT);
|
||||
|
||||
|
|
|
@ -26,6 +26,7 @@ int main(int argc, char* argv[])
|
|||
initLCD();
|
||||
configureCF();
|
||||
configureButtons();
|
||||
configureMotor();
|
||||
|
||||
for (;;) {
|
||||
clearLCD();
|
||||
|
|
|
@ -12,5 +12,6 @@ int main(int argc, char* argv[])
|
|||
(void)argv;
|
||||
|
||||
configureCF();
|
||||
configureMotor();
|
||||
freewheel();
|
||||
}
|
||||
|
|
26
chef/src/testPuissance.c
Normal file
26
chef/src/testPuissance.c
Normal file
|
@ -0,0 +1,26 @@
|
|||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "CF.h"
|
||||
#include "motor.h"
|
||||
#include "debug.h"
|
||||
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
|
||||
(void)argc;
|
||||
(void)argv;
|
||||
|
||||
configureDebug();
|
||||
configureCF();
|
||||
configureMotor();
|
||||
|
||||
setMoteurTension(24, 24);
|
||||
sleep(10);
|
||||
setMoteurTension(0, 0);
|
||||
sleep(3);
|
||||
|
||||
deconfigureMotor();
|
||||
deconfigureCF();
|
||||
deconfigureDebug();
|
||||
}
|
|
@ -36,6 +36,7 @@ int main(int argc, char* argv[])
|
|||
initLCD();
|
||||
configureCF();
|
||||
configureButtons();
|
||||
configureMotor();
|
||||
|
||||
for (;;) {
|
||||
setPWMTensionWrapper(VITL, VITR);
|
||||
|
|
|
@ -12,6 +12,7 @@ int main(int argc, char* argv[])
|
|||
(void)argv;
|
||||
|
||||
configureCF();
|
||||
configureMotor();
|
||||
stop();
|
||||
|
||||
}
|
||||
|
|
|
@ -38,6 +38,7 @@ int main(int argc, char* argv[])
|
|||
initLCD();
|
||||
configureCF();
|
||||
configureButtons();
|
||||
configureMotor();
|
||||
|
||||
for (;;) {
|
||||
for (int i = 0; i < UP_TIME; i += INTERVAL) {
|
||||
|
@ -56,6 +57,22 @@ int main(int argc, char* argv[])
|
|||
}
|
||||
changerMoteursWrapper(0, 0);
|
||||
delay(LOW_TIME);
|
||||
for (int i = 0; i < UP_TIME; i += INTERVAL) {
|
||||
float p = (float)i / (float)UP_TIME;
|
||||
changerMoteursWrapper(-p * MAXI, -p * MAXI);
|
||||
delay(INTERVAL);
|
||||
}
|
||||
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 * MAXI, -p * MAXI);
|
||||
delay(INTERVAL);
|
||||
}
|
||||
changerMoteursWrapper(0, 0);
|
||||
delay(LOW_TIME);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
Loading…
Reference in a new issue