diff --git a/hardsup/el734_utility.c b/hardsup/el734_utility.c index 36ee0736..e8ef0b27 100644 --- a/hardsup/el734_utility.c +++ b/hardsup/el734_utility.c @@ -1,4 +1,4 @@ -#define ident "1D05" +#define ident "1D08" #ifdef VAXC #module EL734_Utility ident #endif @@ -59,6 +59,7 @@ ** EL734_EncodeMSR - Encode the MSR status into text. ** EL734_EncodeSS - Encode the SS flags into text. ** EL734_ErrInfo - Return detailed status from last operation. +** EL734_GetAirCush - Get W and AC register values. ** EL734_GetEncGearing - Get FD register values. ** EL734_GetId - Get ID register value. ** EL734_GetLimits - Get H register values. @@ -68,6 +69,7 @@ ** EL734_GetPrecision - Get A register value. ** EL734_GetRefMode - Get K register value. ** EL734_GetRefParam - Get Q register value. +** EL734_GetSpeeds - Get G, J and E register values. ** EL734_GetStatus - Get MSR/SS/U register values. ** EL734_GetZeroPoint - Get zero-point of motor. ** EL734_MoveNoWait - Move motor and don't wait for completion. @@ -76,7 +78,11 @@ ** EL734_PutOffline - Put the EL734 off-line. ** EL734_PutOnline - Put the EL734 on-line. ** EL734_SendCmnd - Send a command to RS232C server. +** EL734_SetAirCush - Set the air-cushion (AC register). ** EL734_SetErrcode - Set up EL734_errcode. +** EL734_SetHighSpeed - Set the max speed (J register). +** EL734_SetLowSpeed - Set the start/stop speed (G register). +** EL734_SetRamp - Set the start/stop ramp (E register). ** EL734_Stop - Send a stop command to motor. ** EL734_WaitIdle - Wait till MSR goes to zero. ** EL734_ZeroStatus - Zero the "ored-MSR" and fault counters. @@ -239,6 +245,19 @@ ** Returns detailed status of the last operation. Once an error has been ** detected, the error status is frozen until this routine has been called. **------------------------------------------------------------------------- +** int EL734_GetAirCush (&handle, &present, &state) +** ---------------- +** Get W and AC register values. +** Input Args: +** void **handle - The pointer to the structure returned by EL734_Open. +** Output Args: +** int *present - The W register. If non-zero, motor has an air-cushion. +** int *state - The AC register. If non-zero, air-cushion is up. +** Description: +** The routine is the same as EL734_GetEncGearing except that it issues +** a "W" command and then an "AC" command instead of an "FD" command to +** the controller. +**------------------------------------------------------------------------- ** int EL734_GetEncGearing (&handle, &numerator, &denominator) ** ------------------- ** Get FD register values. @@ -360,6 +379,20 @@ ** The routine is the same as EL734_GetEncGearing except that it issues ** a "Q" command instead of an "FD" command to the controller. **------------------------------------------------------------------------- +** int EL734_GetSpeeds (&handle, &lo, &hi, &ramp) +** --------------- +** Get G, J and E register values. +** Input Args: +** void **handle - The pointer to the structure returned by EL734_Open. +** Output Args: +** int *lo - The start/stop speed (G register). Units = Steps/sec. +** int *hi - The maximum speed (J register). Units = Steps/sec. +** int *ramp - The start/stop ramp (E register). Units = kHz/sec. +** Description: +** The routine is the same as EL734_GetEncGearing except that it issues +** a "G", "J" and "E" commands instead of an "FD" command to the +** controller. +**------------------------------------------------------------------------- ** int EL734_GetStatus (&handle, &msr, &ored_msr, &fp_cntr, &fr_cntr, ** --------------- &ss, &ist_posit) ** Get MSR/SS/U register values. @@ -597,6 +630,38 @@ ** Description: ** The command is passed to AsynSrv_SendCmnds and the reply extracted. **------------------------------------------------------------------------- +** int EL734_SetAirCush (&handle, state) +** ---------------- +** Set AC register value. +** Input Args: +** void **handle - The pointer to the structure returned by EL734_Open. +** Input Args: +** int state - The new state of the AC register. 0 --> down +** non-zero --> up +** Output Args: +** none +** Modified Args: +** none +** Return status: +** True if no problems detected, otherwise False and EL734_ErrInfo +** can be called to identify the problem. Values of Errcode set by +** EL734_SetAirCush are (other values may be set by the called +** routines): +** EL734__BAD_TMO, BAD_LOC, BAD_CMD, BAD_OFL, +** BAD_ADR, EMERG_STOP --> see EL734_Open. +** EL734__VFY_ERR --> the value for the AC register returned by the +** call to EL734_GetAirCush was not the value which +** was sent via the AC command. This could happen +** if there is noise on the RS232C connection to +** the EL734. +** Routines called: +** EL734_AddCallStack, AsynSrv_SendCmnds, AsynSrv_GetReply, +** EL734_GetAirCush +** Description: +** The routine issues an "AC" command to the controller to set the air- +** cushions of the motor up or down. It then calls EL734_GetAirCush +** to check that the air-cushions were set correctly. +**------------------------------------------------------------------------- ** int EL734_SetErrcode (&info_ptr, &response, &cmnd) ** ---------------- ** Set up EL734_errcode (for internal use only) @@ -618,6 +683,60 @@ ** extra action is undertaken to try to see if the emergency stop state ** is active or not. **------------------------------------------------------------------------- +** int EL734_SetHighSpeed (&handle, hi) +** ------------------ +** Set J register value. +** Input Args: +** void **handle - The pointer to the structure returned by EL734_Open. +** Input Args: +** int hi - The maximum speed (J register). Units = Steps/sec. +** Output Args: +** none +** Modified Args: +** none +** Return status: +** True if no problems detected, otherwise False and EL734_ErrInfo +** can be called to identify the problem. Values of Errcode set by +** EL734_SetHighSpeed are (other values may be set by the called +** routines): +** EL734__BAD_TMO, BAD_LOC, BAD_CMD, BAD_OFL, +** BAD_ADR, EMERG_STOP --> see EL734_Open. +** EL734__VFY_ERR --> the value for the J register returned by the call +** to EL734_GetSpeeds was not the value which was +** sent via the J command. This could happen if +** there is noise on the RS232C connection to +** the EL734. +** Routines called: +** EL734_AddCallStack, AsynSrv_SendCmnds, AsynSrv_GetReply, EL734_GetSpeeds +** Description: +** The routine issues a "J" command to the controller to set the max speed +** of the motor. It then calls EL734_GetSpeeds to check that the speed +** was set correctly. +**------------------------------------------------------------------------- +** int EL734_SetLowSpeed (&handle, hi) +** ----------------- +** Set G register value. +** Input Args: +** void **handle - The pointer to the structure returned by EL734_Open. +** Input Args: +** int lo - The start/stop speed (G register). Units = Steps/sec. +** Description: +** The routine is identical to the EL734_SetHighSpeed routine except that +** a "G" command rather than a "J" command is issued to the controller to +** set the start/stop speed. +**------------------------------------------------------------------------- +** int EL734_SetRamp (&handle, ramp) +** ------------- +** Set E register value. +** Input Args: +** void **handle - The pointer to the structure returned by EL734_Open. +** Input Args: +** int ramp - The start/stop ramp (E register). Units = kHz/sec. +** Description: +** The routine is identical to the EL734_SetHighSpeed routine except that +** an "E" command rather than a "J" command is issued to the controller to +** set the start/stop ramp. +**------------------------------------------------------------------------- ** int EL734_Stop (&handle) ** ---------- ** Send a stop command to motor @@ -676,9 +795,6 @@ #include #include #include -#ifndef LINUX -#include -#endif #include #include #include @@ -688,9 +804,12 @@ #include #ifdef __VMS -#include + #include #else -#include + #include + #ifdef FORTIFY + #include + #endif #endif /*-----------------------------------------------------------------*/ #include @@ -707,6 +826,7 @@ static char EL734_routine[5][64]; static int EL734_errcode = 0; static int EL734_errno, EL734_vaxc_errno; + char EL734_IllgText[256]; /* **--------------------------------------------------------------------------- ** EL734_AddCallStack: Add a routine name to the call stack. @@ -1087,6 +1207,65 @@ } /* **--------------------------------------------------------------------------- +** EL734_GetAirCush: Get W and AC register values. +*/ + int EL734_GetAirCush ( +/* ================ +*/ void **handle, + int *present, + int *state) { + + int status; + struct EL734info *info_ptr; + char cmnd0[10], cmnd1[10]; + char *rply_ptr, *rply_ptr0, *rply_ptr1; + /*---------------------------------------------- + */ + *present = *state = 0; + info_ptr = (struct EL734info *) *handle; + + if (!EL734_AddCallStack (info_ptr, "EL734_GetAirCush")) return False; + /*---------------------------------------------- + ** Send W and AC cmnds to EL734 + */ + sprintf (cmnd0, "w %d\r", info_ptr->motor); + sprintf (cmnd1, "ac %d\r", info_ptr->motor); + status = AsynSrv_SendCmnds (&info_ptr->asyn_info, + &info_ptr->to_host, &info_ptr->from_host, + cmnd0, cmnd1, NULL); + if (!status) { + *present = *state = 0; + EL734_errcode = EL734__BAD_ASYNSRV; + return False; + }else { + rply_ptr1 = NULL; + rply_ptr0 = AsynSrv_GetReply ( + &info_ptr->asyn_info, &info_ptr->from_host, NULL); + if (rply_ptr0 != NULL) rply_ptr1 = AsynSrv_GetReply ( + &info_ptr->asyn_info, &info_ptr->from_host, rply_ptr0); + if (rply_ptr0 == NULL) rply_ptr0 = "?no_response"; + if (rply_ptr1 == NULL) rply_ptr1 = "?no_response"; + + if ((sscanf (rply_ptr0, "%d", present) != 1) || + (sscanf (rply_ptr1, "%d", state) != 1)) { + if (*rply_ptr0 == '?') { + rply_ptr = rply_ptr0; + }else if (*rply_ptr1 == '?') { + rply_ptr = rply_ptr1; + }else { + rply_ptr = "?funny_response"; + } + *present = *state = 0; + EL734_SetErrcode (info_ptr, rply_ptr, "W\" or \"AC"); + return False; + } + } + if (EL734_errcode != 0) return False; + EL734_call_depth--; + return True; + } +/* +**--------------------------------------------------------------------------- ** EL734_GetEncGearing: Get FD register values. */ int EL734_GetEncGearing ( @@ -1486,6 +1665,80 @@ } /* **--------------------------------------------------------------------------- +** EL734_GetSpeeds: Get G/J/E register values. +*/ + int EL734_GetSpeeds ( +/* =============== +*/ void **handle, + int *lo, + int *hi, + int *ramp) { + + int status; + struct EL734info *info_ptr; + char cmnd0[10]; + char cmnd1[10]; + char cmnd2[10]; + char *rply_ptr; + char *rply_ptr0; + char *rply_ptr1; + char *rply_ptr2; + /*---------------------------------------------- + */ + *lo = *hi = *ramp = 0; + info_ptr = (struct EL734info *) *handle; + + if (!EL734_AddCallStack (info_ptr, "EL734_GetSpeeds")) return False; + /*---------------------------------------------- + ** Send G, J and E cmnds to EL734 + */ + sprintf (cmnd0, "g %d\r", info_ptr->motor); + sprintf (cmnd1, "j %d\r", info_ptr->motor); + sprintf (cmnd2, "e %d\r", info_ptr->motor); + + status = AsynSrv_SendCmnds (&info_ptr->asyn_info, + &info_ptr->to_host, &info_ptr->from_host, + cmnd0, cmnd1, cmnd2, NULL); + if (!status) { + *lo = *hi = *ramp = 0; + EL734_errcode = EL734__BAD_ASYNSRV; + return False; + }else { + rply_ptr1 = rply_ptr2 = NULL; + rply_ptr0 = AsynSrv_GetReply ( + &info_ptr->asyn_info, &info_ptr->from_host, NULL); + if (rply_ptr0 != NULL) rply_ptr1 = AsynSrv_GetReply ( + &info_ptr->asyn_info, &info_ptr->from_host, rply_ptr0); + if (rply_ptr1 != NULL) rply_ptr2 = AsynSrv_GetReply ( + &info_ptr->asyn_info, &info_ptr->from_host, rply_ptr1); + + if (rply_ptr0 == NULL) rply_ptr0 = "?no_response"; + if (rply_ptr1 == NULL) rply_ptr1 = "?no_response"; + if (rply_ptr2 == NULL) rply_ptr2 = "?no_response"; + + if ((sscanf (rply_ptr0, "%d", lo) != 1) || + (sscanf (rply_ptr1, "%d", hi) != 1) || + (sscanf (rply_ptr2, "%d", ramp) != 1)) { + if (*rply_ptr0 == '?') { + rply_ptr = rply_ptr0; + }else if (*rply_ptr1 == '?') { + rply_ptr = rply_ptr1; + }else if (*rply_ptr2 == '?') { + rply_ptr = rply_ptr2; + }else { + rply_ptr = "?funny_response"; + } + *lo = *hi = *ramp = 0; + EL734_SetErrcode (info_ptr, rply_ptr, "G\", \"J\" or \"E"); + return False; + } + } + if (EL734_errcode != 0) return False; + EL734_call_depth--; + return True; + } +/* +**--------------------------------------------------------------------------- ** EL734_GetStatus: Get MSR/SS/U register values. */ int EL734_GetStatus ( @@ -1507,7 +1760,6 @@ char *rply_ptr0; char *rply_ptr1; char *rply_ptr2; - char buffer[50]; /*---------------------------------------------- */ *msr = *ored_msr = *fp_cntr = *fr_cntr = *ss = -1; *ist_posit = 0.0; @@ -1564,13 +1816,7 @@ rply_ptr = "?funny_response"; } *msr = *ored_msr = *fp_cntr = *fr_cntr = *ss = -1; *ist_posit = 0.0; - strcpy(buffer,cmnd0); - strcat(buffer,cmnd1); - strcat(buffer,cmnd2); - EL734_SetErrcode (info_ptr, rply_ptr, buffer); - if (EL734_errcode == EL734__BAD_ILLG) - fprintf (stderr, " Unrecognised response to \"MSR\", \"SS\"" - " or \"U\" command: \"%s\"\n", rply_ptr); + EL734_SetErrcode (info_ptr, rply_ptr, "MSR\", \"SS\" or \"U"); return False; } } @@ -2020,10 +2266,58 @@ } /* **--------------------------------------------------------------------------- +** EL734_SetAirCush: Set the air-cushion register (AC register) +*/ + int EL734_SetAirCush ( +/* ================ +*/ void **handle, + int state) { + + int status, dum1, my_state; + struct EL734info *info_ptr; + char cmnd0[32]; + char *rply_ptr0; + /*---------------------------------------------- + */ + info_ptr = (struct EL734info *) *handle; + + if (!EL734_AddCallStack (info_ptr, "EL734_SetAirCush")) return False; + /*---------------------------------------------- + ** Send AC cmnd to EL734 + */ + if (state != 0) state = 1; + sprintf (cmnd0, "ac %d %d\r", info_ptr->motor, state); + status = AsynSrv_SendCmnds (&info_ptr->asyn_info, + &info_ptr->to_host, &info_ptr->from_host, + cmnd0, NULL); + if (!status) { + EL734_errcode = EL734__BAD_ASYNSRV; + return False; + }else { + rply_ptr0 = AsynSrv_GetReply ( + &info_ptr->asyn_info, &info_ptr->from_host, NULL); + if (rply_ptr0 == NULL) rply_ptr0 = "?no_response"; + if (*rply_ptr0 == '\0') { + /* + ** The command was accepted - check value is set OK. + */ + status = EL734_GetAirCush (handle, &dum1, &my_state); + if (!status) return False; + if (state != my_state) { + EL734_errcode = EL734__VFY_ERR; + return False; + } + EL734_call_depth--; + return True; + } + EL734_SetErrcode (info_ptr, rply_ptr0, "AC"); + return False; + } + } +/* +**--------------------------------------------------------------------------- ** EL734_SetErrcode - Set up EL734_errcode */ -char cILLGText[256]; - int EL734_SetErrcode ( /* ================ */ struct EL734info *info_ptr, @@ -2040,20 +2334,11 @@ char cILLGText[256]; if (strcmp (response, "?CMD") == 0) EL734_errcode = EL734__BAD_CMD; if (strcmp (response, "?LOC") == 0) EL734_errcode = EL734__BAD_LOC; if (strcmp (response, "?ADR") == 0) EL734_errcode = EL734__BAD_ADR; - if (strcmp (response, "?BSY") == 0) EL734_errcode = EL734__BAD_BSY; if (strcmp (response, "?RNG") == 0) EL734_errcode = EL734__BAD_RNG; if (strcmp (response, "*ES") == 0) EL734_errcode = EL734__EMERG_STOP; if (strcmp (response, "*MS" ) == 0) EL734_errcode = EL734__BAD_STP; if (strncmp (response, "?TMO", 4) == 0) EL734_errcode = EL734__BAD_TMO; if (EL734_errcode != EL734__BAD_ILLG) return EL734_errcode; - - /* - * copy bad response to ILLGText for debugging purposes - */ - strcpy(cILLGText,cmnd); - strcat(cILLGText," : "); - strcat(cILLGText,response); - /* ** The response is not recognised. Perhaps the emergency stop ** signal is set. To check this, it is necessary to turn off @@ -2081,13 +2366,164 @@ char cILLGText[256]; } if (strstr (rply, "*ES") != NULL) EL734_errcode = EL734__EMERG_STOP; - if ((EL734_errcode == EL734__BAD_ILLG) && (cmnd != NULL)) + if ((EL734_errcode == EL734__BAD_ILLG) && (cmnd != NULL)) { fprintf (stderr, " Unrecognised response to \"%s\" command: \"%s\"\n", cmnd, response); + strcpy (EL734_IllgText, cmnd); + strcat (EL734_IllgText, " : "); + strcat (EL734_IllgText, response); + } return EL734_errcode; } /* **--------------------------------------------------------------------------- +** EL734_SetHighSpeed: Set the max speed (J register) +*/ + int EL734_SetHighSpeed ( +/* ================== +*/ void **handle, + int hi) { + + int status, my_lo, my_hi, my_ramp; + struct EL734info *info_ptr; + char cmnd0[32]; + char *rply_ptr0; + /*---------------------------------------------- + */ + info_ptr = (struct EL734info *) *handle; + + if (!EL734_AddCallStack (info_ptr, "EL734_SetHighSpeed")) return False; + /*---------------------------------------------- + ** Send J cmnd to EL734 + */ + sprintf (cmnd0, "j %d %d\r", info_ptr->motor, hi); + status = AsynSrv_SendCmnds (&info_ptr->asyn_info, + &info_ptr->to_host, &info_ptr->from_host, + cmnd0, NULL); + if (!status) { + EL734_errcode = EL734__BAD_ASYNSRV; + return False; + }else { + rply_ptr0 = AsynSrv_GetReply ( + &info_ptr->asyn_info, &info_ptr->from_host, NULL); + if (rply_ptr0 == NULL) rply_ptr0 = "?no_response"; + if (*rply_ptr0 == '\0') { + /* + ** The command was accepted - check value is set OK. + */ + status = EL734_GetSpeeds (handle, &my_lo, &my_hi, &my_ramp); + if (!status) return False; + if (hi != my_hi) { + EL734_errcode = EL734__VFY_ERR; + return False; + } + EL734_call_depth--; + return True; + } + EL734_SetErrcode (info_ptr, rply_ptr0, "J"); + return False; + } + } +/* +**--------------------------------------------------------------------------- +** EL734_SetLowSpeed: Set the start/stop speed (G register) +*/ + int EL734_SetLowSpeed ( +/* ================= +*/ void **handle, + int lo) { + + int status, my_lo, my_hi, my_ramp; + struct EL734info *info_ptr; + char cmnd0[32]; + char *rply_ptr0; + /*---------------------------------------------- + */ + info_ptr = (struct EL734info *) *handle; + + if (!EL734_AddCallStack (info_ptr, "EL734_SetLowSpeed")) return False; + /*---------------------------------------------- + ** Send G cmnd to EL734 + */ + sprintf (cmnd0, "g %d %d\r", info_ptr->motor, lo); + status = AsynSrv_SendCmnds (&info_ptr->asyn_info, + &info_ptr->to_host, &info_ptr->from_host, + cmnd0, NULL); + if (!status) { + EL734_errcode = EL734__BAD_ASYNSRV; + return False; + }else { + rply_ptr0 = AsynSrv_GetReply ( + &info_ptr->asyn_info, &info_ptr->from_host, NULL); + if (rply_ptr0 == NULL) rply_ptr0 = "?no_response"; + if (*rply_ptr0 == '\0') { + /* + ** The command was accepted - check value is set OK. + */ + status = EL734_GetSpeeds (handle, &my_lo, &my_hi, &my_ramp); + if (!status) return False; + if (lo != my_lo) { + EL734_errcode = EL734__VFY_ERR; + return False; + } + EL734_call_depth--; + return True; + } + EL734_SetErrcode (info_ptr, rply_ptr0, "G"); + return False; + } + } +/* +**--------------------------------------------------------------------------- +** EL734_SetRamp: Set the start/stop ramp (E register) +*/ + int EL734_SetRamp ( +/* ============= +*/ void **handle, + int ramp) { + + int status, my_lo, my_hi, my_ramp; + struct EL734info *info_ptr; + char cmnd0[32]; + char *rply_ptr0; + /*---------------------------------------------- + */ + info_ptr = (struct EL734info *) *handle; + + if (!EL734_AddCallStack (info_ptr, "EL734_SetRamp")) return False; + /*---------------------------------------------- + ** Send E cmnd to EL734 + */ + sprintf (cmnd0, "e %d %d\r", info_ptr->motor, ramp); + status = AsynSrv_SendCmnds (&info_ptr->asyn_info, + &info_ptr->to_host, &info_ptr->from_host, + cmnd0, NULL); + if (!status) { + EL734_errcode = EL734__BAD_ASYNSRV; + return False; + }else { + rply_ptr0 = AsynSrv_GetReply ( + &info_ptr->asyn_info, &info_ptr->from_host, NULL); + if (rply_ptr0 == NULL) rply_ptr0 = "?no_response"; + if (*rply_ptr0 == '\0') { + /* + ** The command was accepted - check value is set OK. + */ + status = EL734_GetSpeeds (handle, &my_lo, &my_hi, &my_ramp); + if (!status) return False; + if (ramp != my_ramp) { + EL734_errcode = EL734__VFY_ERR; + return False; + } + EL734_call_depth--; + return True; + } + EL734_SetErrcode (info_ptr, rply_ptr0, "E"); + return False; + } + } +/* +**--------------------------------------------------------------------------- ** EL734_Stop: Send a stop command to motor. */ int EL734_Stop ( @@ -2106,7 +2542,7 @@ char cILLGText[256]; /*---------------------------------------------- ** Send S cmnd to EL734 */ - sprintf (cmnd0, "S %d\r", info_ptr->motor); + sprintf (cmnd0, "s %d\r", info_ptr->motor); status = AsynSrv_SendCmnds (&info_ptr->asyn_info, &info_ptr->to_host, &info_ptr->from_host, cmnd0, NULL); @@ -2137,10 +2573,6 @@ char cILLGText[256]; int *fp_cntr, int *fr_cntr, float *ist_posit) { -#ifdef LINUX -#define nanosleep_d9 nanosleep -#endif - #ifdef __VMS #include #define hibernate lib$wait (0.25) @@ -2149,7 +2581,7 @@ char cILLGText[256]; #include struct timespec delay = {0, 250000000}; struct timespec delay_left; -#define hibernate nanosleep_d9 (&delay, &delay_left) +#define hibernate nanosleep (&delay, &delay_left) #endif int msr, ss; struct EL734info *info_ptr;