Show motor_test.c syntax highlighted
/*
*
* Copyright (c) 2003 The Regents of the University of California. All
* rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* - Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* - Neither the name of the University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS''
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
* PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
* PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
* OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/
/*
* simple example of using a motor device
*/
#include <libdev/include/query_client.h>
#include "mobility_control.h"
#define __GNU_SOURCE
#include <getopt.h>
#define MAX_TEST_POINTS 20
buf_t *log_buf=NULL;
typedef struct motor_client_state {
char device_name[100];
char * cmd;
float pt_x[MAX_TEST_POINTS];
float pt_y[MAX_TEST_POINTS];
int finished;
int total_pts;
query_client_ctx_t * qc;
} motor_client_state_t;
/*
* clients for regression test driver
*/
void ms_client_usage(char *n)
{
fprintf(stderr, "%s -d <motor device name> <motor command name>\n", n);
exit(1);
}
void async_client( motor_client_state_t * m_client, float x, float y);
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_SUCCESS) )
elog( LOG_NOTICE, "FAIL: check command again\n");
else if ( ret->state == MOTOR_SUCCESS )
{
elog( LOG_NOTICE, "SUCCESS: reaching (%.2f, %.2f), now move to the next\n", ret->cx, ret->cy);
mcs->finished ++;
if ( mcs->finished < mcs->total_pts )
{
async_client( mcs, mcs->pt_x[mcs->finished], mcs->pt_y[mcs->finished] );
}
//FIXME
// request_sample();
}
buf_free(response);
}
return EVENT_RENEW;
}
void async_client( motor_client_state_t * m_client, float x, float y )
{
/////////////////////////////////////////////////////////////////////
//each time calling "query_client_new()" opens a new file descriptor
//this will potentially LOCK the motor.
//the LOCK mechanism in query device is per file descriptor
//so this is fine if there is only one such call in the process
//as in this program, o/w, this will lock out subsequent requests..
//solution:
//(A) call "query_client_destroy()" in the above callback function
// "async_client_resp()". HOWEVER, this function doesnot work for now..
//(B) keep using the same file descriptor for all the motor movement.
// so donot allocate new one for each motor move.
// consistent_query_client library is for this.
///////////////////////////////////////////////////////////////////////
static consistent_query_client_t * qobj= NULL;
char cmd[256];
char device[50];
sprintf( cmd, "%s:x=%f:y=%f", MOTOR_MOVE_CMD, x, y );
sprintf(device, "%s", m_client->device_name);
if (qobj == NULL)
qobj = consistent_query_client_new(m_client, cmd, async_client_resp, device);
consistent_query_client_issue_cmd( qobj, cmd );
return;
}
/* read test scenario from a configuration file *
* fname: scenario file, in the format of a list of
* x y
* pt_x, pt_y is the output, points for the robot to visit.
* max_pts: max number of points returned in pt_x, and pt_y
* input again.
*/
int read_test_config(char *fname, float * pt_x, float *pt_y, int max_pts)
{
FILE * f_input;
char buf[BUFSIZE];
float d_x, d_y;
int count;
f_input = fopen( fname, "r" );
if ( f_input == NULL )
{
fprintf( stderr, "open file read error\n" );
exit( 1 );
}
count = 0;
while ((fgets( buf, BUFSIZE, f_input ) != NULL) && (count < max_pts))
{
if ( sscanf( buf, "%f %f", &d_x, &d_y) == 2 )
{
pt_x[count] = d_x;
pt_y[count] = d_y;
count ++;
}
}
fclose( f_input );
return count;
}
int main(int argc, char **argv)
{
/*
* check options
*/
motor_client_state_t m_client;
int i;
char fname[100];
char * key;
strcpy( fname, "\0") ;
if ((key = misc_parse_out_option(&argc, argv, "test-file-name", 't'))) {
strcpy(fname, key );
}
else {
elog(LOG_ERR,"No test configuration file is specified for the motor driver!\n");
}
if ((key = misc_parse_out_option(&argc, argv, "motor-device-name", 'm'))) {
strcpy(m_client.device_name, key );
}
else {
elog(LOG_ERR,"No motor device is specified for the motor driver!\n");
exit(1);
}
m_client.finished =0;
if ( strlen(fname) < 1 )
{
elog( LOG_ERR, "No test scenario specified, use default straight line scenario\n" );
for (i=1; i<=10; i++ )
{
m_client.pt_x[i-1] =i * 100;
m_client.pt_y[i-1] =i * 100;
}
m_client.total_pts = 10;
} else
m_client.total_pts = read_test_config(fname, m_client.pt_x, m_client.pt_y, MAX_TEST_POINTS);
async_client( & m_client, m_client.pt_x[0], m_client.pt_y[0] );
g_main();
return 0;
}
See more files for this project here