/**--------------------------------------------------------------------------------------------------
Name : fifoRead.
Description : Function to get a char from front of given buffer.
Argument(s) : Pointer to a fifoType buffer.
Return value : character. (returns zero if buffer empty)
--------------------------------------------------------------------------------------------------**/
int8_t fifoRead(fifoType * buffer, int8_t * elem)
{
if ( !fifoIsEmpty(buffer) )
{
*elem = (int8_t) buffer->data[buffer->ffilled];
buffer->ffilled = (buffer->ffilled + 1) % buffer->size;
return 0; // success
}
else
return -1; // buffer empty
}
void* fifoPopElem(fifo_t* fifo)
{
void* sourceAddr;
void* byte;
CHECK_FIFO_NULL(fifo);
if (fifoIsEmpty(fifo) == 0x01)
return 0;
sourceAddr = (u8_t*)fifo->buffer + fifo->tail * fifo->elemSize;
byte = (u32_t*)(*(u32_t*)sourceAddr);
fifo->tail++;
if (fifo->tail == fifo->size)
fifo->tail = 0;
return byte;
}
/* Calcule les pwm a appliquer pour un asservissement en position
* <> value_pwm_left : la pwm a appliquer sur la roue gauche [-255,255]
* <> value_pwm_right : la pwm a appliquer sur la roue droite [-255,255]
* */
void positionControl(int* value_pwm_left, int* value_pwm_right){
static bool initDone = false;
if(!initDone){
output4Delta = 0;
output4Alpha = 0;
currentDelta = .0;
currentAlpha = .0;
consigneDelta = .0;
consigneAlpha = .0;
pid4DeltaControl.Reset();
pid4DeltaControl.SetInputLimits(-TABLE_DISTANCE_MAX_MM/ENC_TICKS_TO_MM,TABLE_DISTANCE_MAX_MM/ENC_TICKS_TO_MM);
pid4DeltaControl.SetSampleTime(DUREE_CYCLE);
pid4DeltaControl.SetOutputLimits(-current_goal.speed,current_goal.speed); /*composante liee a la vitesse lineaire*/
pid4DeltaControl.SetMode(AUTO);
pid4AlphaControl.Reset();
pid4AlphaControl.SetSampleTime(DUREE_CYCLE);
pid4AlphaControl.SetInputLimits(-M_PI,M_PI);
pid4AlphaControl.SetOutputLimits(-255,255); /*composante lie a la vitesse de rotation*/
pid4AlphaControl.SetMode(AUTO);
initDone = true;
}
/* Gestion de l'arret d'urgence */
if(current_goal.isCanceled){
initDone = false;
current_goal.isReached = true;
current_goal.isCanceled = false;
/* et juste pour etre sur */
(*value_pwm_right) = 0;
(*value_pwm_left) = 0;
return;
}
/* Gestion de la pause */
if(current_goal.isPaused){
(*value_pwm_right) = 0;
(*value_pwm_left) = 0;
return;
}
/*calcul de l'angle alpha a combler avant d'etre aligne avec le point cible
* borne = [-PI PI] */
double angularCoeff = atan2(current_goal.y-robot_state.y,current_goal.x-robot_state.x); /*arctan(y/x) -> [-PI,PI]*/
currentAlpha = moduloPI(angularCoeff - robot_state.angle); /* il faut un modulo ici, c'est sur !
/* En fait, le sens est donne par l'ecart entre le coeff angulaire et l'angle courant du robot.
* Si cet angle est inferieur a PI/2 en valeur absolue, le robot avance en marche avant (il avance quoi)
* Si cet angle est superieur a PI/2 en valeur absolue, le robot recule en marche arriere (= il recule)
*/
int sens = 1;
if(abs(currentAlpha) > M_PI/2){/* c'est a dire qu'on a meilleur temps de partir en marche arriere */
sens = -1;
currentAlpha = moduloPI(M_PI + angularCoeff - robot_state.angle);
}
currentAlpha = -currentAlpha;
double dx = current_goal.x-robot_state.x;
double dy = current_goal.y-robot_state.y;
currentDelta = -sens * sqrt(dx*dx+dy*dy); // - parce que l'ecart doit etre negatif pour partir en avant
/*
Serial.print("coeff:");
Serial.print(angularCoeff);
Serial.print(" angle:");
Serial.print(robot_state.angle);
Serial.print(" alpha:");
Serial.print(currentAlpha);
Serial.print(" delta:");
Serial.print(currentDelta);
Serial.print(" x:");
Serial.print(current_goal.x);
Serial.print(" y:");
Serial.println(current_goal.y);
*/
/* on limite la vitesse lineaire quand on s'approche du but */
if (abs(currentDelta)<250){
pid4DeltaControl.SetOutputLimits(-min(50,current_goal.speed),min(50,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
else if (abs(currentDelta)<500){
pid4DeltaControl.SetOutputLimits(-min(60,current_goal.speed),min(60,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
else if (abs(currentDelta)<750){
pid4DeltaControl.SetOutputLimits(-min(80,current_goal.speed),min(80,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
else if (abs(currentDelta)<1000){
pid4DeltaControl.SetOutputLimits(-min(100,current_goal.speed),min(100,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
else if (abs(currentDelta)<1250){
pid4DeltaControl.SetOutputLimits(-min(150,current_goal.speed),min(150,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
else if (abs(currentDelta)<1500){
pid4DeltaControl.SetOutputLimits(-min(200,current_goal.speed),min(200,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
if(abs(currentDelta) < 5*ENC_MM_TO_TICKS) /*si l'ecart n'est plus que de 6 mm, on considere la consigne comme atteinte*/
current_goal.phase = PHASE_2;
else
current_goal.phase = PHASE_1;
pid4AlphaControl.Compute();
pid4DeltaControl.Compute();
if(current_goal.phase == PHASE_2){
(*value_pwm_right) = 0;
(*value_pwm_left) = 0;
}
else{
double pwm4Delta = 0.0;
//FIXME probleme de debordemment
//alpha 200 delta 255 / delta+alpha = 455 / delta-alpha = 55 / => alpha 200 delta 55
//alpha 200 delta -255 / delta+alpha = -55 / delta-alpha = -455 / => alpha 200 delta -55
//alpha -200 delta 255 / delta+alpha = 55 / delta-alpha = 455 / => alpha -200 delta 55
//alpha -200 delta -255 / delta+alpha = -455 / delta-alpha = -55 / => alpha -200 delta -55
/*
Correction by thomas
if(output4Delta+output4Alpha>255 || output4Delta-output4Alpha>255)
pwm4Delta = 255-output4Alpha;
else if(output4Delta+output4Alpha<-255 || output4Delta-output4Alpha<-255)
pwm4Delta = -255+output4Alpha;
else
pwm4Delta = output4Delta;*/
(*value_pwm_right) = output4Delta+output4Alpha;
(*value_pwm_left) = output4Delta-output4Alpha;
// Correction by thomas
if ((*value_pwm_right) > 255)
(*value_pwm_right) = 255;
else if ((*value_pwm_right) < -255)
(*value_pwm_right) = -255;
if ((*value_pwm_left) > 255)
(*value_pwm_left) = 255;
else if ((*value_pwm_left) < -255)
(*value_pwm_left) = -255;
}
if(current_goal.phase == PHASE_2){
if(current_goal.id != -1 && !current_goal.isMessageSent){
//le message d'arrivee n'a pas encore ete envoye a l'intelligence
//envoi du message
sendMessage(current_goal.id,2);
current_goal.isMessageSent = true;
}
/*condition d'arret = si on a atteint le but et qu'un nouveau but attends dans la fifo*/
if(!fifoIsEmpty()){ //on passe a la tache suivante
current_goal.isReached = true;
initDone = false;
}
}
}
/* Calcule les pwm a appliquer pour un asservissement en angle
* <> value_pwm_left : la pwm a appliquer sur la roue gauche [-255,255]
* <> value_pwm_right : la pwm a appliquer sur la roue droite [-255,255]
* */
void angleControl(int* value_pwm_left, int* value_pwm_right){
static bool initDone = false;
if(!initDone){
pwm = 0;
currentEcart = .0;
consigne = .0;
pid4AngleControl.Reset();
pid4AngleControl.SetInputLimits(-M_PI,M_PI);
pid4AngleControl.SetOutputLimits(-current_goal.speed,current_goal.speed);
pid4AngleControl.SetSampleTime(DUREE_CYCLE);
pid4AngleControl.SetMode(AUTO);
initDone = true;
}
/* Gestion de l'arret d'urgence */
if(current_goal.isCanceled){
initDone = false;
current_goal.isReached = true;
current_goal.isCanceled = false;
/* et juste pour etre sur */
(*value_pwm_right) = 0;
(*value_pwm_left) = 0;
return;
}
/* Gestion de la pause */
if(current_goal.isPaused){
(*value_pwm_right) = 0;
(*value_pwm_left) = 0;
return;
}
/*l'angle consigne doit etre comprise entre ]-PI, PI]
En fait pour simplifier, l'entree du PID sera l'ecart entre le l'angle courant et l'angle cible (consigne - angle courant)
la consigne (SetPoint) du PID sera 0
la sortie du PID sera le double pwm
*/
currentEcart = -moduloPI(current_goal.angle - robot_state.angle);
/*
Serial.print("goal: ");
Serial.print(current_goal.angle);
Serial.print(" current: ");
Serial.print(robot_state.angle);
Serial.print(" ecart: ");
Serial.println(currentEcart);
*/
if(abs(currentEcart) < 3.0f*M_PI/360.0f) /*si l'erreur est inferieur a 3deg, on concidere la consigne atteinte*/
current_goal.phase = PHASE_2;
else
current_goal.phase = PHASE_1;
pid4AngleControl.Compute();
if(current_goal.phase == PHASE_2){
(*value_pwm_right) = 0;
(*value_pwm_left) = 0;
}
else{
(*value_pwm_right) = pwm;
(*value_pwm_left) = -pwm;
}
if(current_goal.phase == PHASE_2){
if(current_goal.id != -1 && !current_goal.isMessageSent){
//le message d'arrivee n'a pas encore ete envoye a l'intelligence
//envoi du message
sendMessage(current_goal.id,2);
current_goal.isMessageSent = true;
}
if(!fifoIsEmpty()){ //on passe a la tache suivante
/*condition d'arret = si on a atteint le but et qu'un nouveau but attends dans la fifo*/
current_goal.isReached = true;
initDone = false;
}
}
}
/* Calcule les pwm a appliquer pour un asservissement en position
* <> value_pwm_left : la pwm a appliquer sur la roue gauche [-255,255]
* <> value_pwm_right : la pwm a appliquer sur la roue droite [-255,255]
* */
void positionControl(int* value_pwm_left, int* value_pwm_right){
static bool initDone = false;
if(!initDone){
output4Delta = 0;
output4Alpha = 0;
currentDelta = .0;
currentAlpha = .0;
consigneDelta = .0;
consigneAlpha = .0;
pid4DeltaControl.Reset();
pid4DeltaControl.SetInputLimits(-TABLE_DISTANCE_MAX_MM/ENC_TICKS_TO_MM,TABLE_DISTANCE_MAX_MM/ENC_TICKS_TO_MM);
pid4DeltaControl.SetSampleTime(DUREE_CYCLE);
pid4DeltaControl.SetOutputLimits(-current_goal.speed,current_goal.speed); /*composante liee a la vitesse lineaire*/
pid4DeltaControl.SetMode(AUTO);
pid4AlphaControl.Reset();
pid4AlphaControl.SetSampleTime(DUREE_CYCLE);
pid4AlphaControl.SetInputLimits(-M_PI,M_PI);
pid4AlphaControl.SetOutputLimits(-255,255); /*composante lie a la vitesse de rotation*/
pid4AlphaControl.SetMode(AUTO);
initDone = true;
}
/* Gestion de l'arret d'urgence */
if(current_goal.isCanceled){
initDone = false;
current_goal.isReached = true;
current_goal.isCanceled = false;
/* et juste pour etre sur */
(*value_pwm_right) = 0;
(*value_pwm_left) = 0;
return;
}
/* Gestion de la pause */
if(current_goal.isPaused){
(*value_pwm_right) = 0;
(*value_pwm_left) = 0;
return;
}
/*calcul de l'angle alpha a combler avant d'etre aligne avec le point cible
* borne = [-PI PI] */
double angularCoeff = atan2(current_goal.y-robot_state.y,current_goal.x-robot_state.x); /*arctan(y/x) -> [-PI,PI]*/
currentAlpha = moduloPI(angularCoeff - robot_state.angle); /* il faut un modulo ici, c'est sur !
/* En fait, le sens est donne par l'ecart entre le coeff angulaire et l'angle courant du robot.
* Si cet angle est inferieur a PI/2 en valeur absolue, le robot avance en marche avant (il avance quoi)
* Si cet angle est superieur a PI/2 en valeur absolue, le robot recule en marche arriere (= il recule)
*/
int sens = 1;
if(current_goal.phase != PHASE_1 and abs(currentAlpha) > M_PI/2){/* c'est a dire qu'on a meilleur temps de partir en marche arriere */
sens = -1;
currentAlpha = moduloPI(M_PI + angularCoeff - robot_state.angle);
}
currentAlpha = -currentAlpha;
double dx = current_goal.x-robot_state.x;
double dy = current_goal.y-robot_state.y;
currentDelta = -sens * sqrt(dx*dx+dy*dy); // - parce que l'ecart doit etre negatif pour partir en avant
switch(current_goal.phase)
{
case PHASE_1:
if(abs(currentDelta)<1500) /*si l'ecart n'est plus que de 6 mm, on considere la consigne comme atteinte*/
{
current_goal.phase = PHASE_DECEL;
}
break;
case PHASE_DECEL:
if (fifoIsEmpty())
{
/* on limite la vitesse lineaire quand on s'approche du but */
if (abs(currentDelta)<250){
pid4DeltaControl.SetOutputLimits(-min(50,current_goal.speed),min(50,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
else if (abs(currentDelta)<500){
pid4DeltaControl.SetOutputLimits(-min(60,current_goal.speed),min(60,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
else if (abs(currentDelta)<750){
pid4DeltaControl.SetOutputLimits(-min(80,current_goal.speed),min(80,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
else if (abs(currentDelta)<1000){
pid4DeltaControl.SetOutputLimits(-min(100,current_goal.speed),min(100,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
else if (abs(currentDelta)<1250){
pid4DeltaControl.SetOutputLimits(-min(150,current_goal.speed),min(150,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
else {
pid4DeltaControl.SetOutputLimits(-min(200,current_goal.speed),min(200,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
}
if(abs(currentDelta) < 5*ENC_MM_TO_TICKS) /*si l'ecart n'est plus que de 6 mm, on considere la consigne comme atteinte*/
{
//envoi du message
sendMessage(current_goal.id,2);
current_goal.isMessageSent = true;
current_goal.phase = PHASE_ARRET;
}
break;
case PHASE_ARRET:
if (abs(currentDelta) > 5*ENC_MM_TO_TICKS)
{
current_goal.phase = PHASE_CORRECTION;
}
break;
case PHASE_CORRECTION:
pid4DeltaControl.SetOutputLimits(-min(50,current_goal.speed),min(50,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
if (abs(currentDelta) < 5*ENC_MM_TO_TICKS)
{
current_goal.phase = PHASE_ARRET;
}
default:
break;
}
if (!fifoIsEmpty() and (current_goal.phase == PHASE_ARRET or current_goal.phase == PHASE_CORRECTION)) { //on passe a la tache suivante si la fifo n'est pas vide
current_goal.isReached = true;
initDone = false;
}
pid4AlphaControl.Compute();
pid4DeltaControl.Compute();
if (current_goal.phase == PHASE_ARRET)
{
(*value_pwm_right) = 0;
(*value_pwm_left) = 0;
}
else
{
(*value_pwm_right) = output4Delta+output4Alpha;
(*value_pwm_left) = output4Delta-output4Alpha;
// Débordement
if ((*value_pwm_right) > 255)
(*value_pwm_right) = 255;
else if ((*value_pwm_right) < -255)
(*value_pwm_right) = -255;
if ((*value_pwm_left) > 255)
(*value_pwm_left) = 255;
else if ((*value_pwm_left) < -255)
(*value_pwm_left) = -255;
}
}
