motor_sensor_dev.c from EmStar at Krugle
Show motor_sensor_dev.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.
*
*/
/*
* Simulated Motor Controller Driver
*
* This device driver provides a qeury device that used by clients
* to control the motor; and a status device which is used to
* monitor the state of the motor, e.g., it can periodically
* update the motor's current position information
*
*/
#include "motors/motor_nims.h"
#include "sim_sensor_nims.h"
#include "data_replay.h"
#include "mote_nims.h"
#define ADD_MOTE_DEVICE
/***************************
*
* Driving the motor...
*
*/
/* the time difference in micro-seconds of (t2 - t1) */
long usec_diff(struct timeval * t1, struct timeval * t2)
{
long tmp = (t2->tv_sec - t1->tv_sec) * 1000000 ;
tmp += (t2->tv_usec - t1->tv_usec);
return tmp;
}
/*
* This generates HUMAN-readable output that describes the current
* state of the motor
*/
int motor_busy(motor_nims_state_t *mcs)
{
return g_timer_isset(mcs->motor_timer);
}
int motor_status_print(status_context_t *info, buf_t *buf)
{
motor_nims_state_t *mcs = (motor_nims_state_t *) sd_data(info);
float cx = mcs->pos.cx;
float cy = mcs->pos.cy;
float tx = mcs->pos.tx;
float ty = mcs->pos.ty;
float px, py, dis, cur_dis;
struct timeval tv;
long usec_time;
if ((cx == tx) && (cy == ty))
{
px = tx;
py = ty;
} else {
dis = sqrt((cx - tx)*(cx - tx) + (cy - ty)*(cy - ty));
if ( gettimeofday( &tv, NULL ) < 0 )
elog( LOG_NOTICE, "Wierd gettimeofday() error!\n");
usec_time = usec_diff( &(mcs->start_time), &tv );
cur_dis = mcs->pos.v * usec_time * 1.0 / 1000000 ;
if ( cur_dis < dis )
{
px = (tx - cx) * cur_dis / dis + cx;
py = (ty - cy) * cur_dis / dis + cy;
} else {
px = tx;
py = ty;
}
}
/*
bufprintf(buf,
"# Motor, position(m), operating velocity (m/s)\n"
"(%.2f, %.2f) %.2f\n",
px, py,
motor_busy(mcs) ? mcs->pos.v : 0 );
*/
bufprintf(buf,
"# Motor, position(m)\n"
"(%.2f, %.2f)\n",
cx, cy );
return STATUS_MSG_COMPLETE;
}
static
int motor_generate_report(void *data, int interval, g_event_t *event)
{
motor_nims_state_t *mcs = (motor_nims_state_t *) data;
//printf( "GENERATING REPORT\n" );
/* notify client */
g_status_dev_notify(mcs->status_ref);
return TIMER_RENEW;
}
/***********************/
static void motor_shutdown(void *data)
{
elog(LOG_NOTICE, "Motor controller service shutting down...");
exit(0);
}
void enable_binary_mode(motor_nims_state_t * mcs )
{
mcs->binary_mode_ = 1;
}
int main(int argc, char *argv[])
{
/*
* Declare our state block and initialize it to 0.
*
* Most emstar structures and types are correctly initialized when zeroed,
* and continuing this policy is highly encouraged. We also always use the
* gcc per-field structure initialization extension. This approach makes it
* easy to safely extend structures with optional fields, because any fields
* that are not explicitly mentioned will be zeroed. Making 0 values a
* valid initialization can mean you don't have to track down every place
* a struct is inialized to make sure that it will be well-formed.
*
* Following this principle, ID's usually use 0 to mean not present (e.g.
* node and interface IDs are non-0), and pointers are initialized to zero
* to indicate no memory allocated.
*/
motor_nims_state_t mcs = {};
/* emrun will trigger this callback to run on shutdown */
emrun_opts_t emrun_opts = {
shutdown: motor_shutdown, /* this function implements our shutdown handler */
data: &mcs /* this pointer will be passed to our shutdown handler */
};
//create query device
query_dev_opts_t opts = {
device: {
devname: NIMS_MOTOR_CONTROLLER_DEV,
device_info: &mcs
},
usage: motor_usage,
process: motor_process
};
#ifdef ADD_MOTE_DEVICE
//create query device
mote_nims_state_t nims_mote;
// nims_mote.serial_fd = nims_sensor.input_stream;
packet_dev_opts_t mote_opts = {
device: {
devname: NIMS_MOTE_CONTROLLER_DEV,
device_info: &nims_mote
},
send: mote_process,
// unparse: unparse_cb,
enable_debug_device: 1
};
/*
query_dev_opts_t mote_opts = {
device: {
devname: NIMS_MOTE_CONTROLLER_DEV,
device_info: &nims_mote
},
process: mote_process
};
*/
#endif
#ifdef ADD_SENSOR_DEVICE
//create sensor device BEGIN
sdev_context_t* ctx = NULL;
sensor_replay_dev_t *sde = g_new0(sensor_replay_dev_t,1);
replay_Opts_t rbO;
sdev_opts_t sensor_opts = {
device: {
devname: SENSORDEV,
destroy: sdev_sim_destroy,
device_info: sde
},
open: sdev_sim_open,
close: sdev_sim_close,
usage: sdev_sim_usage
};
/* generic init, CAN I GET AWAY WITH iT */
misc_init(&argc, argv, CVSTAG);
parse_sensor_options(argc,argv,&rbO);
sensor_opts.r_opts.sample_size = rbO.sampleSz;
sensor_opts.r_opts.num_samples = rbO.size;
sde->mns = &mcs;
sde->sensor_data_fname = rbO.sensor_data_fname;
// sde->data records the replayed sensor data
sde->data = read_input( sde->sensor_data_fname );
if (sdev_new(&sensor_opts, &ctx) < 0) {
elog(LOG_ERR,"FAILURE to register sensor device.\n");
return -1;
}
sde->sdev = ctx;
sde->startSample = 0;
if (rbO.pushRate == 0) {
//default sampling rate is 1 sample per second.
rbO.pushRate = 1000;
}
elog( LOG_INFO, "Sample rate: %f\n", rbO.pushRate);
g_timer_add(rbO.pushRate, sdevNonisoWrite, sde, NULL, NULL);
//create sensor device END
#endif
//some initialization
//?? set speed of the motor, maybe it should be set by the command line, or a command handled by the query device.
mcs.pos.v = 10000;
enable_binary_mode( &mcs );
mcs.binary_mode_ = 1;
#ifndef ADD_SENSOR_DEVICE
misc_init(&argc, argv, CVSTAG);
#endif
/*
* Create our device interface.
*
* We specify the name of the device in devname, and our state block
* as the device_info pointer (it will be passed back to us in our
* callback functions.
*
* Then, we define callback functions to provide responses
* in printable and binary form, and a callback to handle commands
* written by the client.
*
* Whenever a client tries to read, the appropriate handler will be
* called to generate a response.
*
* We save a reference to the device so that we can trigger notification
* when the position changes.
*
*/
{
status_dev_opts_t s_opts = {
device: {
devname: "/dev/motor/status",
device_info: &mcs
},
printable: motor_status_print,
};
if (g_status_dev(&s_opts, &(mcs.status_ref)) < 0) {
elog(LOG_CRIT, "Unable to create status device: %m");
exit(1);
}
}
/*
* add a timer to generate periodic reports to monitor motor status
**/
if (g_timer_add(1000, motor_generate_report, &mcs, NULL, &(mcs.report_timer)) < 0) {
elog(LOG_CRIT, "Failed to install timer: %m");
}
//create query device
if (query_dev_new(&opts, NULL) < 0)
elog( LOG_ERR, "unable to create query device\n");
/*
* Register with emrun. Call this function only after all initialization is
* successfully completed.
*/
/*
nims_mote.query_ref = NULL;
if (query_dev_new(&mote_opts, &(nims_mote.query_ref)) < 0)
elog( LOG_ERR, "unable to create query device for mote\n");
elog( LOG_ERR, "SUC in create query device for mote\n");
*/
nims_mote.pd_ref = NULL;
if (g_packet_dev(& mote_opts, &(nims_mote.pd_ref) ) < 0)
elog( LOG_ERR, "SUC in create packet device for mote\n");
emrun_init(&emrun_opts);
/*
* Go! In an emstar application, all activities are triggered from the
* event loop. So after you have set up all your events and defined
* all the handlers, your program enters the event loop, which will
* control the subsequent control flow of your program. The "g_main()"
* function is entered and never returns.
*/
g_main();
/* this should never be reached */
elog(LOG_ALERT, "event system terminated abnormally");
return 1;
}
See more files for this project here