parse_errors_e fnc_com::execute()
{
switch (_op)
{
case sc_create:
return execute_create();
case sc_free:
return execute_free();
case sc_invoke:
return execute_invoke();
default:
return parse_error_function_expected;
}
}
extern void dispatcher(Queue* in, Queue* out, pthread_mutex_t *mutex){
pthread_mutex_lock(mutex);
if (!is_empty(in)){
uint8_t packet_length = queue_head(in);
if (in->count >= packet_length){
uint8_t count = dequeue(in);
t_command command = dequeue(in);
printf ("%s receive packet with command %i\n", KNRM, command);
switch (command){
case INIT:
init(in);
acknowledge(out);
break;
case SET_DIAM_WHEELS:
set_diam_wheels(in);
acknowledge(out);
break;
case SET_WHEELS_SPACING:
set_wheels_spacing(in);
acknowledge(out);
break;
case SET_PID_TRSL:
set_pid_trsl(in);
acknowledge(out);
break;
case SET_PID_ROT:
set_pid_rot(in);
acknowledge(out);
break;
case SET_TRSL_ACC:
set_trsl_acc(in);
acknowledge(out);
break;
case SET_TRSL_DEC:
set_trsl_dec(in);
acknowledge(out);
break;
case SET_TRSL_MAXSPEED:
set_trsl_max(in);
acknowledge(out);
break;
case SET_ROT_ACC:
set_rot_acc(in);
acknowledge(out);
break;
case SET_ROT_DEC:
set_rot_dec(in);
acknowledge(out);
break;
case SET_ROT_MAXSPEED:
set_rot_max(in);
acknowledge(out);
break;
case SET_DELTA_MAX_ROT:
set_delta_max_rot(in);
acknowledge(out);
break;
case SET_DELTA_MAX_TRSL:
set_delta_max_trsl(in);
acknowledge(out);
break;
case SET_PWM:
set_pwm(in);
acknowledge(out);
break;
case SET_X:
set_x(in);
acknowledge(out);
break;
case SET_Y:
set_y(in);
acknowledge(out);
break;
case SET_THETA:
set_theta(in);
acknowledge(out);
break;
case TELEMETRY:
set_telemetry(in);
acknowledge(out);
break;
case SET_DEBUG:
set_debug(in);
acknowledge(out);
break;
case GOTO_XY:
goto_xy(in);
acknowledge(out);
break;
case GOTO_THETA:
goto_theta(in);
acknowledge(out);
break;
case FREE:
execute_free(in);
acknowledge(out);
break;
case STOP_ASAP:
stop_asap(in);
acknowledge(out);
break;
case MOVE_TRSL:
move_trsl(in);
acknowledge(out);
break;
case MOVE_ROT:
move_rot(in);
acknowledge(out);
break;
case RECALAGE:
execute_recalage(in);
acknowledge(out);
break;
case CURVE:
curve(in);
acknowledge(out);
break;
case GET_POSITION:
getPosition(out);
break;
case GET_PID_TRSL:
getPIDtrsl(out);
break;
case GET_PID_ROT:
getPIDRot(out);
break;
case GET_SPEEDS:
getSpeed(out);
break;
case GET_WHEELS:
getWheels(out);
break;
case GET_DELTA_MAX:
getDeltaMax(out);
break;
case GET_VECTOR_TRSL:
getVectorTrsl (out);
break;
case GET_VECTOR_ROT:
getVectorRot (out);
break;
default:
printf ("/!\\ command error: %i\n", command);
send_error(out, COULD_NOT_READ);
break;
}
}
}
pthread_mutex_unlock(mutex);
}
