static void do_game(game_t *p_game) { while(1) { uint32_t before, after; uint32_t keys; before = vGetTickCount(); /* Get the current ms ticks */ keys = vGetButtonData(); handle_input(p_game, keys); move_hero(p_game, &p_game->player); if (keys & KEY_SELECT) return; draw_game(p_game); after = vGetTickCount(); /* Get the current ms ticks */ /* Every loop iteration should take about SLEEP_PERIOD, see to that */ if ((after - before) < SLEEP_PERIOD) { msSleep( SLEEP_PERIOD - (after-before) ); } } }
int menu_select(menu_t *p_menu, uint32_t available_options, int *p_submenus) { vSetActiveFont(p_menu->p_font); while (vGetButtonData()) ; p_menu->available_options = available_options; while(1) { uint32_t keys; vSetForeColor(vRGB(0,0,0)); vFillRect(p_menu->x1, p_menu->y1, p_menu->x2, p_menu->y2); menu_draw(p_menu); vFlipScreen(1); msSleep(300); keys = wait_key_press(); if (keys & KEY_UP) select_next(p_menu, 0, -1); else if (keys & KEY_DOWN) select_next(p_menu, 0, 1); else if (keys & KEY_LEFT) select_next(p_menu, -1, 0); else if (keys & KEY_RIGHT) select_next(p_menu, 1, 0); else if (keys & KEY_SELECT) return -1; else if (keys & KEY_FIRE || keys & KEY_FIRE2) { int ret = p_menu->cur_sel; int i; if (!is_submenu_title(p_menu, ret)) { for (i=0; i<p_menu->n_submenus; i++) p_submenus[i] = p_menu->p_submenus[i].sel; p_menu->cur_sel = 0; return ret; } } } return -1; }
static int WaitFlag(U32 dwFlag, U32 dwTimeLimit) { U32 tmp; while(dwTimeLimit--) { tmp = I2C_ISR; if((tmp & dwFlag) == dwFlag) { I2C_ISR = dwFlag; if((tmp & ISR_ALD) == ISR_ALD) { I2C_ISR = ISR_ALD; DBG_TraceStrLine("Arbitration Loss"); return(ERR_CODE_I2C_ARBI_LOSS); } return(ERR_CODE_DONE); } msSleep(1); } return(ERR_CODE_I2C_TIMEOUT); }
void *threadSerialPCPrinterFunction(void *message) { int ret; char SerialPOSBuf[BUF_MAX]; time_t CurrentTime = 0 ; time_t LastTime = 0; int i; int WriteFlag = 0; ret = pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL); if (0!=ret) { perror("Thread pthread_setcancelstat failed"); pthread_exit(NULL); } ret = pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED, NULL); if (0!=ret) { perror("Thread pthread_setcanceltype failed"); pthread_exit(NULL); } Message.data_length = 0; while(1) { pthread_mutex_lock(&(Message.data_lock)); pthread_mutex_lock(&(Response.data_lock)); if (Response.data_length>0) { int ret; printf("Write to PC %d Datas!\n", Response.data_length); ret = SerialWrite(SerialReadfd, &(Response.data[0]), Response.data_length); if(ret!=Response.data_length) { printf("Response to PC failed!"); } else { Response.data_length = 0; } } pthread_mutex_unlock(&(Response.data_lock)); if(Message.data_length<=0) { //printf("hhhhhhhhhh\n"); //memset(&(Message.data[0]), '\0', BUF_MAX); //Message.data_length = SerialRead(SerialReadfd, &(Message.data[0]), sizeof(Message.data)); Message.data_length = SerialRead(SerialReadfd, &(Message.data[0]), sizeof(Message.data)); //printf("Data length = %d\n", Message.data_length); if(Message.data_length > 0) { //#define rdtscll(val) __asm__ __volatile__("rdtsc" : "=A" (val)) int n = 0; printf("Received %d bytes data\n", Message.data_length); LastTime = time(NULL); /* for(n=0; n<Message.data_length; ++n) { printf("%x\t", Message.data[n]); }*/ //fflush(stdout); WriteARecord(&Message.data[0], Message.data_length);//Write data to a file; // LOGD("%s(%d)-%s:",__FILE__,__LINE__,__FUNCTION__); WriteFlag = 1; // Message.data_length = 0; pthread_mutex_unlock(&Message.data_lock); msSleep(10); } else { CurrentTime = time(NULL); if (((CurrentTime - LastTime)>5)&&(1 == WriteFlag)) { WriteFlag = 0; WriteARecord(&Message.data[0], 0);//close the crrent file ,and creat a queue member; printf("Creat file!\n"); } /* printf("no data in serialread!\n"); */ pthread_mutex_unlock(&Message.data_lock); msSleep(10); } } else { /* printf("Length>0\n"); */ pthread_mutex_unlock(&Message.data_lock); msSleep(10); } } pthread_exit(0); }
int main(int argc, char **argv) { OptoDAQ daq; OptoPorts ports; msSleep(2500); // We wait some ms to be sure about OptoPorts enumerated PortList SensorConfig config; double fx_gain, fy_gain, fz_gain; // sensitivity gain in counts/N double tx_gain, ty_gain, tz_gain; // sensitivity gain in count/Nm int speed, filter; u_int8_t sens_params_set = 1; OPort* portlist=ports.listPorts(true); if (ports.getLastSize()>0) { daq.open(portlist[0]); if (daq.getVersion()!=_95 && daq.getVersion() != _64) // It is a 3D sensor { OptoPackage pack3D; int size=daq.read(pack3D,false); // Reading Sensor #0 (up to 16 Sensors) std::cout<<"x: "<<pack3D.x<<" y: "<<pack3D.y<<" z: "<<pack3D.z<<std::endl; std::cout<<"Sensor not supported in ROS yet"<<std::endl; //Support should be added later } else // It is a 6D sensor = the only one supported for now { OptoPackage6D pack6D; int size=daq.read6D(pack6D,false); //init ROS node std::cout<<"Found 6DOF sensor -> Starting ROS node"<<std::endl; ros::init(argc, argv, "OForceSensorPublisher"); ros::NodeHandle n("~"); //Creating wrench publisher ros::Publisher wrench_pub_raw = n.advertise<geometry_msgs::WrenchStamped>("OptoForceWrench_raw", 1000); ros::Publisher wrench_pub = n.advertise<geometry_msgs::WrenchStamped>("OptoForceWrench", 1000); // get sensitivy gains from rosparam server n.param("fx_gain", fx_gain, 0.0); n.param("fy_gain", fy_gain, 0.0); n.param("fz_gain", fz_gain, 0.0); n.param("tx_gain", tx_gain, 0.0); n.param("ty_gain", ty_gain, 0.0); n.param("tz_gain", tz_gain, 0.0); n.param("speed", speed, 1000); n.param("filter", filter, 0); double double_thresh = 0.0001; if (fx_gain < 0.0001 || fy_gain < 0.0001 || fz_gain < 0.001) { std::cout<<"Sensitivity force params not set properly. Publishing only raw values."<<std::endl; sens_params_set = 0; } if (tx_gain < 0.0001 || ty_gain < 0.0001 || tz_gain < 0.001) { std::cout<<"Sensitivity torque params not set properly.Publishing only raw values."<<std::endl; sens_params_set = 0; } config = daq.getConfig(); if (speed != 30 && speed != 100 && speed != 333 && speed != 1000) { std::cout<<"The speed of the package not set properly. Using default speed of 100 Hz."<<std::endl; speed = 100; } if (filter != 150 && filter != 50 && filter != 15 && filter != 0){ std::cout<<"The filter of the package not set properly. Using default filter of 0 Hz."<<std::endl; filter = 0; } config.setSpeed(speed); config.setFilter(filter); config.setMode(1); daq.sendConfig(config); std::cout<<"Optoforce sensor speed: "<<daq.getConfig().getSpeed()<<" Hz"<<std::endl; std::cout<<"Optoforce sensor filter "<<daq.getConfig().getFilter()<<" Hz"<<std::endl; std::cout<<"Optoforce sensor mode "<<daq.getConfig().getMode()<<std::endl; //Set ROS rate to speed Hz ros::Rate loop_rate(speed); //Main ROS loop while(ros::ok()) { geometry_msgs::WrenchStamped wrench_msg; // Create msg //Fill msg wrench_msg.header.stamp = ros::Time::now(); wrench_msg.wrench.force.x = pack6D.Fx; wrench_msg.wrench.force.y = pack6D.Fy; wrench_msg.wrench.force.z = pack6D.Fz; wrench_msg.wrench.torque.x = pack6D.Tx; wrench_msg.wrench.torque.y = pack6D.Ty; wrench_msg.wrench.torque.z = pack6D.Tz; wrench_pub_raw.publish(wrench_msg); if (sens_params_set) { wrench_msg.header.stamp = ros::Time::now(); wrench_msg.wrench.force.x = pack6D.Fx / fx_gain; wrench_msg.wrench.force.y = pack6D.Fy / fy_gain; wrench_msg.wrench.force.z = pack6D.Fz / fz_gain; wrench_msg.wrench.torque.x = pack6D.Tx / tx_gain; wrench_msg.wrench.torque.y = pack6D.Ty / ty_gain; wrench_msg.wrench.torque.z = pack6D.Tz / tz_gain; wrench_pub.publish(wrench_msg); } ros::spinOnce(); loop_rate.sleep(); size = daq.read6D(pack6D,false); //std::cout<<size<<std::endl; } //std::cout<<"Fx: "<<pack6D.Fx<<" Fy: "<<pack6D.Fy<<" Fz: "<<pack6D.Fz<<" "; //std::cout<<"Tx: "<<pack6D.Tx<<" Ty: "<<pack6D.Ty<<" Tz: "<<pack6D.Tz<<std::endl; } std::cout<<"Closing sensor connection"<<std::endl; daq.close(); } else { std::cout<<"No sensor available"<<std::endl; } return 0; }