Show sim_motor.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.
*
*/
#include "mobility_control.h"
#include "emrun/emsim.h"
int pt_to_pt( dc_motor_global_info_t *mcs, double x0, double y0, double x1, double y1 );
int send_modify_cmd(dc_motor_global_info_t * mcs);
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"
"command to set motor position is pos:x=[m]:y=[n]\n"
"command to set motor speed is set-speed:s=[speed]\n");
}
/*
* This generates HUMAN-readable output that describes the current
* state of the motor
*/
int motor_status_print(status_context_t *info, buf_t *buf)
{
dc_motor_global_info_t *mcs = (dc_motor_global_info_t *) sd_data(info);
send_modify_cmd(mcs);
bufprintf(buf, "Motor is currently at (%d, %d), %s\n", (int) rint(mcs->c_pos_x), (int) rint(mcs->c_pos_y),
(mcs->busy_ == MOTORS_ACTIVE) ? "moving" : "still" );
elog(LOG_ERR, "Motor is currently at (%d, %d), %s\n", (int) rint(mcs->c_pos_x), (int) rint(mcs->c_pos_y),
(mcs->busy_ == MOTORS_ACTIVE) ? "moving" : "still" );
return STATUS_MSG_COMPLETE;
}
int motor_status_binary(status_context_t *info, buf_t *buf)
{
dc_motor_global_info_t *mcs = (dc_motor_global_info_t *) sd_data(info);
motor_cmd_return_state_t ret;
send_modify_cmd(mcs);
elog(LOG_ERR, "Motor is currently at (%d, %d)\n", (int) rint(mcs->c_pos_x), (int) rint(mcs->c_pos_y) );
ret.state= 0;
ret.cx = (int) rint(mcs->c_pos_x);
ret.cy = (int) rint(mcs->c_pos_y);
bufcpy(buf, &ret, sizeof(ret));
return STATUS_MSG_COMPLETE;
}
/***********************/
void motor_shutdown(void *data)
{
elog(LOG_NOTICE, "Motor controller service shutting down...");
exit(0);
}
static
void enable_binary_mode(dc_motor_global_info_t * mcs)
{
mcs->binary_mode_ = 1;
}
int main(int argc, char** argv)
{
dc_motor_global_info_t mcs = {};
/* emrun will trigger this callback to run on shutdown */
emrun_opts_t emrun_opts = {
shutdown: motor_shutdown,
data: &mcs
};
char * key;
if ((key = misc_parse_out_option(&argc, argv, "speed", 's'))) {
mcs.motion_info.ver_motor_speed = atoi(key);
mcs.motion_info.hor_motor_speed = mcs.motion_info.ver_motor_speed;
}
else {
elog(LOG_ERR,"No motor speed is specified for the motor driver!\n");
exit(1);
}
if ((key = misc_parse_out_option(&argc, argv, "posx", 'x'))) {
mcs.c_pos_x = atof(key);
}
else {
elog(LOG_ERR,"No position x is initialized for the motor driver!\n");
exit(1);
}
if ((key = misc_parse_out_option(&argc, argv, "posy", 'y'))) {
mcs.c_pos_y = atof(key);
}
else {
elog(LOG_ERR,"No position y is initialized for the motor driver!\n");
exit(1);
}
enable_binary_mode( &mcs );
/* generic init */
misc_init(&argc, argv, CVSTAG);
{
status_dev_opts_t s_opts = {
device: {
devname: NIMS_MOTOR_STATUS_DEVICE,
device_info: &mcs
},
printable: motor_status_print,
binary: motor_status_binary
};
if (g_status_dev(&s_opts, &(mcs.status_ref)) < 0) {
elog(LOG_CRIT, "Unable to create status device: %m");
exit(1);
}
}
{
// create query device for motor command
query_dev_opts_t opts = {
device: {
devname: NIMS_MOTOR_CONTROLLER_DEV,
device_info: &mcs
},
usage: motor_usage,
process: motor_process
};
//create query device
if (query_dev_new(&opts, &(mcs.query_ref)) < 0)
elog( LOG_ERR, "unable to create query device for motor\n");
}
emrun_init(&emrun_opts);
/* make sure we're running inside the simulator before we actually start */
// if (!sim_is_component()) {
// elog(LOG_CRIT, "this program should not be run standalone");
// elog(LOG_CRIT, "it should be specified in your emsim config file as a sim-component");
// exit(1);
// }
//FIXME: command line input alg. option
mcs.motion_info.motion_algorithm = ALG0;
mcs.busy_ = MOTORS_IDLE;
g_main();
elog(LOG_ALERT, "event system terminated abnormally");
return 1;
}
int motor_process(query_context_t *q, char *command, size_t size,
buf_t *print, buf_t *bin)
{
double tx = -1;
double ty = -1;
parser_state_t p_state = {
input: command,
input_len: size
};
int tx_para = false;
int ty_para = false;
int vs_set = false;
int vs;
dc_motor_global_info_t *mcs= (dc_motor_global_info_t *) qdev_data(q);
double cx, cy;
motor_cmd_return_state_t ret;
motor_command_t command_type=-1;
elog( LOG_NOTICE, "motor_process called, '%s'\n", command);
if (mcs->flog != NULL)
{
fprintf( mcs->flog, "motor_process called, '%s'\n", command);
fflush( mcs->flog );
}
while (misc_parse_next_kvp(&p_state) >= 0) {
/* check for the key 'move' */
if (strcmp( MOTOR_MOVE_CMD, p_state.key) == 0)
command_type = MOTOR_MOVE;
else if (strcmp( MOTOR_SET_POSITION_CMD, p_state.key) == 0)
command_type = MOTOR_POS;
else if (strcmp( MOTOR_SET_VELOCITY_CMD, p_state.key) == 0)
command_type = MOTOR_SET_VELOCITY;
/* check for the key 'x' */
if (strcmp("x", p_state.key) == 0) {
/* get the x coordinate from the value */
if (p_state.value) {
tx= (atof(p_state.value));
tx_para = true;
}
}
/* check for the key 'y' */
if ( strcmp("y", p_state.key) == 0) {
/* get the y coordinate from the value */
if (p_state.value) {
ty= (atof(p_state.value));
ty_para = true;
}
}
if (strcmp("s", p_state.key) == 0) {
if (p_state.value) {
vs= atoi(p_state.value);
vs_set = true;
}
}
}
if (command_type == MOTOR_SET_VELOCITY)
{
if ( vs_set )
{
mcs->motion_info.ver_motor_speed =vs;
mcs->motion_info.hor_motor_speed =vs;
}
bufprintf(print, "setup SUC\n");
return QUERY_DONE;
}
ret.cx = (int) rint(mcs->c_pos_x);
ret.cy = (int) rint(mcs->c_pos_y);
if ( (! tx_para) || (! ty_para) || ((command_type != MOTOR_POS) && (command_type != MOTOR_MOVE)))
{
elog( LOG_ERR, "INVALID command or target position\n" );
if (mcs->flog != NULL)
fprintf( mcs->flog, "INVALID command or target position\n" );
ret.state = MOTOR_FAIL;
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)
{
mcs->c_pos_x = tx;
mcs->c_pos_y = ty;
bufprintf(print, "setup SUC\n");
g_status_dev_notify(mcs->status_ref);
return QUERY_DONE;
}
//ok, it is a valid target position value
mcs->d_pos_x = tx;
mcs->d_pos_y = ty;
cx = mcs->c_pos_x;
cy = mcs->c_pos_y;
if ((cx==tx) && (cy == ty))
{
elog( LOG_WARNING, "Motor is already in target position\n" );
if (mcs->flog != NULL)
fprintf( mcs->flog, "Motor is already in target position\n" );
ret.state = MOTOR_NO_NEED_TO_MOVE;
bufcpy(bin, &ret, sizeof(motor_cmd_return_state_t) );
bufprintf(print, "Motor reach target position\n" );
return QUERY_DONE;
}
mcs->p_pos_x = mcs->c_pos_x;
mcs->p_pos_y = mcs->c_pos_y;
elog( LOG_WARNING, "Send move command to the motor\n" );
if ( pt_to_pt( mcs, mcs->p_pos_x, mcs->p_pos_y, mcs->d_pos_x, mcs->d_pos_y ) )
{
elog( LOG_ERR, "target position out of boundary\n" );
if (mcs->flog != NULL)
fprintf( mcs->flog, "target position out of boundary\n" );
ret.state = MOTOR_FAIL;
bufcpy(bin, &ret, sizeof(motor_cmd_return_state_t) );
bufprintf(print, "target position out of boundary\n");
return QUERY_DONE;
}
return QUERY_NO_REPLY;
}
static
int motor_reply(void *data, int interval, g_event_t *event)
{
dc_motor_global_info_t *mcs = (dc_motor_global_info_t *) data;
motor_cmd_return_state_t motor_move_ret;
buf_t *bin = qdev_curr_bin(mcs->query_ref);
buf_t *print = qdev_curr_print(mcs->query_ref);
mcs->c_pos_x = mcs->d_pos_x;
mcs->c_pos_y = mcs->d_pos_y;
/*
** trigger status device
** respond to query client.
**/
g_status_dev_notify(mcs->status_ref);
mcs->busy_ = MOTORS_IDLE;
motor_move_ret.state= 0;
motor_move_ret.cx = (int) rint(mcs->c_pos_x);
motor_move_ret.cy = (int) rint(mcs->c_pos_y);
buf_init( bin );
bufcpy(bin, &motor_move_ret, sizeof(motor_cmd_return_state_t) );
bufprintf(print, "Motor reach target position\n" );
qdev_reply(mcs->query_ref, QUERY_DONE);
return TIMER_DONE;
}
int pt_to_pt( dc_motor_global_info_t *mcs, double x0, double y0, double x1, double y1 )
{
double dis = sqrt( (x0 - x1)*(x0 - x1) + (y0 -y1)*(y0 -y1) );
double est_delay = dis / mcs->motion_info.ver_motor_speed;
mcs->busy_ = MOTORS_ACTIVE;
//set up a timer of estimated delay.
if (g_timer_add( (int)(est_delay * 1000), motor_reply, mcs, NULL, NULL) < 0) {
elog(LOG_CRIT, "Failed to install timer: %m");
return 1;
}
return 0;
}
/* modify node location */
int send_modify_cmd(dc_motor_global_info_t * mcs)
{
char cmd[256];
int mID = my_node_id;
FILE * fp = fopen( EMSIM_COMMAND_DEVNAME, "w" );
sprintf(cmd, "id=%d:x=%lf:y=%lf", mID, mcs->c_pos_x, mcs->c_pos_y);
if ( fp == NULL )
{
elog( LOG_ERR, "%s device is NOT there\n", EMSIM_COMMAND_DEVNAME );
return 1;
}
fprintf(fp, "%s", cmd );
fclose( fp );
return 0;
}
See more files for this project here