1 /*
2 *
3 * Copyright (c) 2003 The Regents of the University of California. All
4 * rights reserved.
5 *
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted provided that the following conditions
8 * are met:
9 *
10 * - Redistributions of source code must retain the above copyright
11 * notice, this list of conditions and the following disclaimer.
12 *
13 * - Neither the name of the University nor the names of its
14 * contributors may be used to endorse or promote products derived
15 * from this software without specific prior written permission.
16 *
17 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS''
18 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
19 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
20 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
21 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
22 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
23 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
24 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
25 * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 *
29 */
30
31
32 #include "mobility_control.h"
33 #include "emrun/emsim.h"
34
35 int pt_to_pt( dc_motor_global_info_t *mcs, double x0, double y0, double x1, double y1 );
36 int send_modify_cmd(dc_motor_global_info_t * mcs);
37
38 void motor_usage(query_context_t *q, buf_t *buf)
39 {
40 bufprintf(buf, "command to move motor to a position (m, n) is move:x=[m]:y=[n]\n"
41 "command to set motor position is pos:x=[m]:y=[n]\n"
42 "command to set motor speed is set-speed:s=[speed]\n");
43 }
44
45 /*
46 * This generates HUMAN-readable output that describes the current
47 * state of the motor
48 */
49 int motor_status_print(status_context_t *info, buf_t *buf)
50 {
51 dc_motor_global_info_t *mcs = (dc_motor_global_info_t *) sd_data(info);
52
53 send_modify_cmd(mcs);
54
55 bufprintf(buf, "Motor is currently at (%d, %d), %s\n", (int) rint(mcs->c_pos_x), (int) rint(mcs->c_pos_y),
56 (mcs->busy_ == MOTORS_ACTIVE) ? "moving" : "still" );
57
58 elog(LOG_ERR, "Motor is currently at (%d, %d), %s\n", (int) rint(mcs->c_pos_x), (int) rint(mcs->c_pos_y),
59 (mcs->busy_ == MOTORS_ACTIVE) ? "moving" : "still" );
60 return STATUS_MSG_COMPLETE;
61 }
62
63 int motor_status_binary(status_context_t *info, buf_t *buf)
64 {
65 dc_motor_global_info_t *mcs = (dc_motor_global_info_t *) sd_data(info);
66 motor_cmd_return_state_t ret;
67
68 send_modify_cmd(mcs);
69 elog(LOG_ERR, "Motor is currently at (%d, %d)\n", (int) rint(mcs->c_pos_x), (int) rint(mcs->c_pos_y) );
70
71 ret.state= 0;
72 ret.cx = (int) rint(mcs->c_pos_x);
73 ret.cy = (int) rint(mcs->c_pos_y);
74 bufcpy(buf, &ret, sizeof(ret));
75 return STATUS_MSG_COMPLETE;
76 }
77
78 /***********************/
79
80 void motor_shutdown(void *data)
81 {
82 elog(LOG_NOTICE, "Motor controller service shutting down...");
83 exit(0);
84 }
85
86 static
87 void enable_binary_mode(dc_motor_global_info_t * mcs)
88 {
89 mcs->binary_mode_ = 1;
90 }
91
92 int main(int argc, char** argv)
93 {
94 dc_motor_global_info_t mcs = {};
95
96 /* emrun will trigger this callback to run on shutdown */
97 emrun_opts_t emrun_opts = {
98 shutdown: motor_shutdown,
99 data: &mcs
100 };
101
102 char * key;
103
104 if ((key = misc_parse_out_option(&argc, argv, "speed", 's'))) {
105 mcs.motion_info.ver_motor_speed = atoi(key);
106 mcs.motion_info.hor_motor_speed = mcs.motion_info.ver_motor_speed;
107 }
108 else {
109 elog(LOG_ERR,"No motor speed is specified for the motor driver!\n");
110 exit(1);
111 }
112
113 if ((key = misc_parse_out_option(&argc, argv, "posx", 'x'))) {
114 mcs.c_pos_x = atof(key);
115 }
116 else {
117 elog(LOG_ERR,"No position x is initialized for the motor driver!\n");
118 exit(1);
119 }
120
121 if ((key = misc_parse_out_option(&argc, argv, "posy", 'y'))) {
122 mcs.c_pos_y = atof(key);
123 }
124 else {
125 elog(LOG_ERR,"No position y is initialized for the motor driver!\n");
126 exit(1);
127 }
128
129 enable_binary_mode( &mcs );
130 /* generic init */
131 misc_init(&argc, argv, CVSTAG);
132
133 {
134 status_dev_opts_t s_opts = {
135 device: {
136 devname: NIMS_MOTOR_STATUS_DEVICE,
137 device_info: &mcs
138 },
139 printable: motor_status_print,
140 binary: motor_status_binary
141 };
142
143 if (g_status_dev(&s_opts, &(mcs.status_ref)) < 0) {
144 elog(LOG_CRIT, "Unable to create status device: %m");
145 exit(1);
146 }
147 }
148
149 {
150 // create query device for motor command
151 query_dev_opts_t opts = {
152 device: {
153 devname: NIMS_MOTOR_CONTROLLER_DEV,
154 device_info: &mcs
155 },
156 usage: motor_usage,
157 process: motor_process
158 };
159 //create query device
160 if (query_dev_new(&opts, &(mcs.query_ref)) < 0)
161 elog( LOG_ERR, "unable to create query device for motor\n");
162 }
163
164 emrun_init(&emrun_opts);
165
166 /* make sure we're running inside the simulator before we actually start */
167 // if (!sim_is_component()) {
168 // elog(LOG_CRIT, "this program should not be run standalone");
169 // elog(LOG_CRIT, "it should be specified in your emsim config file as a sim-component");
170 // exit(1);
171 // }
172
173 //FIXME: command line input alg. option
174 mcs.motion_info.motion_algorithm = ALG0;
175 mcs.busy_ = MOTORS_IDLE;
176
177 g_main();
178 elog(LOG_ALERT, "event system terminated abnormally");
179 return 1;
180 }
181
182 int motor_process(query_context_t *q, char *command, size_t size,
183 buf_t *print, buf_t *bin)
184 {
185 double tx = -1;
186 double ty = -1;
187 parser_state_t p_state = {
188 input: command,
189 input_len: size
190 };
191
192 int tx_para = false;
193 int ty_para = false;
194
195 int vs_set = false;
196 int vs;
197
198 dc_motor_global_info_t *mcs= (dc_motor_global_info_t *) qdev_data(q);
199
200 double cx, cy;
201 motor_cmd_return_state_t ret;
202 motor_command_t command_type=-1;
203
204 elog( LOG_NOTICE, "motor_process called, '%s'\n", command);
205 if (mcs->flog != NULL)
206 {
207 fprintf( mcs->flog, "motor_process called, '%s'\n", command);
208 fflush( mcs->flog );
209 }
210
211 while (misc_parse_next_kvp(&p_state) >= 0) {
212
213 /* check for the key 'move' */
214 if (strcmp( MOTOR_MOVE_CMD, p_state.key) == 0)
215 command_type = MOTOR_MOVE;
216 else if (strcmp( MOTOR_SET_POSITION_CMD, p_state.key) == 0)
217 command_type = MOTOR_POS;
218 else if (strcmp( MOTOR_SET_VELOCITY_CMD, p_state.key) == 0)
219 command_type = MOTOR_SET_VELOCITY;
220
221 /* check for the key 'x' */
222 if (strcmp("x", p_state.key) == 0) {
223
224 /* get the x coordinate from the value */
225 if (p_state.value) {
226 tx= (atof(p_state.value));
227 tx_para = true;
228 }
229
230 }
231
232 /* check for the key 'y' */
233 if ( strcmp("y", p_state.key) == 0) {
234
235 /* get the y coordinate from the value */
236 if (p_state.value) {
237 ty= (atof(p_state.value));
238 ty_para = true;
239 }
240
241 }
242
243 if (strcmp("s", p_state.key) == 0) {
244 if (p_state.value) {
245 vs= atoi(p_state.value);
246 vs_set = true;
247 }
248 }
249 }
250
251 if (command_type == MOTOR_SET_VELOCITY)
252 {
253 if ( vs_set )
254 {
255 mcs->motion_info.ver_motor_speed =vs;
256 mcs->motion_info.hor_motor_speed =vs;
257 }
258
259 bufprintf(print, "setup SUC\n");
260 return QUERY_DONE;
261 }
262
263 ret.cx = (int) rint(mcs->c_pos_x);
264 ret.cy = (int) rint(mcs->c_pos_y);
265
266 if ( (! tx_para) || (! ty_para) || ((command_type != MOTOR_POS) && (command_type != MOTOR_MOVE)))
267 {
268 elog( LOG_ERR, "INVALID command or target position\n" );
269 if (mcs->flog != NULL)
270 fprintf( mcs->flog, "INVALID command or target position\n" );
271 ret.state = MOTOR_FAIL;
272
273 bufcpy(bin, &ret, sizeof(motor_cmd_return_state_t) );
274 // bufprintf(print, "Invalid command or target position\n" );
275 bufprintf(print, "1\n");
276 return QUERY_DONE;
277 }
278
279 if (command_type == MOTOR_POS)
280 {
281 mcs->c_pos_x = tx;
282 mcs->c_pos_y = ty;
283 bufprintf(print, "setup SUC\n");
284 g_status_dev_notify(mcs->status_ref);
285 return QUERY_DONE;
286 }
287
288 //ok, it is a valid target position value
289 mcs->d_pos_x = tx;
290 mcs->d_pos_y = ty;
291
292 cx = mcs->c_pos_x;
293 cy = mcs->c_pos_y;
294
295 if ((cx==tx) && (cy == ty))
296 {
297 elog( LOG_WARNING, "Motor is already in target position\n" );
298 if (mcs->flog != NULL)
299 fprintf( mcs->flog, "Motor is already in target position\n" );
300 ret.state = MOTOR_NO_NEED_TO_MOVE;
301
302 bufcpy(bin, &ret, sizeof(motor_cmd_return_state_t) );
303 bufprintf(print, "Motor reach target position\n" );
304
305 return QUERY_DONE;
306 }
307
308 mcs->p_pos_x = mcs->c_pos_x;
309 mcs->p_pos_y = mcs->c_pos_y;
310
311 elog( LOG_WARNING, "Send move command to the motor\n" );
312
313 if ( pt_to_pt( mcs, mcs->p_pos_x, mcs->p_pos_y, mcs->d_pos_x, mcs->d_pos_y ) )
314 {
315 elog( LOG_ERR, "target position out of boundary\n" );
316 if (mcs->flog != NULL)
317 fprintf( mcs->flog, "target position out of boundary\n" );
318 ret.state = MOTOR_FAIL;
319
320 bufcpy(bin, &ret, sizeof(motor_cmd_return_state_t) );
321 bufprintf(print, "target position out of boundary\n");
322 return QUERY_DONE;
323 }
324
325 return QUERY_NO_REPLY;
326 }
327
328 static
329 int motor_reply(void *data, int interval, g_event_t *event)
330 {
331 dc_motor_global_info_t *mcs = (dc_motor_global_info_t *) data;
332 motor_cmd_return_state_t motor_move_ret;
333
334 buf_t *bin = qdev_curr_bin(mcs->query_ref);
335 buf_t *print = qdev_curr_print(mcs->query_ref);
336
337 mcs->c_pos_x = mcs->d_pos_x;
338 mcs->c_pos_y = mcs->d_pos_y;
339 /*
340 ** trigger status device
341 ** respond to query client.
342 **/
343
344 g_status_dev_notify(mcs->status_ref);
345 mcs->busy_ = MOTORS_IDLE;
346
347 motor_move_ret.state= 0;
348 motor_move_ret.cx = (int) rint(mcs->c_pos_x);
349 motor_move_ret.cy = (int) rint(mcs->c_pos_y);
350
351 buf_init( bin );
352 bufcpy(bin, &motor_move_ret, sizeof(motor_cmd_return_state_t) );
353 bufprintf(print, "Motor reach target position\n" );
354 qdev_reply(mcs->query_ref, QUERY_DONE);
355
356 return TIMER_DONE;
357 }
358
359 int pt_to_pt( dc_motor_global_info_t *mcs, double x0, double y0, double x1, double y1 )
360 {
361 double dis = sqrt( (x0 - x1)*(x0 - x1) + (y0 -y1)*(y0 -y1) );
362 double est_delay = dis / mcs->motion_info.ver_motor_speed;
363
364 mcs->busy_ = MOTORS_ACTIVE;
365
366 //set up a timer of estimated delay.
367 if (g_timer_add( (int)(est_delay * 1000), motor_reply, mcs, NULL, NULL) < 0) {
368 elog(LOG_CRIT, "Failed to install timer: %m");
369 return 1;
370 }
371
372 return 0;
373 }
374
375 /* modify node location */
376 int send_modify_cmd(dc_motor_global_info_t * mcs)
377 {
378 char cmd[256];
379 int mID = my_node_id;
380 FILE * fp = fopen( EMSIM_COMMAND_DEVNAME, "w" );
381
382 sprintf(cmd, "id=%d:x=%lf:y=%lf", mID, mcs->c_pos_x, mcs->c_pos_y);
383
384 if ( fp == NULL )
385 {
386 elog( LOG_ERR, "%s device is NOT there\n", EMSIM_COMMAND_DEVNAME );
387 return 1;
388 }
389
390 fprintf(fp, "%s", cmd );
391
392 fclose( fp );
393 return 0;
394 }
395
This page was automatically generated by the
LXR engine.
Visit the LXR main site for more
information.