vrpn 07.35
Virtual Reality Peripheral Network
Loading...
Searching...
No Matches
vrpn_IDEA.C
Go to the documentation of this file.
1// vrpn_IDEA.C
2
3// See http://www.haydonkerk.com/LinkClick.aspx?fileticket=LEcwYeRmKVg%3d&tabid=331
4// for the software manual for this device.
5
6#include <stddef.h> // for size_t
7#include <stdio.h> // for fprintf, stderr, sprintf, etc
8#include <string.h> // for NULL, strlen, strchr, etc
9
10#include "vrpn_BaseClass.h" // for ::vrpn_TEXT_ERROR, etc
11#include "vrpn_IDEA.h"
12#include "vrpn_Serial.h"
13#include "vrpn_Shared.h" // for timeval, vrpn_unbuffer, etc
14
15#define VRPN_TIMESTAMP_MEMBER d_timestamp // Configuration required for vrpn_MessageMacros in this class.
16#include "vrpn_MessageMacros.h" // for VRPN_MSG_INFO, VRPN_MSG_WARNING, VRPN_MSG_ERROR
17
18#undef VERBOSE
19
20// Defines the modes in which the device can find itself.
21#define STATUS_RESETTING (-1) // Resetting the device
22#define STATUS_SYNCING (0) // Looking for the first character of report
23#define STATUS_READING (1) // Looking for the rest of the report
24
25#define TIMEOUT_TIME_INTERVAL (2000000L) // max time between reports (usec)
26#define POLL_INTERVAL (1000000L) // time to poll if no response in a while (usec)
27
28// This creates a vrpn_IDEA and sets it to reset mode. It opens
29// the serial device using the code in the vrpn_Serial_Analog constructor.
30
31vrpn_IDEA::vrpn_IDEA (const char * name, vrpn_Connection * c,const char * port
32 , int run_speed_tics_sec
33 , int start_speed_tics_sec
34 , int end_speed_tics_sec
35 , int accel_rate_tics_sec_sec
36 , int decel_rate_tics_sec_sec
37 , int run_current
38 , int hold_current
39 , int accel_current
40 , int decel_current
41 , int delay
42 , int step
43 , int high_limit_index
44 , int low_limit_index
45 , int output_1_setting
46 , int output_2_setting
47 , int output_3_setting
48 , int output_4_setting
49 , double initial_move
50 , double fractional_c_a
51 , double reset_location):
52 vrpn_Serial_Analog(name, c, port, 57600)
53 , vrpn_Analog_Output(name, c)
54 , vrpn_Button_Filter(name, c)
55 , d_bufcount(0)
56 , d_run_speed_tics_sec(run_speed_tics_sec)
57 , d_start_speed_tics_sec(start_speed_tics_sec)
58 , d_end_speed_tics_sec(end_speed_tics_sec)
59 , d_accel_rate_tics_sec_sec(accel_rate_tics_sec_sec)
60 , d_decel_rate_tics_sec_sec(decel_rate_tics_sec_sec)
61 , d_run_current(run_current)
62 , d_hold_current(hold_current)
63 , d_accel_current(accel_current)
64 , d_decel_current(decel_current)
65 , d_delay(delay)
66 , d_step(step)
67 , d_high_limit_index(high_limit_index)
68 , d_low_limit_index(low_limit_index)
69 , d_output_1_setting(output_1_setting)
70 , d_output_2_setting(output_2_setting)
71 , d_output_3_setting(output_3_setting)
72 , d_output_4_setting(output_4_setting)
73 , d_initial_move(initial_move)
74 , d_fractional_c_a(fractional_c_a)
75 , d_reset_location(reset_location)
76{
77 d_last_poll.tv_sec = 0;
78 d_last_poll.tv_usec = 0;
79
83 channel[0] = 0;
84 last[0] = 0;
85 memset(buttons, 0, sizeof(buttons));
86 memset(lastbuttons, 0, sizeof(buttons));
88
89 // Set the mode to reset
91
92 // Register to receive the message to request changes and to receive connection
93 // messages.
94 if (d_connection != NULL) {
96 this, d_sender_id)) {
97 fprintf(stderr,"vrpn_IDEA: can't register handler\n");
98 d_connection = NULL;
99 }
101 this, d_sender_id)) {
102 fprintf(stderr,"vrpn_IDEA: can't register handler\n");
103 d_connection = NULL;
104 }
106 this, d_sender_id)) {
107 fprintf(stderr,"vrpn_IDEA: can't register handler\n");
108 d_connection = NULL;
109 }
110 } else {
111 fprintf(stderr,"vrpn_IDEA: Can't get connection!\n");
112 }
113
114 // Reset the drive.
115 reset();
116}
117
118// Add a newline-termination to the command and then send it.
119bool vrpn_IDEA::send_command(const char *cmd)
120{
121 char buf[128];
122 size_t len = strlen(cmd);
123 if (len > sizeof(buf)-2) { return false; }
124 strcpy(buf, cmd);
125 buf[len] = '\r';
126 buf[len+1] = '\0';
127 return (static_cast<size_t>(vrpn_write_characters(serial_fd,
128 (const unsigned char *)((void*)(buf)), strlen(buf))) == strlen(buf) );
129}
130
131// Helper function to scale int by a double and get an int.
132static inline int scale_int(int val, double scale)
133{
134 return static_cast<int>(val*scale);
135}
136
137// Commands Responses Meanings
138// M None Move to position
139// i None Interrupt configure
140// Params:
141// distance: distance in 1/64th steps
142// run speed: steps/second
143// start speed: steps/second
144// end speed: steps/second
145// accel rate: steps/second/second
146// decel rate: steps/second/second
147// run current: milliamps
148// hold current: milliamps
149// accel current: milliamps
150// decel current: milliamps
151// delay: milliseconds, waiting to drop to hold current
152// step mode: inverse step size: 4 is 1/4 step.
153// scale: Between 0 and 1. Scales the acceleration and currents down.
154
155bool vrpn_IDEA::send_move_request(vrpn_float64 location_in_steps, double scale)
156{
157 char cmd[512];
158
159 //-----------------------------------------------------------------------
160 // Configure input interrupts. We want a rising-edge trigger for the
161 // inputs, calling the appropriate subroutine. We only enable the
162 // high limit switch when moving forward and only the low one when moving
163 // backwards. If neither limit switch is set, we still send a command so
164 // that they will be disabled (in case the motor was programmed differently
165 // before).
166 // XXX The "i" command is only available in program mode, so we need to
167 // write a program that will call the limit routine if needed and then will
168 // execute our particular move, then we call that program. If we're lucky,
169 // we can do the limit subroutine as another program on another page and
170 // call it from here. If not, we'll need to put the whole program including
171 // the subroutine each time -- then we need to figure out where to branch
172 // to. We'll probably need to talk the GUI program into spitting out the
173 // text it is sending to the motor to figure out what that offset is.
174 {
175 int edge_masks[4] = { 0, 0, 0, 0 };
176 int address_masks[4] = { 0, 0, 0, 0 };
177 int priority_masks[4] = { 1, 1, 1, 1 };
178
179 // If we're moving forward and there is a high limit switch, set
180 // up to use it.
181 if ( (location_in_steps > channel[0]) && (d_high_limit_index > 0) ) {
182 edge_masks[d_high_limit_index - 1] = 2; // Rising edge
183 address_masks[d_high_limit_index - 1] = 1024; // Address of program
184 }
185
186 // If we're moving backwards and there is a low limit switch, set
187 // up to use it.
188 if ( (location_in_steps < channel[0]) && (d_low_limit_index > 0) ) {
189 edge_masks[d_low_limit_index - 1] = 2; // Rising edge
190 address_masks[d_low_limit_index - 1] = 2048; // Address of program
191 }
192
193 if (sprintf(cmd, "i%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d",
194 edge_masks[0], edge_masks[1], edge_masks[2], edge_masks[3],
195 address_masks[0], address_masks[1], address_masks[2], address_masks[3],
196 priority_masks[0], priority_masks[1], priority_masks[2], priority_masks[3]
197 ) <= 0) {
198 VRPN_MSG_ERROR("vrpn_IDEA::send_move_request(): Could not configure interrupt command");
200 return false;
201 }
202 if (!send_command(cmd)) {
203 VRPN_MSG_ERROR("vrpn_IDEA::send_move_request(): Could not configure interrupts");
205 return false;
206 }
207 }
208
209 // If we have a high limit and are moving forward, or a low limit and are
210 // moving backwards, then we need to check the appropriate limit switch
211 // before starting the move so that we won't drive once we've passed the
212 // limit.
213 if ( (location_in_steps > channel[0]) && (d_high_limit_index > 0) ) {
214 if (buttons[d_high_limit_index - 1] != 0) {
215 VRPN_MSG_WARNING("vrpn_IDEA::send_move_request(): Asked to move into limit");
216 return true; // Nothing failed, but we're not moving.
217 }
218 }
219 if ( (location_in_steps < channel[0]) && (d_low_limit_index > 0) ) {
220 if (buttons[d_low_limit_index - 1] != 0) {
221 VRPN_MSG_WARNING("vrpn_IDEA::send_move_request(): Asked to move into limit");
222 return true; // Nothing failed, but we're not moving.
223 }
224 }
225
226 // Send the command to move the motor. It may cause an interrupt which
227 // will stop the motion along the way.
228 long steps_64th = static_cast<long>(location_in_steps*64);
229 sprintf(cmd, "M%ld,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d",
230 steps_64th,
232 scale_int(d_start_speed_tics_sec,scale),
233 scale_int(d_end_speed_tics_sec, scale),
234 scale_int(d_accel_rate_tics_sec_sec, scale),
235 scale_int(d_decel_rate_tics_sec_sec, scale),
236 scale_int(d_run_current, scale),
237 scale_int(d_hold_current, scale),
238 scale_int(d_accel_current, scale),
239 scale_int(d_decel_current, scale),
240 d_delay,
241 d_step);
242 return send_command(cmd);
243}
244
245bool vrpn_IDEA::move_until_done_or_error(vrpn_float64 location_in_steps, double scale)
246{
247 // Send a move command, scaled by the fractional current and
248 // acceleration values.
249 if (!send_move_request(location_in_steps, scale)) {
250 VRPN_MSG_ERROR("Could not do move");
251 return false;
252 }
253
254 // Keep asking whether the motor is moving until it says that it
255 // is not.
256 bool moving = true;
257 int ret;
258 unsigned char inbuf[1024];
259 do {
260 if (!send_command("o")) {
261 VRPN_MSG_ERROR("Could not request movement status");
262 return false;
263 }
264
265 struct timeval timeout;
266 timeout.tv_sec = 0;
267 timeout.tv_usec = 30000;
268 ret = vrpn_read_available_characters(serial_fd, inbuf, sizeof(inbuf), &timeout);
269 if (ret < 0) {
270 VRPN_MSG_ERROR("Error reading movement status");
271 return false;
272 }
273 if ( (ret < 8) || (inbuf[ret-1] != '\r') ) {
274 inbuf[ret] = '\0';
275 VRPN_MSG_ERROR("Bad movement status report");
276 return false;
277 }
278 inbuf[ret] = '\0';
279
280 if ( (inbuf[0] != '`') || (inbuf[1] != 'o') ) {
281 VRPN_MSG_ERROR("Bad movement status report");
282 return false;
283 }
284 moving = (inbuf[2] == 'Y');
285 } while (moving);
286
287 return true;
288}
289
290// This routine will parse a location response from the drive.
291// Commands Responses Meanings
292// l `l<value>[cr]`l#[cr] Location of the drive
293
295{
296 // Make sure that the last character is [cr] and that we
297 // have another [cr] in the record somewhere (there should be
298 // two). This makes sure that we have a complete report.
299 char *firstindex = strchr((char*)(buf), '\r');
300 char *lastindex = strrchr((char*)(buf), '\r');
301 if (buf[strlen((char*)(buf))-1] != '\r') { return 0; }
302 if (firstindex == lastindex) { return 0; }
303
304 // See if we can convert the number.
305 int data;
306 if (sscanf((char *)(buf), "`l%d\r`l#\r", &data) != 1) {
307 return -1;
308 }
309
310 // The location of the drive is in 64th-of-a-tick units, so need
311 // to divide by 64 to find the actual location. Store this in our
312 // analog channel.
313 channel[0] = data/64.0;
314 return 1;
315}
316
317// This routine will parse an I/O response from the drive.
318// Commands Responses Meanings
319// l `:<value>[cr]`:#[cr] Bitmask of the in/outputs
320
322{
323 // Make sure that the last character is [cr] and that we
324 // have another [cr] in the record somewhere (there should be
325 // two). This makes sure that we have a complete report.
326 char *firstindex = strchr((char*)(buf), '\r');
327 char *lastindex = strrchr((char*)(buf), '\r');
328 if (buf[strlen((char*)(buf))-1] != '\r') { return 0; }
329 if (firstindex == lastindex) { return 0; }
330
331 // See if we can read the status into an integer.
332 int io_status;
333 if (sscanf((char *)(buf), "`:%d\r`:#\r", &io_status) != 1) {
334 return -1;
335 }
336
337 // Store the results from the first four inputs (low-order bits,
338 // input 1 is lowest) into our buttons.
339 vrpn_Button::buttons[0] = (0 != (io_status & (1 << 0)) );
340 vrpn_Button::buttons[1] = (0 != (io_status & (1 << 1)) );
341 vrpn_Button::buttons[2] = (0 != (io_status & (1 << 2)) );
342 vrpn_Button::buttons[3] = (0 != (io_status & (1 << 3)) );
343
344 // If one of our limit-switch buttons has just toggled on, report this
345 // as a warning.
348 VRPN_MSG_WARNING("Encountered high limit");
349 }
352 VRPN_MSG_WARNING("Encountered low limit");
353 }
354
355 // We got a report.
356 return 1;
357}
358
359// This routine will reset the IDEA drive.
360// Commands Responses Meanings
361// R None Software reset
362// f `f<value>[cr]`f#[cr] Request fault status
363// l `l<value>[cr]`l#[cr] Location of the drive
364// A None Abort
365// P `P[program size][cr]`P#[cr] (or none) Program
366// L None Goto If
367// : `:[value][cr]`:#[cr] Read I/O
368// O None Set output
369// Z None Set position value
370// o `oYES[cr]`o#[cr] (or NO) Is the motor moving?
371
373{
374 struct timeval timeout;
375 unsigned char inbuf[128];
376 char cmd[512];
377 int ret;
378
379 //-----------------------------------------------------------------------
380 // Drain the input buffer to make sure we start with a fresh slate.
381 // Also wait a bit to let things clear out.
382 vrpn_SleepMsecs(250);
384
385 //-----------------------------------------------------------------------
386 // Reset the driver, then wait briefly to let it reset.
387 if (!send_command("R")) {
388 fprintf(stderr,"vrpn_IDEA::reset(): Could not send reset command\n");
389 return -1;
390 }
391 vrpn_SleepMsecs(250);
392
393 //-----------------------------------------------------------------------
394 // Ask for the fault status of the drive. This should cause it to respond
395 // with an "f" followed by a number and a carriage return. We want the
396 // number to be zero.
397 if (!send_command("f")) {
398 fprintf(stderr,"vrpn_IDEA::reset(): Could not request fault status\n");
399 return -1;
400 }
401
402 timeout.tv_sec = 0;
403 timeout.tv_usec = 30000;
404
405 ret = vrpn_read_available_characters(serial_fd, inbuf, sizeof(inbuf), &timeout);
406 if (ret < 0) {
407 perror("vrpn_IDEA::reset(): Error reading fault status from device");
408 return -1;
409 }
410 if ( (ret < 8) || (inbuf[ret-1] != '\r') ) {
411 inbuf[ret] = '\0';
412 fprintf(stderr,"vrpn_IDEA::reset(): Bad fault status report (length %d): %s\n", ret, inbuf);
413 VRPN_MSG_ERROR("Bad fault status report");
414 return -1;
415 }
416 inbuf[ret] = '\0';
417
418 int fault_status;
419 if (sscanf((char *)(inbuf), "`f%d\r`f#\r", &fault_status) != 1) {
420 fprintf(stderr,"vrpn_IDEA::reset(): Bad fault status report: %s\n", inbuf);
421 VRPN_MSG_ERROR("Bad fault status report");
422 return -1;
423 }
424 if (fault_status != 0) {
425 VRPN_MSG_ERROR("Drive reports a fault");
426 return -1;
427 }
428
429 //-----------------------------------------------------------------------
430 // Reset the drive count at the present location to 0, so that we can
431 // know how far we got on our initial move later.
432 if (!send_command("Z0")) {
433 fprintf(stderr,"vrpn_IDEA::reset(): Could not set position to 0\n");
434 return -1;
435 }
436
437 //-----------------------------------------------------------------------
438 // Set the outputs to the desired state. If a setting is -1, then we
439 // don't change the value. If it is 0 or 1 then we set the value to
440 // what was requested.
441 int value = 0;
442 if (d_output_1_setting >= 0) {
443 value |= 1 << (4 + 0);
444 value |= (d_output_1_setting != 0) << (0);
445 }
446 if (d_output_2_setting >= 0) {
447 value |= 1 << (4 + 1);
448 value |= (d_output_2_setting != 0) << (1);
449 }
450 if (d_output_3_setting >= 0) {
451 value |= 1 << (4 + 2);
452 value |= (d_output_3_setting != 0) << (2);
453 }
454 if (d_output_4_setting >= 0) {
455 value |= 1 << (4 + 3);
456 value |= (d_output_4_setting != 0) << (3);
457 }
458 if (sprintf(cmd, "O%d", value) <= 0) {
459 VRPN_MSG_ERROR("vrpn_IDEA::send_output(): Could not configure output command");
461 return -1;
462 }
463 if (!send_command(cmd)) {
464 VRPN_MSG_ERROR("vrpn_IDEA::send_output(): Could not configure outputs");
466 return -1;
467 }
468 //printf("XXX Sending output command: %s\n", cmd);
469 vrpn_SleepMsecs(100);
470
471 //-----------------------------------------------------------------------
472 // Read the input/output values from the drive (debugging).
473 if (!send_command(":")) {
474 fprintf(stderr,"vrpn_IDEA::reset(): Could not request I/O status\n");
475 return -1;
476 }
477
478 timeout.tv_sec = 0;
479 timeout.tv_usec = 30000;
480
481 ret = vrpn_read_available_characters(serial_fd, inbuf, sizeof(inbuf), &timeout);
482 if (ret < 0) {
483 perror("vrpn_IDEA::reset(): Error reading I/O status from device");
484 return -1;
485 }
486 if ( (ret < 8) || (inbuf[ret-1] != '\r') ) {
487 inbuf[ret] = '\0';
488 fprintf(stderr,"vrpn_IDEA::reset(): Bad I/O status report (length %d): %s\n", ret, inbuf);
489 VRPN_MSG_ERROR("Bad I/O status report");
490 return -1;
491 }
492 inbuf[ret] = '\0';
493
494 if (convert_report_to_buttons(inbuf) != 1) {
495 fprintf(stderr,"vrpn_IDEA::reset(): Bad I/O status report: %s\n", inbuf);
496 VRPN_MSG_ERROR("Bad I/O status report");
497 return -1;
498 }
499
500 //-----------------------------------------------------------------------
501 // Ask for the position of the drive and make sure we can read it.
502 if (!send_command("l")) {
503 fprintf(stderr,"vrpn_IDEA::reset(): Could not request position\n");
504 return -1;
505 }
506
507 timeout.tv_sec = 0;
508 timeout.tv_usec = 30000;
509
510 ret = vrpn_read_available_characters(serial_fd, inbuf, sizeof(inbuf), &timeout);
511 if (ret < 0) {
512 perror("vrpn_IDEA::reset(): Error reading position from device");
513 return -1;
514 }
515 if ( (ret < 8) || (inbuf[ret-1] != '\r') ) {
516 inbuf[ret] = '\0';
517 fprintf(stderr,"vrpn_IDEA::reset(): Bad position report: %s\n", inbuf);
518 VRPN_MSG_ERROR("Bad position report");
519 return -1;
520 }
521 inbuf[ret] = '\0';
522
523 if (convert_report_to_position(inbuf) != 1) {
524 fprintf(stderr,"vrpn_IDEA::reset(): Bad position report: %s\n", inbuf);
525 VRPN_MSG_ERROR("Bad position report");
526 return -1;
527 }
528
529 //-----------------------------------------------------------------------
530 // Write the limit-switch subroutine, which should abort a move when
531 // the drive is moving in a positive direction and we get a rising
532 // edge trigger on the associated input. If the index of the input
533 // to use is -1, we aren't using this. The motion command will set
534 // the interrupt handlers appropriately, given the direction of travel.
535 // Here, we write the programs for the high limit switch and the low
536 // limit switch that will be called when they are triggered.
537
538 {
539 // The program name must be exactly ten characters.
540 // The first integer (second parameter) is the page number that the
541 // program starts on. Pages are 1024 bytes long. There are pages
542 // 1-85 available. We put this program on page 1 (which we hope is
543 // address 0).
544 // We put this program at memory location 1024 (must be multiple of 1024).
545 if (!send_command("Pfoundlimit,1,1")) {
546 fprintf(stderr,"vrpn_IDEA::reset(): Could not start limit program\n");
547 return -1;
548 }
549
550 // The program should cause an abort instruction when run
551 if (!send_command("A")) {
552 fprintf(stderr,"vrpn_IDEA::reset(): Could not send abort to limit program\n");
553 return -1;
554 }
555
556 // "Return" instruction from the subroutine back to the main program.
557 if (!send_command("X")) {
558 fprintf(stderr,"vrpn_IDEA::reset(): Could not send return to limit program\n");
559 return -1;
560 }
561
562 // The program description is done
563 if (!send_command("P")) {
564 fprintf(stderr,"vrpn_IDEA::reset(): Could not finish limit program\n");
565 return -1;
566 }
567
568 timeout.tv_sec = 0;
569 timeout.tv_usec = 300000;
570
571 // Get a response saying that the program has been received; it will
572 // say what the size of the program is.
573 ret = vrpn_read_available_characters(serial_fd, inbuf, sizeof(inbuf), &timeout);
574 if (ret < 0) {
575 perror("vrpn_IDEA::reset(): Error reading limit program response from device");
576 return -1;
577 }
578 if ( (ret < 8) || (inbuf[ret-1] != '\r') ) {
579 inbuf[ret] = '\0';
580 fprintf(stderr,"vrpn_IDEA::reset(): Bad limit program response report: %s\n", inbuf);
581 VRPN_MSG_ERROR("Could not write limit program");
582 return -1;
583 }
584 inbuf[ret] = '\0';
585
586 int program_length;
587 if (sscanf((char *)(inbuf), "`P%d\r`P#\r", &program_length) != 1) {
588 fprintf(stderr,"vrpn_IDEA::reset(): Bad limit program report: %s\n", inbuf);
589 VRPN_MSG_ERROR("Bad limit program report");
590 return -1;
591 }
592 }
593
594 //-----------------------------------------------------------------------
595 // If we have an initial-move value (to run into the rails), then execute
596 // a move with acceleration and currents scaled by the fractional_c_a
597 // value used in the constructor. Wait until the motor stops moving
598 // after this command before proceeding.
599
600 if (d_initial_move != 0) {
602 VRPN_MSG_ERROR("Could not do initial move");
603 return -1;
604 }
605 }
606
607 //-----------------------------------------------------------------------
608 // XXX Once the interrupt abort is working...
609 // Ask for the position of the drive and see if we moved to where we
610 // wanted to. If not, report where we ended up.
611 // XXX To make this work, we'll need to write a program to do each
612 // move, which sets the interrupt handlers as needed for the limit
613 // switches (the "i" command is only available in program mode).
614
615 // XXX Until we can fix the limit-switch program to abort a move, we'll
616 // do the homing a different way. If there is a limit switch enabled
617 // in the direction of the initial move, we will repeat that initial
618 // move until the limit switch is engaged. This only works for the
619 // high limit switch; it is a hack until we get the interrupts working.
620 while ( (d_initial_move > 0) && (d_high_limit_index > 0) ) {
621
622 //-----------------------------------------------------------------------
623 // Ask for the position of the drive and parse it.
624 if (!send_command("l")) {
625 fprintf(stderr,"vrpn_IDEA::reset(): Could not request position\n");
626 return -1;
627 }
628
629 timeout.tv_sec = 0;
630 timeout.tv_usec = 30000;
631
632 ret = vrpn_read_available_characters(serial_fd, inbuf, sizeof(inbuf), &timeout);
633 if (ret < 0) {
634 perror("vrpn_IDEA::reset(): Error reading position from device");
635 return -1;
636 }
637 if ( (ret < 8) || (inbuf[ret-1] != '\r') ) {
638 inbuf[ret] = '\0';
639 fprintf(stderr,"vrpn_IDEA::reset(): Bad position report: %s\n", inbuf);
640 VRPN_MSG_ERROR("Bad position report");
641 return -1;
642 }
643 inbuf[ret] = '\0';
644
645 if (convert_report_to_position(inbuf) != 1) {
646 fprintf(stderr,"vrpn_IDEA::reset(): Bad position report: %s\n", inbuf);
647 VRPN_MSG_ERROR("Bad position report");
648 return -1;
649 }
650
651 // Check to see if the limit switch is on. If so, we're done, so we
652 // break out of the loop.
653 if (!send_command(":")) {
654 fprintf(stderr,"vrpn_IDEA::reset(): Could not request I/O status in limit hunt\n");
655 return -1;
656 }
657
658 timeout.tv_sec = 0;
659 timeout.tv_usec = 30000;
660
661 ret = vrpn_read_available_characters(serial_fd, inbuf, sizeof(inbuf), &timeout);
662 if (ret < 0) {
663 perror("vrpn_IDEA::reset(): Error reading I/O status from device in limit hunt");
664 return -1;
665 }
666 if ( (ret < 8) || (inbuf[ret-1] != '\r') ) {
667 inbuf[ret] = '\0';
668 fprintf(stderr,"vrpn_IDEA::reset(): Bad I/O status report (length %d) in limit hunt: %s\n", ret, inbuf);
669 VRPN_MSG_ERROR("Bad I/O status report");
670 return -1;
671 }
672 inbuf[ret] = '\0';
673
674 if (convert_report_to_buttons(inbuf) != 1) {
675 fprintf(stderr,"vrpn_IDEA::reset(): Bad I/O status report in limit hunt: %s\n", inbuf);
676 VRPN_MSG_ERROR("Bad I/O status report");
677 return -1;
678 }
679
680 // Break out if we're done.
682 break;
683 }
684
685 // Issue another move request in the same direction of the same
686 // magnitude as the first one, until we get to the rails.
687 printf("XXX vrpn_IDEA: moving to %lf to find limit\n",
690 VRPN_MSG_ERROR("Could not do limit-hunting move");
691 return -1;
692 }
693 }
694
695 //-----------------------------------------------------------------------
696 // Reset the drive count at the present location to the value set in the
697 // constructor. We need to multiply by 64 to get into microticks.
698 long reset_location = static_cast<long>(64 * d_reset_location);
699 sprintf(cmd, "Z%ld", reset_location);
700 if (!send_command(cmd)) {
701 fprintf(stderr,"vrpn_IDEA::reset(): Could not set position to 1280\n");
702 return -1;
703 }
704
705 //-----------------------------------------------------------------------
706 // Ask for the position of the drive so that it will start sending.
707 // them to us. Each time we finish getting a report, we request
708 // another one.
709 if (!send_command("l")) {
710 fprintf(stderr,"vrpn_IDEA::reset(): Could not request position\n");
711 return -1;
712 }
713
714 // We're now waiting for any responses from devices
715 VRPN_MSG_WARNING("reset complete (this is good)");
716 vrpn_gettimeofday(&d_timestamp, NULL); // Set watchdog now
718 return 0;
719}
720
721// This function will read characters until it has a full report, then
722// put that report into analog fields and call the report methods on these.
723// The time stored is that of the first character received as part of the
724// report.
725
727{
728 int ret; // Return value from function call to be checked
729
730 //--------------------------------------------------------------------
731 // If we're SYNCing, then the next character we get should be the start
732 // of a report. If we recognize it, go into READing mode and tell how
733 // many characters we expect total. If we don't recognize it, then we
734 // must have misinterpreted a command or something; clear the buffer
735 // and try another read after sending a warning in the hope that this
736 // is a one-shot glitch. If it persists, we'll eventually timeout and
737 // reset.
738 //--------------------------------------------------------------------
739
740 if (status == STATUS_SYNCING) {
741 // Try to get a character. If none, just return.
743 return 0;
744 }
745
746 // Make sure that the first character is a single back-quote. If
747 // not, we need to flush the buffer and then send another
748 // request for reading (probably dropped a character).
749 if (d_buffer[0] != '`') {
750 char msg[256];
751 sprintf(msg, "Bad character (got %c, expected `), re-syncing", d_buffer[0]);
752 VRPN_MSG_WARNING(msg);
753 vrpn_SleepMsecs(10);
755 if (!send_command("l")) {
756 VRPN_MSG_ERROR("Could not send position request in re-sync, resetting");
758 return 0;
759 }
760 return 0;
761 }
762
763 // Got the first character of a report -- go into READING mode
764 // and record that we got one character at this time. The next
765 // bit of code will attempt to read the rest of the report.
766 // The time stored here is as close as possible to when the
767 // report was generated.
768 d_bufcount = 1;
771#ifdef VERBOSE
772 printf("... Got the 1st char\n");
773#endif
774 }
775
776 //--------------------------------------------------------------------
777 // Read as many bytes of this report as we can, storing them
778 // in the buffer. We keep track of how many have been read so far
779 // and only try to read the rest.
780 //--------------------------------------------------------------------
781
783 sizeof(d_buffer)-d_bufcount);
784 if (ret == -1) {
785 VRPN_MSG_ERROR("Error reading");
787 return 0;
788 }
789 d_bufcount += ret;
790#ifdef VERBOSE
791 if (ret != 0) printf("... got %d characters (%d total)\n",ret, d_bufcount);
792#endif
793
794 //--------------------------------------------------------------------
795 // See if we can parse this report as position. If so, it is stored into
796 // our analog channel and we request an I/O report next (we toggle back
797 // and forth between requesting position and requesting I/O values).
798 //--------------------------------------------------------------------
799
800 d_buffer[d_bufcount] = '\0';
801 int pos_ret;
803 if (pos_ret == 1) {
804
805 //--------------------------------------------------------------------
806 // Request an I/O report so we can keep them coming.
807 //--------------------------------------------------------------------
808
809#ifdef VERBOSE
810 printf("got a complete report (%d)!\n", d_bufcount);
811#endif
812 if (!send_command(":")) {
813 VRPN_MSG_ERROR("Could not send I/O request, resetting");
815 return 0;
816 }
817
818 //--------------------------------------------------------------------
819 // Done with the decoding, send the reports and go back to syncing
820 //--------------------------------------------------------------------
821
824 d_bufcount = 0;
825
826 return 1;
827 }
828
829 //--------------------------------------------------------------------
830 // See if we can parse this report as I/O. If so, it is stored into
831 // our button channels and we request a position report next (we toggle back
832 // and forth between requesting position and requesting I/O values).
833 //--------------------------------------------------------------------
834
835 d_buffer[d_bufcount] = '\0';
836 int but_ret;
838 if (but_ret == 1) {
839
840 //--------------------------------------------------------------------
841 // Request a position report so we can keep them coming.
842 //--------------------------------------------------------------------
843
844#ifdef VERBOSE
845 printf("got a complete report (%d)!\n", d_bufcount);
846#endif
847 if (!send_command("l")) {
848 VRPN_MSG_ERROR("Could not send position request, resetting");
850 return 0;
851 }
852
853 //--------------------------------------------------------------------
854 // Done with the decoding, send the reports and go back to syncing
855 //--------------------------------------------------------------------
856
859 d_bufcount = 0;
860
861 return 1;
862 }
863
864 // If we got an error for both types of reports, trouble!
865 // Flush things and ask for a new position report.
866 if ( (pos_ret == -1) && (but_ret == -1) ) {
867 // Error during parsing, maybe we got off by a half-report.
868 // Try clearing the input buffer and re-requesting a report.
869 VRPN_MSG_WARNING("Flushing input and requesting new position report");
871 d_bufcount = 0;
872 vrpn_SleepMsecs(10);
874 if (!send_command("l")) {
875 VRPN_MSG_ERROR("Could not send position request in convert failure, resetting");
877 return 0;
878 }
879 }
880
881 // We've not gotten a report, nor an error.
882 return 0;
883}
884
886{
887 const char *bufptr = p.buffer;
888 vrpn_int32 chan_num;
889 vrpn_int32 pad;
890 vrpn_float64 value;
892
893 // Read the parameters from the buffer
894 vrpn_unbuffer(&bufptr, &chan_num);
895 vrpn_unbuffer(&bufptr, &pad);
896 vrpn_unbuffer(&bufptr, &value);
897
898 // Set the position to the appropriate value, if the channel number is in the
899 // range of the ones we have.
900 if ( (chan_num < 0) || (chan_num >= me->o_num_channel) ) {
901 char msg[1024];
902 sprintf(msg,"vrpn_IDEA::handle_request_message(): Index out of bounds (%d of %d), value %lg\n",
903 chan_num, me->num_channel, value);
905 return 0;
906 }
907 // This will get set when we read from the motoer me->channel[chan_num] = value;
908 me->send_move_request(value);
909
910 return 0;
911}
912
914{
915 int i;
916 const char* bufptr = p.buffer;
917 vrpn_int32 num;
918 vrpn_int32 pad;
920
921 // Read the values from the buffer
922 vrpn_unbuffer(&bufptr, &num);
923 vrpn_unbuffer(&bufptr, &pad);
924 if (num > me->o_num_channel) {
925 char msg[1024];
926 sprintf(msg,"vrpn_IDEA::handle_request_channels_message(): Index out of bounds (%d of %d), clipping\n",
927 num, me->o_num_channel);
929 num = me->o_num_channel;
930 }
931 for (i = 0; i < num; i++) {
932 vrpn_unbuffer(&bufptr, &(me->o_channel[i]));
933 me->send_move_request(me->o_channel[i]);
934 }
935
936 return 0;
937}
938
942{
944
946 return 0;
947}
948
949void vrpn_IDEA::report_changes(vrpn_uint32 class_of_service)
950{
953
954 vrpn_Analog::report_changes(class_of_service);
956}
957
958void vrpn_IDEA::report(vrpn_uint32 class_of_service)
959{
962
963 vrpn_Analog::report(class_of_service);
965}
966
975{
976 char errmsg[256];
977
979
980 switch(status) {
981 case STATUS_RESETTING:
982 reset();
983 break;
984
985 case STATUS_SYNCING:
986 case STATUS_READING:
987 {
988 // It turns out to be important to get the report before checking
989 // to see if it has been too long since the last report. This is
990 // because there is the possibility that some other device running
991 // in the same server may have taken a long time on its last pass
992 // through mainloop(). Trackers that are resetting do this. When
993 // this happens, you can get an infinite loop -- where one tracker
994 // resets and causes the other to timeout, and then it returns the
995 // favor. By checking for the report here, we reset the timestamp
996 // if there is a report ready (ie, if THIS device is still operating).
997 while (get_report()) {}; // Keep getting reports so long as there are more
998
999 struct timeval current_time;
1000 vrpn_gettimeofday(&current_time, NULL);
1001 if ( vrpn_TimevalDuration(current_time,d_timestamp) > POLL_INTERVAL) {
1002
1004 // Send another request to the unit, in case we've somehow
1005 // dropped a request.
1006 if (!send_command("l")) {
1007 VRPN_MSG_ERROR("Could not request position");
1009 }
1011 } else {
1012 return;
1013 }
1014 }
1015
1017 sprintf(errmsg,"Timeout, resetting... current_time=%ld:%ld, timestamp=%ld:%ld",
1018 current_time.tv_sec, static_cast<long>(current_time.tv_usec),
1019 d_timestamp.tv_sec, static_cast<long>(d_timestamp.tv_usec));
1020 VRPN_MSG_ERROR(errmsg);
1022 }
1023 }
1024 break;
1025
1026 default:
1027 VRPN_MSG_ERROR("Unknown mode (internal error)");
1028 break;
1029 }
1030}
1031
vrpn_float64 o_channel[vrpn_CHANNEL_MAX]
vrpn_int32 request_channels_m_id
vrpn_float64 last[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:39
vrpn_float64 channel[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:38
struct timeval timestamp
Definition: vrpn_Analog.h:41
vrpn_int32 num_channel
Definition: vrpn_Analog.h:40
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY, const struct timeval time=vrpn_ANALOG_NOW)
Send a report whether something has changed or not (for servers) Optionally, tell what time to stamp ...
Definition: vrpn_Analog.C:94
virtual void report_changes(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY, const struct timeval time=vrpn_ANALOG_NOW)
Send a report only if something has changed (for servers) Optionally, tell what time to stamp the val...
Definition: vrpn_Analog.C:71
int register_autodeleted_handler(vrpn_int32 type, vrpn_MESSAGEHANDLER handler, void *userdata, vrpn_int32 sender=vrpn_ANY_SENDER)
Registers a handler with the connection, and remembers to delete at destruction.
vrpn_Connection * d_connection
Connection that this object talks to.
vrpn_int32 d_sender_id
Sender ID registered with the connection.
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
int send_text_message(const char *msg, struct timeval timestamp, vrpn_TEXT_SEVERITY type=vrpn_TEXT_NORMAL, vrpn_uint32 level=0)
Sends a NULL-terminated text message from the device d_sender_id.
vrpn_int32 d_ping_message_id
Ask the server if they are there.
All button servers should derive from this class, which provides the ability to turn any of the butto...
Definition: vrpn_Button.h:66
virtual void report_changes(void)
Definition: vrpn_Button.C:383
vrpn_int32 num_buttons
Definition: vrpn_Button.h:48
struct timeval timestamp
Definition: vrpn_Button.h:49
virtual void report_changes(void)
Definition: vrpn_Button.C:423
unsigned char lastbuttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:46
unsigned char buttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:45
Generic connection class not specific to the transport mechanism.
double d_initial_move
Definition: vrpn_IDEA.h:81
int d_accel_rate_tics_sec_sec
Definition: vrpn_IDEA.h:67
bool send_command(const char *cmd)
Appends carriage-return and then sends the command.
Definition: vrpn_IDEA.C:119
struct timeval d_timestamp
Definition: vrpn_IDEA.h:62
int d_high_limit_index
Definition: vrpn_IDEA.h:75
int d_delay
Definition: vrpn_IDEA.h:73
int d_end_speed_tics_sec
Definition: vrpn_IDEA.h:66
struct timeval d_last_poll
Definition: vrpn_IDEA.h:84
int d_run_current
Definition: vrpn_IDEA.h:69
bool send_move_request(vrpn_float64 location_in_steps, double scale=1.0)
Request a move from the motor to the specified location.
Definition: vrpn_IDEA.C:155
int d_accel_current
Definition: vrpn_IDEA.h:71
int d_decel_rate_tics_sec_sec
Definition: vrpn_IDEA.h:68
int d_hold_current
Definition: vrpn_IDEA.h:70
int d_low_limit_index
Definition: vrpn_IDEA.h:76
int d_output_3_setting
Definition: vrpn_IDEA.h:79
static int VRPN_CALLBACK handle_request_message(void *userdata, vrpn_HANDLERPARAM p)
Responds to a request to change one of the values by setting the channel to that value.
Definition: vrpn_IDEA.C:885
int d_run_speed_tics_sec
Definition: vrpn_IDEA.h:64
int d_output_2_setting
Definition: vrpn_IDEA.h:78
virtual int get_report(void)
Definition: vrpn_IDEA.C:726
vrpn_IDEA(const char *name, vrpn_Connection *c, const char *port, int run_speed_tics_sec=3200, int start_speed_tics_sec=1200, int end_speed_tics_sec=2000, int accel_rate_tics_sec_sec=40000, int decel_rate_tics_sec_sec=100000, int run_current=290, int hold_current=0, int accel_current=290, int decel_current=290, int delay=50, int step=8, int high_limit_index=-1, int low_limit_index=-1, int output_1_setting=-1, int output_2_setting=-1, int output_3_setting=-1, int output_4_setting=-1, double initial_move=0, double fractional_c_a=1.0, double reset_location=0.0)
Definition: vrpn_IDEA.C:31
static int VRPN_CALLBACK handle_request_channels_message(void *userdata, vrpn_HANDLERPARAM p)
Responds to a request to change multiple channels at once.
Definition: vrpn_IDEA.C:913
int d_step
Definition: vrpn_IDEA.h:74
int convert_report_to_buttons(unsigned char *buf)
Parses an input/output report. Returns -1 on failure, 0 on no value.
Definition: vrpn_IDEA.C:321
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_RELIABLE)
send report whether or not changed
Definition: vrpn_IDEA.C:958
virtual int reset(void)
Definition: vrpn_IDEA.C:372
int d_decel_current
Definition: vrpn_IDEA.h:72
int convert_report_to_position(unsigned char *buf)
Parses a position report. Returns -1 on failure, 0 on no value.
Definition: vrpn_IDEA.C:294
virtual void mainloop()
Called once through each main loop iteration to handle updates.
Definition: vrpn_IDEA.C:974
unsigned char d_buffer[512]
Definition: vrpn_IDEA.h:59
bool move_until_done_or_error(vrpn_float64 location_in_steps, double scale=1.0)
Send a move request and then wait for the move to complete. Repeat.
Definition: vrpn_IDEA.C:245
int d_output_1_setting
Definition: vrpn_IDEA.h:77
unsigned d_bufcount
Definition: vrpn_IDEA.h:60
double d_fractional_c_a
Definition: vrpn_IDEA.h:82
double d_reset_location
Definition: vrpn_IDEA.h:83
static int VRPN_CALLBACK handle_connect_message(void *userdata, vrpn_HANDLERPARAM p)
Responds to a connection request with a report of the values.
Definition: vrpn_IDEA.C:941
int d_start_speed_tics_sec
Definition: vrpn_IDEA.h:65
int d_output_4_setting
Definition: vrpn_IDEA.h:80
This structure is what is passed to a vrpn_Connection message callback.
const char * buffer
#define STATUS_SYNCING
#define STATUS_READING
#define STATUS_RESETTING
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
@ vrpn_TEXT_ERROR
#define TIMEOUT_TIME_INTERVAL
const vrpn_uint32 vrpn_CONNECTION_RELIABLE
Classes of service for messages, specify multiple by ORing them together Priority of satisfying these...
#define POLL_INTERVAL
Definition: vrpn_IDEA.C:26
Header containing macros formerly duplicated in a lot of implementation files.
#define VRPN_MSG_ERROR(msg)
#define VRPN_MSG_WARNING(msg)
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
Definition: vrpn_Serial.C:651
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
Definition: vrpn_Serial.C:438
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
Definition: vrpn_Serial.C:515
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
VRPN_API int vrpn_unbuffer(const char **buffer, timeval *t)
Utility routine for taking a struct timeval from a buffer that was sent as a message.
Definition: vrpn_Shared.C:321
unsigned long vrpn_TimevalDuration(struct timeval endT, struct timeval startT)
Return number of microseconds between startT and endT.
Definition: vrpn_Shared.C:138
void vrpn_SleepMsecs(double dMilliSecs)
Definition: vrpn_Shared.C:166
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:99