motor_command.c from EmStar at Krugle
Show motor_command.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.
*
*/
/*
* use a query device to simulate a motor driver, which accept and interprets
* commands to the motor
*/
#include "sim_motor_nims.h"
void motor_usage(query_context_t *q, buf_t *buf)
{
bufprintf(buf, "command to move motor to a position (m, n) is move:x=m:y=n\n");
}
int motor_complete_process(void *data, int interval, g_event_t *event)
{
query_context_t *context = (query_context_t *)data;
buf_t *print = qdev_curr_print(context);
buf_t *bin = qdev_curr_bin(context);
motor_nims_state_t *mcs;
motor_cmd_return_state_t ret;
elog( LOG_NOTICE, "motor_complete_process called\n");
mcs = (motor_nims_state_t *) qdev_data( context );
mcs->pos.cx = mcs->pos.tx;
mcs->pos.cy = mcs->pos.ty;
if ( mcs->binary_mode_ )
{
ret.state = SUCCESS;
ret.cx = mcs->pos.cx;
ret.cy = mcs->pos.cy;
bufcpy(bin, &ret, sizeof(motor_cmd_return_state_t) );
// bufprintf(print, "I reach (%.2f, %.2f)\n", mcs->pos.cx, mcs->pos.cy );
bufprintf(print, "0 %d %d\n", (int) mcs->pos.cx, (int) mcs->pos.cy );
} else
bufprintf(print, "I reach (%.2f, %.2f)\n", mcs->pos.cx, mcs->pos.cy );
qdev_reply(context, QUERY_LOCK);
// qdev_reply(context, QUERY_DONE);
return TIMER_DONE;
}
int motor_process(query_context_t *q, char *command, size_t size,
buf_t *print, buf_t *bin)
{
parse_state_type_t parse_state = PS_INVALID;
float tx = -1;
float ty = -1;
parser_state_t p_state = {
input: command,
input_len: size
};
motor_nims_state_t *mcs;
motor_command_t command_type;
float cx, cy, dis, vel, est_t ;
motor_cmd_return_state_t ret;
elog( LOG_ERR, "motor_process called, '%s'\n", command);
//initialize them to negative value, suppose
//valid target coordinated are non-negative.
while (misc_parse_next_kvp(&p_state) >= 0) {
/* check for the key 'move' */
if (strcmp("move", p_state.key) == 0) {
parse_state = PS_BEGIN;
}
/* check for the key 'move' */
if ((strcmp("move", p_state.key) == 0) || (strcmp("pos", p_state.key) == 0)) {
parse_state = PS_BEGIN;
if (strcmp("move", p_state.key) == 0)
command_type = MOTOR_MOVE;
else if (strcmp("pos", p_state.key) == 0)
command_type = MOTOR_POS;
}
/* check for the key 'x' */
if ( (parse_state > PS_INVALID) && (strcmp("x", p_state.key) == 0)) {
/* get the x coordinate from the value */
if (p_state.value) {
tx= (float) (atof(p_state.value));
parse_state ++;
}
}
/* check for the key 'y' */
if ( (parse_state > PS_INVALID) && (strcmp("y", p_state.key)) == 0) {
/* get the y coordinate from the value */
if (p_state.value) {
ty= (float) (atof(p_state.value));
parse_state ++;
}
}
}
mcs = (motor_nims_state_t *) qdev_data(q);
if (parse_state < PS_END)
{
elog( LOG_ERR, "INVALID command or target position\n" );
ret.state = FAIL;
ret.cx = mcs->pos.cx;
ret.cy = mcs->pos.cy;
bufcpy(bin, &ret, sizeof(motor_cmd_return_state_t) );
// bufprintf(print, "Invalid command or target position\n" );
bufprintf(print, "1\n" );
return QUERY_DONE;
}
if (command_type == MOTOR_POS)
{
ret.state = MOTOR_POS_SUC;
mcs->pos.cx = tx;
mcs->pos.cy = ty;
ret.cx = mcs->pos.cx;
ret.cy = mcs->pos.cy;
bufcpy(bin, &ret, sizeof(motor_cmd_return_state_t) );
// bufprintf(print, "DONE. At:(%f, %f)\n", mcs->pos.cx, mcs->pos.cy);
bufprintf(print, "0\n");
return QUERY_DONE;
}
//ok, it is a valid target position value
mcs->pos.tx = tx;
mcs->pos.ty = ty;
cx = mcs->pos.cx;
cy = mcs->pos.cy;
//for now, suppose the motor go in a straight line
dis = sqrt((cx - tx)*(cx - tx) + (cy - ty)*(cy - ty));
vel = mcs->pos.v;
est_t = dis / vel;
if ( gettimeofday( &(mcs->start_time), NULL ) < 0 )
elog( LOG_ERR, "Wierd gettimeofday() error!\n");
elog( LOG_NOTICE, "v = %f, est time = %f\n", vel, est_t );
// the timer value is in mili sec.
g_timer_add( est_t * 1000, motor_complete_process, q, NULL, &(mcs->motor_timer));
return QUERY_NO_REPLY;
}
See more files for this project here