void *timer_handler(void *ptr) { if(timer_disable == 1) { return NULL; } struct timespec _t; static struct timeval now, prev; double dt = 0; clock_gettime(CLOCK_REALTIME, &_t); static PxController ctrlr; // SioClient initialization ------------------------------------------------- SioClientWrapper client; sio::message::ptr data; //発生するイベント名一覧をstd::vector<std::string>としてclientに渡す std::vector<std::string> eventList(0); eventList.push_back("landing"); eventList.push_back("direction"); eventList.push_back("px_bounce"); eventList.push_back("px_start"); eventList.push_back("px_position"); eventList.push_back("px_velocity"); client.setEventList(eventList); //自身を表す部屋名を設定する(Phenoxなら例えば"Phenox"と決める) client.setMyRoom("Phenox"); //データの送信宛先となる部屋名を設定する(Gameサーバなら例えば"Game") client.setDstRoom("Game"); //URLを指定して接続開始 client.start("http://localhost:8000"); while(1) { pxset_keepalive(); pxset_systemlog(); pxset_img_seq(cameraid); static unsigned long msec_cnt = 0; msec_cnt++; gettimeofday(&now, NULL); dt = (now.tv_sec - prev.tv_sec) + (now.tv_usec - prev.tv_usec) * 1.0E-6; if(dt < 0){ cout << "dt < 0" << endl; continue; } prev = now; // vision control static px_selfstate st; static Vector2f pos; pxget_selfstate(&st); pos << st.vision_tx, st.vision_ty; Vector2f norm, norm_start; Vector2f norm2, norm_start2; static Vector2f input(0,0); // ------------------------------------------------------------------- // get boundary norm ------------------------------------------------- // ------------------------------------------------------------------- int boundary_cnt = 0; if(pthread_mutex_trylock(&mutex) != EBUSY){ norm = gnorm; norm_start = gnorm_start; norm2 = gnorm2; norm_start2 = gnorm_start2; boundary_cnt = gboundary_cnt; //cout << "boundary cnt = " << boundary_cnt << endl; //cout << "norm = \n" << norm << endl; //cout << "norm2 = \n" << norm2 << endl; pthread_mutex_unlock(&mutex); ctrlr.boundHandler(boundary_cnt,norm,norm2,pos); } // -------------------------------------------------------------------- // get landing and direction------------------------------------------- // -------------------------------------------------------------------- //"landing"に対応するデータが来ているかチェック if (client.isUpdated("landing")){ data = client.getData("landing");//データをsio::message::ptrとして取得 parseLanding(data);//データ抽出用関数に渡す std::cout << "landing=" << landing << std::endl; } //"direction"に対応するデータが来ているかチェック if (client.isUpdated("direction")){ data = client.getData("direction");//データをsio::message::ptrとして取得 parseDirection(data);//データ抽出用関数に渡す std::cout << "direction = [" << direction[0] << ", " << direction[1] << "]" << std::endl; } // -------------------------------------------------------------------- // Sample of data sending --------------------------------------------- // -------------------------------------------------------------------- // 送りたいところに移動してね Vector2f px_position(0, 0); Vector2f px_velocity(0, 0); client.sendData("px_start", makePxStart());// client.sendData("px_bounce", makePxBounce());// client.sendData("px_position", makePxPosition());// client.sendData("px_velocity", makePxVelocity());// //cout << ctrlr.vx() << "," << ctrlr.vy() << endl; // save log static ofstream ofs_deg("output_deg"); ofs_deg << st.degx << "," << st.degy << endl; static ofstream ofs_ctl("output_v"); ofs_ctl << ctrlr.vx() << "," << ctrlr.vy() << "," << norm.x() << "," << norm.y() << endl; static ofstream ofs_vision("output_vision"); ofs_vision << st.vision_tx << "," << st.vision_ty << "," << input.x() << "," << input.y() << endl; // if(!(msec_cnt % 30)){ // printf("%.2f %.2f %.2f | %.2f %.2f %.2f | %.2f | \n",st.degx,st.degy,st.degz,st.vision_tx,st.vision_ty,st.vision_tz,st.height); // } static int hover_cnt = 0; static Vector2f origin(0,0); if(pxget_operate_mode() == PX_UP){ pxset_visualselfposition(0, 0); hover_cnt = 0; } if(pxget_operate_mode() == PX_HOVER) { if(hover_cnt < 500){ hover_cnt++; } else if (hover_cnt == 500) { cout << "start control" << endl; ctrlr.init(0,1,origin,pos); hover_cnt++; } else{ input = ctrlr.controlStep(pos, dt); pxset_visioncontrol_xy(input.x(),input.y()); } } static int prev_operatemode = PX_HALT; if((prev_operatemode == PX_UP) && (pxget_operate_mode() == PX_HOVER)) { origin << st.vision_tx, st.vision_ty; pxset_visioncontrol_xy(origin.x(),origin.y()); } prev_operatemode = pxget_operate_mode(); if(pxget_whisle_detect() == 1) { if(pxget_operate_mode() == PX_HOVER) { pxset_operate_mode(PX_DOWN); } else if(pxget_operate_mode() == PX_HALT) { pxset_rangecontrol_z(150); pxset_operate_mode(PX_UP); } } if(pxget_battery() == 1) { timer_disable = 1; system("shutdown -h now\n"); exit(1); } struct timespec remains; _t.tv_nsec += 10000000; clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &_t, &remains); // while (get_time() - t < 10.0) { // usleep(500); // } clock_gettime(CLOCK_REALTIME, &_t); } }