#define ident "1E07" #define Active_Motor 1 #ifdef __DECC #pragma module EL734_TEST ident #endif /* ** +--------------------------------------------------------------+ ** | Paul Scherrer Institute | ** | SINQ Project | ** | | ** | This software may be used freely by non-profit organizations.| ** | It may be copied provided that the name of P.S.I. and of the | ** | author is included. Neither P.S.I. nor the author assume any | ** | responsibility for the use of this software outside of P.S.I.| ** +--------------------------------------------------------------+ ** ** Link_options - Here is the Linker Option File **!$! **!$! To build on LNSA09 ... **!$! $ build_cc_select :== decc **!$! $ import tasmad **!$! $ def/job deltat_c_tlb sinq_c_tlb **!$! $ bui tas_src:[utils]el734_test debug **!$! **!$! To build on PSICLC ... **!$! $ build_cc_select :== decc **!$! $ set default usr_scroot:[maden] **!$! $ copy lnsa09::ud0:[maden.motor]el734_test.c [] **!$! $ copy lnsa09::tasmad_disk:[mad.lib]sinq_dbg.olb [] **!$! $ copy lnsa09::tasmad_disk:[mad.lib]sinq_c.tlb [] **!$! $ def/job sinq_olb usr_scroot:[maden]sinq_dbg.olb **!$! $ def/job sinq_c_tlb usr_scroot:[maden]sinq_c.tlb **!$! $ def/job deltat_c_tlb sinq_c_tlb **!$! $ bui el734_test debug **!$! **!$ if p1 .eqs. "DEBUG" then dbg1 := /debug **!$ if p1 .eqs. "DEBUG" then dbg2 := _dbg **!$ link 'dbg1'/exe=el734_test'dbg2'.exe sys$input/options **! el734_test **! sinq_olb/lib **! sys$share:decw$xmlibshr12/share **! sys$share:decw$xtlibshrr5/share **! sys$share:decw$xlibshr/share **!$ purge/nolog el734_test'dbg2'.exe **!$ set prot=w:re el734_test'dbg2'.exe **!$ my_dir = f$element (0, "]", f$environment ("procedure")) + "]" **!$ write sys$output "Exec file is ''my_dir'EL734_TEST''DBG2'.EXE **!$ exit **!$! ** Link_options_end ** ** Building on Alpha OSF/1: ** ** cc -std1 -g -o ~/motor/el734_test -I/public/lib/include \ ** -transitive_link -L/public/lib -ldeltat \ ** -lrt -lXm -lXt -lX11 -lXext -lm \ ** ~/motor/el734_test.c ** or ** alias makep /usr/opt/posix/usr/bin/make <-- Posix Vn of "make" ** makep -f ~/motor/motor.make ~/motor/el734_test ** ** To run the program, one might need to set the environment variable: ** ** setenv LD_LIBRARY_PATH /public/lib ** ** if the -soname option was not specified to ld when the deltat shared ** library was build. ** ** Resources and Flags File: decw$user_defaults:SinQ_rc.dat ** ------------------- or $HOME/SinQ_rc ** ** Resource Flag Default Description ** -------- ---- ------- ----------- ** - -name el734_test Name to use when looking up the ** resources. The default is the ** image file name. ** ... ** A value given via -name will be converted to lowercase before being used. **+ **--------------------------------------------------------------------------- ** Module Name . . . . . . . . : [...MOTOR]EL734_TEST.C ** ** Author . . . . . . . . . . : D. Maden ** Date of creation . . . . . . : Nov 1995 ** ** Purpose ** ======= ** EL734_TEST is a test program for the EL734 Motor Controller. ** Use: ** === ** 1) On VMS, define a foreign command, e.g. ** ** $ el734_test :== $psi_public:[exe_axp]el734_test.exe ** ** On Unix systems, ensure el734_test is in a suitable PATH directory. ** ** 2) Issue commands of the form: ** ** el734_test -host lnsp22 -chan 5 -m 6 -p 25.0 ** ** where ** -host specifies the name of the computer to which the EL734 is ** attached. This computer must be running the ** RS232C_SRV program or equivalent. ** -chan specifies the serial channel number to which the EL734 is ** attached. ** -m specifies the motor number to be driven and ** -p specifies that a positioning command is to be executed. ** ** For a full list of options, issue the command: ** ** el734_test -help ** ** Updates: ** 1A01 25-Nov-1994 DM. Initial version. ** 1C01 12-Sep-1996 DM. Use SINQ.OLB and SINQ_C.TLB on VMS. ** 1D01 6-Nov-1996 DM. Add EC command to SAVE command list. ** 1E01 8-May-1997 DM. Add RESTORE and NO_RESTORE to LOAD command list. **- **==================================================================== */ #include #include #include #include #include #ifdef __VMS #include #else #include #endif #include /* **==================================================================== */ #include #include /* **-------------------------------------------------------------------------- ** Define global structures and constants. */ #define NIL '\0' #ifndef True #define True 1 #define False 0 #endif /* ** Define the file idents for stdin, stdout and stderr */ #define STDIN 0 #define STDOUT 1 #define STDERR 2 #define N_ELEMENTS(arg) (sizeof (arg)/sizeof (arg[0])) /*------------------------------------------------------------- ** Global Variables */ static int C_gbl_status; /* Return status from C_... routines */ static int Ctrl_C_has_happened; void *Hndl = NULL; static XrmOptionDescRec OpTable_0[] = { {"-name", ".name", XrmoptionSepArg, (XPointer) NULL}, {"-?", ".el734HelpItem", XrmoptionSepArg, (XPointer) NULL}, {"-?cmd", ".el734HelpCmd", XrmoptionNoArg, (XPointer) "1"}, {"-?msg", ".el734HelpMsg", XrmoptionNoArg, (XPointer) "1"}, {"-?par", ".el734HelpPar", XrmoptionNoArg, (XPointer) "1"}, {"-?res", ".el734HelpRes", XrmoptionNoArg, (XPointer) "1"}, {"-chan", ".el734Chan", XrmoptionSepArg, (XPointer) NULL}, {"-f", ".el734Frequency", XrmoptionSepArg, (XPointer) NULL}, {"-fb", ".el734Fb", XrmoptionNoArg, (XPointer) "-1"}, {"-ff", ".el734Ff", XrmoptionNoArg, (XPointer) "-1"}, {"-frequency",".el734Frequency", XrmoptionSepArg, (XPointer) NULL}, {"-help", ".el734Help", XrmoptionNoArg, (XPointer) "1"}, {"-hi", ".el734High", XrmoptionSepArg, (XPointer) NULL}, {"-high", ".el734High", XrmoptionSepArg, (XPointer) NULL}, {"-host", ".el734Host", XrmoptionSepArg, (XPointer) NULL}, {"-hunt", ".el734Hunt", XrmoptionNoArg, (XPointer) "1"}, {"-id", ".el734Id", XrmoptionSepArg, (XPointer) NULL}, {"-limits", ".el734Limits", XrmoptionSepArg, (XPointer) NULL}, {"-lo", ".el734Low", XrmoptionSepArg, (XPointer) NULL}, {"-load", ".el734Load", XrmoptionSepArg, (XPointer) NULL}, {"-low", ".el734Low", XrmoptionSepArg, (XPointer) NULL}, {"-m", ".el734Motor", XrmoptionSepArg, (XPointer) NULL}, {"-motor", ".el734Motor", XrmoptionSepArg, (XPointer) NULL}, {"-n", ".el734N", XrmoptionSepArg, (XPointer) NULL}, {"-p", ".el734Position", XrmoptionSepArg, (XPointer) NULL}, {"-port", ".el734Port", XrmoptionSepArg, (XPointer) NULL}, {"-position", ".el734Position", XrmoptionSepArg, (XPointer) NULL}, {"-random", ".el734Random", XrmoptionNoArg, (XPointer) "1"}, {"-ref", ".el734Ref", XrmoptionNoArg, (XPointer) "1"}, {"-rndm", ".el734Random", XrmoptionNoArg, (XPointer) "1"}, {"-s", ".el734Stop", XrmoptionNoArg, (XPointer) "1"}, {"-save", ".el734Save", XrmoptionSepArg, (XPointer) NULL}, {"-saw", ".el734Saw", XrmoptionNoArg, (XPointer) "1"}, {"-sb", ".el734Sb", XrmoptionNoArg, (XPointer) "-1"}, {"-scan", ".el734Scan", XrmoptionNoArg, (XPointer) "-1"}, {"-seed", ".el734Seed", XrmoptionSepArg, (XPointer) NULL}, {"-sf", ".el734Sf", XrmoptionNoArg, (XPointer) "-1"}, {"-step", ".el734Step", XrmoptionSepArg, (XPointer) NULL}, {"-stop", ".el734Stop", XrmoptionNoArg, (XPointer) "1"}, {"-tmo", ".el734Tmo", XrmoptionSepArg, (XPointer) NULL}, {"-ur@", ".el734SetPos", XrmoptionSepArg, (XPointer) NULL}, {"-verbose", ".el734Verbose", XrmoptionSepArg, (XPointer) NULL}, {"-wait", ".el734Wait", XrmoptionSepArg, (XPointer) NULL}, }; char El734_host[20]; int El734_port; /* TCP/IP Port number for socket */ int El734_chan; /* Asynch channel number */ char El734_id0[20]; /* The 1st EL734 identifier string */ char El734_id1[20]; /* The 2nd EL734 identifier string */ int Check_EL734_id; int Motor; /* Motor number */ int Enc_typ; int Enc_num; int Enc_par; int N_moves; float Lo_arg, Hi_arg; int Lo_arg_present, Hi_arg_present; int Do_help, Do_posit, Do_rndm, Do_saw, Do_scan, Do_step; int Do_ref, Do_hunt, Do_ff, Do_fb, Do_sf, Do_sb, Do_stop; int Do_save, Do_load; int Do_limits, Do_setpos; float Lim_arg_lo, Lim_arg_hi, Ist_arg; float Tmo, Tmo_ref; unsigned int Seed; int Seed_present; int Verbose; int Wait_time, Frequency; float Soll_posit; float Step; char Save_file[80]; char Load_file[80]; int Dec_pt = 3; int Enc_fact_0, Enc_fact_1; int Mot_fact_0, Mot_fact_1; float Inertia_tol; int Ramp; int Loop_mode; int Slow_hz; float Lo, Hi; char Ctrl_id[32]; int Fast_hz; int Ref_mode; int Backlash; int Pos_tol; char Mot_mem[16]; char Mot_name[16]; float Ref_param; int Is_sided; int Null_pt; int Ac_par; int Enc_circ; int Stat_pos; int Stat_pos_flt; int Stat_pos_fail; int Stat_cush_fail; float Ist_pos; int Prop; int Integ; int Deriv; char *Errstack; int Errcode, Errno, Vaxc_errno; /* **-------------------------------------------------------------------------- ** PrintErrInfo: print out error information. */ void PrintErrInfo (char *text) { /* ============ */ EL734_ErrInfo (&Errstack, &Errcode, &Errno, &Vaxc_errno); fprintf (stderr, "\n\007" " Error return from %s\n" " Errstack = \"%s\"\n" " Errcode = %d Errno = %d Vaxc$errno = %d\n", text, Errstack, Errcode, Errno, Vaxc_errno); switch (Errcode) { case EL734__BAD_ADR: fprintf (stderr, " Address error\n"); break; case EL734__BAD_CMD: fprintf (stderr, " Command error\n"); break; case EL734__BAD_ILLG: fprintf (stderr, " Illegal response\n"); break; case EL734__BAD_LOC: fprintf (stderr, " EL734 is in manual mode.\n"); break; case EL734__BAD_MALLOC: fprintf (stderr, " Call to \"malloc\" failed\n"); perror (text); break; case EL734__BAD_OFL: fprintf (stderr, " Connection to asynch port lost\n"); break; case EL734__BAD_OVFL: fprintf (stderr, " Overflow: may be due to bad encoder gearing factor\n"); break; case EL734__BAD_PAR: fprintf (stderr, " Illegal parameter specified\n"); break; case EL734__BAD_RNG: fprintf (stderr, " Attempted to exceed lower or upper limit\n"); break; case EL734__BAD_SOCKET: fprintf (stderr, " Call to \"AsynSrv_Open\" failed\n"); perror (text); break; case EL734__BAD_STP: fprintf (stderr, " Motor is disabled: \"Stop\" signal is active!"); break; case EL734__BAD_TMO: fprintf (stderr, " Time-out of EL734 response.\n"); break; default: if ((Errno != 0) || (Vaxc_errno != 0)) perror (text); } return; } /* **-------------------------------------------------------------------------- ** GetKHVQZ: read the K, H, V, Q and Zero-point parameters */ int GetKHVQZ ( /* ======== */ int *k, float *lo, float *hi, int *v, float *q, float *z) { int status; status = EL734_GetRefMode (&Hndl, k); if (!status) { printf ("\n\007"); printf ("Bad status from EL734_GetRefMode.\n"); return False; } status = EL734_GetLimits (&Hndl, lo, hi); if (!status) { printf ("\n\007"); printf ("Bad status from EL734_GetLimits.\n"); return False; } status = EL734_GetNullPoint (&Hndl, v); if (!status) { printf ("\n\007"); printf ("Bad status from EL734_GetZeroPoint.\n"); return False; } status = EL734_GetRefParam (&Hndl, q); if (!status) { printf ("\n\007"); printf ("Bad status from EL734_GetRefParam.\n"); return False; } status = EL734_GetZeroPoint (&Hndl, z); if (!status) { printf ("\n\007"); printf ("Bad status from EL734_GetZeroPoint.\n"); return False; } return True; } /* **--------------------------------------------------------------------------- ** My_WaitIdle: Wait till MSR goes to zero or ** This routine is similar to EL734_WaitIdle ** with the extra verbose argument and a test ** for . */ int My_WaitIdle ( /* =========== */ void **handle, int verbose, /* Width of display field */ int *ored_msr, int *fp_cntr, int *fr_cntr, float *ist_posit) { #ifdef __VMS #include #define hibernate lib$wait (0.25) #else #include #include struct timespec delay = {0, 250000000}; struct timespec delay_left; #define hibernate nanosleep_d9 (&delay, &delay_left) #endif int i, msr, ss, s_stat; int my_verbose; float last_posit; char buff[64]; my_verbose = verbose; if (my_verbose*2 > sizeof (buff)) my_verbose = sizeof (buff)/2; if (my_verbose > 0) { s_stat = EL734_GetStatus (handle, &msr, ored_msr, fp_cntr, fr_cntr, &ss, ist_posit); if (!s_stat) { PrintErrInfo ("My_WaitIdle/EL734_GetStatus"); return False; } last_posit = *ist_posit; sprintf (buff, "%*.*f", my_verbose, Dec_pt, last_posit); printf (buff); fflush (NULL); for (i=0; i 0) && (*ist_posit != last_posit)) { last_posit = *ist_posit; sprintf (&buff[my_verbose], "%*.*f", my_verbose, Dec_pt, last_posit); printf (buff); fflush (NULL); } if ((msr & MSR__BUSY) == 0) return True; hibernate; if (Ctrl_C_has_happened) return False; } PrintErrInfo ("My_WaitIdle/EL734_GetStatus"); /* Error detected in ** EL734_GetStatus */ return False; } /* **-------------------------------------------------------------------------- ** LoadCheckTwoInteger: routine to check that a command specifying ** two integers set correctly. */ int LoadCheckTwoInteger (char *cmnd) { /* =================== */ int status, len; char my_cmnd[80], rd_cmnd[40], buff[40]; char *cmnd_tok, *motor_tok, *par0_tok, *par1_tok, *rd0_tok, *rd1_tok; StrJoin (my_cmnd, sizeof (my_cmnd), cmnd, ""); /* Make a copy of cmnd */ len = strlen (my_cmnd); if (my_cmnd[len-1] == '\r') my_cmnd[len-1] = NIL; cmnd_tok = strtok (my_cmnd, " "); motor_tok = strtok (NULL, " "); par0_tok = strtok (NULL, " "); par1_tok = strtok (NULL, " "); if ((cmnd_tok == NULL) || (motor_tok == NULL) || (par0_tok == NULL) || (par1_tok == NULL)) { printf ("\007Software problem in LoadCheckTwoInteger\n"); return False; } StrJoin (buff, sizeof (buff), cmnd_tok, " "); StrJoin (rd_cmnd, sizeof (rd_cmnd), buff, motor_tok); len = strlen (rd_cmnd); rd_cmnd[len] = '\r'; rd_cmnd[len+1] = NIL; status = EL734_SendCmnd (&Hndl, rd_cmnd, buff, sizeof (buff)); if (status) { rd0_tok = strtok (buff, " "); rd1_tok = strtok (NULL, " "); if ((rd0_tok == NULL) || (rd1_tok == NULL) || (strcmp (par0_tok, rd0_tok) != 0) || (strcmp (par1_tok, rd1_tok) != 0)) { if (rd0_tok == NULL) rd0_tok = ""; if (rd1_tok == NULL) rd1_tok = ""; printf ("\007Verify error for command \"%s %s %s %s\"\n", cmnd_tok, motor_tok, par0_tok, par1_tok); printf ("Values set in EL734 controller are \"%s %s\"\n" " They should be \"%s %s\"\n", rd0_tok, rd1_tok, par0_tok, par1_tok); return False; }else { return True; /* The parameter has been set correctly! */ } }else { printf ("\007LoadCheckTwoInteger -- error from EL734_SendCmnd\n"); printf ("Command being checked was \"%s %s %s %s\"\n", cmnd_tok, motor_tok, par0_tok, par1_tok); return False; } } /* **-------------------------------------------------------------------------- ** LoadCheckTwoFloat: routine to check that a command specifying ** two real values set correctly. */ int LoadCheckTwoFloat (char *cmnd, int n_dec) { /* ================= */ int status, len; char my_cmnd[80], rd_cmnd[40], buff[40], par0[40], par1[40]; char *cmnd_tok, *motor_tok, *par0_tok, *par1_tok, *rd0_tok, *rd1_tok; char *whole_tok, *frac_tok; StrJoin (my_cmnd, sizeof (my_cmnd), cmnd, ""); /* Make a copy of cmnd */ len = strlen (my_cmnd); if (my_cmnd[len-1] == '\r') my_cmnd[len-1] = NIL; cmnd_tok = strtok (my_cmnd, " "); motor_tok = strtok (NULL, " "); par0_tok = strtok (NULL, " "); par1_tok = strtok (NULL, " "); if ((cmnd_tok == NULL) || (motor_tok == NULL) || (par0_tok == NULL) || (par1_tok == NULL)) { printf ("\007Software problem in LoadCheckTwoFloat\n"); return False; } /*--------------------------------------------------- ** Check that the number of decimal places in the first set ** parameter agrees with the setting of the EL734. */ StrJoin (par0, sizeof (par0), par0_tok, ""); whole_tok = strtok (par0, "."); frac_tok = strtok (NULL, "."); if (frac_tok == NULL) { /* Check for a decimal point */ len = strlen (whole_tok); /* None there, so put in a ".0" */ frac_tok = whole_tok + len + 1; frac_tok[0] = '0'; frac_tok[1] = NIL; } len = strlen (frac_tok); if (len > n_dec) { /* Param has too many decimal places */ /* Try to remove trailing zeros */ while ((len >= 0) && (frac_tok[len-1] == '0')) { len = len - 1; frac_tok[len] = NIL; if (len == n_dec) break; } if (len != n_dec) { printf ("Don't expect the parameter to verify correctly.\n" "You have specified too many decimal places!\n"); } }else if (len < n_dec) { /* Param has too few decimal places */ while (len < n_dec) { /* Pad with zeros */ frac_tok[len] = '0'; len = len + 1; frac_tok[len] = NIL; } } len = strlen (whole_tok); /* Re-join the parts of param again */ whole_tok[len] = '.'; /*--------------------------------------------------- ** Check that the number of decimal places in the second set ** parameter agrees with the setting of the EL734. */ StrJoin (par1, sizeof (par1), par1_tok, ""); whole_tok = strtok (par1, "."); frac_tok = strtok (NULL, "."); if (frac_tok == NULL) { /* Check for a decimal point */ len = strlen (whole_tok); /* None there, so put in a ".0" */ frac_tok = whole_tok + len + 1; frac_tok[0] = '0'; frac_tok[1] = NIL; } len = strlen (frac_tok); if (len > n_dec) { /* Param has too many decimal places */ /* Try to remove trailing zeros */ while ((len >= 0) && (frac_tok[len-1] == '0')) { len = len - 1; frac_tok[len] = NIL; if (len == n_dec) break; } if (len != n_dec) { printf ("Don't expect the parameter to verify correctly.\n" "You have specified too many decimal places!\n"); } }else if (len < n_dec) { /* Param has too few decimal places */ while (len < n_dec) { /* Pad with zeros */ frac_tok[len] = '0'; len = len + 1; frac_tok[len] = NIL; } } len = strlen (whole_tok); /* Re-join the parts of param again */ whole_tok[len] = '.'; /* End of checking number of decimal places **--------------------------------------------------- */ StrJoin (buff, sizeof (buff), cmnd_tok, " "); StrJoin (rd_cmnd, sizeof (rd_cmnd), buff, motor_tok); len = strlen (rd_cmnd); rd_cmnd[len] = '\r'; rd_cmnd[len+1] = NIL; status = EL734_SendCmnd (&Hndl, rd_cmnd, buff, sizeof (buff)); if (status) { rd0_tok = strtok (buff, " "); rd1_tok = strtok (NULL, " "); if ((rd0_tok == NULL) || (rd1_tok == NULL) || (strcmp (par0, rd0_tok) != 0) || (strcmp (par1, rd1_tok) != 0)) { if (rd0_tok == NULL) rd0_tok = ""; if (rd1_tok == NULL) rd1_tok = ""; printf ("\007Verify error for command \"%s %s %s %s\"\n", cmnd_tok, motor_tok, par0, par1); printf ("Value set in EL734 controller is \"%s %s\"\n" " It should be \"%s %s\"\n", rd0_tok, rd1_tok, par0, par1); return False; }else { return True; /* The parameter has been set correctly! */ } }else { printf ("\007LoadCheckTwoFloat -- error from EL734_SendCmnd\n"); printf ("Command being checked was \"%s %s %s %s\"\n", cmnd_tok, motor_tok, par0, par1); return False; } } /* **-------------------------------------------------------------------------- ** LoadFloatJuggle: routine to try to get around an EL734 problem. ** ** The problem is that the EL734 does not at the moment ** handle binary <--> float conversion correctly. */ int LoadFloatJuggle ( /* =============== */ char *cmnd, /* The command to be issued */ char *motor, /* The motor index being loaded */ char *param, /* The desired parameter */ int n_dec) { /* The number of decimal places */ /* ** It is assumed that all parameters are consistent (especially ** param and n_dec) since this is an internal routine and that ** they are not terminated with . */ int status, i, incr0, incr1, incr2; char set_cmnd[80], read_cmnd[40], my_par[40], buff[40]; char *rd_tok; printf ("Trying to juggle the \"%s\" parameter of Motor %s" " to be %s ..\n .. ", cmnd, motor, param); sprintf (read_cmnd, "%s %s\r", cmnd, motor); /* Prepare the param rd cmnd */ /* ** Find indices of last 3 chars to be incremented */ incr0 = strlen (param); incr0--; /* incr0 now indexes the last digit of param */ if (!isdigit (param[incr0])) incr0--; /* Be careful not to increment .. */ /* .. a decimal point! */ incr1 = incr0 - 1; if (!isdigit (param[incr1])) incr1--; /* Be careful not to increment .. */ /* .. a decimal point! */ incr2 = incr1 - 1; if (!isdigit (param[incr2])) incr2--; /* Be careful not to increment .. */ /* .. a decimal point! */ if ((!isdigit (param[incr0])) || (!isdigit (param[incr1])) || (!isdigit (param[incr2]))) { printf ("LoadFloatJuggle: software problem with decimal point\n" " The routine probably needs to be enhanced!\n"); return False; } /*---------------------------------------------------------------- ** First try incrementing the last digit of the set value of the ** parameter 5 times. */ StrJoin (my_par, sizeof (my_par), param, ""); /* Make a copy of param */ for (i = 0; i < 5; i++) { if (my_par[incr0] != '9') { /* Check for carry to next digit */ my_par[incr0]++; }else { my_par[incr0] = '0'; if (my_par[incr1] != '9') { my_par[incr1]++; }else { my_par[incr1] = '0'; if (my_par[incr2] != '9') { my_par[incr2]++; }else { my_par[incr2] = '0'; } } } printf ("%s .. ", my_par); sprintf (set_cmnd, "%s %s %s\r", cmnd, motor, my_par); status = EL734_SendCmnd (&Hndl, set_cmnd, buff, sizeof (buff)); if (status) status = EL734_SendCmnd (&Hndl, read_cmnd, buff, sizeof (buff)); if (!status) { printf ("\nError with \"%s %s %s\"\n" "Abandoning parameter juggle!\n", cmnd, motor, my_par); return False; } rd_tok = strtok (buff, " "); /* Skip leading spaces */ if (strcmp (param, rd_tok) == 0) { printf ("\n Success. Parameter as sent was %s\n" " Parameter as read is %s\n", my_par, param); return True; } } /*---------------------------------------------------------------- ** Now try decrementing the last digit of the set value of the ** parameter 5 times. */ StrJoin (my_par, sizeof (my_par), param, ""); /* Make a copy of param */ for (i = 0; i < 5; i++) { if (my_par[incr0] != '0') { /* Check for carry to next digit */ my_par[incr0]--; }else { my_par[incr0] = '9'; if (my_par[incr1] != '0') { my_par[incr1]--; }else { my_par[incr1] = '9'; if (my_par[incr2] != '0') { my_par[incr2]--; }else { my_par[incr2] = '9'; } } } printf ("%s .. ", my_par); sprintf (set_cmnd, "%s %s %s\r", cmnd, motor, my_par); status = EL734_SendCmnd (&Hndl, set_cmnd, buff, sizeof (buff)); if (status) status = EL734_SendCmnd (&Hndl, read_cmnd, buff, sizeof (buff)); if (!status) { printf ("\nError with \"%s %s %s\"\n" "Abandoning parameter juggle!\n", cmnd, motor, my_par); return False; } rd_tok = strtok (buff, " "); /* Skip leading spaces */ if (strcmp (param, rd_tok) == 0) { printf ("\n Success. Parameter as sent was %s\n" " Parameter as read is %s\n", my_par, param); return True; } } /* ** Failed - go back to original setting */ sprintf (set_cmnd, "%s %s %s\r", cmnd, motor, param); status = EL734_SendCmnd (&Hndl, set_cmnd, buff, sizeof (buff)); if (status) status = EL734_SendCmnd (&Hndl, read_cmnd, buff, sizeof (buff)); if (status) { rd_tok = strtok (buff, " "); printf ("\n Failed. Parameter value is set to %s\n", rd_tok); }else { printf ("\n Failed. Parameter value is unknown due to error\n"); } return False; } /* **-------------------------------------------------------------------------- ** LoadIntJuggle: routine to try to get around an EL734 problem. ** ** The problem is that some integer parameters (e.g. E) ** do not set correctly. */ int LoadIntJuggle ( /* ============= */ char *cmnd, /* The command to be issued */ char *motor, /* The motor index being loaded */ char *param) { /* The desired parameter */ /* ** It is assumed that the parameters are trimmed (especially ** param) and are not terminated with . */ int status, i, my_par; char set_cmnd[80], read_cmnd[40], buff[40]; char *rd_tok; printf ("Trying to juggle the \"%s\" parameter of Motor %s" " to be %s ..\n .. ", cmnd, motor, param); sprintf (read_cmnd, "%s %s\r", cmnd, motor); /* Prepare the param rd cmnd */ /*---------------------------------------------------------------- ** First try incrementing the parameter 5 times. */ sscanf (param, "%d", &my_par); /* Gen binary value of param */ for (i = 0; i < 5; i++) { my_par++; printf ("%d .. ", my_par); sprintf (set_cmnd, "%s %s %d\r", cmnd, motor, my_par); status = EL734_SendCmnd (&Hndl, set_cmnd, buff, sizeof (buff)); if (status) status = EL734_SendCmnd (&Hndl, read_cmnd, buff, sizeof (buff)); if (!status) { printf ("\nError with \"%s %s %d\"\n" "Abandoning parameter juggle!\n", cmnd, motor, my_par); return False; } rd_tok = strtok (buff, " "); /* Skip leading spaces */ if (strcmp (param, rd_tok) == 0) { printf ("\n Success. Parameter as sent was %d\n" " Parameter as read is %s\n", my_par, param); return True; } } /*---------------------------------------------------------------- ** Now try decrementing the last digit of the set value of the ** parameter 5 times. */ sscanf (param, "%d", &my_par); /* Gen binary value of param */ for (i = 0; i < 5; i++) { my_par--; printf ("%d .. ", my_par); sprintf (set_cmnd, "%s %s %d\r", cmnd, motor, my_par); status = EL734_SendCmnd (&Hndl, set_cmnd, buff, sizeof (buff)); if (status) status = EL734_SendCmnd (&Hndl, read_cmnd, buff, sizeof (buff)); if (!status) { printf ("\nError with \"%s %s %d\"\n" "Abandoning parameter juggle!\n", cmnd, motor, my_par); return False; } rd_tok = strtok (buff, " "); /* Skip leading spaces */ if (strcmp (param, rd_tok) == 0) { printf ("\n Success. Parameter as sent was %d\n" " Parameter as read is %s\n", my_par, param); return True; } } /* ** Failed - go back to original setting */ sprintf (set_cmnd, "%s %s %s\r", cmnd, motor, param); status = EL734_SendCmnd (&Hndl, set_cmnd, buff, sizeof (buff)); if (status) status = EL734_SendCmnd (&Hndl, read_cmnd, buff, sizeof (buff)); if (status) { rd_tok = strtok (buff, " "); printf ("\n Failed. Parameter value is set to %s\n", rd_tok); }else { printf ("\n Failed. Parameter value is unknown due to error\n"); } return False; } /* **-------------------------------------------------------------------------- ** LoadCheckOneInteger: routine to check that a command specifying ** a single integer set correctly. */ int LoadCheckOneInteger (char *cmnd) { /* =================== */ int status, len; char my_cmnd[80], rd_cmnd[40], buff[40]; char *cmnd_tok, *motor_tok, *param_tok, *rd_tok; StrJoin (my_cmnd, sizeof (my_cmnd), cmnd, ""); /* Make a copy of cmnd */ len = strlen (my_cmnd); if (my_cmnd[len-1] == '\r') my_cmnd[len-1] = NIL; cmnd_tok = strtok (my_cmnd, " "); motor_tok = strtok (NULL, " "); param_tok = strtok (NULL, " "); if ((cmnd_tok == NULL) || (motor_tok == NULL) || (param_tok == NULL)) { printf ("\007Software problem in LoadCheckOneInteger\n"); return False; } StrJoin (buff, sizeof (buff), cmnd_tok, " "); StrJoin (rd_cmnd, sizeof (rd_cmnd), buff, motor_tok); len = strlen (rd_cmnd); rd_cmnd[len] = '\r'; rd_cmnd[len+1] = NIL; status = EL734_SendCmnd (&Hndl, rd_cmnd, buff, sizeof (buff)); if (status) { rd_tok = strtok (buff, " "); if ((rd_tok == NULL) || (strcmp (param_tok, rd_tok) != 0)) { if (rd_tok == NULL) rd_tok = ""; printf ("\007Verify error for command \"%s %s %s\"\n", cmnd_tok, motor_tok, param_tok); printf ("Value set in EL734 controller is \"%s\"\n" " It should be \"%s\"\n", rd_tok, param_tok); status = LoadIntJuggle (cmnd_tok, motor_tok, param_tok); return status; }else { return True; /* The parameter has been set correctly! */ } }else { printf ("\007LoadCheckOneInteger -- error from EL734_SendCmnd\n"); printf ("Command being checked was \"%s %s %s\"\n", cmnd_tok, motor_tok, param_tok); return False; } } /* **-------------------------------------------------------------------------- ** LoadCheckOneFloat: routine to check that a command specifying ** a single real value set correctly. */ int LoadCheckOneFloat (char *cmnd, int n_dec) { /* ================= */ int status, len, n_dec_ok; char my_cmnd[80], rd_cmnd[40], buff[40], param[40]; char *cmnd_tok, *motor_tok, *param_tok, *rd_tok; char *whole_tok, *frac_tok; StrJoin (my_cmnd, sizeof (my_cmnd), cmnd, ""); /* Make a copy of cmnd */ len = strlen (my_cmnd); if (my_cmnd[len-1] == '\r') my_cmnd[len-1] = NIL; cmnd_tok = strtok (my_cmnd, " "); motor_tok = strtok (NULL, " "); param_tok = strtok (NULL, " "); if ((cmnd_tok == NULL) || (motor_tok == NULL) || (param_tok == NULL)) { printf ("\007Software problem in LoadCheckOneFloat\n"); return False; } /* ** Check that the number of decimal places in the set parameter ** agrees with the setting of the EL734. */ n_dec_ok = True; /* Assume it will be OK */ StrJoin (param, sizeof (param), param_tok, ""); whole_tok = strtok (param, "."); frac_tok = strtok (NULL, "."); if (frac_tok == NULL) { /* Check for a decimal point */ len = strlen (whole_tok); /* None there, so put in a ".0" */ frac_tok = whole_tok + len + 1; frac_tok[0] = '0'; frac_tok[1] = NIL; } len = strlen (frac_tok); if (len > n_dec) { /* Param has too many decimal places */ /* Try to remove trailing zeros */ while ((len >= 0) && (frac_tok[len-1] == '0')) { len = len - 1; frac_tok[len] = NIL; if (len == n_dec) break; } if (len != n_dec) { printf ("Don't expect the parameter to verify correctly.\n" "You have specified too many decimal places!\n"); n_dec_ok = False; /* Remember it (to suppress retries) */ } }else if (len < n_dec) { /* Param has too few decimal places */ while (len < n_dec) { /* Pad with zeros */ frac_tok[len] = '0'; len = len + 1; frac_tok[len] = NIL; } } len = strlen (whole_tok); /* Re-join the parts of param again */ whole_tok[len] = '.'; StrJoin (buff, sizeof (buff), cmnd_tok, " "); StrJoin (rd_cmnd, sizeof (rd_cmnd), buff, motor_tok); len = strlen (rd_cmnd); rd_cmnd[len] = '\r'; rd_cmnd[len+1] = NIL; status = EL734_SendCmnd (&Hndl, rd_cmnd, buff, sizeof (buff)); if (status) { rd_tok = strtok (buff, " "); if ((rd_tok == NULL) || (strcmp (param, rd_tok) != 0)) { if (rd_tok == NULL) rd_tok = ""; printf ("\007Verify error for command \"%s %s %s\"\n", cmnd_tok, motor_tok, param); printf ("Value set in EL734 controller is \"%s\"\n" " It should be \"%s\"\n", rd_tok, param); if (n_dec_ok) { status = LoadFloatJuggle (cmnd_tok, motor_tok, param, n_dec); } return status; }else { return True; /* The parameter has been set correctly! */ } }else { printf ("\007LoadCheckOneFloat -- error from EL734_SendCmnd\n"); printf ("Command being checked was \"%s %s %s\"\n", cmnd_tok, motor_tok, param); return False; } } /* **-------------------------------------------------------------------------- ** DoStop: Send a STOP command to the motor. If has ** been detected, assume that this is an emergency ** stop and do fewer tests. */ int DoStop () { /* ====== */ int status, no_errors, i, len, act_len; int msr, ored_msr, fp_cntr, fr_cntr, ss; char recd[132], buff[132], cmnd[80]; float f_tmp; no_errors = True; if (!Ctrl_C_has_happened) { EL734_ZeroStatus (&Hndl); printf ("Sending STOP command to motor %d ...", Motor); }else { printf ("\n\007 detected: Sending STOP command to motor %d ...", Motor); } status = EL734_Stop (&Hndl); if (!status) no_errors = False; if (no_errors) { if (Ctrl_C_has_happened) { printf (" OK.\n"); return True; }else { printf ("\nWwaiting for motor to become idle ... "); status = My_WaitIdle (&Hndl, Verbose, &ored_msr, &fp_cntr, &fr_cntr, &Ist_pos); if (!status) { if (Ctrl_C_has_happened) { printf ("\n\007 detected: Wait-for-idle abandoned!\n"); }else { printf ("\n\007 Error return status from My_WaitIdle!\n"); } return False; } if ((ored_msr & MSR__STOPPED) == 0) { printf ("\n\007 Warning -- MSR STOP bit is not set!\n"); return False; } if ((ored_msr & (~MSR__STOPPED)) != 0) { printf ("\n\007 ... unexpected MSR obtained!\n"); printf (" %s\n", EL734_EncodeMSR (buff, sizeof (buff), 0, ored_msr, fp_cntr, fr_cntr)); return False; } printf (" OK.\n"); } return True; }else { printf ("\n\007 STOP command not accepted!\n"); return False; } } /* **-------------------------------------------------------------------------- ** DoLimits: Set the lower and upper software limits */ int DoLimits ( /* ======== */ float lo, float hi) { int status, no_errors, i, len, act_len; int msr, ored_msr, fp_cntr, fr_cntr, ss; char recd[132], buff[132], cmnd[80]; float f_tmp; no_errors = False; printf ("Sending command \"h %d %.*f %.*f\" ...", Motor, Dec_pt, lo, Dec_pt, hi); sprintf (cmnd, "h %d %.*f %.*f\r", Motor, Dec_pt, lo, Dec_pt, hi); status = EL734_SendCmnd (&Hndl, cmnd, buff, sizeof (buff)); if (status && (buff[0] == NIL)) no_errors = True; if (no_errors) { printf (" OK.\n"); return True; }else { printf ("\n\007 Command not accepted!\n"); return False; } } /* **-------------------------------------------------------------------------- ** DoSimpleMove: Send a simple move command to the motor and wait for idle */ int DoSimpleMove ( /* ============ */ char *a_cmnd, int test_status) { int status, i, len, act_len; int msr, ored_msr, fp_cntr, fr_cntr, ss; char recd[132], buff[132], cmnd[80]; float f_tmp; char *etxt; EL734_ZeroStatus (&Hndl); sprintf (cmnd, a_cmnd, Motor); printf ("Sending \"%s\" command ...", cmnd); i = strlen (cmnd); cmnd[i] = '\r'; cmnd[i+1] = NIL; status = EL734_SendCmnd (&Hndl, cmnd, buff, sizeof (buff)); if (!status) { PrintErrInfo ("EL734_SendCmnd"); return False; } if (buff[0] != NIL) { printf ("\007 response was \"%s\".\n", buff); etxt = "Unrecognised response!"; if (strcmp (buff, "?BSY") == 0) etxt = "Motor busy!"; if (strcmp (buff, "?CMD") == 0) etxt = "Bad command!"; if (strcmp (buff, "?LOC") == 0) etxt = "Controller is in manual mode!"; if (strcmp (buff, "?ADR") == 0) etxt = "Bad motor number!"; if (strcmp (buff, "?RNG") == 0) etxt = "Range error! Check low/high limits."; if (strcmp (buff, "*MS") == 0) etxt = "Motor is disabled: \"Stop\" signal is active!"; if (strcmp (buff, "*ES") == 0) etxt = "Motor is disabled: \"Emergency Stop\" signal is active!"; if (strncmp (buff, "?TMO", 4) == 0) etxt = "Time-out! You should check the cables, perhaps."; printf ("%s\n", etxt); return False; } printf ("\nWaiting for motor to become idle ..."); fflush (NULL); status = My_WaitIdle (&Hndl, Verbose, &ored_msr, &fp_cntr, &fr_cntr, &Ist_pos); if (!status) { if (Ctrl_C_has_happened) DoStop (); return False; } if (test_status && ((ored_msr & (~MSR__BUSY)) != MSR__OK)) { printf ("\n\007 ... unexpected MSR obtained!\n"); printf (" %s\n", EL734_EncodeMSR (buff, sizeof (buff), 0, ored_msr, fp_cntr, fr_cntr)); return False; } printf (" OK.\nPosition = %.*f\n", Dec_pt, Ist_pos); return True; } /* **-------------------------------------------------------------------------- ** DoSimpleSet: Send a parameter set command to the motor and ** check for null response. */ int DoSimpleSet ( /* =========== */ char *a_cmnd) { int status, i; char cmnd[80], buff[40]; sprintf (cmnd, a_cmnd, Motor); printf ("Sending \"%s\" command ...", cmnd); i = strlen (cmnd); cmnd[i] = '\r'; cmnd[i+1] = NIL; status = EL734_SendCmnd (&Hndl, cmnd, buff, sizeof (buff)); if (status && (buff[0] == NIL)) { printf (" OK.\n"); return True; }else if (!status) { printf ("\n\007"); PrintErrInfo ("EL734_SendCmnd"); return False; }else { printf ("\n\007Error response from the motor: \"%s\"!\n", buff); return False; } } /* **-------------------------------------------------------------------------- ** DoSetPos: Set the current position */ int DoSetPos (float ist) { /* ======== */ int status; char cmnd[80]; sprintf (cmnd, "UU %%d %.*f", Dec_pt, ist); status = DoSimpleSet (cmnd); if (status) printf ("Position set to %.*f\n", Dec_pt, ist); return status; } /* **-------------------------------------------------------------------------- ** DoRef: Perform a Reference Seek */ int DoRef ( /* ===== */ float *shift) { int status, no_restore, i, len, act_len; int msr, ored_msr, fp_cntr, fr_cntr, ss; char recd[132], buff[132], cmnd[80]; int k, v; float lo, hi, q, zero_pt, targ; float f_tmp; /*----------------------------------------------------------------- ** Absolute encoder? */ status = GetKHVQZ (&k, &lo, &hi, &v, &q, &zero_pt); if (!status) return status; if (k == 0) { printf ("\n\007Absolute encoder, K = 0, " "\"-ref\" option is not meaningful!\n"); return False; }else { printf ("Performing reference point seek.\n"); sprintf (recd, "%.*f", Dec_pt, zero_pt); if (k == -1 || k == -11) { printf ("Reference point = %s (lower limit switch) ...", recd); }else if (k == 1 || k == 11) { printf ("Reference point = %s (upper limit switch) ...", recd); }else if (k == 2 || k == 12) { printf ("Reference point = %s (separate limit switch) ...", recd); }else { printf ("Reference point = %s (reference mode = %d (unrecognised)) ...", recd, k); } } /*---------------------------------------------------------------*/ *shift = 0.0; status = EL734_GetStatus (&Hndl, &msr, &ored_msr, &fp_cntr, &fr_cntr, &ss, &Ist_pos); if ((!status) || (msr == -1)) { printf ("\n\007"); printf ("Bad status from EL734_GetStatus.\n" " ... failed to do reference seek.\n"); return False; } EL734_ZeroStatus (&Hndl); sprintf (cmnd, "rf %d\r", Motor); /* Start reference seek */ status = EL734_SendCmnd (&Hndl, cmnd, buff, sizeof (buff)); if (!status) { printf ("\n\007" " ... failed to initiate reference seek.\n"); PrintErrInfo ("EL734_SendCmnd"); return False; }else { if (buff[0] != NIL) { printf ("\n\007" " ... error response when initiating reference seek:" " \"%s\".\n" " Operation abandoned.\n", buff); return False; } } status = My_WaitIdle (&Hndl, Verbose, &ored_msr, &fp_cntr, &fr_cntr, &Ist_pos); if (!status) { if (Ctrl_C_has_happened) { DoStop (); }else { PrintErrInfo ("My_WaitIdle"); } return False; } if ((ored_msr & MSR__REF_OK) != 0) { printf (" OK.\n"); }else { if ((ored_msr & MSR__REF_FAIL) != 0) { printf ("\007 failed!\n"); }else { printf ("\007 unexpected MSR obtained!\n"); } printf (" %s\n", EL734_EncodeMSR (recd, sizeof (recd), 0, ored_msr, fp_cntr, fr_cntr)); return False; } *shift = Ist_pos - zero_pt; sprintf (recd, "%.*f", Dec_pt, *shift); printf ("Position = %.*f, Zero-point error = %s\n", Dec_pt, Ist_pos, recd); sscanf (recd, "%f", &f_tmp); if (f_tmp != 0.0) { sprintf (recd, "%.*f", Dec_pt, zero_pt); printf ("\007Setting current position to be %s\n", recd); status = DoSetPos (zero_pt); if (!status) return False; } if ((zero_pt < lo) || (zero_pt > hi)) { /* Move into range? */ if (zero_pt < lo) targ = lo; /* Yes */ if (zero_pt > lo) targ = hi; printf ("Moving into low-high range ...\n"); sprintf (cmnd, "P %%d %.*f", Dec_pt, targ); status = DoSimpleMove (cmnd, True); } return True; } /* **-------------------------------------------------------------------------- ** DoFF: Send a FF command to the motor */ int DoFF () { /* ==== */ int status; char cmnd[32]; if (Frequency > 0) { sprintf (cmnd, "FF %%d %d", Frequency); }else { strcpy (cmnd, "FF %d"); } status = DoSimpleMove (cmnd, True); return status; } /* **-------------------------------------------------------------------------- ** DoFB: Send a FB command to the motor */ int DoFB () { /* ==== */ int status; char cmnd[32]; if (Frequency > 0) { sprintf (cmnd, "FB %%d %d", Frequency); }else { strcpy (cmnd, "FB %d"); } status = DoSimpleMove (cmnd, True); return status; } /* **-------------------------------------------------------------------------- ** DoSF: Send a SF command to the motor */ int DoSF () { /* ==== */ int status; char cmnd[32]; if (Frequency > 0) { sprintf (cmnd, "SF %%d %d", Frequency); }else { strcpy (cmnd, "SF %d"); } status = DoSimpleMove (cmnd, True); return status; } /* **-------------------------------------------------------------------------- ** DoSB: Send a SB command to the motor */ int DoSB () { /* ==== */ int status; char cmnd[32]; if (Frequency > 0) { sprintf (cmnd, "SB %%d %d", Frequency); }else { strcpy (cmnd, "SB %d"); } status = DoSimpleMove (cmnd, True); return status; } /* **-------------------------------------------------------------------------- ** DoHunt: hunt for the motor's reference point. */ int DoHunt () { /* ====== */ int status; int k; float lo, hi, q, shift, zero; int v; int msr, ored_msr, fp_cntr, fr_cntr, ss; float step, targ, f_tmp; char cmnd[20]; status = GetKHVQZ (&k, &lo, &hi, &v, &q, &zero); if (!status) return status; /*----------------------------------------------------------------- ** Absolute encoder */ if (k == 0) { printf ("\n\007Absolute encoder, K = 0, " "-hunt option is not meaningful!\n"); return False; /*----------------------------------------------------------------- ** Lo-Lim is Ref Pt */ }else if ((k == -1) || (k == -11)) { printf ("Reference point = %.*f (low limit switch)\n", Dec_pt, zero); if (q <= 0) { printf ("\n\007Q = %.*f. This is inconsistent with K = %d!\n", Dec_pt, q, k); return False; } status = DoSimpleSet ("H %d -8000 8000"); if (!status) return status; status = DoSetPos (0.0); if (!status) return status; status = DoSimpleMove ("FB %d", False); /* Do FB but don't test MSR at end */ if (!status) return status; DoLimits (lo, hi); /* Reset lo/hi limits */ status = EL734_GetStatus (&Hndl, &msr, &ored_msr, &fp_cntr, &fr_cntr, &ss, &Ist_pos); if (msr == -1) { printf ("\n\007Bad status from EL734_GetStatus!"); return False; } if ((ored_msr & MSR__LO_LIM) == 0) { printf ("\n\007Low-limit switch was not reached!"); return False; } status = DoSetPos (zero); if (!status) return status; if ((ss & SS__LSX) != 0) { printf ("\n\007Reference-point is still active!"); return False; } status = DoRef (&shift); return status; /*----------------------------------------------------------------- ** Hi-Lim is Ref Pt */ }else if ((k == 1) || (k == 11)) { printf ("Reference point = %.*f (high limit switch)\n", Dec_pt, zero); if (q <= 0) { printf ("\n\007Q = %.*f. This is inconsistent with K = %d!\n", Dec_pt, q, k); return False; } status = DoSimpleSet ("H %d -8000 8000"); if (!status) return status; status = DoSetPos (0.0); if (!status) return status; status = DoSimpleMove ("FF %d", False); /* Do FF but don't test MSR at end */ if (!status) return status; DoLimits (lo, hi); /* Reset lo/hi limits */ status = EL734_GetStatus (&Hndl, &msr, &ored_msr, &fp_cntr, &fr_cntr, &ss, &Ist_pos); if (msr == -1) { printf ("\n\007Bad status from EL734_GetStatus!"); return False; } if ((ored_msr & MSR__HI_LIM) == 0) { printf ("\n\007High-limit switch was not reached!"); return False; } status = DoSetPos (zero); if (!status) return status; if ((ss & SS__LSX) != 0) { printf ("\n\007Reference-point is still active!"); return False; } status = DoRef (&shift); return status; /*----------------------------------------------------------------- ** Separate Ref Pt */ }else if ((k == 2) || (k == 12)) { printf ("Reference point = %.*f (separate switch)\n", Dec_pt, zero); if (q == 0.0) { printf ("\n\007Q = %.*f. This is inconsistent with K = %d!\n", Dec_pt, q, k); return False; }else { sprintf (cmnd, "%.*f", Dec_pt, q); /* Check Q param is not too small */ sscanf (cmnd, "%f", &f_tmp); if (f_tmp == 0.0) { printf ("\n\007Q = %f. This is too small!\n", q); return False; } } status = DoSimpleSet ("H %d -8000 8000"); if (!status) return status; sprintf (cmnd, "P %%d %.*f", Dec_pt, (zero - (q/2.0))); printf ("Moving to start position.\n"); status = DoSimpleMove (cmnd, False); status = DoSetPos (0.0); if (!status) { DoLimits (lo, hi); return status; } step = 0.95 * q; targ = 0.0; printf ("Low-to-High distance = %.*f\n", Dec_pt, (hi - lo)); printf ("Step size = %.*f\n", Dec_pt, step); if (step > 0) { printf ("Stepping to low-limit switch looking for ref-point ...\n"); }else { printf ("Stepping to high-limit switch looking for ref-point ...\n"); } fflush (NULL); status = EL734_GetStatus (&Hndl, &msr, &ored_msr, &fp_cntr, &fr_cntr, &ss, &Ist_pos); while (status && ((ored_msr & MSR__LO_LIM) == 0) && ((ored_msr & MSR__HI_LIM) == 0) && ((ss & SS__LSX) == 0)) { targ = targ - step; sprintf (cmnd, "P %%d %.*f", Dec_pt, targ); status = DoSimpleMove (cmnd, False); if (!status) { if (Ctrl_C_has_happened) { DoStop (); }else { PrintErrInfo ("DoSimpleMove"); } DoLimits (lo, hi); return False; } status = EL734_GetStatus (&Hndl, &msr, &ored_msr, &fp_cntr, &fr_cntr, &ss, &Ist_pos); } if (!status) return False; if ((ored_msr & (MSR__LO_LIM | MSR__HI_LIM)) != 0) { printf ("Got to limit switch. Ref-point not found. " "Returning to Start.\n"); fflush (NULL); status = DoSimpleMove ("P %d 0.0", False); if (!status) { if (Ctrl_C_has_happened) { DoStop (); }else { PrintErrInfo ("DoSimpleMove"); } DoLimits (lo, hi); return False; } targ = 0.0; if (step > 0) { printf ("Stepping to high-limit switch looking for ref-point ...\n"); }else { printf ("Stepping to low-limit switch looking for ref-point ...\n"); } status = EL734_GetStatus (&Hndl, &msr, &ored_msr, &fp_cntr, &fr_cntr, &ss, &Ist_pos); while (status && ((ored_msr & MSR__LO_LIM) == 0) && ((ored_msr & MSR__HI_LIM) == 0) && ((ss & SS__LSX) == 0)) { targ = targ + step; sprintf (cmnd, "P %%d %.*f", Dec_pt, targ); status = DoSimpleMove (cmnd, False); if (!status) { if (Ctrl_C_has_happened) { DoStop (); }else { PrintErrInfo ("DoSimpleMove"); } DoLimits (lo, hi); return False; } status = EL734_GetStatus (&Hndl, &msr, &ored_msr, &fp_cntr, &fr_cntr, &ss, &Ist_pos); } if (!status) return False; if ((ored_msr & (MSR__LO_LIM | MSR__HI_LIM)) != 0) { printf ("\n\007Got to limit switch. Ref-point not found!\n"); printf ("Hunt operation abandoned.\n"); DoLimits (lo, hi); if (step > 0) DoSetPos (hi); else DoSetPos (lo); return False; } } DoLimits (lo, hi); /* Reset lo/hi limits */ if ((ss & SS__LSX) == 0) { printf ("\n\007Ref-point not found!\n"); DoLimits (lo, hi); return False; } status = DoRef (&shift); return status; }else { printf ("\n\007Reference Mode, K = %d. Unrecognised value!\n", k); return False; } } /* **-------------------------------------------------------------------------- ** DoSave: Get all parameter settings of motor. */ int DoSave () { /* ====== */ int status, no_errors; char buff[80]; int msr, ored_msr, fp_cntr, fr_cntr, ss; int air_cush, inp_state, act_mot; FILE *lun; time_t time_now; struct EL734info *info_ptr; char cmnd00[10], cmnd01[10], cmnd02[10], cmnd03[10], cmnd04[10]; char cmnd05[10], cmnd06[10], cmnd07[10], cmnd08[10], cmnd09[10]; char cmnd10[10], cmnd11[10], cmnd12[10], cmnd13[10], cmnd14[10]; char cmnd15[10], cmnd16[10], cmnd17[10], cmnd18[10], cmnd19[10]; char cmnd20[10], cmnd21[10], cmnd22[10], cmnd23[10], cmnd24[10]; char cmnd25[10], cmnd26[10], cmnd27[10], cmnd28[10], cmnd29[10]; char cmnd30[10], cmnd31[10]; char *rptr00, *rptr01, *rptr02, *rptr03, *rptr04; char *rptr05, *rptr06, *rptr07, *rptr08, *rptr09; char *rptr10, *rptr11, *rptr12, *rptr13, *rptr14; char *rptr15, *rptr16, *rptr17, *rptr18, *rptr19; char *rptr20, *rptr21, *rptr22, *rptr23, *rptr24; char *rptr25, *rptr26, *rptr27, *rptr28, *rptr29; char *rptr30, *rptr31; int no_EC_cmnd = True; int no_A_cmnd = True; int no_FD_cmnd = True; int no_FM_cmnd = True; int no_D_cmnd = True; int no_E_cmnd = True; int no_F_cmnd = True; int no_G_cmnd = True; int no_H_cmnd = True; int no_J_cmnd = True; int no_K_cmnd = True; int no_L_cmnd = True; int no_M_cmnd = True; int no_Q_cmnd = True; int no_T_cmnd = True; int no_V_cmnd = True; int no_W_cmnd = True; int no_Z_cmnd = True; int no_SP_cmnd = True; int no_ST_cmnd = True; int no_SR_cmnd = True; int no_SA_cmnd = True; int no_AC_cmnd = True; int no_RI_cmnd = True; int no_AM_cmnd = True; int no_EP_cmnd = True; int no_KP_cmnd = True; int no_KI_cmnd = True; int no_KD_cmnd = True; if ((strcmp (Save_file, "-") == 0) || (strcmp (Save_file, "=") == 0)) { /* Use standard output? */ lun = stdout; /* Yes */ }else { lun = fopen (Save_file, "w"); if (lun == NULL) return False; printf ("Writing motor parameters to file %s ...", Save_file); } time_now = time (NULL); fprintf (lun, "! EL734 Status at %s", asctime (localtime (&time_now))); fprintf (lun, "! ============\n"); sprintf (cmnd00, "id\r"); sprintf (cmnd01, "mn %d\r", Motor); sprintf (cmnd02, "mem %d\r", Motor); sprintf (cmnd03, "ec %d\r", Motor); sprintf (cmnd04, "a %d\r", Motor); sprintf (cmnd05, "fd %d\r", Motor); sprintf (cmnd06, "fm %d\r", Motor); sprintf (cmnd07, "d %d\r", Motor); sprintf (cmnd08, "e %d\r", Motor); sprintf (cmnd09, "f %d\r", Motor); sprintf (cmnd10, "g %d\r", Motor); sprintf (cmnd11, "h %d\r", Motor); sprintf (cmnd12, "j %d\r", Motor); sprintf (cmnd13, "k %d\r", Motor); sprintf (cmnd14, "l %d\r", Motor); sprintf (cmnd15, "m %d\r", Motor); sprintf (cmnd16, "q %d\r", Motor); sprintf (cmnd17, "t %d\r", Motor); sprintf (cmnd18, "v %d\r", Motor); sprintf (cmnd19, "w %d\r", Motor); sprintf (cmnd20, "z %d\r", Motor); sprintf (cmnd21, "sp %d\r", Motor); sprintf (cmnd22, "st %d\r", Motor); sprintf (cmnd23, "sr %d\r", Motor); sprintf (cmnd24, "sa %d\r", Motor); sprintf (cmnd25, "ac %d\r", Motor); sprintf (cmnd26, "ri %d\r", Motor); sprintf (cmnd27, "am\r"); sprintf (cmnd28, "ep %d\r", Motor); sprintf (cmnd29, "kp %d\r", Motor); sprintf (cmnd30, "ki %d\r", Motor); sprintf (cmnd31, "kd %d\r", Motor); no_errors = True; info_ptr = (struct EL734info *) Hndl; status = AsynSrv_SendCmnds (&info_ptr->asyn_info, &info_ptr->to_host, &info_ptr->from_host, cmnd00, cmnd01, cmnd02, cmnd03, cmnd04, cmnd05, cmnd06, cmnd07, cmnd08, cmnd09, cmnd10, cmnd11, cmnd12, cmnd13, cmnd14, cmnd15, cmnd16, cmnd17, cmnd18, cmnd19, cmnd20, cmnd21, cmnd22, cmnd23, cmnd24, cmnd25, cmnd26, cmnd27, cmnd28, cmnd29, cmnd30, cmnd31, NULL); if (status) { rptr00 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, NULL); rptr01 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr00); rptr02 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr01); rptr03 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr02); rptr04 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr03); rptr05 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr04); rptr06 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr05); rptr07 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr06); rptr08 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr07); rptr09 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr08); rptr10 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr09); rptr11 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr10); rptr12 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr11); rptr13 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr12); rptr14 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr13); rptr15 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr14); rptr16 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr15); rptr17 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr16); rptr18 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr17); rptr19 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr18); rptr20 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr19); rptr21 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr20); rptr22 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr21); rptr23 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr22); rptr24 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr23); rptr25 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr24); rptr26 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr25); rptr27 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr26); rptr28 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr27); rptr29 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr28); rptr30 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr29); rptr31 = AsynSrv_GetReply (&info_ptr->asyn_info, &info_ptr->from_host, rptr30); if ((rptr00 == NULL) || (rptr01 == NULL) || (rptr02 == NULL) || (rptr03 == NULL) || (rptr04 == NULL) || (rptr05 == NULL) || (rptr06 == NULL) || (rptr07 == NULL) || (rptr08 == NULL) || (rptr09 == NULL) || (rptr10 == NULL) || (rptr11 == NULL) || (rptr12 == NULL) || (rptr13 == NULL) || (rptr14 == NULL) || (rptr15 == NULL) || (rptr16 == NULL) || (rptr17 == NULL) || (rptr18 == NULL) || (rptr19 == NULL) || (rptr20 == NULL) || (rptr21 == NULL) || (rptr22 == NULL) || (rptr23 == NULL) || (rptr24 == NULL) || (rptr25 == NULL) || (rptr26 == NULL) || (rptr27 == NULL) || (rptr28 == NULL) || (rptr29 == NULL) || (rptr30 == NULL) || (rptr31 == NULL)) { no_errors = False; }else { StrJoin (Ctrl_id, sizeof (Ctrl_id), rptr00, ""); StrJoin (Mot_name, sizeof (Mot_name), rptr01, ""); StrJoin (Mot_mem, sizeof (Mot_mem), rptr02, ""); if (sscanf (rptr03, "%d %d", &Enc_typ, &Enc_num) == 2) no_EC_cmnd = False; if (sscanf (rptr04, "%d", &Dec_pt) == 1) no_A_cmnd = False; if (sscanf (rptr05, "%d %d", &Enc_fact_0, &Enc_fact_1) == 2) no_FD_cmnd = False; if (sscanf (rptr06, "%d %d", &Mot_fact_0, &Mot_fact_1) == 2) no_FM_cmnd = False; if (sscanf (rptr07, "%f", &Inertia_tol) == 1) no_D_cmnd = False; if (sscanf (rptr08, "%d", &Ramp) == 1) no_E_cmnd = False; if (sscanf (rptr09, "%d", &Loop_mode) == 1) no_F_cmnd = False; if (sscanf (rptr10, "%d", &Slow_hz) == 1) no_G_cmnd = False; if (sscanf (rptr11, "%f %f", &Lo, &Hi) == 2) no_H_cmnd = False; if (sscanf (rptr12, "%d", &Fast_hz) == 1) no_J_cmnd = False; if (sscanf (rptr13, "%d", &Ref_mode) == 1) no_K_cmnd = False; if (sscanf (rptr14, "%d", &Backlash) == 1) no_L_cmnd = False; if (sscanf (rptr15, "%d", &Pos_tol) == 1) no_M_cmnd = False; if (sscanf (rptr16, "%f", &Ref_param) == 1) no_Q_cmnd = False; if (sscanf (rptr17, "%d", &Is_sided) == 1) no_T_cmnd = False; if (sscanf (rptr18, "%d", &Null_pt) == 1) no_V_cmnd = False; if (sscanf (rptr19, "%d", &Ac_par) == 1) no_W_cmnd = False; if (sscanf (rptr20, "%d", &Enc_circ) == 1) no_Z_cmnd = False; if (sscanf (rptr21, "%d", &Stat_pos) == 1) no_SP_cmnd = False; if (sscanf (rptr22, "%d", &Stat_pos_flt) == 1) no_ST_cmnd = False; if (sscanf (rptr23, "%d", &Stat_pos_fail) == 1) no_SR_cmnd = False; if (sscanf (rptr24, "%d", &Stat_cush_fail) == 1) no_SA_cmnd = False; if (sscanf (rptr25, "%d", &air_cush) == 1) no_AC_cmnd = False; if (sscanf (rptr26, "%d", &inp_state) == 1) no_RI_cmnd = False; if (sscanf (rptr27, "%x", &act_mot) == 1) no_AM_cmnd = False; if (sscanf (rptr28, "%d", &Enc_par) == 1) no_EP_cmnd = False; if (sscanf (rptr29, "%d", &Prop) == 1) no_KP_cmnd = False; if (sscanf (rptr30, "%d", &Integ) == 1) no_KI_cmnd = False; if (sscanf (rptr31, "%d", &Deriv) == 1) no_KD_cmnd = False; } }else { no_errors = False; } if (no_errors) { EL734_GetStatus (&Hndl, &msr, &ored_msr, &fp_cntr, &fr_cntr, &ss, &Ist_pos); if (msr == -1) no_errors = False; } if (no_errors) goto ds_do; printf ("\007"); fprintf (lun, "!\n"); fprintf (lun, "! Failed to get status of motor\n"); if (lun != stdout) { fclose (lun); printf ("\007error detected.\n"); } return False; ds_do: if (no_K_cmnd) { fprintf (lun, "!\n" "! EL734 ID = \"%s\"\n" "! Server \"%s\"\n" "! Port %5d\n" "! Channel %5d\n" "! Motor %5d\n", Ctrl_id, El734_host, El734_port, El734_chan, Motor); }else { fprintf (lun, "!\n" "! Reference mode information: EL734 ID = \"%s\"\n" "! K = -11 = LoLim + Index is ref. pt. Server \"%s\"\n" "! -1 = LoLim is ref. pt. Port %5d\n" "! 0 = Abs encoder Channel %5d\n" "! 1 = HiLim is ref. pt. Motor %5d\n" "! 2 = Separate ref. pt.\n" "! 11 = HiLim + Index is ref. pt.\n" "! 12 = Separate + Index ref. pt.\n", Ctrl_id, El734_host, El734_port, El734_chan, Motor); } fprintf (lun, "!\n"); if (!no_SP_cmnd) fprintf (lun, "! # of positionings, SP = %d\n", Stat_pos); if (!no_ST_cmnd) fprintf (lun, "! # of positioning faults, ST = %d\n", Stat_pos_flt); if (!no_SR_cmnd) fprintf (lun, "! # of positioning failures, SR = %d\n", Stat_pos_fail); if (!no_SA_cmnd) fprintf (lun, "! # of air-cushion failures, SA = %d\n", Stat_cush_fail); fprintf (lun, "! %s", EL734_EncodeMSR (buff, sizeof (buff), msr, ored_msr, fp_cntr, fr_cntr)); fprintf (lun, " %s\n", EL734_EncodeSS (buff, sizeof (buff), ss)); if (!no_W_cmnd) { if (Ac_par == 0) { switch (air_cush) { case 0: break; /* Don't mention air cushions in this case! */ case 1: fprintf (lun, "! Air-cushion status is \"on\".\n"); break; default: fprintf (lun, "! Air-cushion status = %d.\n", air_cush); } }else { switch (air_cush) { case 0: fprintf (lun, "! Air-cushion is \"down\"\n"); break; case 1: fprintf (lun, "! Air-cushion is \"up\"\n"); break; default: fprintf (lun, "! Air-cushion status = %d.\n", air_cush); } } } if (!no_RI_cmnd) { switch (inp_state) { case 0: fprintf (lun, "! Input status is \"off\".\n"); break; case 1: fprintf (lun, "! Input status is \"on.\"\n"); break; default: fprintf (lun, "! Input status = %d.\n", inp_state); } } if (!no_AM_cmnd) { if (act_mot != 0) { fprintf (lun, "! Active motor status = 0x%03X\n", act_mot); }else { fprintf (lun, "! No motors are active.\n"); } } fprintf (lun, "!\n"); if (Mot_name[0] == NIL) { sprintf (buff, " mn %%d ..............."); }else { sprintf (buff, " mn %%d %s", Mot_name); } fprintf (lun, "%-32s! %s\n", buff, "Motor name"); if (!no_EC_cmnd) { sprintf (buff, " ec %%d 0 0"); fprintf (lun, "%-32s! %s\n", buff, "Zero the encoder mapping"); sprintf (buff, " ec %%d %s", rptr03); fprintf (lun, "%-32s! %s\n", buff, "Encoder mapping (type/number)"); } if (!no_EP_cmnd) { sprintf (buff, " ep %%d %s", rptr28); fprintf (lun, "%-32s! %s\n", buff, "Encoder magic parameter"); } if (!no_A_cmnd) { sprintf (buff, " a %%d %s", rptr04); fprintf (lun, "%-32s! %s\n", buff, "Precision"); } if (!no_FD_cmnd) { sprintf (buff, " fd %%d %s", rptr05); fprintf (lun, "%-32s! %s\n", buff, "Encoder gearing (numer/denom)"); } if (!no_FM_cmnd) { sprintf (buff, " fm %%d %s", rptr06); fprintf (lun, "%-32s! %s\n", buff, "Motor gearing (numer/denom)"); } if (!no_D_cmnd) { sprintf (buff, " d %%d %s", rptr07); fprintf (lun, "%-32s! %s\n", buff, "Inertia tolerance"); } if (!no_E_cmnd) { sprintf (buff, " e %%d %s", rptr08); fprintf (lun, "%-32s! %s\n", buff, "Start/stop ramp (kHz/sec)"); } if (!no_F_cmnd) { sprintf (buff, " f %%d %s", rptr09); fprintf (lun, "%-32s! %s\n", buff, "Open loop/Closed loop (0/1)"); } if (!no_G_cmnd) { sprintf (buff, " g %%d %s", rptr10); fprintf (lun, "%-32s! %s\n", buff, "Start/stop frequency (Mot-S/sec)"); } if (!no_H_cmnd) { sprintf (buff, " h %%d %s", rptr11); fprintf (lun, "%-32s! %s\n", buff, "Low/High Software Limits"); } if (!no_J_cmnd) { sprintf (buff, " j %%d %s", rptr12); fprintf (lun, "%-32s! %s\n", buff, "Top speed (Mot-S/sec)"); } if (!no_K_cmnd) { sprintf (buff, " k %%d %s", rptr13); fprintf (lun, "%-32s! %s\n", buff, "Reference mode"); } if (!no_L_cmnd) { sprintf (buff, " l %%d %s", rptr14); fprintf (lun, "%-32s! %s\n", buff, "Backlash/Spielausgleich (Mot-S)"); } if (!no_M_cmnd) { sprintf (buff, " m %%d %s", rptr15); fprintf (lun, "%-32s! %s\n", buff, "Position tolerance (Enc-Steps)"); } if (!no_Q_cmnd) { sprintf (buff, " q %%d %s", rptr16); fprintf (lun, "%-32s! %s\n", buff, "Reference switch width"); } if (!no_T_cmnd) { sprintf (buff, " t %%d %s", rptr17); fprintf (lun, "%-32s! %s\n", buff, "One-sided operation flag (0 = no)"); } if (!no_V_cmnd) { sprintf (buff, " v %%d %s", rptr18); fprintf (lun, "%-32s! %s\n", buff, "Null point"); } if (!no_W_cmnd) { sprintf (buff, " w %%d %s", rptr19); fprintf (lun, "%-32s! %s\n", buff, "Air-cushion dependency"); } if (!no_Z_cmnd) { sprintf (buff, " z %%d %s", rptr20); fprintf (lun, "%-32s! %s\n", buff, "Circumf. of encoder (Enc-Steps)"); } if (!no_KP_cmnd) { sprintf (buff, " kp %%d %s", rptr29); fprintf (lun, "%-32s! %s\n", buff, "Proportional"); } if (!no_KI_cmnd) { sprintf (buff, " ki %%d %s", rptr30); fprintf (lun, "%-32s! %s\n", buff, "Integral"); } if (!no_KD_cmnd) { sprintf (buff, " kd %%d %s", rptr31); fprintf (lun, "%-32s! %s\n", buff, "Differential"); } if (Mot_mem[0] == NIL) { sprintf (buff, " mem %%d ..............."); }else { sprintf (buff, " mem %%d %s", Mot_mem); } fprintf (lun, "%-32s! %s\n", buff, "User data register"); if (Ref_mode != 0) { fprintf (lun, "%-32s! %s\n", " restore", "Incr. encoder" " - specify position restore"); } fprintf (lun, "!\n"); fprintf (lun, "! Current position is %.*f\n", Dec_pt, Ist_pos); fprintf (lun, "!\n"); if (lun != stdout) { fclose (lun); chmod (Save_file, 0644); printf (" OK.\n"); } return True; } /* **-------------------------------------------------------------------------- ** DoLoad: Load parameter settings from a file. */ int DoLoad () { /* ====== */ int status, go_on, no_errors, no_restore, i, len, act_len; int msr, ored_msr, fp_cntr, fr_cntr, ss; FILE *lun; char recd[132], buff[132], cmnd[80], cmnd_prnt[80]; /* ** Setting motor parameters usually causes the current ** position to get lost. Read it now so that it can be ** restored at the end if required. */ printf ("The current position, "); status = EL734_GetPrecision (&Hndl, &Dec_pt); if (status) { EL734_GetStatus (&Hndl, &msr, &ored_msr, &fp_cntr, &fr_cntr, &ss, &Ist_pos); if (msr == -1) status = False; } if (!status) { printf ("\n ... failed to get current position.\n"); return False; }else { printf ("%.*f, can be restored at end of load operation if\n" "a \"RESTORE\" command is given. Executing a \"U\" or a \"UU\"" " or any motion command\n" "will cancel the effect of a \"RESTORE\" command.\n", Dec_pt, Ist_pos); no_restore = True; } if ((strcmp (Load_file, "-") == 0) || (strcmp (Load_file, "=") == 0)) { /* Use standard input? */ lun = stdin; /* Yes */ printf ("Getting motor parameters from standard input ...\n> "); }else { lun = fopen (Load_file, "r"); if (lun == NULL) { printf ("\007Error opening file %s ... load failed.\n", Load_file); return False; } printf ("Getting motor parameters from file %s ...\n", Load_file); } go_on = True; no_errors = True; while (go_on && (fgets (recd, sizeof (recd), lun) != NULL)) { len = strlen (recd); if (len <= 1) { if (lun == stdin) {printf ("> "); fflush (NULL);} continue; } if (recd[len-1] != '\n') { recd[20] = NIL; printf ("\007 Record not terminated by \"\\n\". " "It is probably too long!\n" " The record starts thus: %s ...\n" " It has been skipped.\n", recd); if (lun == stdin) {printf ("> "); fflush (NULL);} continue; } recd[len-1] = NIL; /* Remove the terminating "\n" */ /* ** Strip off any trailing stuff (but keep it around so that we ** can print it out). "Trailing stuff" is anything after a "!". */ act_len = strcspn (recd, "!"); len = sizeof (buff); StrEdit (buff, recd, "trim compress uncomment", &len); /* ** If the remainder is just white-space, do nothing. */ if (len <= 0) { if (lun == stdin) {printf ("> "); fflush (NULL);} continue; } if (strlen (buff) >= sizeof (cmnd)) { recd[20] = NIL; printf ("\007 Record has a dubious format!!\n" " The record starts thus: %s ...\n" " It has been skipped.\n", recd); if (lun == stdin) {printf ("> "); fflush (NULL);} continue; } if (sprintf (cmnd, buff, Motor) >= sizeof (cmnd)) { fprintf (stderr, "\007 Record has generated a command which is too long.\n" " This may have corrupted the program. To be safe,\n" " we are now going to do an emergency exit. Bye.\n"); exit (False); } if ((lun != stdin) && (len > 0)) { /* Show user what's going on */ strcpy (cmnd_prnt, cmnd); MakePrint (cmnd_prnt); printf ("%-32s%s\n", cmnd_prnt, &recd[act_len]); } len = sizeof (cmnd); StrEdit (cmnd, cmnd, "upcase compress", &len); if (strncmp (cmnd, "EXIT", 4) == 0) { go_on = False; continue; }else if (strncmp (cmnd, "QUIT", 4) == 0) { go_on = False; continue; }else if (strncmp (cmnd, "NO_RESTORE", 10) == 0) { no_restore = True; printf ("The restore operation has been suppressed via " "the \"NO_RESTORE\" command.\n"); }else if (strncmp (cmnd, "RESTORE", 7) == 0) { no_restore = False; printf ("The restore operation has been requested via " "the \"RESTORE\" command.\n"); }else if (strncmp (cmnd, "WAIT", 4) == 0) { status = My_WaitIdle (&Hndl, Verbose, &ored_msr, &fp_cntr, &fr_cntr, &Ist_pos); if (!status) { go_on = no_errors = False; if (Ctrl_C_has_happened) DoStop (); continue; }else { if ((ored_msr & ~(MSR__BUSY | MSR__OK)) != 0) { printf ("! %s\n", EL734_EncodeMSR (buff, sizeof (buff), 0, ored_msr, fp_cntr, fr_cntr)); } } }else { len = strlen (cmnd); if (len == 2 && cmnd[0] == '\\' && cmnd[1] == 'R') len = 0; if (len == 2 && cmnd[0] == '\\' && cmnd[1] == '0') { cmnd[0] = NIL; /* Null command */ }else { cmnd[len] = '\r'; /* Terminate command with a */ cmnd[len+1] = NIL; } status = EL734_SendCmnd (&Hndl, cmnd, buff, sizeof (buff)); if (!status) { go_on = no_errors = False; continue; }else { if (buff[0] == NIL) { len = sizeof (cmnd); if ((strncmp (cmnd, "U ", 2) == 0) || (strncmp (cmnd, "UU ", 3) == 0) || (strncmp (cmnd, "P ", 2) == 0) || (strncmp (cmnd, "PD ", 3) == 0) || (strncmp (cmnd, "PR ", 3) == 0) || (strncmp (cmnd, "R ", 2) == 0) || (strncmp (cmnd, "FF ", 3) == 0) || (strncmp (cmnd, "FB ", 3) == 0) || (strncmp (cmnd, "SF ", 3) == 0) || (strncmp (cmnd, "SB ", 3) == 0)) { no_restore = True; }else if ((strncmp (cmnd, "A ", 2) == 0) || (strncmp (cmnd, "E ", 2) == 0) || (strncmp (cmnd, "EP ", 3) == 0) || (strncmp (cmnd, "F ", 2) == 0) || (strncmp (cmnd, "G ", 2) == 0) || (strncmp (cmnd, "J ", 2) == 0) || (strncmp (cmnd, "K ", 2) == 0) || (strncmp (cmnd, "L ", 2) == 0) || (strncmp (cmnd, "M ", 2) == 0) || (strncmp (cmnd, "T ", 2) == 0) || (strncmp (cmnd, "V ", 2) == 0) || (strncmp (cmnd, "W ", 2) == 0) || (strncmp (cmnd, "Z ", 2) == 0)) { LoadCheckOneInteger (cmnd); if (strncmp (cmnd, "A ", 2) == 0) { status = EL734_GetPrecision (&Hndl, &i); if (status) Dec_pt = i; } }else if ((strncmp (cmnd, "EC ", 3) == 0) || (strncmp (cmnd, "FD ", 3) == 0) || (strncmp (cmnd, "FM ", 3) == 0)) { LoadCheckTwoInteger (cmnd); }else if ((strncmp (cmnd, "D ", 2) == 0)) { LoadCheckOneFloat (cmnd, 1); /* D cmnd only has 1 Dec Place */ }else if ((strncmp (cmnd, "Q ", 2) == 0)) { LoadCheckOneFloat (cmnd, Dec_pt); }else if ((strncmp (cmnd, "H ", 2) == 0)) { LoadCheckTwoFloat (cmnd, Dec_pt); } }else { if (buff[0] == '?') { printf ("%s\n", buff); if (lun != stdin) { /* If input from file .. */ go_on = no_errors = False; /* .. quit */ continue; } }else { if (strncmp (cmnd, "MSR ", 4) == 0) { sscanf (buff, "%x", &ored_msr); printf ("%s ! %s\n", buff, EL734_EncodeMSR (buff, sizeof (buff), ored_msr, ored_msr, 0, 0)); }else if (strncmp (cmnd, "SS ", 3) == 0) { sscanf (buff, "%x", &ss); printf ("%s ! %s\n", buff, EL734_EncodeSS (buff, sizeof (buff), ss)); }else { printf ("%s\n", buff); } } } } } if (lun == stdin) {printf ("> "); fflush (NULL);} } /* ** Restore the current motor position. */ if (no_errors && !no_restore) { EL734_GetPrecision (&Hndl, &Dec_pt); printf ("Restoring %.*f as current motor position ...\n", Dec_pt, Ist_pos); sprintf (cmnd, "uu %d %.*f\r", Motor, Dec_pt, Ist_pos); status = EL734_SendCmnd (&Hndl, cmnd, buff, sizeof(buff)); if (status) { if (buff[0] != NIL) { no_errors = False; } }else { no_errors = False; } } if (lun != stdin) fclose (lun); if (no_errors) { printf ("\"load\" completed.\n"); EL734_GetStatus (&Hndl, &msr, &ored_msr, &fp_cntr, &fr_cntr, &ss, &Ist_pos); printf ("The motor position is %.*f\n", Dec_pt, Ist_pos); }else { printf ("\007Failed to load motor parameters.\n"); } return no_errors; } /* **--------------------------------------------------------------------------- ** DoWait - wait (if necessary) */ void DoWait ( /* ====== */ int print_flag) { int my_wait, irand; float my_rand; if (Wait_time == 0) return; if (Wait_time > 0) { if (print_flag) printf (" waiting %d secs ...", Wait_time); sleep (Wait_time); if (print_flag) printf ("\n"); return; } my_wait = -Wait_time; irand = rand () & 0x7FFF; my_rand = ((float) irand)/32768.0; my_rand = my_rand * ((float) (my_wait)); irand = (int) (my_rand + 1); if (print_flag) printf (" waiting %d secs ...", irand); sleep (irand); if (print_flag) printf ("\n"); } /* **-------------------------------------------------------------------------- ** PrintUsage: Auxilliary routine for ShowUsage and ShowItemUsage */ int PrintUsage (char **txt, int n_recd) { /* ========== */ int i = 0; int ans; printf ("\n"); while (i < n_recd) { printf ("%s\n", txt[i]); i++; if ((i % 24 == 0) && isatty (STDOUT)) { printf ("More? "); ans = getchar (); if ((ans == EOF) || (toupper (ans) == 'Q')) return False; } } printf ("\n"); } /* **-------------------------------------------------------------------------- ** ShowUsage: A simple help routine. */ void ShowUsage (int level) { /* ========= */ char *short_help_txt[] = { "\007To get help on running the program issue the command:", "", " el734_test -help"}; char *help_txt[] = { " Usage: el734_test [options ...]", "", " Valid options are:", " -help Generates this help text.", " -?