WURDE: /home/fwph/code/wurde/modules/ptu/ptu.cpp Source File

00001 /*************************************************************************
00002 *****                  PTU BINARY DRIVER CODE FILE                   ****/
00003 #define PTU_CPI_CODE_VERSION   "v1.09.12"
00004 /****                                                                *****
00005 *****             (C)1992-1999, Directed Perception, Inc.            *****
00006 *****                     All Rights Reserved.                       *****
00007 *****                                                                *****
00008 *****   Licensed users may freely distribute compiled code including *****
00009 *****   this code and data. Source data and code may NOT be          *****
00010 *****   distributed without the prior written consent from           *****
00011 *****   Directed Perception, Inc.                                    *****
00012 *****           Directed Perception, Inc. reserves the right to make *****
00013 *****   changes without further notice to any content herein to      *****
00014 *****   improve reliability, function or design. Directed Perception *****
00015 *****   shall not assume any liability arising from the application  *****
00016 *****   or use of this code, data or function.                       *****
00017 *****                                                                *****
00018 **************************************************************************
00019 
00020 CHANGE HISTORY:
00021     12/10/99: v1.09.12   added a write and wait after open in open_host_port 
00022                          for networked operation
00023     11/10/99: v1.09.11   Added asynchronous event status handling functions.
00024      8/18/99: v1.09.10   Select_unit_ID binary opcode code added and commented
00025                          for users wishing to use binary unit selects (i.e.,
00026                                      by uncommenting the binary op and commenting out the ASCII op).
00027      7/28/99: v1.09.09r2 In firmware_version_OK, made sure it works even when
00028                                      the controller is in feedback terse mode. Also added 
00029                          10 sec timeout on read to allow fall-out from non-responsive PTU.
00030     5/12/99: v1.09.09r1  select_unit declarations cleaned up
00031     2/ 8/99: v1.09.09.   select_unit operation performed using ASCII select to aid
00032                                      debugging for developers.
00033     1/26/99: v1.09.08.   select_unit was blocking for a getchar that doesn't happen...
00034     9/23/98: v1.09.07.   Added additional commands supported in firmware version v1.9.7
00035                                  and above. Unit commands modified to work (unsigned short int).
00036     8/10/98: v1.08.09.   In firmware_version_OK, removed addressing to string constant.
00037     9/27/97: v1.08.08.   Win32. Removed writestring in openserial.
00038                          Set 0 read timeout in setserial. Peek improved.
00039     7/10/97: v1.08.00.   Firmware version check modified so illegal version
00040                              numbers (e.g., from bad read operations) are detected
00041     11/2/95:  v1.07.07d. Firmware version check bug fixed.
00042     7/11/95:  v1.07.06d. Updated opcode structure and added new support.
00043     2/19/95:  v1.07.04d. Generalized for Windows, DOS, & UNIX.
00044                              Added networking. 
00045     10/12/94: v1.07.03d. Pre-release working version.
00046                              XON/XOFF removed from PTU firmware to allow for binary mode.
00047 
00048 
00049 **************************************************************************/
00050 
00051 #define PTU_OPCODE_VERSION                                      "v1.07.07d"
00052 /* this code supports controller firmware versions equal or above */
00053 #define PTU_modelVersion                                                1
00054 #define PTU_codeVersion                                             7
00055 #define PTU_revision                                                    6
00056 
00057 /* The binary PTU driver code */
00058 /* Assumes serial support for read_byte and write_byte */
00059 
00060 /* #include <windows.h> */
00061 
00062 
00063 /* #include <dos.h>   */
00064 /* #include <conio.h> */
00065 #include <stdio.h>         
00066 #include <ctype.h>
00067 #include <string.h>
00068 /* #include <stdlib.h> */
00069 /* #include <time.h>   */
00070 /* #include <dir.h>    */
00071 #include "ptu.h"
00072 
00073 static char err;
00074 
00075 static portstream_fd current_host_port;
00076 
00077 static char speed_control_mode = PTU_INDEPENDENT_SPEED_CONTROL_MODE;
00078 
00079 
00080 
00081 /*** asynchronous event handling ***/
00082 /* This mechanism is invoked when a pan/tilt limit is hit or a cable disconnect is detected */
00083 
00084 /* The default asynchronous event handler does nothing. Useful for defining
00085    your own callback function. Returns TRUE when called (used to determine
00086    if the handler has been installed properly). */
00087 unsigned char default_async_event_handler(unsigned char async_event)
00088 {   printf("\n\n\n\ndefault_async_event_handler called: ");
00089         switch (async_event) {
00090         case PAN_LIMIT_HIT:             printf("PAN_LIMIT_HIT\n\n\n");
00091                                                                     break;
00092         case TILT_LIMIT_HIT:            /* printf("TILT_LIMIT_HIT\n\n\n"); */
00093                                             break;
00094         case CABLE_DISCONNECT_DETECTED: /* printf("CABLE_DISCONNECT_DETECTED\n\n\n"); */
00095                                             break;
00096         default:                        /* printf("ERROR (unknown async_event signalled)\n\n\n"); */
00097                                                         break;
00098         }
00099         return(TRUE);
00100 }
00101 
00102 
00103 static event_handler_fn_ptr_type async_event_handler_fn_ptr = (unsigned char (*) (unsigned char)) default_async_event_handler;
00104 
00105 
00106 /* Call this function to set the function handler for asynchronous events.
00107    Defaults to a null function. Example call: 
00108              if ( set_async_event_handler( (unsigned char (*) (unsigned char)) default_async_event_handler ) )
00109                       printf("ASYNCH handler installed properly!");
00110                  else printf("ERROR: ASYNCH handler not installed properly!");
00111 */
00112 unsigned char set_async_event_handler( void (*async_event_handler) (unsigned char) )
00113 { 
00114         async_event_handler_fn_ptr = (unsigned char (*) (unsigned char)) default_async_event_handler;
00115         return ( (*async_event_handler_fn_ptr) (0) );
00116 }
00117 
00118 
00119 
00120 /************************** PTU COMMANDS *****************************/
00121 
00122 
00123 /* open_host_port(<portname>) ==> <portstream> */
00124 portstream_fd open_host_port(char *portname)
00125 { char out_string[10] = "    "; 
00126         
00127   current_host_port = openserial(portname);
00128 
00129   /* added this to allow serial ports to clear for networked startup */
00130   SerialBytesOut(current_host_port, (unsigned char *) out_string, strlen(out_string));
00131   do_delay(2000);
00132 
00133   return current_host_port;
00134 }
00135 
00136 
00137 /* close_host_port(<portstream>) ==> <status> */
00138 char close_host_port(portstream_fd portstream)
00139         {   current_host_port = PORT_NOT_OPENED;
00140             return( closeserial(portstream) );
00141         }
00142 
00143 
00144 unsigned char GetSerialChar(char await_char)
00145         {  unsigned char c;
00146 
00147                 for (;;) {
00148                         err = SerialBytesIn(current_host_port, &c, 1, -1);
00149                         if (err < 0) return err;
00150                         else if (err > 0) return c;
00151                                   else if (await_char != TRUE)
00152                                                          return 0;
00153                         }
00154         }
00155 
00156 
00157 /* Call this function to return a single byte binary command return status.
00158    Handles asynchronous events, and makes any required event handling callbacks. */
00159 unsigned char get_binary_command_return_status() 
00160   {     unsigned char status;
00161 
00162         status = GetSerialChar(TRUE);
00163         while ( ASYNCHRONOUS_EVENT(status) )
00164                 { /* call the asynchronous event handler */
00165                   (*async_event_handler_fn_ptr) (status);
00166                   status = GetSerialChar(TRUE);
00167         }
00168         return (status);
00169 }
00170 
00171 
00172 
00173 char SerialOut(unsigned char outchar)
00174 {    return SerialBytesOut(current_host_port, &outchar, 1);
00175 }
00176 
00177 
00178 /* use this function to switch the host port currently being controlled */
00179 char select_host_port(portstream_fd portstream)
00180         {  current_host_port = portstream;
00181                 return(0);
00182         }
00183 
00184 
00185 
00186 /* reset_PTU() ==> <status> */
00187 char reset_ptu(void)
00188         {  unsigned char c;
00189 
00190                 SerialOut(UNIT_RESET);
00191                 while ( ((c = GetSerialChar(TRUE)) == PAN_LIMIT_HIT) ||
00192                                   (c == TILT_LIMIT_HIT) );
00193                 return(c);
00194         }
00195 
00196 
00197 
00198 /* an internal function that verifies that the PTU controller firmware
00199    supports this binary interface. TRUE when OK, otherwise error code.
00200    This function communicates in ASCII to ensure proper unit communication. 
00201    Added 10 sec timeout on read to allow fall-out from non-responsive PTU. */
00202 char firmware_version_OK(void)
00203 {   unsigned char *tmpStr;
00204     char c1, c2;
00205     int modelVersion, codeVersion, revision;
00206     unsigned char versionSTR[256];
00207     int charsRead=0;
00208     int status;
00209     unsigned char initString[] = "    v ";
00210 
00211         do_delay(500);
00212         status = FlushInputBuffer(current_host_port);   
00213         if (status != TRUE)
00214            printf ("\nERROR(firmware_version_OK): FlushInputBuffer failed with status (%d)\n", status);
00215         tmpStr = initString;
00216         if ( (status = SerialBytesOut(current_host_port, tmpStr, 6)) != TRUE )
00217            { printf ("\nERROR(firmware_version_OK): SerialBytesOut error %d\n", status);
00218              return(FALSE);
00219         }
00220         do_delay(500);
00221 
00222         switch ( ReadSerialLine(current_host_port, versionSTR, 10000, &charsRead) ) {
00223                 case TRUE:                                      
00224                                 break;
00225                 case TIMEOUT_CHAR_READ:         
00226                                 printf("\nERROR(firmware_version_OK): timeout on ReadSerialLine (%d read)\n", charsRead);
00227                                 return(TIMEOUT_CHAR_READ);
00228                 default:                                        
00229                                 printf("\nERROR(firmware_version_OK): ReadSerialLine error\n");
00230                                 return(FALSE);
00231         }                                      
00232         
00233         /* parse to the beginning of the version ID (fix for ft mode on 7/28/99) */
00234         tmpStr = versionSTR;
00235         while ( tolower(*tmpStr) != '*' ) tmpStr++;
00236         while ( tolower(*tmpStr) != 'v' ) tmpStr++;
00237         tmpStr++;
00238         sscanf((char *) tmpStr, "%1d %c %2d %c %2d",
00239                                 &modelVersion, &c1, &codeVersion, &c2, &revision);
00240             
00241         
00242         if ( /* ensure the version numbers seem reasonable as version numbers, and
00243                     the version number is high enough */
00244                  ((modelVersion < 999)  && (modelVersion >= PTU_modelVersion)) ||
00245                  ((codeVersion  < 999)  && (codeVersion  >= PTU_codeVersion))  ||
00246                  ((revision     < 9999) && (revision     >= PTU_revision)) ) 
00247                   { printf("\n\nController firmware v%d.%d.%d is compatible\n\n",
00248                                           modelVersion, codeVersion, revision);
00249                          return(TRUE); /* the controller firmware is compatible */
00250                   }
00251         else { printf("\n\nPTU Controller firmware version v%d.%d.%d is NOT compatible:\n\tversion v%d.%d.%d and higher is required\n   charsRead='%d'\n",
00252                                           modelVersion, codeVersion, revision,
00253                                           PTU_modelVersion, PTU_codeVersion, PTU_revision,
00254                                           charsRead);
00255                          return(FALSE);
00256                   }
00257 
00258 }
00259 
00260 
00261 
00262 /* Flushes the PTU parser so that pending command parses are terminated,
00263         and the call blocks until the PTU is ready to accept the next command
00264         or it times out before the PTU responds that it is OK. */
00265 /* reset_PTU_parser(<timeout_in_msec>) ==> [PTU_OK|PTU_NOT_RESPONDING] */
00266 char reset_PTU_parser(long timeout_in_msec)
00267         { long elapsed_time = 250;
00268           char status;
00269 
00270           do_delay(500);                                   /* allows pending PTU commands to complete */
00271           SerialOut(' '); SerialOut(' '); SerialOut(' ');  /* terminates any pending parses */
00272           do_delay(250);      /* allows the return of any PTU info from the termination */
00273           FlushInputBuffer(current_host_port);             /* flushes the PTU return info */
00274 
00275           if ( (status = firmware_version_OK()) != TRUE )
00276              return(status);
00277 
00278           /* now make sure the PTU is responding to commands */
00279           for (;;)
00280                  { /* issue a command and ensure it looks legal */
00281                         SerialOut(PAN_HOLD_POWER_QUERY);
00282                         /* do_delay(250);  */
00283                         status = GetSerialChar(FALSE);
00284                         if ( (status >= PTU_REG_POWER) & (status <= PTU_OFF_POWER) )
00285                                 { /* things look OK, so flush and unblock */
00286                                   FlushInputBuffer(current_host_port); /* flushes the PTU return info */
00287                                   return(PTU_OK);
00288                                 }
00289                         else
00290                                 { /* there's a problem, so flush, delay, and retry */
00291                                   FlushInputBuffer(current_host_port); /* flushes the PTU return info */
00292                                   do_delay(500);
00293                                   elapsed_time += 750;
00294                                   if (elapsed_time > timeout_in_msec)
00295                                           return(PTU_NOT_RESPONDING);
00296                                 }}
00297         }
00298 
00299 
00300 /* set_desired([PAN|TILT],
00301                 [POSITION|SPEED|ACCELERATION|BASE|UPPER|LOWER],
00302                 [<position>|<speed>|<acceleration>],
00303                 [RELATIVE|ABSOLUTE]) ==> <status>
00304         set_desired([PAN|TILT],
00305                 [HOLD_POWER_LEVEL,MOVE_POWER_LEVEL],
00306                 <power mode>,
00307                 NULL) ==> <status>                              */
00308 char set_desired(char axis, char kinematic_property,
00309                      PTU_PARM_PTR *value, char movement_mode)
00310         { unsigned short int uvalue;
00311           long lvalue;
00312           char cvalue;
00313 
00314           switch (kinematic_property)   {
00315                         case POSITION:
00316                                         switch (axis)  {
00317                                                 case PAN:
00318                                                          switch (movement_mode) {
00319                                                                 case RELATIVE: SerialOut(PAN_SET_REL_POS);
00320                                                                                                         break;
00321                                                                 case ABSOLUTE: SerialOut(PAN_SET_ABS_POS);
00322                                                                                                         break;
00323                                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00324                                                          }
00325                                                          break;
00326                                                 case TILT:
00327                                                          switch (movement_mode) {
00328                                                                 case RELATIVE: SerialOut(TILT_SET_REL_POS);
00329                                                                                                         break;
00330                                                                 case ABSOLUTE: SerialOut(TILT_SET_ABS_POS);
00331                                                                                                         break;
00332                                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00333                                                          }
00334                                                          break;
00335                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);          }
00336                                         PutSignedShort(current_host_port, (signed short *) value);
00337                                         break;
00338                         case SPEED:
00339                                         switch (axis)  {
00340                                                 case PAN:
00341                                                          switch (movement_mode) {
00342                                                                 case RELATIVE: SerialOut(PAN_SET_REL_SPEED);
00343                                                                                            PutSignedShort(current_host_port, (signed short *) value);
00344                                                                                            break;
00345                                                                 case ABSOLUTE: SerialOut(PAN_SET_ABS_SPEED);
00346                                                                                            if (speed_control_mode == PTU_INDEPENDENT_SPEED_CONTROL_MODE)
00347                                                                                                   { uvalue = *((unsigned short *) value);
00348                                                                                                     PutUnsignedShort(current_host_port, &uvalue);               }
00349                                                                                            else
00350                                                                                                   { PutSignedShort(current_host_port, (signed short *) value);          }
00351                                                                                            break;
00352                                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00353                                                          }
00354                                                          break;
00355                                                 case TILT:
00356                                                          switch (movement_mode) {
00357                                                                 case RELATIVE: SerialOut(TILT_SET_REL_SPEED);
00358                                                                                            PutSignedShort(current_host_port, (signed short *) value);
00359                                                                                            break;
00360                                                                 case ABSOLUTE: SerialOut(TILT_SET_ABS_SPEED);
00361                                                                                            if (speed_control_mode == PTU_INDEPENDENT_SPEED_CONTROL_MODE)
00362                                                                                                   { uvalue = *((unsigned short *) value);
00363                                                                                                         PutUnsignedShort(current_host_port, &uvalue);           }
00364                                                                                            else
00365                                                                                                   { PutSignedShort(current_host_port, (signed short *) value);          }
00366                                                                                            break;
00367                                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00368                                                          }
00369                                                          break;
00370                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);          }
00371                                                 break;
00372                         case ACCELERATION:
00373                                         lvalue = *((long *)value);
00374                                         switch (axis)  {
00375                                                 case PAN:
00376                                                          switch (movement_mode) {
00377                                                                 case RELATIVE: lvalue += get_current(PAN,ACCELERATION);
00378                                                                                                         SerialOut(PAN_SET_ACCEL);
00379                                                                                                         break;
00380                                                                 case ABSOLUTE: SerialOut(PAN_SET_ACCEL);
00381                                                                                                         break;
00382                                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00383                                                          }
00384                                                          break;
00385                                                 case TILT:
00386                                                          switch (movement_mode) {
00387                                                                 case RELATIVE: lvalue += get_current(TILT,ACCELERATION);
00388                                                                                                         SerialOut(TILT_SET_ACCEL);
00389                                                                                                         break;
00390                                                                 case ABSOLUTE: SerialOut(TILT_SET_ACCEL);
00391                                                                                                         break;
00392                                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00393                                                          }
00394                                                          break;
00395                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);          }
00396                                         PutSignedLong(current_host_port, &lvalue);
00397                                   break;
00398                         case BASE:
00399                                         switch (axis)  {
00400                                                 case PAN:
00401                                                          switch (movement_mode) {
00402                                                                 case RELATIVE:
00403                                                                 case ABSOLUTE: SerialOut(PAN_SET_BASE_SPEED);
00404                                                                                                         break;
00405                                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00406                                                          }
00407                                                          break;
00408                                                 case TILT:
00409                                                          switch (movement_mode) {
00410                                                                 case RELATIVE:
00411                                                                 case ABSOLUTE: SerialOut(TILT_SET_BASE_SPEED);
00412                                                                                                         break;
00413                                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00414                                                          }
00415                                                          break;
00416                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);          }
00417                                         uvalue = *((unsigned short int*) value);
00418                                         PutUnsignedShort(current_host_port, &uvalue);
00419                                   break;
00420                         case UPPER_SPEED_LIMIT:
00421                                         switch (axis)  {
00422                                                 case PAN:
00423                                                          switch (movement_mode) {
00424                                                                 case RELATIVE:
00425                                                                 case ABSOLUTE: SerialOut(PAN_SET_UPPER_SPEED_LIMIT);
00426                                                                                            break;
00427                                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00428                                                          }
00429                                                          break;
00430                                                 case TILT:
00431                                                          switch (movement_mode) {
00432                                                                 case RELATIVE:
00433                                                                 case ABSOLUTE: SerialOut(TILT_SET_UPPER_SPEED_LIMIT);
00434                                                                                                         break;
00435                                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00436                                                          }
00437                                                          break;
00438                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);          }
00439                                         uvalue = *((unsigned short int*) value);
00440                                         PutUnsignedShort(current_host_port, &uvalue);
00441                                   break;
00442                         case LOWER_SPEED_LIMIT:
00443                                         switch (axis)  {
00444                                                 case PAN:
00445                                                          switch (movement_mode) {
00446                                                                 case RELATIVE:
00447                                                                 case ABSOLUTE: SerialOut(PAN_SET_LOWER_SPEED_LIMIT);
00448                                                                                                         break;
00449                                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00450                                                          }
00451                                                          break;
00452                                                 case TILT:
00453                                                          switch (movement_mode) {
00454                                                                 case RELATIVE:
00455                                                                 case ABSOLUTE: SerialOut(TILT_SET_LOWER_SPEED_LIMIT);
00456                                                                                                         break;
00457                                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00458                                                          }
00459                                                          break;
00460                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);          }
00461                                         uvalue = *((unsigned short int*) value);
00462                                         PutUnsignedShort(current_host_port, &uvalue);
00463                                   break;
00464                         case HOLD_POWER_LEVEL:
00465                                         switch (axis)  {
00466                                                 case PAN:
00467                                                          switch (movement_mode) {
00468                                                                 case RELATIVE:
00469                                                                 case ABSOLUTE: SerialOut(PAN_SET_HOLD_POWER);
00470                                                                                                         break;
00471                                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00472                                                          }
00473                                                          break;
00474                                                 case TILT:
00475                                                          switch (movement_mode) {
00476                                                                 case RELATIVE:
00477                                                                 case ABSOLUTE: SerialOut(TILT_SET_HOLD_POWER);
00478                                                                                                         break;
00479                                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00480                                                          }
00481                                                          break;
00482                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);          }
00483                                         cvalue = *((unsigned char*) value);
00484                                         SerialOut((unsigned char) cvalue);
00485                                   break;
00486                         case MOVE_POWER_LEVEL:
00487                                         switch (axis)  {
00488                                                 case PAN:
00489                                                          switch (movement_mode) {
00490                                                                 case RELATIVE:
00491                                                                 case ABSOLUTE: SerialOut(PAN_SET_MOVE_POWER);
00492                                                                                                         break;
00493                                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00494                                                          }
00495                                                          break;
00496                                                 case TILT:
00497                                                          switch (movement_mode) {
00498                                                                 case RELATIVE:
00499                                                                 case ABSOLUTE: SerialOut(TILT_SET_MOVE_POWER);
00500                                                                                                         break;
00501                                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00502                                                          }
00503                                                          break;
00504                                                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);          }
00505                                         cvalue = *((unsigned char*) value);
00506                                         SerialOut((unsigned char) cvalue);
00507                                   break;
00508                         default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00509                         }
00510 
00511 
00512         return( get_binary_command_return_status() );    /* return the command execution status */
00513 
00514         }
00515 
00516 
00517 /* await_completion() ==> <status> */
00518 char await_completion(void)
00519         { SerialOut(AWAIT_COMMAND_COMPLETION);
00520           return( get_binary_command_return_status() );
00521           }
00522 
00523 
00524 /* get_current([PAN|TILT],
00525                 [POSITION|SPEED|ACCELERATION|BASE|
00526                  UPPER_SPEED_LIMIT|LOWER_SPEED_LIMIT|
00527                  MINIMUM_POSITION|MAXIMUM_POSITION|
00528                  RESOLUTION|
00529                  HOLD_POWER_LEVEL|MOVE_POWER_LEVEL|]) ==> <value> */
00530 long get_current(char axis, char kinematic_property)
00531         { unsigned short int uvalue;
00532           signed short int value;
00533      long long_value;
00534 
00535           switch (kinematic_property)   {
00536                         case POSITION:
00537                                         switch (axis)  {
00538                                                 case PAN:       SerialOut(PAN_CURRENT_POS_QUERY);
00539                                                                         goto get_and_return_signed_short_int;
00540                                                 case TILT:  SerialOut(TILT_CURRENT_POS_QUERY);
00541                                                                         goto get_and_return_signed_short_int;
00542                                                 default:        return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00543                                                 }
00544                         case SPEED:
00545                                         switch (axis)  {
00546                                                 case PAN:   SerialOut(PAN_CURRENT_SPEED_QUERY);
00547                                                                         if (speed_control_mode == PTU_INDEPENDENT_SPEED_CONTROL_MODE)
00548                                                                            goto get_and_return_unsigned_short_int;
00549                                                                         else
00550                                                                            goto get_and_return_signed_short_int;
00551                                                 case TILT:  SerialOut(TILT_CURRENT_SPEED_QUERY);
00552                                                                         if (speed_control_mode == PTU_INDEPENDENT_SPEED_CONTROL_MODE)
00553                                                                            goto get_and_return_unsigned_short_int;
00554                                                                         else
00555                                                                            goto get_and_return_signed_short_int;
00556                                                 default:        return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00557                                                 }
00558                         case ACCELERATION:
00559                                         switch (axis)  {
00560                                                 case PAN:       SerialOut(PAN_ACCEL_QUERY);
00561                                                                         goto get_and_return_long;
00562                                                 case TILT:  SerialOut(TILT_ACCEL_QUERY);
00563                                                                         goto get_and_return_long;
00564                                                 default:        return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00565                                                 }
00566                         case BASE:
00567                                         switch (axis)  {
00568                                                 case PAN:       SerialOut(PAN_BASE_SPEED_QUERY);
00569                                                                         goto get_and_return_unsigned_short_int;
00570                                                 case TILT:  SerialOut(TILT_BASE_SPEED_QUERY);
00571                                                                         goto get_and_return_unsigned_short_int;
00572                                                 default:        return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00573                                                 }
00574                         case UPPER_SPEED_LIMIT:
00575                                         switch (axis)  {
00576                                                 case PAN:       SerialOut(PAN_UPPER_SPEED_LIMIT_QUERY);
00577                                                                         goto get_and_return_unsigned_short_int;
00578                                                 case TILT:      SerialOut(TILT_UPPER_SPEED_LIMIT_QUERY);
00579                                                                         goto get_and_return_unsigned_short_int;
00580                                                 default:        return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00581                                                 }
00582                         case LOWER_SPEED_LIMIT:
00583                                         switch (axis)  {
00584                                                 case PAN:       SerialOut(PAN_LOWER_SPEED_LIMIT_QUERY);
00585                                                                         goto get_and_return_unsigned_short_int;
00586                                                 case TILT:  SerialOut(TILT_LOWER_SPEED_LIMIT_QUERY);
00587                                                                         goto get_and_return_unsigned_short_int;
00588                                                 default:    return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00589                                                 }
00590                         case MINIMUM_POSITION:
00591                                         switch (axis)  {
00592                                                 case PAN:       SerialOut(PAN_MIN_POSITION_QUERY);
00593                                                                         goto get_and_return_signed_short_int;
00594                                                 case TILT:  SerialOut(TILT_MIN_POSITION_QUERY);
00595                                                                         goto get_and_return_signed_short_int;
00596                                                 default:        return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00597                                                 }
00598                         case MAXIMUM_POSITION:
00599                                         switch (axis)  {
00600                                                 case PAN:       SerialOut(PAN_MAX_POSITION_QUERY);
00601                                                                         goto get_and_return_signed_short_int;
00602                                                 case TILT:  SerialOut(TILT_MAX_POSITION_QUERY);
00603                                                                         goto get_and_return_signed_short_int;
00604                                                 default:        return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00605                                                 }
00606                         case RESOLUTION:
00607                                         switch (axis)  {
00608                                                 case PAN:       SerialOut(PAN_RESOLUTION_QUERY);
00609                                                                         goto get_and_return_long;
00610                                                 case TILT:  SerialOut(TILT_RESOLUTION_QUERY);
00611                                                                         goto get_and_return_long;
00612                                                 default:        return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00613                                                 }
00614                         case HOLD_POWER_LEVEL:
00615                                         switch (axis)  {
00616                                                 case PAN:       SerialOut(PAN_HOLD_POWER_QUERY);
00617                                                                         goto get_and_return_char;
00618                                                 case TILT:  SerialOut(TILT_HOLD_POWER_QUERY);
00619                                                                         goto get_and_return_char;
00620                                                 default:        return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00621                                                 }
00622                         case MOVE_POWER_LEVEL:
00623                                         switch (axis)  {
00624                                                 case PAN:       SerialOut(PAN_MOVE_POWER_QUERY);
00625                                                                         goto get_and_return_char;
00626                                                 case TILT:  SerialOut(TILT_MOVE_POWER_QUERY);
00627                                                                         goto get_and_return_char;
00628                                                 default:        return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00629                                                 }
00630                         default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00631                 }
00632 
00633         get_and_return_unsigned_short_int:
00634                 err = GetUnsignedShort(current_host_port, &uvalue, -1);
00635                 long_value = uvalue;
00636                 return(long_value);
00637 
00638         get_and_return_signed_short_int:
00639                 err = GetSignedShort(current_host_port, &value, -1);
00640                 long_value = value;
00641                 return(long_value);
00642 
00643         get_and_return_long:
00644                 err = GetSignedLong(current_host_port, &long_value, -1);
00645                 return(long_value);
00646 
00647         get_and_return_char:
00648                 long_value = (long) GetSerialChar(TRUE);
00649                 return(long_value);
00650                 
00651 }
00652 
00653 
00654 
00655 /* get_desired([PAN|TILT],
00656                 [POSITION|SPEED|ACCELERATION|BASE|
00657                  UPPER_SPEED_LIMIT|LOWER_SPEED_LIMIT|
00658                  MINIMUM_POSITION|MAXIMUM_POSITION|
00659                  RESOLUTION|
00660                  HOLD_POWER_LEVEL|MOVE_POWER_LEVEL]) ==> <ptr to value> */
00661 long get_desired(char axis, char kinematic_property)
00662         { unsigned short int uvalue;
00663           signed short int value;
00664       long long_value;
00665 
00666           switch (kinematic_property)   {
00667                         case POSITION:
00668                                         switch (axis)  {
00669                                                 case PAN:   SerialOut(PAN_DESIRED_POS_QUERY);
00670                                                                         goto get_and_return_signed_short_int;
00671                                                 case TILT:  SerialOut(TILT_DESIRED_POS_QUERY);
00672                                                                         goto get_and_return_signed_short_int;
00673                                                 default:        return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00674                                                 }
00675                         case SPEED:
00676                                         switch (axis)  {
00677                                                 case PAN:   SerialOut(PAN_DESIRED_SPEED_QUERY);
00678                                                                         if (speed_control_mode == PTU_INDEPENDENT_SPEED_CONTROL_MODE)
00679                                                                            goto get_and_return_unsigned_short_int;
00680                                                                         else
00681                                                                            goto get_and_return_signed_short_int;
00682                                                 case TILT:  SerialOut(TILT_DESIRED_SPEED_QUERY);
00683                                                                         if (speed_control_mode == PTU_INDEPENDENT_SPEED_CONTROL_MODE)
00684                                                                            goto get_and_return_unsigned_short_int;
00685                                                                         else
00686                                                                            goto get_and_return_signed_short_int;
00687                                                 default:    return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00688                                                 }
00689                         case ACCELERATION:
00690                                         switch (axis)  {
00691                                                 case PAN:   SerialOut(PAN_ACCEL_QUERY);
00692                                                                         goto get_and_return_long;
00693                                                 case TILT:  SerialOut(TILT_ACCEL_QUERY);
00694                                                                         goto get_and_return_long;
00695                                                 default:    return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00696                                                 }
00697                         case BASE:
00698                                         switch (axis)  {
00699                                                 case PAN:       SerialOut(PAN_BASE_SPEED_QUERY);
00700                                                                         goto get_and_return_unsigned_short_int;
00701                                                 case TILT:  SerialOut(TILT_BASE_SPEED_QUERY);
00702                                                                         goto get_and_return_unsigned_short_int;
00703                                                 default:        return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00704                                                 }
00705                         case UPPER_SPEED_LIMIT:
00706                                         switch (axis)  {
00707                                                 case PAN:       SerialOut(PAN_UPPER_SPEED_LIMIT_QUERY);
00708                                                                         goto get_and_return_unsigned_short_int;
00709                                                 case TILT:      SerialOut(TILT_UPPER_SPEED_LIMIT_QUERY);
00710                                                                         goto get_and_return_unsigned_short_int;
00711                                                 default:        return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00712                                                 }
00713                         case LOWER_SPEED_LIMIT:
00714                                         switch (axis)  {
00715                                                 case PAN:       SerialOut(PAN_LOWER_SPEED_LIMIT_QUERY);
00716                                                                         goto get_and_return_unsigned_short_int;
00717                                                 case TILT:  SerialOut(TILT_LOWER_SPEED_LIMIT_QUERY);
00718                                                                         goto get_and_return_unsigned_short_int;
00719                                                 default:        return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00720                                                 }
00721                         case MINIMUM_POSITION:
00722                                         switch (axis)  {
00723                                                 case PAN:       SerialOut(PAN_MIN_POSITION_QUERY);
00724                                                                         goto get_and_return_signed_short_int;
00725                                                 case TILT:  SerialOut(TILT_MIN_POSITION_QUERY);
00726                                                                         goto get_and_return_signed_short_int;
00727                                                 default: break;
00728                                                 }
00729                         case MAXIMUM_POSITION:
00730                                         switch (axis)  {
00731                                                 case PAN:       SerialOut(PAN_MAX_POSITION_QUERY);
00732                                                                         goto get_and_return_signed_short_int;
00733                                                 case TILT:  SerialOut(TILT_MAX_POSITION_QUERY);
00734                                                                         goto get_and_return_signed_short_int;
00735                                                 default:        return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00736                                                 }
00737                         case RESOLUTION:
00738                                         switch (axis)  {
00739                                                 case PAN:   SerialOut(PAN_RESOLUTION_QUERY);
00740                                                                         goto get_and_return_long;
00741                                                 case TILT:  SerialOut(TILT_RESOLUTION_QUERY);
00742                                                                         goto get_and_return_long;
00743                                                 default:    return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00744                                                 }
00745                         case HOLD_POWER_LEVEL:
00746                                         switch (axis)  {
00747                                                 case PAN:       SerialOut(PAN_HOLD_POWER_QUERY);
00748                                                                         goto get_and_return_char;
00749                                                 case TILT:  SerialOut(TILT_HOLD_POWER_QUERY);
00750                                                                         goto get_and_return_char;
00751                                                 default:        return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00752                                                 }
00753                         case MOVE_POWER_LEVEL:
00754                                         switch (axis)  {
00755                                                 case PAN:       SerialOut(PAN_MOVE_POWER_QUERY);
00756                                                                         goto get_and_return_char;
00757                                                 case TILT:  SerialOut(TILT_MOVE_POWER_QUERY);
00758                                                                         goto get_and_return_char;
00759                                                 default:        return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00760                                                 }
00761                         default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00762 
00763                 }
00764 
00765         get_and_return_unsigned_short_int:
00766                 err = GetUnsignedShort(current_host_port, &uvalue,-1);
00767                 long_value = uvalue;
00768                 return(long_value);
00769 
00770         get_and_return_signed_short_int:
00771                 err = GetSignedShort(current_host_port, &value,-1);
00772                 long_value = value;
00773                 return(long_value);
00774 
00775         get_and_return_long:
00776                 err = GetSignedLong(current_host_port, &long_value,-1);
00777                 return(long_value);
00778 
00779         get_and_return_char:
00780                 long_value = (long) GetSerialChar(TRUE);
00781                 return(long_value);
00782 
00783 }
00784 
00785 
00786 /* set_mode(COMMAND_EXECUTION_MODE,
00787                 [EXECUTE_IMMEDIATELY|EXECUTE_UPON_IMMEDIATE_OR_AWAIT]) ==> <status>
00788    set_mode(ASCII_VERBOSE_MODE, [VERBOSE|TERSE|QUERY_MODE]) ==> <status>
00789    set_mode(ASCII_ECHO_MODE, [ON_MODE|OFF_MODE|QUERY_MODE] ==> <status>
00790    set_mode(POSITION_LIMITS_MODE, [ON_MODE|OFF_MODE|QUERY_MODE] ==> <status>
00791    set_mode(DEFAULTS,[SAVE_CURRENT_SETTINGS|RESTORE_SAVED_SETTINGS|RESTORE_FACTORY_SETTINGS]) ==> <status>
00792    *** below is only supported by PTU firmware versions 1.9.7 and higher.                        ***
00793    *** This call must be made before pure velocity speed control mode made be used by CPI calls. ***
00794    set_mode(SPEED_CONTROL_MODE, [PTU_INDEPENDENT_SPEED_CONTROL_MODE |
00795                                  PTU_PURE_VELOCITY_SPEED_CONTROL_MODE | QUERY_MODE] ==> <status>
00796 */ 
00797 char set_mode(char mode_type, char mode_parameter)
00798         { switch (mode_type) {
00799                 case DEFAULTS:
00800                                 {switch (mode_parameter) {
00801                                         case SAVE_CURRENT_SETTINGS:
00802                                                                 SerialOut(SAVE_DEFAULTS);
00803                                                                 goto return_status;
00804                                         case RESTORE_SAVED_SETTINGS:
00805                                                                 SerialOut(RESTORE_SAVED_DEFAULTS);
00806                                                                 goto return_status;
00807                                         case RESTORE_FACTORY_SETTINGS:
00808                                                                 SerialOut(RESTORE_FACTORY_DEFAULTS);
00809                                                                 goto return_status;
00810                                         default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00811                                         }}
00812                 case ASCII_ECHO_MODE:
00813                                 {switch (mode_parameter) {
00814                                         case ON_MODE:
00815                                                                 SerialOut(ENABLE_ECHO);
00816                                                                 goto return_status;
00817                                         case OFF_MODE:
00818                                                                 SerialOut(DISABLE_ECHO);
00819                                                                 goto return_status;
00820                                         case QUERY_MODE:
00821                                                                 SerialOut(ECHO_QUERY);
00822                                                                 goto return_status;
00823                                         default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00824                                         }}
00825                 case COMMAND_EXECUTION_MODE:
00826                                 switch (mode_parameter) {
00827                                         case EXECUTE_IMMEDIATELY:
00828                                                                 SerialOut(SET_IMMEDIATE_COMMAND_MODE);
00829                                                                 break;
00830                                         case EXECUTE_UPON_IMMEDIATE_OR_AWAIT:
00831                                                                 SerialOut(SET_SLAVED_COMMAND_MODE);
00832                                                                 break;
00833                                         default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00834                                         }
00835                                 break;
00836                 case ASCII_VERBOSE_MODE:
00837                                 {switch (mode_parameter) {
00838                                         case VERBOSE:
00839                                                                 SerialOut(SET_VERBOSE_ASCII_ON);
00840                                                                 break;
00841                                         case TERSE:
00842                                                                 SerialOut(SET_VERBOSE_ASCII_OFF);
00843                                                                 break;
00844                                         case QUERY_MODE:
00845                                                                 SerialOut(VERBOSE_QUERY);
00846                                                                 break;
00847                                         default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00848                                         }}
00849                                 break;
00850                 case POSITION_LIMITS_MODE:
00851                                 {switch (mode_parameter) {
00852                                         case ON_MODE:
00853                                                                 SerialOut(ENABLE_POSITION_LIMITS);
00854                                                                 break;
00855                                         case OFF_MODE:
00856                                                                 SerialOut(DISABLE_POSITION_LIMITS);
00857                                                                 break;
00858                                         case QUERY_MODE:
00859                                                                 SerialOut(POSITION_LIMITS_QUERY);
00860                                                                 break;
00861                                         default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00862                                         }}
00863                                 break;
00864                 case SPEED_CONTROL_MODE:
00865                                 {switch (mode_parameter) {
00866                                         case PTU_INDEPENDENT_SPEED_CONTROL_MODE:
00867                                                                 speed_control_mode = PTU_INDEPENDENT_SPEED_CONTROL_MODE;
00868                                                         SerialOut(SET_INDEPENDENT_CONTROL_MODE);
00869                                                                 break;
00870                                         case PTU_PURE_VELOCITY_SPEED_CONTROL_MODE:
00871                                                                 speed_control_mode = PTU_PURE_VELOCITY_SPEED_CONTROL_MODE;
00872                                                         SerialOut(SET_PURE_VELOCITY_CONTROL_MODE);
00873                                                                 break;
00874                                         case QUERY_MODE:
00875                                                                 SerialOut(QUERY_SPEED_CONTROL_MODE);
00876                                                                 break;
00877                                         default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00878                                         }}
00879                                 break;          
00880                 default: return(PTU_ILLEGAL_COMMAND_ARGUMENT);
00881         }
00882   return_status:
00883         return( get_binary_command_return_status() );   /* return <status> */
00884 }
00885 
00886 
00887 /* halt([ALL|PAN|TILT]) ==> <status>    */
00888 char halt(char halt_type)
00889         { switch(halt_type) {
00890                 case PAN:   SerialOut(HALT_PAN);
00891                                                 break;
00892                 case TILT:  SerialOut(HALT_TILT);
00893                                                 break;
00894                 default:    SerialOut(HALT);
00895                                                 break;
00896                 }
00897           return( get_binary_command_return_status() );
00898         }
00899 
00900 
00901 /* firmware_version() ==> <version ID string> */
00902 char* firmware_version(void)
00903 {  static unsigned char version_ID_string[256];
00904         int charsRead;
00905 
00906         SerialOut(FIRMWARE_VERSION_QUERY);
00907         do_delay(1000);
00908         ReadSerialLine(current_host_port, version_ID_string,0,&charsRead);
00909         return((char *) version_ID_string);
00910 }
00911 
00912 
00913 
00914 /* modified 5/12/99  */
00915 char select_unit(UID_fd unit_ID)
00916         { char UID_select[10];
00917 
00918           sprintf(UID_select, "  _%d ", unit_ID);
00919           SerialBytesOut(current_host_port, (unsigned char*) UID_select, strlen(UID_select));
00920                 
00921           /* the below only works for PTU firmware v1.9.11r3 and above */
00922           /* SerialOut(SELECT_UNIT_ID); */
00923           /* PutUnsignedShort(current_host_port, &unit_ID); */
00924 
00925           return TRUE;
00926         }
00927 
00928 
00929 
00930 /*  modified 10/19/98  */
00931 char set_unit_id(UID_fd unit_ID)
00932         {   SerialOut(SET_UNIT_ID);
00933         PutUnsignedShort(current_host_port, &unit_ID); 
00934         return GetSerialChar(TRUE);
00935         }
00936 
00937 
00938