move_n_sample.cc from EmStar at Krugle
Show move_n_sample.cc syntax highlighted
/* ex: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */
/*HACK */
//#define SENSORDEV "dse_parNT533733-2"
#include <devel/as-emstar/ds.h>
__BEGIN_DECLS
#include <libdev/include/query_client.h>
#include <motors/motor_nims.h>
#include "sensor_nims.h"
__END_DECLS
#include "devel/as-emstar/sensor_state.h"
typedef struct motor_client_state {
Adaptive_Sampling * as;
char * cmd;
query_client_ctx_t * qc;
} motor_client_state_t;
int async_client_resp(query_client_ctx_t *qc, int retval, buf_t *response)
{
motor_cmd_return_state_t * ret;
motor_client_state_t * mcs = (motor_client_state_t * )query_client_data( qc );
if (retval) {
elog( LOG_ERR, "got error: %s\n", strerror(retval));
}
if (response) {
ret = ( motor_cmd_return_state_t * ) response->buf;
if ( (! ret) || (ret->state == MOTOR_FAIL) )
elog( LOG_NOTICE, "FAIL: check command again\n");
else if ( ret->state == MOTOR_POS_SUC )
elog( LOG_NOTICE, "success in setting motor position\n");
else {
if ( ret->state == MOTOR_NO_NEED_TO_MOVE )
elog( LOG_NOTICE, "Already in the target position, now take sample\n");
else if ( ret->state == MOTOR_SUCCESS )
elog( LOG_NOTICE, "MOTOR_SUCCESS: reaching (%.2f, %.2f), now taking samples\n", ret->cx, ret->cy);
//mcs->as.pos = (ret->cx, ret->cy) ;
mcs->as->pos.x = ret->cx;
mcs->as->pos.y = ret->cy;
request_sample( mcs->as );
}
// fprintf(stderr, "got back '%s'\n", response->buf);
buf_free(response);
}
return EVENT_RENEW;
// return EVENT_DONE;
}
void command_motor(Adaptive_Sampling *as, loc_world ori_pos, motor_command_t type)
{
char str[256];
int length;
static int flag =0;
loc_world pos;
float vga_x, vga_y, world_x, world_y;
pos.x = ori_pos.x;
pos.y = ori_pos.y;
//check pos should stay within the boundary of
//AS's world coordinate system.
if ( type == MOTOR_MOVE )
{
if ( (pos.x < as->field->world_upper_left.x) || (pos.y < as->field->world_upper_left.y) || (pos.x > (as->field->world_lower_right.x+as->field->pixel_size_x)) || (pos.y > (as->field->world_lower_right.y+as->field->pixel_size_y)) )
{
elog( LOG_ERR, "Robot tries to go beyond the world coordinate boundary(%f, %f)\n", pos.x, pos.y);
return;
}
//convert pos to the VGA mode:
if ( as->field->VGA_mode )
{
vga_x = as->field->VGA_lower_right.x - as->field->VGA_upper_left.x;
vga_y = as->field->VGA_lower_right.y - as->field->VGA_upper_left.y;
world_x = as->field->world_lower_right.x - as->field->world_upper_left.x;
world_y = as->field->world_lower_right.y - as->field->world_upper_left.y;
pos.x = as->field->VGA_upper_left.x + (ori_pos.x - as->field->world_upper_left.x) * vga_x / world_x;
pos.y = as->field->VGA_upper_left.y + (ori_pos.y - as->field->world_upper_left.y) * vga_y / world_y;
}
}
motor_client_state_t * m_client;
if (flag == 0)
{
m_client = g_new0(motor_client_state_t, 1);
as->motor_opts->private_data = m_client;
m_client->as = as;
m_client->qc = NULL;
as->motor_opts->device = NIMS_MOTOR_CONTROLLER_DEV;
as->motor_opts->response = async_client_resp;
query_client_new( (as->motor_opts), &(m_client->qc) );
} else {
m_client = (motor_client_state_t *)as->motor_opts->private_data;
}
switch ( type )
{
case MOTOR_POS:
sprintf( str, "pos:x=%f:y=%f", pos.x, pos.y );
break;
case MOTOR_MOVE:
default:
sprintf( str, "move:x=%f:y=%f", pos.x, pos.y );
break;
}
length = strlen( str) ;
//fprintf( stderr, "before free m_client->cmd\n" );
if ( m_client->cmd != NULL )
free( m_client->cmd );
//fprintf( stderr, "after free m_client->cmd\n" );
m_client->cmd = g_new0( char, (length + 1) );
strcpy( m_client->cmd, str );
flag = 1;
elog(LOG_ERR, "BEFORE command motor: %s\n", str);
query_client_issue_s( m_client->qc, m_client->cmd, 0);
}
//take sample at location stored at the head of the sample_list
void take_first_sample(Adaptive_Sampling *as, Samples * sample_list)
{
Sample * s_p;
loc_world loc_world;
bool goto_sample = false;
//move to actual locations, take samples.
while ( ! sample_list->empty() )
{
s_p = sample_list->front();
sample_list->pop_front();
//convert pixel to world coordinates (in terms of meters)
if ( ! as->field->pixel_2_world( &(s_p->loc), & loc_world ) )
{
elog( LOG_ERR, "Invalid pixel index, out of range\n" );
continue;
}
//check pos should stay within the boundary of
//AS's world coordinate system.
if ( (loc_world.x < as->field->world_upper_left.x) || (loc_world.y < as->field->world_upper_left.y) || (loc_world.x > (as->field->world_lower_right.x + as->field->pixel_size_x)) || (loc_world.y > (as->field->world_lower_right.y + as->field->pixel_size_y)) )
{
elog( LOG_ERR, "Robot try to go beyond the world coordinate boundary(%f, %f)\n", loc_world.x, loc_world.y);
continue;
}
goto_sample = true;
break;
}
if ( goto_sample )
{
as->cur_sp = s_p;
command_motor(as, loc_world, MOTOR_MOVE );
}
}
void request_sample(Adaptive_Sampling *as)
{
static int first_time = 1;
static Nims_sensor_state_t * state;
if ( first_time )
{
elog(LOG_ERR,"Set new state\n");
state = g_new0(Nims_sensor_state_t, 1);
state->binary = 1;
state->printHdr = 1;
state->as = as;
first_time =0;
}
elog(LOG_ERR,"state: %x\n", (int) state);
sc_get_next_sample(as->sensor_device_name, 0,
retrieve_sample_value, state, NULL);
}
See more files for this project here
EmStar is a software system for developing and deploying wireless sensor networks involving Linux-based platforms. As the wireless sensor network community has attempted to deploy more complex designs---large-scale, long-lived systems that need self-organization and adaptivity---a number of difficult software design issues have arisen. Advances in software design have not kept pace with the capabilities of hardware. This is because designing for an adaptive, efficient, and useful sensor network has turned out to be surprisingly complex and difficult. EmStar is a Linux-based software framework, whose goal is to dramatically reduce this complexity, enabling work to be shared and reused, and simplifying and speeding the design of new sensor network applications.
Project homepage:
http://cvs.cens.ucla.edu/emstar/
Programming language(s): C,Shell Script
License: other
as-misc.cc
as-restart.cc
as-with-ta.cc
as.cc
as_process.cc
move_n_sample.cc
sim_sensor_client.cc
socket_server.cc
tcp_client.cc
tcp_func.c