int
skin_inf_ask (void)
{
static int tick = 0;
sw_struct sw;
printf ("inferior ask\n");
sw_init (&sw);
sw_set_name (&sw, "MyEncoder");
sw.type = SW_SEN_ENCODER;
sw.op = SW_SEN_ENCODER_SET;
sw.data.encoder.resolution = 0.001;
sw.data.encoder.mount.x = 1;
sw.data.encoder.mount.y = 2;
sw.data.encoder.mount.z = -3;
sw.data.encoder.mount.roll = -0.1;
sw.data.encoder.mount.pitch = -0.2;
sw.data.encoder.mount.yaw = 0.3;
sup_tell (&sw);
ulapi_wait (500000000);
sw.op = SW_SEN_ENCODER_STAT;
sw.time = ulapi_time ();
sw.data.encoder.tick = tick++;
sup_tell (&sw);
ulapi_wait (500000000);
return 0;
}
static void emove_client_write_task_code(emove_client_write_task_args *args)
{
ulapi_task_struct *task;
int id;
int period_nsecs;
enum {OUTBUF_SIZE = 1024};
char outbuf[OUTBUF_SIZE];
int nchars;
nist_kitting::emove_stat emove_stat;
task = args->task;
id = args->id;
period_nsecs = args->period_nsecs;
free(args);
while (true) {
ros::spinOnce();
ulapi_mutex_take(&emove_stat_mutex);
emove_stat = emove_stat_buf;
ulapi_mutex_give(&emove_stat_mutex);
/*
uint8 type
uint8 serial_number
uint8 state
uint8 status
uint8 heartbeat
float32 period
float32 cycle
float32 duration
extern char *kitting_cmd_to_string(int s);
extern char *rcs_state_to_string(int s);
extern char *rcs_status_to_string(int s);
*/
ulapi_snprintf(outbuf, sizeof(outbuf), "%s %d %s %s %d %s %s %s\n",
kitting_cmd_to_string(emove_stat.stat.type),
emove_stat.stat.serial_number,
rcs_state_to_string(emove_stat.stat.state),
rcs_status_to_string(emove_stat.stat.status),
emove_stat.stat.heartbeat,
emove_stat.name.c_str(), emove_stat.line.c_str(), emove_stat.crcl.c_str());
outbuf[sizeof(outbuf)-1] = 0;
nchars = ulapi_socket_write(id, outbuf, strlen(outbuf));
if (nchars <= 0) break;
ulapi_wait(period_nsecs);
}
ulapi_socket_close(id);
if (debug) printf("emove client write handler %d done\n", id);
ulapi_task_delete(task);
return;
}
rtapi_result
rtapi_wait(rtapi_integer period_nsec)
{
return ulapi_wait(period_nsec);
}
void client_code(void *args)
{
void *client_task;
ulapi_integer client_id;
client_db_struct *client_db_ptr;
ulapi_integer debug;
enum {BUFFERLEN = 256};
char inbuf[BUFFERLEN];
char outbuf[BUFFERLEN];
ulapi_integer nchars;
int number;
int lastnumber = 0;
client_task = ((client_args *) args)->client_task;
client_id = ((client_args *) args)->client_id;
client_db_ptr = ((client_args *) args)->client_db_ptr;
debug = ((client_args *) args)->debug;
free(args);
/*
The client thread asks for an update every second, blocks until
it gets a response, prints it, and loops again.
*/
for (;;) {
ulapi_mutex_take(client_db_ptr->mutex);
number = client_db_ptr->number;
ulapi_mutex_give(client_db_ptr->mutex);
if (number != lastnumber) {
ulapi_snprintf(outbuf, sizeof(outbuf), "write %d", number);
lastnumber = number;
} else {
ulapi_snprintf(outbuf, sizeof(outbuf), "read");
}
ulapi_socket_write(client_id, outbuf, strlen(outbuf)+1);
nchars = ulapi_socket_read(client_id, inbuf, sizeof(inbuf)-1);
if (-1 == nchars) {
if (debug) printf("connection closed\n");
break;
}
if (0 == nchars) {
if (debug) printf("end of file\n");
break;
}
inbuf[nchars] = 0;
/*
Parse and handle the message here as your application requires.
*/
printf("%s\n", inbuf);
ulapi_wait(1000000000);
}
ulapi_socket_close(client_id);
}
static void state_connection_thread_code(state_connection_thread_args *args)
{
void *thread;
int id;
double period;
joint_traj_pt_state_message jsmsg;
robot_status_message rsmsg;
object_state_message obj_state(3);
enum {OUTBUF_LEN = 1024};
char outbuf[OUTBUF_LEN];
int nchars, len;
int which = 0;
thread = args->thread;
id = args->id;
period = args->period;
free(args);
for (int t = 0; t < obj_state.object_number(); t++) {
obj_state.objects[t].id = t + 1;
obj_state.objects[t].x = t + 1.2;
obj_state.objects[t].y = t - 3.4;
obj_state.objects[t].z = t + 5.6;
obj_state.objects[t].qx = 0;
obj_state.objects[t].qy = 0;
obj_state.objects[t].qz = sin(0.1*t);
obj_state.objects[t].qw = cos(0.1*t);
}
while (true) {
float p;
int i;
switch (which) {
case 0:
ulapi_mutex_take(&robot_mutex);
for (int t = 0; t < JOINT_MAX; t++) {
if (the_robot.get_robot_joint_pos(&p, t)) jsmsg.set_pos(p, t);
}
ulapi_mutex_give(&robot_mutex);
nchars = ulapi_socket_write(id, reinterpret_cast<char *>(&jsmsg), sizeof(jsmsg));
if (nchars < 0) break;
break;
case 1:
ulapi_mutex_take(&robot_mutex);
the_robot.get_drives_powered(&i);
rsmsg.set_drives_powered(i);
the_robot.get_e_stopped(&i);
rsmsg.set_e_stopped(i);
the_robot.get_in_error(&i);
rsmsg.set_in_error(i);
the_robot.get_in_motion(&i);
rsmsg.set_in_motion(i);
the_robot.get_mode(&i);
rsmsg.set_mode(i);
the_robot.get_motion_possible(&i);
rsmsg.set_motion_possible(i);
ulapi_mutex_give(&robot_mutex);
nchars = ulapi_socket_write(id, reinterpret_cast<char *>(&rsmsg), sizeof(rsmsg));
if (nchars < 0) break;
break;
case 2:
#undef DO_OBJECTS
#ifdef DO_OBJECTS
len = obj_state.size();
if (len <= sizeof(outbuf)) {
obj_state.write_object_state(outbuf, sizeof(outbuf));
nchars = ulapi_socket_write(id, outbuf, len);
}
#endif // DO_OBJECTS
break;
} // switch (which)
if (++which > 2) which = 0;
ulapi_wait(period * 1e9);
} // while (true)
ulapi_socket_close(id);
if (debug) printf("joint state connection handler %d done\n", id);
free(thread);
return;
}
