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(); */ }