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);
}
}
