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);
}
int pVehicle::initWheels(int flags)
{
getWheels().clear();
int nbShapes = getActor()->getNbShapes();
NxShape ** slist = (NxShape **)getActor()->getShapes();
for (NxU32 j=0; j<nbShapes; j++)
{
NxShape *s = slist[j];
if (s)
{
pSubMeshInfo *sinfo = static_cast<pSubMeshInfo*>(s->userData);
if (sinfo && sinfo->wheel !=NULL)
{
pWheel *wheel = sinfo->wheel;
pWheel2* wheel2 = (pWheel2*)wheel;
NxWheelShape *wShape = wheel2->getWheelShape();
wheel2->setEntity(static_cast<CK3dEntity*>(GetPMan()->GetContext()->GetObject(wheel2->getEntID())));
if (!wShape) continue;
if (wheel2->getWheelFlag(WF_VehicleControlled) )
{
int& pOptions = getProcessOptions();
wheel2->setProcessOptions(pOptions);
if ( wShape->getWheelFlags() & NX_WF_AXLE_SPEED_OVERRIDE )
wheel2->_createInternalContactModifyCallback();
getWheels().push_back(wheel);
wheel2->setVehicle(this);
}
}
}
}
return getWheels().size();
/*
int result = 0 ;
if (!getBody() || !getBody()->isValid() )
{
return result;
}
getWheels().clear();
CK3dEntity* subEntity = NULL;
while (subEntity= getBody()->GetVT3DObject()->HierarchyParser(subEntity) )
{
if (subEntity->HasAttribute(GetPMan()->GetPAttribute()))
{
pObjectDescr *subDescr = pFactory::Instance()->createPObjectDescrFromParameter(subEntity->GetAttributeParameter(GetPMan()->GetPAttribute()));
if (subDescr->flags & BF_SubShape)
{
if (subDescr->hullType == HT_Wheel)
{
if (subEntity->HasAttribute(GetPMan()->att_wheelDescr ))
{
CKParameter *par = subEntity->GetAttributeParameter(GetPMan()->att_wheelDescr );
if (par)
{
pWheelDescr *wDescr = pFactory::Instance()->createWheelDescrFromParameter(par);
if (wDescr)
{
pWheel *wheel = pFactory::Instance()->createWheel(getBody(),*wDescr);
if (wheel)
{
NxWheelShape *wShape = static_cast<NxWheelShape*>(getBody()->_getSubShapeByEntityID(subEntity->GetID()));
if(wDescr->wheelFlags & E_WF_USE_WHEELSHAPE)
{
pWheel2 *wheel2 = static_cast<pWheel2*>(wheel);
if (wheel2)
{
if(wShape)
wheel2->setWheelShape(wShape);
}
}
wheel->setEntID(subEntity->GetID());
getWheels().push_back(wheel);
// subEntity->SetParent(NULL);
}
}
}
}
}
}
}
}
return getWheels().size();
*/
}
