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