void goalPos(goal_t *goal) {
int x, y;
float dx, dy, da, dd, goal_a;
x = goal->data.pos_data.x;
y = goal->data.pos_data.y;
dx = x - current_pos.x;
dy = y - current_pos.y;
goal_a = atan2(dy, dx);
da = (goal_a - current_pos.angle);
da = moduloTwoPI(da);
dd = sqrt(pow(dx, 2.0)+pow(dy, 2.0));
if (goal->data.pos_data.d == ANY) {
if (abs(da) > CONE_ALIGNEMENT) {
da = moduloPI(da);
dd = - dd;
}
} else if (goal->data.pos_data.d == BACKWARD) {
dd = - dd;
da = moduloTwoPI(da+M_PI);
}
if (controlPos(dd, da) & POS_REACHED) {
goal->is_reached = 1;
}
applyPID();
}
/* Implementation du modele d'evolution du robot a partir de l'odometrie
* A appeler a intervalle regulier (a voir pour la mettre sur une interruption timer)
* */
void computeRobotState(){
static long prev_value_left_enc = 0;
static long prev_value_right_enc = 0;
static unsigned long prevTime = 0;
/*calcul du deplacement depuis la derniere fois en ticks */
long dl = value_left_enc - prev_value_left_enc;
long dr = value_right_enc - prev_value_right_enc;
/*preparation de la prochaine iteration*/
prev_value_left_enc = value_left_enc;
prev_value_right_enc = value_right_enc;
/*calcul de la vitesse en mm/s */
unsigned long duree = micros() - prevTime;
double speed_left = ((double)dl/(double)duree)*1000000.0;
double speed_right = ((double)dr/(double)duree)*1000000.0;
double speed = (speed_left+speed_right)/2.0; /*estimation : simple moyenne*/
prevTime = micros();
/* calcul du changement d'orientation en rad */
double delta_angle = (double)(dr-dl)/(double)ENC_CENTER_DIST_TICKS;
/* calcul du deplacement */
double delta_dist = (double)(dr+dl)/2.0;
/* mise a jour de la position en ticks
* on utilise des cos et des sin et c'est pas tres opti.
* A voir si l'approximation par un dev limite d'ordre 2 est plus efficace
*/
double dx = delta_dist*cos(robot_state.angle+delta_angle); //FIXME y a peut etre un delta_angle/2 ici, a verifier
double dy = delta_dist*sin(robot_state.angle+delta_angle);
/*mise a jour de l'etat du robot */
robot_state.speed = speed;
robot_state.angle = moduloPI(robot_state.angle + delta_angle);
robot_state.x += dx;
robot_state.y += dy;
}
void RobotObserver::compute(TICKS left_value, TICKS right_value) {
TICKS dl = (left_value - _prev_left_value);
TICKS dr = (right_value - _prev_right_value);
// vitesse de rotation
_speed_a = (dr-dl)/(double)ENC_CENTER_DIST_TICKS;
//_speed_a = atan2(dr-dl, ENC_CENTER_DIST_TICKS);
// vitesse
_speed = (dr+dl)/2.0;
// mise a jour de l'etat du robot
_a = moduloPI(((double)(right_value - left_value)) / (double)ENC_CENTER_DIST_TICKS);
TICKS_100 delta = (dr+dl) * 50; // (dr+dl)*100/2
_x += ((double)delta)*cos(_a);//(1.0-0.5*_a*_a);//cos(_a);
_y += ((double)delta)*sin(_a);//(_a-_a*_a*_a/6.0);//sin(_a);
// preparation de la prochaine iteration
_prev_left_value = left_value;
_prev_right_value = right_value;
}
/* 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;
}
}
double alpha_diff(double a, double b)
{
//return atan2(sin(a-b), cos(a-b));
return moduloPI(a - b);
}
/**
* Analyse le message et effectue les actions associees
*
* @param id : l'identifiant associe au message
* @param header : le type de message (en-tete)
* @param args : le tableau d'entier contenant les arguments
* */
void cmd(int id, int id_cmd, int* args, int size){
/* On analyse le message en fonction de son type */
switch(id_cmd){
case QA_ID: /* Identification */
{
sendMessage(id, ID_ASSERV, (char*)"asserv");
break;
}
case QA_PING:
{
sendMessage(id, (char*)"Pong");
break;
}
case QA_SETPID:
{
//ENC_CENTER_DIST_MM = args[0]/10.0;
break;
}
case QA_GOTO:
{
if (size < 3)
sendMessage(id, E_INVALID_PARAMETERS_NUMBERS);
else
{
pushGoalPosition(id,(double)args[0]*ENC_MM_TO_TICKS, (double)args[1]*ENC_MM_TO_TICKS, (double)args[2]);
sendMessage(id, 1);
}
break;
}
case QA_GOTOR:
{
if (size < 3)
sendMessage(id, E_INVALID_PARAMETERS_NUMBERS);
else
{
double co = cos(robot_get_angle());
double si = sin(robot_get_angle());
pushGoalPosition(id,((double)args[0]*co-(double)args[1]*si)*18+robot_get_x()*0.01, ((double)args[0]*si+(double)args[1]*co)*18+robot_get_y()*0.01, (double)args[2]);
sendMessage(id, 1);
}
break;
}
case QA_TURN:
{
if (size < 2)
sendMessage(id, E_INVALID_PARAMETERS_NUMBERS);
else
{
double angle = moduloPI(((double)args[0]) * DEG_TO_RAD);
sendMessage(-1, (int)(angle*100.0));
pushGoalOrientation(id,angle,args[1]);
sendMessage(id, 1);
}
break;
}
case QA_TURNR:
{
if (size < 2)
sendMessage(id, E_INVALID_PARAMETERS_NUMBERS);
else
{
double angle = moduloPI(((double)args[0]) * DEG_TO_RAD + robot_get_angle());
pushGoalOrientation(id,angle,args[1]);
sendMessage(id, 1);
}
break;
}
case QA_POS:
{
int x_mm = robot_get_x()*0.01*ENC_TICKS_TO_MM;
int y_mm = robot_get_y()*0.01*ENC_TICKS_TO_MM;
int a_deg = robot_get_angle()*RAD_TO_DEG;
int tab[] = {x_mm,y_mm,a_deg};
sendMessage(id,tab,3);
break;
}
case QA_SET_POS:
{
if (size < 2)
sendMessage(id, E_INVALID_PARAMETERS_NUMBERS);
else {
robot_set_mm_x(args[0]);
robot_set_mm_y(args[1]);
robot_set_deg_angle(args[2]);
value_left_enc = 0;
value_right_enc = 0;
sendMessage(id, 0);
}
break;
}
case QA_PWM:
{
if (size < 2)
sendMessage(id, E_INVALID_PARAMETERS_NUMBERS);
else
{
pushGoalPwm(id,args[0],args[1],args[2]);
sendMessage(id, 1);
}
break;
}
/*
case Q_MODIF_GOAL_ABS:
{
if (size < 3)
sendMessage(id, E_INVALID_PARAMETERS_NUMBERS);
else
{
current_goal.type = TYPE_POSITION;
current_goal.isReached = false;
current_goal.x = (double)args[0]*ENC_MM_TO_TICKS;
current_goal.y = (double)args[1]*ENC_MM_TO_TICKS;
current_goal.speed = args[2];
sendMessage(id, 1);
}
break;
}
case Q_MODIF_GOAL_REL:
{
if (size < 3)
sendMessage(id, E_INVALID_PARAMETERS_NUMBERS);
else
{
double co = cos(robot_state.angle);
double si = sin(robot_state.angle);
current_goal.type = TYPE_POSITION;
current_goal.isReached = false;
current_goal.x = (args[0]*co-args[1]*si)*18+robot_state.x;
current_goal.y = (args[0]*si+args[1]*co)*18+robot_state.y;
current_goal.speed = args[2];
sendMessage(id, 1);
}
break;
}
*/
case QA_CANCEL: /* comme stop */
{
clearGoals();
current_goal.isCanceled = true;
sendMessage(id, 0);
break;
}
case QA_STOP: /* comme pause */
{
current_goal.isPaused = true;
sendMessage(id, 0);
break;
}
case QA_RESUME: /* comme resume */
{
current_goal.isPaused = false;
sendMessage(id, 0);
break;
}
case QA_RESET:
{
clearGoals();
current_goal.isCanceled = true;
current_goal.isPaused = false;
value_left_enc = 0;
value_right_enc = 0;
robot_set_rad_angle(0.0);
robot_set_x(0);
robot_set_y(0);
sendMessage(id, 0);
break;
}
case QA_GETSENS:
{
if (value_pwm_right > 0)
sendMessage(id, AVANT);
else if (value_pwm_right < 0)
sendMessage(id, ARRIERE);
else
sendMessage(id, ARRET);
break;
}
case QA_GETENC:
{
int tab[2] = {value_left_enc,value_right_enc};
sendMessage(id, tab, 2);
break;
}
case Q_DEBUG : //TODO a degager quand tout marche
{
/*Serial.print("?,_________________§");
Serial.print("uptime: ");Serial.print(millis());
Serial.print("§angle: ");Serial.print(robot_state.angle, DEC);
Serial.print("§speed: ");Serial.print(robot_state.speed*ENC_TICKS_TO_MM, DEC);
Serial.print("§speedt: ");Serial.print(robot_state.speed, DEC);
Serial.print("§x: ");Serial.print(robot_state.x*ENC_TICKS_TO_MM, DEC);
Serial.print("§xt: ");Serial.print(robot_state.x, DEC);
Serial.print("§y: ");Serial.print(robot_state.y*ENC_TICKS_TO_MM, DEC);
Serial.print("§yt: ");Serial.print(robot_state.y, DEC);
Serial.print("§encL: ");Serial.print(value_left_enc);
Serial.print("§encR: ");Serial.println(value_right_enc);*/
break;
}
default:
{
sendMessage(id,E_INVALID_CMD);
break;
}
}
}
