From ad1e2f3181bd2fdf90f8f37ad22829d175205ecb Mon Sep 17 00:00:00 2001 From: jsullivan-anl Date: Thu, 31 Aug 2006 15:42:31 +0000 Subject: [PATCH] Major rewite to be compatable with factor default command response mode (ERRLVL 4). The original driver was designed to response mode (ERRLVL 1) which was the mode required for MX. --- motorApp/PC6KSrc/6K_STARTP_EXAMPLE | 59 + motorApp/PC6KSrc/6krev_info | 16 + motorApp/PC6KSrc/Makefile | 44 +- motorApp/PC6KSrc/ParkerRegister.cc | 166 +-- motorApp/PC6KSrc/ParkerRegister.h | 94 +- motorApp/PC6KSrc/README | 438 +++---- motorApp/PC6KSrc/devPC6K.cc | 745 ++++++------ motorApp/PC6KSrc/devPC6K.dbd | 12 +- motorApp/PC6KSrc/drvPC6K.cc | 1801 ++++++++++++++-------------- motorApp/PC6KSrc/drvPC6K.h | 247 ++-- 10 files changed, 1907 insertions(+), 1715 deletions(-) create mode 100755 motorApp/PC6KSrc/6K_STARTP_EXAMPLE create mode 100644 motorApp/PC6KSrc/6krev_info diff --git a/motorApp/PC6KSrc/6K_STARTP_EXAMPLE b/motorApp/PC6KSrc/6K_STARTP_EXAMPLE new file mode 100755 index 00000000..55644a7f --- /dev/null +++ b/motorApp/PC6KSrc/6K_STARTP_EXAMPLE @@ -0,0 +1,59 @@ +SCLA 1,1,1,1 +SCLD 1,1,1,1 +SCLV 1,1,1,1 +SCALE1 + +DEL SETUP +DEF SETUP +PORT1 +ECHO0 +DRIVE0000 +FOLMAS 0,0,0,0,0,0,0,0 +FOLEN00000000 +AXSDEF 0000 +DRFLVL 1111 +DRFEN 0000 +DRES 200,200,200,200 +PULSE 0.3,0.3,0.3,0.3 +DSTALL 0000 +ERES ,,, +EFAIL XXXX +ENCPOL XXXX +ENCSND XXXX +ESTALL XXXX +ESK XXXX +ENCCNT XXXX +LH 3,3,3,3 +LHAD 1000,1000,1000,1000 +LHADA 1000,1000,1000,1000 +LSNEG 0,0,0,0 +LSPOS 0,0,0,0 +HOMA 400,400,400,400 +HOMAA 400,400,400,400 +HOMV 200,200,200,200 +HOMAD 1000,1000,1000,1000 +HOMADA 1000,1000,1000,1000 +HOMBAC 0000 +HOMZ 0000 +HOMDF 0000 +HOMVF 0.1,0.1,0.1,0.1 +HOMEDG 0000 +LIMLVL 000000000000 +OPTEN0 +NTADDR164,54,200,13 +NTMASK255,255,255,128 +NTFEN2 +COMEXC1 +END + +DEL MAIN +DEF MAIN +GOSUB SETUP +DRIVE1111 +MA0 +A800,800,800,800 +V400,400,400,400 +PSET 0,0,0,0 +END + +STARTP MAIN diff --git a/motorApp/PC6KSrc/6krev_info b/motorApp/PC6KSrc/6krev_info new file mode 100644 index 00000000..918f13fd --- /dev/null +++ b/motorApp/PC6KSrc/6krev_info @@ -0,0 +1,16 @@ +This version of the driver has been tested against +the following Compumotor 6K Controller (TREV Cmnd). + +IMCA-CAT + 6K4: 92-016740-01-6.5.0 6K (8/8/06) + + + 6K8: 92-016740-01-5.1.2 6K + SN#99060900593 + TNT: + *Ethernet enabled: NTFEN1 + *6K IP address: 164.54.9.33 + *6K Network Mask: 255.255.252.0 + *6K Ethernet address: 0-144-85-0-1-208 (decimal) + *6K Ethernet address: 00-90-55-00-01-D0 (hex) + *6K Ethernet not connected diff --git a/motorApp/PC6KSrc/Makefile b/motorApp/PC6KSrc/Makefile index 3043cfe8..a13acb14 100755 --- a/motorApp/PC6KSrc/Makefile +++ b/motorApp/PC6KSrc/Makefile @@ -1,22 +1,22 @@ -# Makefile -TOP = ../.. -include $(TOP)/configure/CONFIG - -# The following are used for debugging messages. -#USR_CXXFLAGS += -DDEBUG - -OPT_CXXFLAGS = - -DBD += devPC6K.dbd - -LIBRARY_IOC = Parker - -# Intelligent Motion Systems driver support. -SRCS += ParkerRegister.cc -SRCS += devPC6K.cc drvPC6K.cc - -Parker_LIBS += motor asyn -Parker_LIBS += $(EPICS_BASE_IOC_LIBS) - -include $(TOP)/configure/RULES - +# Makefile +TOP = ../.. +include $(TOP)/configure/CONFIG + +# The following are used for debugging messages. +#USR_CXXFLAGS += -DDEBUG + +OPT_CXXFLAGS = -g -O0 + +DBD += devPC6K.dbd + +LIBRARY_IOC = Parker + +# Intelligent Motion Systems driver support. +SRCS += ParkerRegister.cc +SRCS += devPC6K.cc drvPC6K.cc + +Parker_LIBS += motor asyn +Parker_LIBS += $(EPICS_BASE_IOC_LIBS) + +include $(TOP)/configure/RULES + diff --git a/motorApp/PC6KSrc/ParkerRegister.cc b/motorApp/PC6KSrc/ParkerRegister.cc index 7d5ae87f..c5230ca1 100755 --- a/motorApp/PC6KSrc/ParkerRegister.cc +++ b/motorApp/PC6KSrc/ParkerRegister.cc @@ -1,83 +1,83 @@ -/* -FILENAME... ParkerRegister.cc -USAGE... Register Parker/Compumotor motor device driver shell commands. - -Version: $Revision: 1.1 $ -Modified By: $Author: rivers $ -Last Modified: $Date: 2005-12-21 22:58:24 $ -*/ - -/***************************************************************** - COPYRIGHT NOTIFICATION -***************************************************************** - -(C) COPYRIGHT 1993 UNIVERSITY OF CHICAGO - -This software was developed under a United States Government license -described on the COPYRIGHT_UniversityOfChicago file included as part -of this distribution. -**********************************************************************/ - -#include -#include "ParkerRegister.h" -#include "epicsExport.h" - -extern "C" -{ - -// Parker Setup arguments -static const iocshArg setupArg0 = {"Max. controller count", iocshArgInt}; -static const iocshArg setupArg1 = {"Polling rate", iocshArgInt}; - -// Parker Config arguments -static const iocshArg configArg0 = {"Card being configured", iocshArgInt}; -static const iocshArg configArg1 = {"asyn port name", iocshArgString}; - - // Parker File Upload Argument */ -static const iocshArg upLoadArg0 = {"Controller Card#", iocshArgInt}; -static const iocshArg upLoadArg1 = {"Upload File Path", iocshArgString}; -static const iocshArg upLoadArg2 = {"Program Name ", iocshArgString}; - - -static const iocshArg * const ParkerSetupArgs[2] = {&setupArg0, &setupArg1}; - -static const iocshArg * const ParkerConfigArgs[2] = {&configArg0, &configArg1}; - - static const iocshArg * const ParkerUpLoadArgs[3] = {&upLoadArg0, &upLoadArg1, &upLoadArg2}; - -static const iocshFuncDef setupPC6K = {"PC6KSetup",2, ParkerSetupArgs}; - -static const iocshFuncDef configPC6K = {"PC6KConfig", 2, ParkerConfigArgs}; - -static const iocshFuncDef upLoadPC6K = {"PC6KUpLoad", 3, ParkerUpLoadArgs}; - - -static void setupPC6KCallFunc(const iocshArgBuf *args) -{ - PC6KSetup(args[0].ival, args[1].ival); -} - -static void configPC6KCallFunc(const iocshArgBuf *args) -{ - PC6KConfig(args[0].ival, args[1].sval); -} - -static void upLoadPC6KCallFunc(const iocshArgBuf *args) -{ - PC6KUpLoad(args[0].ival, args[1].sval, args[2].sval); -} - - -static void ParkerRegister(void) -{ - - iocshRegister(&setupPC6K, setupPC6KCallFunc); - - iocshRegister(&configPC6K, configPC6KCallFunc); - - iocshRegister(&upLoadPC6K, upLoadPC6KCallFunc); -} - -epicsExportRegistrar(ParkerRegister); - -} // extern "C" +/* +FILENAME... ParkerRegister.cc +USAGE... Register Parker/Compumotor motor device driver shell commands. + +Version: $Revision: 1.2 $ +Modified By: $Author: sullivan $ +Last Modified: $Date: 2006-08-31 15:42:31 $ +*/ + +/***************************************************************** + COPYRIGHT NOTIFICATION +***************************************************************** + +(C) COPYRIGHT 1993 UNIVERSITY OF CHICAGO + +This software was developed under a United States Government license +described on the COPYRIGHT_UniversityOfChicago file included as part +of this distribution. +**********************************************************************/ + +#include +#include "ParkerRegister.h" +#include "epicsExport.h" + +extern "C" +{ + +// Parker Setup arguments +static const iocshArg setupArg0 = {"Max. controller count", iocshArgInt}; +static const iocshArg setupArg1 = {"Polling rate", iocshArgInt}; + +// Parker Config arguments +static const iocshArg configArg0 = {"Card being configured", iocshArgInt}; +static const iocshArg configArg1 = {"asyn port name", iocshArgString}; + + // Parker File Upload Argument */ +static const iocshArg upLoadArg0 = {"Controller Card#", iocshArgInt}; +static const iocshArg upLoadArg1 = {"Upload File Path", iocshArgString}; +static const iocshArg upLoadArg2 = {"Program Name ", iocshArgString}; + + +static const iocshArg * const ParkerSetupArgs[2] = {&setupArg0, &setupArg1}; + +static const iocshArg * const ParkerConfigArgs[2] = {&configArg0, &configArg1}; + + static const iocshArg * const ParkerUpLoadArgs[3] = {&upLoadArg0, &upLoadArg1, &upLoadArg2}; + +static const iocshFuncDef setupPC6K = {"PC6KSetup",2, ParkerSetupArgs}; + +static const iocshFuncDef configPC6K = {"PC6KConfig", 2, ParkerConfigArgs}; + +static const iocshFuncDef upLoadPC6K = {"PC6KUpLoad", 3, ParkerUpLoadArgs}; + + +static void setupPC6KCallFunc(const iocshArgBuf *args) +{ + PC6KSetup(args[0].ival, args[1].ival); +} + +static void configPC6KCallFunc(const iocshArgBuf *args) +{ + PC6KConfig(args[0].ival, args[1].sval); +} + +static void upLoadPC6KCallFunc(const iocshArgBuf *args) +{ + PC6KUpLoad(args[0].ival, args[1].sval, args[2].sval); +} + + +static void ParkerRegister(void) +{ + + iocshRegister(&setupPC6K, setupPC6KCallFunc); + + iocshRegister(&configPC6K, configPC6KCallFunc); + + iocshRegister(&upLoadPC6K, upLoadPC6KCallFunc); +} + +epicsExportRegistrar(ParkerRegister); + +} // extern "C" diff --git a/motorApp/PC6KSrc/ParkerRegister.h b/motorApp/PC6KSrc/ParkerRegister.h index d1da54cf..e68ae6ee 100755 --- a/motorApp/PC6KSrc/ParkerRegister.h +++ b/motorApp/PC6KSrc/ParkerRegister.h @@ -1,47 +1,47 @@ -/* -FILENAME... ParkerRegister.h -USAGE... This file contains function prototypes for Parker IOC shell commands. - -Version: 1.4 -Modified By: rivers -Last Modified: 2004/07/28 18:45:16 -*/ - -/* - * Original Author: Ron Sluiter - * Date: 05/19/03 - * - * Experimental Physics and Industrial Control System (EPICS) - * - * Copyright 1991, the Regents of the University of California, - * and the University of Chicago Board of Governors. - * - * This software was produced under U.S. Government contracts: - * (W-7405-ENG-36) at the Los Alamos National Laboratory, - * and (W-31-109-ENG-38) at Argonne National Laboratory. - * - * Initial development by: - * The Controls and Automation Group (AT-8) - * Ground Test Accelerator - * Accelerator Technology Division - * Los Alamos National Laboratory - * - * Co-developed with - * The Controls and Computing Group - * Accelerator Systems Division - * Advanced Photon Source - * Argonne National Laboratory - * - * Modification Log: - * ----------------- - */ - -#include "motor.h" -#include "motordrvCom.h" - -/* Function prototypes. */ -extern RTN_STATUS PC6KSetup(int, int); -extern RTN_STATUS PC6KConfig(int, const char *); -extern RTN_STATUS PC6KUpLoad(int, const char *, const char *); - - +/* +FILENAME... ParkerRegister.h +USAGE... This file contains function prototypes for Parker IOC shell commands. + +Version: 1.4 +Modified By: rivers +Last Modified: 2004/07/28 18:45:16 +*/ + +/* + * Original Author: Ron Sluiter + * Date: 05/19/03 + * + * Experimental Physics and Industrial Control System (EPICS) + * + * Copyright 1991, the Regents of the University of California, + * and the University of Chicago Board of Governors. + * + * This software was produced under U.S. Government contracts: + * (W-7405-ENG-36) at the Los Alamos National Laboratory, + * and (W-31-109-ENG-38) at Argonne National Laboratory. + * + * Initial development by: + * The Controls and Automation Group (AT-8) + * Ground Test Accelerator + * Accelerator Technology Division + * Los Alamos National Laboratory + * + * Co-developed with + * The Controls and Computing Group + * Accelerator Systems Division + * Advanced Photon Source + * Argonne National Laboratory + * + * Modification Log: + * ----------------- + */ + +#include "motor.h" +#include "motordrvCom.h" + +/* Function prototypes. */ +extern RTN_STATUS PC6KSetup(int, int); +extern RTN_STATUS PC6KConfig(int, const char *); +extern RTN_STATUS PC6KUpLoad(int, const char *, const char *); + + diff --git a/motorApp/PC6KSrc/README b/motorApp/PC6KSrc/README index 780bb925..a30ca447 100755 --- a/motorApp/PC6KSrc/README +++ b/motorApp/PC6KSrc/README @@ -1,212 +1,226 @@ - - Parker 6K8 Motor Controller Series -============================================================================== - -Ethernet Configuration ----------------------------------- - -Default Ethernet Address: 192.168.10.30 - - -Ascii Command Port: 5002 - - -Note: This controller's ethernet port only accepts one server at a - time. If the RS232 is in use then the ethernet port is closed. - -Controller Configuration ------------------------------------------- -Disable Echo - ECHO0 -Enable continuous command mode - COMEXEC1 - - ----------------- Network Commands ------------------- -NTADDRxx,xx,xx,xx = network IP -NTMASKxxx,xxx,xxx,xxx = network mask -NTFEN1 = network enable - - -Commands ------------------------------------------- - -REVISION --------- -TREV -*TREV92-016740-01-6.4.0 GEM6K GT6K-L8 -(60 characters) - -CONTINUOUS COMMAND MODE ------------------------- -COMEXC1 = set on, required for status updates during motion. - ****(No Reply)*** - -LIMIT SWITCH ENABE ------------------------ -LH3 - enable hardware LS -LH0 - disable hardware LS - -ENABLE DRIVE -------------- -DRIVE1 - enable drive (must be renable after a dreset) - *****(No Reply when Set)********* - -DRIVE -*DRIVE 0 or 1 - -MOTION MODE -------------- -MA<0/1> - Set incremental or absolute mode - 0 = incremental - 1 = absolute - -MC1 - Coninutous Mode (Jog) - -MOTION PARAMETERS ------------------- -A - set acceleration units/sec^2 - -A -*A#### - prints acceleration - -V - velocity - -D - set distance - -GO - start motion - - -ie: motion - -MA1: A20: V25: D25000: GO - -Setting absolute postion -<#>PSET - - -Velociy Feedback: - -<#>TVEL - commanded velocity -<#>TVELA - actual - -Position Feedback: - -<#>TPC - commanded position - *1TPC+0 -<#>TPE - encoder position - *1TPE+0 - -SCALING: -SCALE0/1 - disable/enable -SCLA - acceleration -SCLV - velocity -SCLD - distance - -DRES - Drive Resolution (steps/rev) **** Stepper Only ***** -ERES - Encoder Resolution (steps/rev) -DRESET - drive reset - enables new scaling factors - -STATUS: -<#>TAS -*<#>TAS0000_0000_0000_0000_0000_0000_0000_0000 (32 bits) - ^ bit 1 ^bit 32 - -*** Returns - *0 or *1 -TAS.1 - in motion (commanded - will be off during settling time) -TAS.2 - direction is negative -TAS.5 - home successfull -TAS.13 - drive shutdown -TAS.14 - drive fault -TAS.15 - Plus Hardware Travel Limit -TAS.16 - Minus Hardware Travel Limit -TAS.17 - Plus Software TL (LSPOS) -TAS.18 - Minus Software TL (LSNEG) -TAS.24 - In Target Zone -ie. -1TAS -*1TAS0000_0000_0000_0000_0000_0000_000_00000 - --------------------- Jog Method -------------- -MC1 - Use continuous motion mode for jogging - - ----------------- Program Handling ------------ - -TDIR - list downloaded programs - -DEL - delete program - -DEF - -. -. -END - end of program creation - -TPROG - transfer program contents - - - - - -============== Build Info ====================== - -xxxApp/src - Makefile - -------- - xxx_Common_LIBS += Parker - - xxCommonInclude.dbd - ------------------- - include "devPC6K.dbd" - - -============= IOC Boot info ====================== - -iocBoot/iocLinux - serial.cmd - ---------- - - # serial 1 is a RS232 link to a Parker Gemini 6K Controller - drvAsynSerialPortConfigure("serial1", "/dev/ttyUSB0", 0, 0, 0) - asynOctetSetInputEos("serial1",0,">") - asynOctetSetOutputEos("serial1",0,"\r") - asynOctetConnect("serial1", "serial1") - - # serial 3 is ETHERNET link to the Parker Gemini 6K Controller - drvAsynIPPortConfigure("serial3", "192.168.10.29:5002", 0, 0, 0) - asynOctetConnect("serial3", "serial3") - asynOctetSetInputEos("serial3",0,">") - asynOctetSetOutputEos("serial3",0,"\r") - - . - - # Parker/Compumotor - Gemini 6K driver setup parameters: - # (1) maximum number of controllers in system - # (2) motor task polling rate (min=1Hz, max=60Hz) - PC6KSetup(2, 60) - - # Parker/Compumotor - Gemini 6K driver configuration parameters: - # (1) controller being configured - # (2) asyn port name (string) - PC6KConfig(0, "serial1") - PC6KConfig(1, "serial3") - - - motor.substitutions - ------------------- - Set the DTYPE column to "PC6K" - - -============== IOC Runtime Info (Post iocInit) ======================== - -# PC5KUpLoad(Controller #, , ) - -### Send setup files to 6K8 -PC6KUpLoad(0,"6k_setup","setup") -### Set startup file to setup -PC6KUpLoad(0,"startp") - - - + + Parker 6K8 Motor Controller Series +============================================================================== + +Ethernet Configuration +---------------------------------- + +Default Ethernet Address: 192.168.10.30 + + +Ascii Command Port: 5002 + + +Note: This controller's ethernet port only accepts one server at a + time. If the RS232 is in use then the ethernet port is closed. + +Controller Configuration +------------------------------------------ +Disable Echo + ECHO0 +Enable continuous command mode + COMEXEC1 + +Communication Settings + EOL13,10,0 + EOT13,0,0 + ERRLVL4 + ERRDEF13,10,45,32 + ERROK13,10,62,32 + ERRBAD13,10,63,32 + +---------------- Network Commands ------------------- +NTADDRxx,xx,xx,xx = network IP +NTMASKxxx,xxx,xxx,xxx = network mask +NTFEN2 = network enable + +Workstations cannot link to 6K Controllers without +an explicit arp table entry using the 'arp' command. + +arp -s <6K IP addr> <6K MAC addr> + +* Use the 6K 'TNT' command to see the ip and MAC address + + +Commands +------------------------------------------ + +REVISION +-------- +TREV +*TREV92-016740-01-6.4.0 GEM6K GT6K-L8 +(60 characters) + +CONTINUOUS COMMAND MODE +------------------------ +COMEXC1 = set on, required for status updates during motion. + ****(No Reply)*** + +LIMIT SWITCH ENABE +----------------------- +LH3 - enable hardware LS +LH0 - disable hardware LS + +ENABLE DRIVE +------------- +DRIVE1 - enable drive (must be renable after a dreset) + *****(No Reply when Set)********* + +DRIVE +*DRIVE 0 or 1 + +MOTION MODE +------------- +MA<0/1> - Set incremental or absolute mode + 0 = incremental + 1 = absolute + +MC1 - Coninutous Mode (Jog) + +MOTION PARAMETERS +------------------ +A - set acceleration units/sec^2 + +A +*A#### - prints acceleration + +V - velocity + +D - set distance + +GO - start motion + + +ie: motion + +MA1: A20: V25: D25000: GO + +Setting absolute postion +<#>PSET + + +Velociy Feedback: + +<#>TVEL - commanded velocity +<#>TVELA - actual + +Position Feedback: + +<#>TPC - commanded position + *1TPC+0 +<#>TPE - encoder position + *1TPE+0 + +SCALING: +SCALE0/1 - disable/enable +SCLA - acceleration +SCLV - velocity +SCLD - distance + +DRES - Drive Resolution (steps/rev) **** Stepper Only ***** +ERES - Encoder Resolution (steps/rev) +DRESET - drive reset - enables new scaling factors + +STATUS: +<#>TAS +*<#>TAS0000_0000_0000_0000_0000_0000_0000_0000 (32 bits) + ^ bit 1 ^bit 32 + +*** Returns - *0 or *1 +TAS.1 - in motion (commanded - will be off during settling time) +TAS.2 - direction is negative +TAS.5 - home successfull +TAS.13 - drive shutdown +TAS.14 - drive fault +TAS.15 - Plus Hardware Travel Limit +TAS.16 - Minus Hardware Travel Limit +TAS.17 - Plus Software TL (LSPOS) +TAS.18 - Minus Software TL (LSNEG) +TAS.24 - In Target Zone +ie. +1TAS +*1TAS0000_0000_0000_0000_0000_0000_000_00000 + +-------------------- Jog Method -------------- +MC1 - Use continuous motion mode for jogging + + +---------------- Program Handling ------------ + +TDIR - list downloaded programs + +DEL - delete program + +DEF + +. +. +END - end of program creation + +TPROG - transfer program contents + + + + + +============== Build Info ====================== + +xxxApp/src + Makefile + -------- + xxx_Common_LIBS += Parker + + xxCommonInclude.dbd + ------------------- + include "devPC6K.dbd" + + +============= IOC Boot info ====================== + +iocBoot/iocLinux + serial.cmd + ---------- + + # serial 1 is a RS232 link to a Parker Gemini 6K Controller + drvAsynSerialPortConfigure("serial1", "/dev/ttyUSB0", 0, 0, 0) + asynOctetSetInputEos("serial1",0,">") + asynOctetSetOutputEos("serial1",0,"\r") + asynOctetConnect("serial1", "serial1") + + # serial 3 is ETHERNET link to the Parker Gemini 6K Controller + drvAsynIPPortConfigure("serial3", "192.168.10.29:5002", 0, 0, 0) + asynOctetConnect("serial3", "serial3") + asynOctetSetInputEos("serial3",0,">") + asynOctetSetOutputEos("serial3",0,"\r") + + . + + # Parker/Compumotor - Gemini 6K driver setup parameters: + # (1) maximum number of controllers in system + # (2) motor task polling rate (min=1Hz, max=60Hz) + PC6KSetup(2, 60) + + # Parker/Compumotor - Gemini 6K driver configuration parameters: + # (1) controller being configured + # (2) asyn port name (string) + PC6KConfig(0, "serial1") + PC6KConfig(1, "serial3") + + + motor.substitutions + ------------------- + Set the DTYPE column to "PC6K" + + +============== IOC Runtime Info (Post iocInit) ======================== + +# PC5KUpLoad(Controller #, , ) + +### Send setup files to 6K8 +PC6KUpLoad(0,"6k_setup","setup") +### Set startup file to setup +PC6KUpLoad(0,"startp") + + + diff --git a/motorApp/PC6KSrc/devPC6K.cc b/motorApp/PC6KSrc/devPC6K.cc index d44abe45..6d256e84 100755 --- a/motorApp/PC6KSrc/devPC6K.cc +++ b/motorApp/PC6KSrc/devPC6K.cc @@ -1,334 +1,411 @@ -/* -FILENAME... devPC6K.cc -USAGE... Motor record device level support for Parker Compumotor drivers - -Version: $Revision: 1.1 $ -Modified By: $Author: rivers $ -Last Modified: $Date: 2005-12-21 22:58:24 $ -*/ - -/* - * Original Author: Mark Rivers - * Date: 10/20/97 - * - * Experimental Physics and Industrial Control System (EPICS) - * - * Copyright 1991, the Regents of the University of California, - * and the University of Chicago Board of Governors. - * - * This software was produced under U.S. Government contracts: - * (W-7405-ENG-36) at the Los Alamos National Laboratory, - * and (W-31-109-ENG-38) at Argonne National Laboratory. - * - * Initial development by: - * The Controls and Automation Group (AT-8) - * Ground Test Accelerator - * Accelerator Technology Division - * Los Alamos National Laboratory - * - * Co-developed with - * The Controls and Computing Group - * Accelerator Systems Division - * Advanced Photon Source - * Argonne National Laboratory - * - * Modification Log: - * ----------------- - * .01 04-07-05 jps initialized from devMM4000.cc - */ - - -#include -#include -#include "motorRecord.h" -#include "motor.h" -#include "motordevCom.h" -#include "drvPC6K.h" -#include "epicsExport.h" - -#define STATIC static - -extern struct driver_table PC6K_access; - -/* ----------------Create the dsets for devPC6K----------------- */ -STATIC struct driver_table *drvtabptr; -STATIC long PC6K_init(void *); -STATIC long PC6K_init_record(void *); -STATIC long PC6K_start_trans(struct motorRecord *); -STATIC RTN_STATUS PC6K_build_trans(motor_cmnd, double *, struct motorRecord *); -STATIC RTN_STATUS PC6K_end_trans(struct motorRecord *); - -struct motor_dset devPC6K = -{ - {8, NULL, (DEVSUPFUN) PC6K_init, (DEVSUPFUN) PC6K_init_record, NULL}, - motor_update_values, - PC6K_start_trans, - PC6K_build_trans, - PC6K_end_trans -}; - -extern "C" {epicsExportAddress(dset,devPC6K);} - -/* --------------------------- program data --------------------- */ - -/* This table is used to define the command types */ -/* WARNING! this must match "motor_cmnd" in motor.h */ - -static msg_types PC6K_table[] = { - MOTION, /* MOVE_ABS */ - MOTION, /* MOVE_REL */ - MOTION, /* HOME_FOR */ - MOTION, /* HOME_REV */ - IMMEDIATE, /* LOAD_POS */ - IMMEDIATE, /* SET_VEL_BASE */ - IMMEDIATE, /* SET_VELOCITY */ - IMMEDIATE, /* SET_ACCEL */ - IMMEDIATE, /* GO */ - IMMEDIATE, /* SET_ENC_RATIO */ - INFO, /* GET_INFO */ - MOVE_TERM, /* STOP_AXIS */ - VELOCITY, /* JOG */ - IMMEDIATE, /* SET_PGAIN */ - IMMEDIATE, /* SET_IGAIN */ - IMMEDIATE, /* SET_DGAIN */ - IMMEDIATE, /* ENABLE_TORQUE */ - IMMEDIATE, /* DISABL_TORQUE */ - IMMEDIATE, /* PRIMITIVE */ - IMMEDIATE, /* SET_HIGH_LIMIT */ - IMMEDIATE, /* SET_LOW_LIMIT */ - VELOCITY /* JOG_VELOCITY */ -}; - - -static struct board_stat **PC6K_cards; - -/* --------------------------- program data --------------------- */ - - -/* initialize device support for PC6K stepper motor */ -STATIC long PC6K_init(void *arg) -{ - long rtnval; - int after = (int) arg; - - if (after == 0) - { - drvtabptr = &PC6K_access; - (drvtabptr->init)(); - } - - rtnval = motor_init_com(after, *drvtabptr->cardcnt_ptr, drvtabptr, &PC6K_cards); - return(rtnval); -} - - -/* initialize a record instance */ -STATIC long PC6K_init_record(void *arg) -{ - struct motorRecord *mr = (struct motorRecord *) arg; - return(motor_init_record_com(mr, *drvtabptr->cardcnt_ptr, drvtabptr, PC6K_cards)); -} - - -/* start building a transaction */ -STATIC long PC6K_start_trans(struct motorRecord *mr) -{ - return(motor_start_trans_com(mr, PC6K_cards)); -} - - -/* end building a transaction */ -STATIC RTN_STATUS PC6K_end_trans(struct motorRecord *mr) -{ - return(motor_end_trans_com(mr, drvtabptr)); -} - - -/* add a part to the transaction */ -STATIC RTN_STATUS PC6K_build_trans(motor_cmnd command, double *parms, struct motorRecord *mr) -{ - struct motor_trans *trans = (struct motor_trans *) mr->dpvt; - struct mess_node *motor_call; - struct controller *brdptr; - struct mess_info *motor_info; - struct PC6KController *cntrl; - char buff[110]; - int signal, axis, card, intval; - int maxdigits; - double dval, cntrl_units; - unsigned int size; - RTN_STATUS rtnval; - - rtnval = OK; - buff[0] = '\0'; - - /* Protect against NULL pointer with WRTITE_MSG(GO/STOP_AXIS/GET_INFO, NULL). */ - intval = (parms == NULL) ? 0 : NINT(parms[0]); - dval = (parms == NULL) ? 0 : *parms; - - motor_call = &(trans->motor_call); - card = motor_call->card; - signal = motor_call->signal; - axis = signal+1; /* Motors start at 1 */ - brdptr = (*trans->tabptr->card_array)[card]; - if (brdptr == NULL) - return(rtnval = ERROR); - - cntrl = (struct PC6KController *) brdptr->DevicePrivate; - cntrl_units = dval * cntrl->drive_resolution[signal]; - maxdigits = cntrl->res_decpts[signal]; - - - if (PC6K_table[command] > motor_call->type) - motor_call->type = PC6K_table[command]; - - if (trans->state != BUILD_STATE) - return(rtnval = ERROR); - - if (command == PRIMITIVE && mr->init != NULL && strlen(mr->init) != 0) - { - strcat(motor_call->message, mr->init); - strcat(motor_call->message, PC6K_OUT_EOS); - } - - switch (command) - { - case MOVE_ABS: - case MOVE_REL: - case HOME_FOR: - case HOME_REV: - case JOG: - if (strlen(mr->prem) != 0) - { - strcat(motor_call->message, mr->prem); - strcat(motor_call->message, ":"); - } - if (strlen(mr->post) != 0) - motor_call->postmsgptr = (char *) &mr->post; - break; - - default: - break; - } - - - switch (command) - { - case MOVE_ABS: - sprintf(buff, "%dMA1:%dMC0:%dD%d:", axis, axis, axis, intval); - break; - - case MOVE_REL: - sprintf(buff, "%dMA0:%dMC0:%dD%d:", axis, axis, axis, intval); - break; - - case HOME_FOR: - sprintf(buff, "%dHOM0:", axis); - break; - case HOME_REV: - sprintf(buff, "%dHOM1:", axis); - break; - - case LOAD_POS: - /* The Feedback position follows the Reference set position */ - sprintf(buff, "%dPSET%d:", axis, intval); - break; - - case SET_VEL_BASE: - break; /* PC6K does not use base velocity */ - - case SET_VELOCITY: - sprintf(buff, "%dV%.*f:", axis, maxdigits, cntrl_units); - break; - - case SET_ACCEL: - /* Set Accel, Avg Accel, Decel, Avg Decel - assume avg to be 1/2 accel */ - sprintf(buff, "%dA%.*f:%dAA%.*f:%dAD%.*f:%dADA%.*f:", - axis, maxdigits, cntrl_units, - axis, maxdigits, cntrl_units/2, - axis, maxdigits, cntrl_units, - axis, maxdigits, cntrl_units/2); - - break; - - case GO: - sprintf(buff, "%dGO:", axis); - break; - - case SET_ENC_RATIO: - // sprintf(buff, "%dERES%d:%dDRESET:", axis, intval, axis); - break; - - case GET_INFO: - /* These commands are not actually done by sending a message, but - rather they will indirectly cause the driver to read the status - of all motors */ - break; - - case STOP_AXIS: - sprintf(buff, "!%dK:", axis); - break; - - case JOG: - if (intval > 0) - sprintf(buff, "%dMC1:%dV%.*f:%dD+:%dGO:", axis, axis, maxdigits, cntrl_units, axis, axis); - else - sprintf(buff, "%dMC1:%dV%.*f:%dD-:%dGO:", axis, axis, maxdigits, cntrl_units, axis, axis); - break; - - case SET_PGAIN: - sprintf(buff, "%dSGP%d:", axis, intval); - break; - - case SET_IGAIN: - sprintf(buff, "%dSGI%d:", axis, intval); - break; - - case SET_DGAIN: - sprintf(buff, "%dSGV%d:", axis, intval); /* Velocity Gain */ - break; - - case ENABLE_TORQUE: - sprintf(buff, "%dDRIVE1:", axis); - break; - - case DISABL_TORQUE: - sprintf(buff, "%dDRIVE0:", axis); - break; - - case SET_HIGH_LIMIT: - motor_info = &(*trans->tabptr->card_array)[card]->motor_info[signal]; - trans->state = IDLE_STATE; /* No command sent to the controller. */ - - if (motor_info->high_limit > motor_info->low_limit && - intval > motor_info->high_limit) - { - mr->dhlm = motor_info->high_limit * mr->mres; - rtnval = ERROR; - } - break; - - case SET_LOW_LIMIT: - motor_info = &(*trans->tabptr->card_array)[card]->motor_info[signal]; - trans->state = IDLE_STATE; /* No command sent to the controller. */ - - if (motor_info->low_limit < motor_info->high_limit && - intval < motor_info->low_limit) - { - mr->dllm = motor_info->low_limit * mr->mres; - rtnval = ERROR; - } - break; - - default: - rtnval = ERROR; - } - - size = strlen(buff); - if (size > sizeof(buff) || (strlen(motor_call->message) + size) > MAX_MSG_SIZE) - errlogMessage("PC6K_build_trans(): buffer overflow.\n"); - else - strcat(motor_call->message, buff); - - return(rtnval); -} +/* +FILENAME... devPC6K.cc +USAGE... Motor record device level support for Parker Compumotor drivers + +Version: $Revision: 1.2 $ +Modified By: $Author: sullivan $ +Last Modified: $Date: 2006-08-31 15:42:30 $ +*/ + +/* + * Original Author: Mark Rivers + * Date: 10/20/97 + * + * Experimental Physics and Industrial Control System (EPICS) + * + * Copyright 1991, the Regents of the University of California, + * and the University of Chicago Board of Governors. + * + * This software was produced under U.S. Government contracts: + * (W-7405-ENG-36) at the Los Alamos National Laboratory, + * and (W-31-109-ENG-38) at Argonne National Laboratory. + * + * Initial development by: + * The Controls and Automation Group (AT-8) + * Ground Test Accelerator + * Accelerator Technology Division + * Los Alamos National Laboratory + * + * Co-developed with + * The Controls and Computing Group + * Accelerator Systems Division + * Advanced Photon Source + * Argonne National Laboratory + * + * Modification Log: + * ----------------- + * .01 04-07-05 jps initialized from devMM4000.cc + */ + + +#include +#include +#include "motorRecord.h" +#include "motor.h" +#include "motordevCom.h" +#include "drvPC6K.h" +#include "epicsExport.h" + +#define STATIC static + +extern struct driver_table PC6K_access; + +/* ----------------Create the dsets for devPC6K----------------- */ +STATIC struct driver_table *drvtabptr; +STATIC long PC6K_init(void *); +STATIC long PC6K_init_record(void *); +STATIC long PC6K_start_trans(struct motorRecord *); +STATIC RTN_STATUS PC6K_build_trans(motor_cmnd, double *, struct motorRecord *); +STATIC RTN_STATUS PC6K_end_trans(struct motorRecord *); + +struct motor_dset devPC6K = +{ + {8, NULL, (DEVSUPFUN) PC6K_init, (DEVSUPFUN) PC6K_init_record, NULL}, + motor_update_values, + PC6K_start_trans, + PC6K_build_trans, + PC6K_end_trans +}; + +extern "C" {epicsExportAddress(dset,devPC6K);} + +/* --------------------------- program data --------------------- */ + +/* This table is used to define the command types */ +/* WARNING! this must match "motor_cmnd" in motor.h */ + +static msg_types PC6K_table[] = { + MOTION, /* MOVE_ABS */ + MOTION, /* MOVE_REL */ + MOTION, /* HOME_FOR */ + MOTION, /* HOME_REV */ + IMMEDIATE, /* LOAD_POS */ + IMMEDIATE, /* SET_VEL_BASE */ + IMMEDIATE, /* SET_VELOCITY */ + IMMEDIATE, /* SET_ACCEL */ + IMMEDIATE, /* GO */ + IMMEDIATE, /* SET_ENC_RATIO */ + INFO, /* GET_INFO */ + MOVE_TERM, /* STOP_AXIS */ + VELOCITY, /* JOG */ + IMMEDIATE, /* SET_PGAIN */ + IMMEDIATE, /* SET_IGAIN */ + IMMEDIATE, /* SET_DGAIN */ + IMMEDIATE, /* ENABLE_TORQUE */ + IMMEDIATE, /* DISABL_TORQUE */ + IMMEDIATE, /* PRIMITIVE */ + IMMEDIATE, /* SET_HIGH_LIMIT */ + IMMEDIATE, /* SET_LOW_LIMIT */ + VELOCITY /* JOG_VELOCITY */ +}; + + +static struct board_stat **PC6K_cards; + +/* --------------------------- program data --------------------- */ + + +/* initialize device support for PC6K stepper motor */ +STATIC long PC6K_init(void *arg) +{ + long rtnval; + int after = (int) arg; + + if (after == 0) + { + drvtabptr = &PC6K_access; + (drvtabptr->init)(); + } + + rtnval = motor_init_com(after, *drvtabptr->cardcnt_ptr, drvtabptr, &PC6K_cards); + return(rtnval); +} + + +/* initialize a record instance */ +STATIC long PC6K_init_record(void *arg) +{ + struct motorRecord *mr = (struct motorRecord *) arg; + return(motor_init_record_com(mr, *drvtabptr->cardcnt_ptr, drvtabptr, PC6K_cards)); +} + + +/* start building a transaction */ +STATIC long PC6K_start_trans(struct motorRecord *mr) +{ + return(motor_start_trans_com(mr, PC6K_cards)); +} + + +/* end building a transaction */ +STATIC RTN_STATUS PC6K_end_trans(struct motorRecord *mr) +{ + return(motor_end_trans_com(mr, drvtabptr)); +} + + +/* add a part to the transaction */ +STATIC RTN_STATUS PC6K_build_trans(motor_cmnd command, double *parms, struct motorRecord *mr) +{ + struct motor_trans *trans = (struct motor_trans *) mr->dpvt; + struct mess_node *motor_call; + struct controller *brdptr; + struct mess_info *motor_info; + struct PC6KController *cntrl; + char buff[110]; + int signal, axis, card, intval; + int maxdigits; + int cmndArg; + double dval, cntrl_units; + unsigned int size; + bool sendMsg; + RTN_STATUS rtnval; + + rtnval = OK; + buff[0] = '\0'; + sendMsg = true; + + /* Protect against NULL pointer with WRTITE_MSG(GO/STOP_AXIS/GET_INFO, NULL). */ + intval = (parms == NULL) ? 0 : NINT(parms[0]); + dval = (parms == NULL) ? 0 : *parms; + + motor_start_trans_com(mr, PC6K_cards); + + motor_call = &(trans->motor_call); + card = motor_call->card; + signal = motor_call->signal; + axis = signal+1; /* Motors start at 1 */ + + motor_call->type = PC6K_table[command]; + + brdptr = (*trans->tabptr->card_array)[card]; + if (brdptr == NULL) + return(rtnval = ERROR); + + cntrl = (struct PC6KController *) brdptr->DevicePrivate; + /* 6K Controllers expect Velocity and Acceleration settings in Revs/sec/sec */ + cntrl_units = dval * cntrl->drive_resolution[signal]; + maxdigits = cntrl->res_decpts[signal]; + + + if (PC6K_table[command] > motor_call->type) + motor_call->type = PC6K_table[command]; + + if (trans->state != BUILD_STATE) + return(rtnval = ERROR); + + if (command == PRIMITIVE && mr->init != NULL && strlen(mr->init) != 0) + { + strcat(motor_call->message, mr->init); + strcat(motor_call->message, PC6K_OUT_EOS); + } + + switch (command) + { + case MOVE_ABS: + case MOVE_REL: + case HOME_FOR: + case HOME_REV: + case JOG: + if (strlen(mr->prem) != 0) + { + strcat(motor_call->message, mr->prem); + rtnval = motor_end_trans_com(mr, drvtabptr); + rtnval = (RTN_STATUS) motor_start_trans_com(mr, PC6K_cards); + motor_call->type = PC6K_table[command]; + } + if (strlen(mr->post) != 0) + motor_call->postmsgptr = (char *) &mr->post; + break; + + default: + break; + } + + /* Parker 6K controllers do not support multiple commands per line */ + switch (command) + { + case MOVE_ABS: + case MOVE_REL: + { + cmndArg = (command == MOVE_ABS) ? 1 : 0; + + sprintf(buff, "%dMA%d", axis, cmndArg); + strcat(motor_call->message, buff); + rtnval = motor_end_trans_com(mr, drvtabptr); + rtnval = (RTN_STATUS) motor_start_trans_com(mr, PC6K_cards); + motor_call->type = PC6K_table[command]; + + sprintf(buff, "%dMC0", axis); + strcat(motor_call->message, buff); + rtnval = motor_end_trans_com(mr, drvtabptr); + rtnval = (RTN_STATUS) motor_start_trans_com(mr, PC6K_cards); + motor_call->type = PC6K_table[command]; + + sprintf(buff, "%dD%d", axis, intval); + } + + break; + + case HOME_FOR: + sprintf(buff, "%dHOM0", axis); + break; + case HOME_REV: + sprintf(buff, "%dHOM1", axis); + break; + + case LOAD_POS: + /* The Feedback position follows the Reference set position */ + sprintf(buff, "%dPSET%d", axis, intval); + break; + + case SET_VEL_BASE: + sendMsg = false; + break; /* PC6K does not use base velocity */ + + case SET_VELOCITY: + // sprintf(buff, "%dV%.*f", axis, maxdigits, cntrl_units); + sprintf(buff, "%dV%d", axis, intval); + break; + + case SET_ACCEL: + /* Set Accel, Avg Accel, Decel, Avg Decel - assume avg to be 1/2 accel */ + // sprintf(buff, "%dA%.*f", axis, maxdigits, cntrl_units); + sprintf(buff, "%dA%d", axis, intval); + strcat(motor_call->message, buff); + rtnval = motor_end_trans_com(mr, drvtabptr); + rtnval = (RTN_STATUS) motor_start_trans_com(mr, PC6K_cards); + motor_call->type = PC6K_table[command]; + + // sprintf(buff, "%dAA%.*f", axis, maxdigits, cntrl_units/2); + sprintf(buff, "%dAA%d", axis, intval/2); + strcat(motor_call->message, buff); + rtnval = motor_end_trans_com(mr, drvtabptr); + rtnval = (RTN_STATUS) motor_start_trans_com(mr, PC6K_cards); + motor_call->type = PC6K_table[command]; + + // sprintf(buff, "%dAD%.*f", axis, maxdigits, cntrl_units); + sprintf(buff, "%dAD%d", axis, intval); + strcat(motor_call->message, buff); + rtnval = motor_end_trans_com(mr, drvtabptr); + rtnval = (RTN_STATUS) motor_start_trans_com(mr, PC6K_cards); + motor_call->type = PC6K_table[command]; + + // sprintf(buff, "%dADA%.*f", axis, maxdigits, cntrl_units/2); + sprintf(buff, "%dADA%d", axis, intval/2); + break; + + case GO: + sprintf(buff, "%dGO", axis); + break; + + case SET_ENC_RATIO: + // sprintf(buff, "%dERES%d:%dDRESET:", axis, intval, axis); + sendMsg = false; + break; + + case GET_INFO: + /* These commands are not actually done by sending a message, but + rather they will indirectly cause the driver to read the status + of all motors */ + sendMsg = false; + break; + + case STOP_AXIS: + sprintf(buff, "!%dS", axis); + break; + + case JOG: + + sprintf(buff, "%dMC1", axis); + strcat(motor_call->message, buff); + rtnval = motor_end_trans_com(mr, drvtabptr); + rtnval = (RTN_STATUS) motor_start_trans_com(mr, PC6K_cards); + motor_call->type = PC6K_table[command]; + + // sprintf(buff, "%dV%.*f", axis, maxdigits, cntrl_units); + sprintf(buff, "%dV%d", axis, intval); + strcat(motor_call->message, buff); + rtnval = motor_end_trans_com(mr, drvtabptr); + rtnval = (RTN_STATUS) motor_start_trans_com(mr, PC6K_cards); + motor_call->type = PC6K_table[command]; + + + if (intval >= 0) + sprintf(buff, "%dD+", axis); + else + sprintf(buff, "%dD-", axis); + + strcat(motor_call->message, buff); + rtnval = motor_end_trans_com(mr, drvtabptr); + rtnval = (RTN_STATUS) motor_start_trans_com(mr, PC6K_cards); + motor_call->type = PC6K_table[command]; + + sprintf(buff, "%dGO", axis); + break; + + case SET_PGAIN: + sprintf(buff, "%dSGP%d", axis, intval); + break; + + case SET_IGAIN: + sprintf(buff, "%dSGI%d", axis, intval); + break; + + case SET_DGAIN: + sprintf(buff, "%dSGV%d", axis, intval); /* Velocity Gain */ + break; + + case ENABLE_TORQUE: + sprintf(buff, "%dDRIVE1", axis); + break; + + case DISABL_TORQUE: + sprintf(buff, "%dDRIVE0", axis); + break; + + case SET_HIGH_LIMIT: + motor_info = &(*trans->tabptr->card_array)[card]->motor_info[signal]; + trans->state = IDLE_STATE; /* No command sent to the controller. */ + + if (motor_info->high_limit > motor_info->low_limit && + intval > motor_info->high_limit) + { + mr->dhlm = motor_info->high_limit * mr->mres; + rtnval = ERROR; + } + sendMsg = false; + break; + + case SET_LOW_LIMIT: + motor_info = &(*trans->tabptr->card_array)[card]->motor_info[signal]; + trans->state = IDLE_STATE; /* No command sent to the controller. */ + + if (motor_info->low_limit < motor_info->high_limit && + intval < motor_info->low_limit) + { + mr->dllm = motor_info->low_limit * mr->mres; + rtnval = ERROR; + } + sendMsg = false; + break; + + default: + sendMsg = false; + rtnval = ERROR; + } + + if (sendMsg == true) + { + size = strlen(buff); + if (size > sizeof(buff) || (strlen(motor_call->message) + size) > MAX_MSG_SIZE) + errlogMessage("PC6K_build_trans(): buffer overflow.\n"); + else + { + strcat(motor_call->message, buff); + motor_end_trans_com(mr, drvtabptr); + } + } + + return(rtnval); +} diff --git a/motorApp/PC6KSrc/devPC6K.dbd b/motorApp/PC6KSrc/devPC6K.dbd index 4bd7ceb5..8a758bb8 100755 --- a/motorApp/PC6KSrc/devPC6K.dbd +++ b/motorApp/PC6KSrc/devPC6K.dbd @@ -1,7 +1,5 @@ -# Parker/Compumotor Device Driver support. -device(motor,VME_IO,devPC6K,"PC6K") -driver(drvPC6K) -registrar(ParkerRegister) -#variable(drvPC6Kdebug) - - +# Parker/Compumtor Device Driver support. +device(motor,VME_IO,devPC6K,"PC6K") +driver(drvPC6K) +registrar(ParkerRegister) +variable(drvPC6Kdebug) diff --git a/motorApp/PC6KSrc/drvPC6K.cc b/motorApp/PC6KSrc/drvPC6K.cc index 2d5f7877..80b8a082 100755 --- a/motorApp/PC6KSrc/drvPC6K.cc +++ b/motorApp/PC6KSrc/drvPC6K.cc @@ -1,888 +1,913 @@ -/* -FILENAME... drvPC6K.cc -USAGE... Motor record driver level support for Parker Compumotor - 6K Series motor controllers - -Version: $Revision: 1.3 $ -Modified By: $Author: sluiter $ -Last Modified: $Date: 2006-01-31 21:59:13 $ - -*/ - -/* - * Original Author: Mark Rivers - * Date: 10/20/97 - * Current Author: J. Sullivan - * - * Experimental Physics and Industrial Control System (EPICS) - * - * Copyright 1991, the Regents of the University of California, - * and the University of Chicago Board of Governors. - * - * This software was produced under U.S. Government contracts: - * (W-7405-ENG-36) at the Los Alamos National Laboratory, - * and (W-31-109-ENG-38) at Argonne National Laboratory. - * - * Initial development by: - * The Controls and Automation Group (AT-8) - * Ground Test Accelerator - * Accelerator Technology Division - * Los Alamos National Laboratory - * - * Co-developed with - * The Controls and Computing Group - * Accelerator Systems Division - * Advanced Photon Source - * Argonne National Laboratory - * - * Modification Log: - * ----------------- - * .01 04-08-05 jps initialized from drvMM4000.cc - */ - - -#include -#include /* isascii functions */ -#include -#include -#include -#include -#include "motor.h" -#include "ParkerRegister.h" -#include "drvPC6K.h" -#include "asynOctetSyncIO.h" -#include "epicsExport.h" - -#define CMD_STATUS "TAS" -#define CMD_POS "TPC" -#define CMD_EA_POS "TPE" -#define CMD_VEL "TVELA" -#define CMD_DRIVE "DRIVE" -#define CMD_HIGHLS "LSPOS" /* Software travel limit */ -#define CMD_LOWLS "LSNEG" -#define CMD_ERES "ERES" -#define CMD_DRES "DRES" -#define CMD_SCLA "SCLA" -#define CMD_SCLV "SCLV" -#define CMD_SCLD "SCLD" -#define CMD_SCALE "SCALE" -#define CMD_AXSDEF "AXSDEF" /* Axis definition xxxx_xxxx , 0=stepper, 1=servo */ - - -#define STOP_ALL "!K" -#define MOTOR_ON "%dDRIVE1" -#define COMEXEC_ENA "COMEXC1" /* Continuous command mode ON */ - - -#define GET_IDENT "TREV" -#define REPLY_CHAR '*' - -#define PC6K_NUM_CARDS 16 -#define BUFF_SIZE 120 /* Maximum length of string to/from PC6K */ - -#define TIMEOUT 1.0 /* Command timeout in sec. */ - -/* Delay after START_MOTION before a status update is possible - * *** Limitation of Parker 6K controller *** -*/ -#define MOTION_DELAY 0.05 - -/*----------------debugging-----------------*/ -#ifdef __GNUG__ - #ifdef DEBUG - #define Debug(l, f, args...) { if(l<=drvPC6Kdebug) printf(f,## args); } - #else - #define Debug(l, f, args...) - #endif -#else - #define Debug() -#endif -volatile int drvPC6Kdebug = 0; -extern "C" {epicsExportAddress(int, drvPC6Kdebug);} - -/* --- Local data. --- */ -int PC6K_num_cards = 0; - -/* Local data required for every driver; see "motordrvComCode.h" */ -#include "motordrvComCode.h" - -/* This is a temporary fix to introduce a delayed reading of the motor - * position after a move completes - */ -volatile double drvPC6KReadbackDelay = 0.; - -/* NOTICE !!!! Command order must match drvPC6K.h/PC6K_query_types !!!! */ -static char *queryCmnds[]= {CMD_STATUS, CMD_POS, CMD_EA_POS, CMD_VEL, CMD_DRIVE}; - -#define QUERY_NUM (sizeof(queryCmnds)/sizeof(char *)) - - -/*----------------functions-----------------*/ -static int recv_mess(int card, char *com, int flag); -static RTN_STATUS send_mess(int card, char const *, char *name); -static int send_recv_mess(int card, char const *send_com, char *recv_com); -static int send_recv_mess(int card, char const *send_com, char *recv_com, - char const *temp_eos); -static int set_status(int card, int signal); -static long report(int level); -static long init(); -static int motor_init(); -static void query_done(int, int, struct mess_node *); - -/*----------------functions-----------------*/ - -struct driver_table PC6K_access = -{ - motor_init, - motor_send, - motor_free, - motor_card_info, - motor_axis_info, - &mess_queue, - &queue_lock, - &free_list, - &freelist_lock, - &motor_sem, - &motor_state, - &total_cards, - &any_motor_in_motion, - send_mess, - recv_mess, - set_status, - query_done, - NULL, - &initialized, - NULL -}; - -struct -{ - long number; -#ifdef __cplusplus - long (*report) (int); - long (*init) (void); -#else - DRVSUPFUN report; - DRVSUPFUN init; -#endif -} drvPC6K = {2, report, init}; - -extern "C" {epicsExportAddress(drvet, drvPC6K);} - -static struct thread_args targs = {SCAN_RATE, &PC6K_access, MOTION_DELAY}; - -/********************************************************* - * Print out driver status report - *********************************************************/ -static long report(int level) -{ - int card; - - if (PC6K_num_cards <=0) - printf(" No PC6K controllers configured.\n"); - else - { - for (card = 0; card < PC6K_num_cards; card++) - { - struct controller *brdptr = motor_state[card]; - - if (brdptr == NULL) - printf(" PC6K controller %d connection failed.\n", card); - else - { - struct PC6KController *cntrl; - - cntrl = (struct PC6KController *) brdptr->DevicePrivate; - printf(" PC6K controller %d, port=%s, address=%d, id: %s \n", - card, cntrl->asyn_port, cntrl->asyn_address, - brdptr->ident); - } - } - } - return(OK); -} - - -static long init() -{ - /* - * We cannot call motor_init() here, because that function can do GPIB I/O, - * and hence requires that the drvGPIB have already been initialized. - * That cannot be guaranteed, so we need to call motor_init from device - * support - */ - /* Check for setup */ - if (PC6K_num_cards <= 0) - { - Debug(1, "init(): PC6K driver disabled. PC6KSetup() missing from startup script.\n"); - } - return((long) 0); -} - - -static void query_done(int card, int axis, struct mess_node *nodeptr) -{ -} - -/********************************************************* - * Read the status and position of all motors on a card - * start_status(int card) - * if card == -1 then start all cards - *********************************************************/ -// static void start_status(int card) -//{ -//} - -/************************************************************** - * Parse status and position strings for a card and signal - * set_status() - ************************************************************/ - -static int set_status(int card, int signal) -{ - struct PC6KController *cntrl; - struct mess_node *nodeptr; - register struct mess_info *motor_info; - char send_buff[80]; - char *strstrRtn[QUERY_NUM]; - char *recvStr; - double vel; - int rtn_state; - int recvCnt; - int motorData; - int motor; - unsigned int qindex; - bool recvRetry, recvNext; - bool plusdir, ls_active = false; - msta_field status; - - cntrl = (struct PC6KController *) motor_state[card]->DevicePrivate; - motor_info = &(motor_state[card]->motor_info[signal]); - status.All = motor_info->status.All; - motor = signal+1; - - /* LOOP: send all status queries and check for valid response - * EXIT LOOP: if communication timeout or invalid response - * but allow one retry; - */ - qindex = 0; - recvRetry = recvNext = false; - do - { - strstrRtn[qindex] = NULL; - sprintf(send_buff, "%d%s", motor, queryCmnds[qindex]); - if ((recvCnt = send_recv_mess(card, send_buff, cntrl->recv_string[qindex])) && - (cntrl->recv_string[qindex][0] == REPLY_CHAR)) - { - strstrRtn[qindex] = &cntrl->recv_string[qindex][1]; - recvRetry = false; - recvNext = (++qindex >= QUERY_NUM) ? false : true; - } - else - { - recvNext = false; - recvRetry = (recvRetry ? false : true); - } - } while (recvRetry || recvNext); - - - /* Check for normal look termination - all queries successful */ - if (qindex >= QUERY_NUM) - cntrl->status = NORMAL; - else - { - if (cntrl->status == NORMAL) - { - epicsThreadSleep(MOTION_DELAY); - cntrl->status = RETRY; - } - else - cntrl->status = COMM_ERR; - } - - if (cntrl->status != NORMAL) - { - if (cntrl->status == COMM_ERR) - { - status.Bits.CNTRL_COMM_ERR = 1; - status.Bits.RA_PROBLEM = 1; - rtn_state = 1; - goto exit; - } - else - { - rtn_state = 0; - goto exit; - } - } - else - status.Bits.CNTRL_COMM_ERR = 0; - - nodeptr = motor_info->motor_motion; - - /* - * Parse the status/fault string - */ - recvStr = strstrRtn[QSTATUS]; - - Debug(5, "set_status(): status = %s\n", recvStr); - - status.Bits.RA_DIRECTION = (recvStr[TAS_NEG] == '0') ? 1 : 0; - - status.Bits.RA_HOME = (recvStr[TAS_HOME] == '1') ? 1 : 0; - - plusdir = (status.Bits.RA_DIRECTION) ? true : false; - - status.Bits.RA_DONE = (recvStr[TAS_INMOTION] == '0') ? 1 : 0; - - /* Set Travel limit switch status bits. */ - if ((recvStr[TAS_HPLUSTL] == '0' && recvStr[TAS_SPLUSTL] == '0') || - status.Bits.RA_HOME) - status.Bits.RA_PLUS_LS = 0; - else - { - status.Bits.RA_PLUS_LS = 1; - if (plusdir == true) - ls_active = true; - } - - if ((recvStr[TAS_HMINUSTL] == '0' && recvStr[TAS_SMINUSTL] == '0') || - status.Bits.RA_HOME) - status.Bits.RA_MINUS_LS = 0; - else - { - status.Bits.RA_MINUS_LS = 1; - if (plusdir == false) - ls_active = true; - } - - - /* Position maintence enabled */ - recvStr = strstrRtn[QDRIVE]; - status.Bits.EA_POSITION = (*recvStr == '1') ? 1: 0; - - /* encoder status */ - status.Bits.EA_SLIP = 0; - status.Bits.EA_SLIP_STALL = 0; - status.Bits.EA_HOME = 0; - - /* - * Parse motor position - */ - recvStr = strstrRtn[QPOS]; - motorData = (int) atof(recvStr); - - if (motorData == motor_info->position) - { - if (nodeptr != 0) /* Increment counter only if motor is moving. */ - motor_info->no_motion_count++; - } - else - { - motor_info->position = motorData; - if (motor_state[card]->motor_info[signal].encoder_present == YES) - { - recvStr = strstrRtn[QEA_POS]; - motor_info->encoder_position = atoi(recvStr); - } - else - motor_info->encoder_position = 0; - - motor_info->no_motion_count = 0; - } - - status.Bits.RA_PROBLEM = 0; - - /* Parse motor velocity? */ - /* NEEDS WORK */ - - recvStr = strstrRtn[QVEL]; - vel = atof(recvStr); - motor_info->velocity = (int)vel; - - rtn_state = (!motor_info->no_motion_count || ls_active == true || - status.Bits.RA_DONE | status.Bits.RA_PROBLEM) ? 1 : 0; - - /* Test for post-move string. */ - if ((status.Bits.RA_DONE || ls_active == true) && nodeptr != 0 && - nodeptr->postmsgptr != 0) - { - strncpy(send_buff, nodeptr->postmsgptr, 80); - send_mess(card, send_buff, (char) NULL); - nodeptr->postmsgptr = NULL; - } - -exit: - motor_info->status.All = status.All; - return(rtn_state); -} - - -/*****************************************************/ -/* send_receive a message to the PC6K board */ -/* send_recv_mess() */ -/*****************************************************/ -static int send_recv_mess(int card, char const *send_com, char *recv_com) -{ - return(send_recv_mess(card, send_com, recv_com, NULL)); -} - -static int send_recv_mess(int card, char const *send_com, char *recv_com, - const char *temp_eos) -{ - struct PC6KController *cntrl; - int size; - size_t nwrite; - size_t nread = 0; - double timeout = 0.; - asynStatus status; - int eomReason; - - - size = strlen(send_com); - recv_com[0] = '\0'; - - if (size > MAX_MSG_SIZE) - { - errlogMessage("drvPC6K.c:send_recv_mess(); message size violation.\n"); - return(ERROR); - } - else if (size == 0) /* Normal exit on empty input message. */ - return(OK); - - if (!motor_state[card]) - { - errlogPrintf("drvPC6K.c:send_recv_mess() - invalid card #%d\n", card); - return(ERROR); - } - Debug(2, "send_recv_mess(): message = %s\n", send_com); - - cntrl = (struct PC6KController *) motor_state[card]->DevicePrivate; - - /* Enable temporary changes to EOS - ie: program creation "-" */ - if (temp_eos != NULL && strlen(temp_eos)) - pasynOctetSyncIO->setInputEos(cntrl->pasynUser,temp_eos,strlen(temp_eos)); - - timeout = TIMEOUT; - /* flush any junk at input port - should not be any data available */ - // pasynOctetSyncIO->flush(cntrl->pasynUser); this is done by asyn - - /* Perform atomic write/read operation */ - status = pasynOctetSyncIO->writeRead(cntrl->pasynUser, send_com, strlen(send_com), - recv_com, PC6K_MSG_SIZE, - TIMEOUT, &nwrite, &nread, &eomReason); - - if ((status != asynSuccess) || (nread <= 0)) - { - recv_com[0] = '\0'; - nread = 0; - } - - Debug(2, "send_recv_mess(): recv message = \"%s\"\n", recv_com); - - /* Return to standard EOS */ - if (temp_eos != NULL && strlen(temp_eos)) - pasynOctetSyncIO->setInputEos(cntrl->pasynUser, - PC6K_IN_EOS,strlen(PC6K_IN_EOS)); - - return(nread); -} -/*****************************************************/ -/* send a message to the PC6K board */ -/* send_mess() */ -/*****************************************************/ -static RTN_STATUS send_mess(int card, char const *com, char *name) -{ - struct PC6KController *cntrl; - int size; - size_t nwrite; - - size = strlen(com); - - if (size > MAX_MSG_SIZE) - { - errlogMessage("drvPC6K.c:send_mess(); message size violation.\n"); - return(ERROR); - } - else if (size == 0) /* Normal exit on empty input message. */ - return(OK); - - if (!motor_state[card]) - { - errlogPrintf("drvPC6K.c:send_mess() - invalid card #%d\n", card); - return(ERROR); - } - - if (name != NULL) - { - errlogPrintf("drvPC6K.c:send_mess() - invalid argument = %s\n", name); - return(ERROR); - } - - Debug(2, "send_mess(): message = %s\n", com); - - cntrl = (struct PC6KController *) motor_state[card]->DevicePrivate; - - /* flush any junk at input port - should not be any data available */ - // pasynOctetSyncIO->flush(cntrl->pasynUser); - - pasynOctetSyncIO->write(cntrl->pasynUser, com, strlen(com), - TIMEOUT, &nwrite); - - return(OK); -} - - -/* - * FUNCTION... recv_mess(int card, char *com, int flag) - * - * INPUT ARGUMENTS... - * card - controller card # (0,1,...). - * *com - caller's response buffer. - * flag | FLUSH = this flag is ignored - the receive buffer is flushed - * on every write (see write_mess()) - * LOGIC... - * IF controller card does not exist. - * ERROR RETURN. - * ENDIF - * NORMAL RETURN. - */ - -static int recv_mess(int card, char *com, int flag) -{ - struct PC6KController *cntrl; - double timeout = 0.; - size_t nread = 0; - asynStatus status; - int eomReason; - - /* Check that card exists */ - if (!motor_state[card]) - return(ERROR); - - cntrl = (struct PC6KController *) motor_state[card]->DevicePrivate; - - timeout = TIMEOUT; - - status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE, - timeout, &nread, &eomReason); - - if (nread > 0) - Debug(2, "recv_mess(): message = \"%s\"\n", com); - - if ((status != asynSuccess) || (nread <= 0)) - { - com[0] = '\0'; - nread = 0; - } - - return(nread); -} - - -/*****************************************************/ -/* Setup system configuration */ -/* PC6KSetup() */ -/*****************************************************/ -RTN_STATUS -PC6KSetup(int num_cards, /* maximum number of controllers in system. */ - int scan_rate) /* polling rate - 1/60 sec units. */ -{ - int itera; - - if (num_cards < 1 || num_cards > PC6K_NUM_CARDS) - PC6K_num_cards = PC6K_NUM_CARDS; - else - PC6K_num_cards = num_cards; - - /* Set motor polling task rate */ - if (scan_rate >= 1 && scan_rate <= 60) - targs.motor_scan_rate = scan_rate; - else - targs.motor_scan_rate = SCAN_RATE; - - /* - * Allocate space for motor_state structures. Note this must be done - * before PC6KConfig is called, so it cannot be done in motor_init() - * This means that we must allocate space for a card without knowing - * if it really exists, which is not a serious problem - */ - motor_state = (struct controller **) malloc(PC6K_num_cards * - sizeof(struct controller *)); - - for (itera = 0; itera < PC6K_num_cards; itera++) - motor_state[itera] = (struct controller *) NULL; - - return(OK); -} - - -/*****************************************************/ -/* Configure a controller */ -/* PC6KConfig() */ -/*****************************************************/ -RTN_STATUS -PC6KConfig(int card, /* card being configured */ - const char *name) /* asyn port name */ -{ - struct PC6KController *cntrl; - - if (card < 0 || card >= PC6K_num_cards) - return(ERROR); - - motor_state[card] = (struct controller *) malloc(sizeof(struct controller)); - motor_state[card]->DevicePrivate = malloc(sizeof(struct PC6KController)); - cntrl = (struct PC6KController *) motor_state[card]->DevicePrivate; - - strcpy(cntrl->asyn_port, name); - return(OK); -} - - -/*****************************************************/ -/* Upload file to Controller */ -/* PC6KConfig() */ -/*****************************************************/ - -RTN_STATUS -PC6KUpLoad(int card, /* Controller Number */ - const char *file, /* full path to upload file */ - const char *progName) /* PC6K program name - NULL=execute */ -{ - FILE *fd; - char nextLine[BUFF_SIZE]; - // char replyBuff[BUFF_SIZE]; - // char eos_str[] = "-"; - // char *eos_ptr = NULL; - // int recvCnt; - int i, lineLen; - - if (card < 0 || card >= total_cards) - { - printf("{PC6KUpLoad: Controller does not exist - %d\n",card); - return(ERROR); - } - - if (motor_state[card] == NULL) - { - printf("PC6KUpLoad: Controller is not configured - %d\n",card); - return(ERROR); - } - - - if ((fd=fopen(file, "r")) == NULL) - { - printf("PC6KUpLoad: File does not exist - %s\n",file); - return(ERROR); - } - - if (progName && strlen(progName)) - { - /* Copy file into PC6K Program */ - sprintf(nextLine, "DEL %s", progName); - // recvCnt = send_recv_mess(card, nextLine, replyBuff); - send_mess(card, nextLine, (char) NULL); - // eos_ptr = eos_str; - sprintf(nextLine, "DEF %s", progName); - // recvCnt = send_recv_mess(card, nextLine, replyBuff, eos_ptr); - // recvCnt = send_recv_mess(card, nextLine, replyBuff); - send_mess(card, nextLine, (char) NULL); - } - - while (fgets(nextLine, BUFF_SIZE, fd) != NULL) - { - /* Clear control characters */ - for (i = 0, lineLen = strlen(nextLine); i < lineLen; i++) - if (!isprint(nextLine[i])) - nextLine[i] = ' '; - - // recvCnt = send_recv_mess(card, nextLine, replyBuff, eos_ptr); - // recvCnt = send_recv_mess(card, nextLine, replyBuff); - send_mess(card, nextLine, (char) NULL); - } - - fclose(fd); - - if (progName && strlen(progName)) - /* End PC6K Program */ - // recvCnt = send_recv_mess(card, "END", replyBuff); - send_mess(card, "END", (char) NULL); - - return(OK); -} - - - -/*****************************************************/ -/* initialize all software and hardware */ -/* This is called from the initialization routine in */ -/* device support. */ -/* motor_init() */ -/*****************************************************/ -static int motor_init() -{ - struct controller *brdptr; - struct PC6KController *cntrl; - int card_index, motor_index; - char buff[BUFF_SIZE]; - char send_buff[80]; - int total_axis = 0; - int recvCnt, retryCnt; - int digits; - bool nextAxis; - bool cardFound = false; - asynStatus success_rtn; - - initialized = true; /* Indicate that driver is initialized. */ - - /* Check for setup */ - if (PC6K_num_cards <= 0) - return(ERROR); - - for (card_index = 0; card_index < PC6K_num_cards; card_index++) - { - if (!motor_state[card_index]) - continue; - - brdptr = motor_state[card_index]; - brdptr->cmnd_response = false; - total_cards = card_index + 1; - cntrl = (struct PC6KController *) brdptr->DevicePrivate; - - /* Initialize communications channel */ - success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port, - cntrl->asyn_address, &cntrl->pasynUser, NULL); - - if (success_rtn == asynSuccess) - { - - /* Set command End-of-string */ - pasynOctetSyncIO->setInputEos(cntrl->pasynUser, - PC6K_IN_EOS,strlen(PC6K_IN_EOS)); - pasynOctetSyncIO->setOutputEos(cntrl->pasynUser, - PC6K_OUT_EOS,strlen(PC6K_OUT_EOS)); - - /* Send a message to the board, see if it exists */ - retryCnt = 0; - - do - { - /* Return value is length of received string */ - recvCnt = send_recv_mess(card_index, GET_IDENT, buff); - /* Check for valid response -- if not retry */ - if ((recvCnt > 0) && (buff[0] == REPLY_CHAR) && strstr(buff,"6K")) - cardFound = true; - } while(!cardFound && ++retryCnt < 3); - } - - if (cardFound) - { - - strcpy(brdptr->ident, buff); /* Save Controller ID */ - - brdptr->localaddr = (char *) NULL; - brdptr->motor_in_motion = 0; - /* Stop all motors */ - send_mess(card_index, STOP_ALL, (char) NULL); - // All stop requires a delay before the controller starts responding - // again - handshake on some command - retryCnt = 0; - do { - recvCnt = send_recv_mess(card_index, CMD_DRIVE, buff); - } while (!recvCnt && ++retryCnt < 3); - - - send_mess(card_index, COMEXEC_ENA, (char) NULL); /* Enable continuous commands */ - - /* The find how many axes this controller has */ - total_axis = 0; - - do { - brdptr->motor_info[total_axis].motor_motion = NULL; - - sprintf(send_buff, "%d%s", total_axis+1, CMD_POS); - recvCnt = send_recv_mess(card_index, send_buff, buff); - nextAxis = (recvCnt > 0 && (buff[0] == REPLY_CHAR)) ? true : false; - if (nextAxis) - total_axis++; - } - while (nextAxis); - - brdptr->total_axis = total_axis; - - for (motor_index = 0; motor_index < total_axis; motor_index++) - { - struct mess_info *motor_info = &brdptr->motor_info[motor_index]; - - motor_info->status.All = 0; - motor_info->no_motion_count = 0; - motor_info->encoder_position = 0; - motor_info->position = 0; - - - /* Encoder Enable - both STEPPER and DC motors have encoder support */ - motor_info->encoder_present = YES; - motor_info->status.Bits.EA_PRESENT = 1; - motor_info->pid_present = YES; - motor_info->status.Bits.GAIN_SUPPORT = 1; - - /* Determine if motor type = servo */ - sprintf(send_buff, "%d%s", motor_index+1, CMD_AXSDEF); - if (send_recv_mess(card_index, send_buff, buff) > 0 && - (buff[0] == REPLY_CHAR) && - (buff[1] == '1')) - cntrl->type[motor_index] = DC; - else - cntrl->type[motor_index] = STEPPER; - - - /* Determin drive resolution */ - if (cntrl->type[motor_index] == DC) - sprintf(send_buff, "%d%s", motor_index+1, CMD_ERES); - else - sprintf(send_buff, "%d%s", motor_index+1, CMD_DRES); - - if (send_recv_mess(card_index, send_buff, buff) > 0 && (buff[0] == REPLY_CHAR)) - cntrl->drive_resolution[motor_index] = 1.0 / atof(&buff[1]); - - digits = (int) -log10(cntrl->drive_resolution[motor_index]) + 2; - if (digits < 1) - digits = 1; - cntrl->res_decpts[motor_index] = digits; - - /* Determine low limit */ - sprintf(send_buff, "%d%s", motor_index+1, CMD_LOWLS); - if (send_recv_mess(card_index, send_buff, buff) > 0 && (buff[0] == REPLY_CHAR)) - motor_info->low_limit = atof(&buff[1]); - - /* Determine high limit */ - sprintf(send_buff, "%d%s", motor_index+1, CMD_HIGHLS); - if (send_recv_mess(card_index, send_buff, buff) > 0 && (buff[0] == REPLY_CHAR)) - motor_info->high_limit = atof(&buff[1]); - - - set_status(card_index, motor_index); /* Read status of each motor */ - } - } - else - motor_state[card_index] = (struct controller *) NULL; - } - - any_motor_in_motion = 0; - - mess_queue.head = (struct mess_node *) NULL; - mess_queue.tail = (struct mess_node *) NULL; - - free_list.head = (struct mess_node *) NULL; - free_list.tail = (struct mess_node *) NULL; - - // epicsThreadCreate((char *) "PC6K_motor", 64, 5000, (EPICSTHREADFUNC) motor_task, (void *) &targs); - epicsThreadCreate((char *) "PC6K_motor", - epicsThreadPriorityMedium, - epicsThreadGetStackSize(epicsThreadStackMedium), - (EPICSTHREADFUNC) motor_task, (void *) &targs); - - - return(OK); -} - +/* +FILENAME... drvPC6K.cc +USAGE... Motor record driver level support for Parker Computmotor + 6K Series motor controllers + +Version: $Revision: 1.4 $ +Modified By: $Author: sullivan $ +Last Modified: $Date: 2006-08-31 15:42:30 $ + +*/ + +/* + * Original Author: Mark Rivers + * Date: 10/20/97 + * Current Author: J. Sullivan + * + * Experimental Physics and Industrial Control System (EPICS) + * + * Copyright 1991, the Regents of the University of California, + * and the University of Chicago Board of Governors. + * + * This software was produced under U.S. Government contracts: + * (W-7405-ENG-36) at the Los Alamos National Laboratory, + * and (W-31-109-ENG-38) at Argonne National Laboratory. + * + * Initial development by: + * The Controls and Automation Group (AT-8) + * Ground Test Accelerator + * Accelerator Technology Division + * Los Alamos National Laboratory + * + * Co-developed with + * The Controls and Computing Group + * Accelerator Systems Division + * Advanced Photon Source + * Argonne National Laboratory + * + * Modification Log: + * ----------------- + * .01 04-08-05 jps initialized from drvMM4000.cc + */ + + +#include +#include /* isascii functions */ +#include +#include +#include +#include +#include "motor.h" +#include "ParkerRegister.h" +#include "drvPC6K.h" +#include "asynOctetSyncIO.h" +#include "epicsExport.h" + +#define CMD_STATUS "TAS" +#define CMD_POS "TPC" +#define CMD_EA_POS "TPE" +#define CMD_VEL "TVELA" +#define CMD_DRIVE "DRIVE" +#define CMD_HIGHLS "LSPOS" /* Software travel limit */ +#define CMD_LOWLS "LSNEG" +#define CMD_ERES "ERES" +#define CMD_DRES "DRES" +#define CMD_SCLA "SCLA1" +#define CMD_SCLV "SCLV1" +#define CMD_SCLD "SCLD1" +#define CMD_SCALE "SCALE1" +#define CMD_AXSDEF "AXSDEF" /* Axis definition xxxx_xxxx , 0=stepper, 1=servo */ +#define CMD_ECHO "ECHO0" /* Echo must be off */ + + + +#define STOP_ALL "!K" +#define MOTOR_ON "%dDRIVE1" +#define COMEXEC_ENA "COMEXC1" /* Continuous command mode ON */ + + +#define GET_IDENT "TREV" +#define REPLY_CHAR '*' + +#define PC6K_NUM_CARDS 16 +#define BUFF_SIZE 120 /* Maximum length of string to/from PC6K */ + +#define TIMEOUT 1.0 /* Command timeout in sec. */ + +/* Delay after START_MOTION before a status update is possible + * *** Limitation of Parker 6K controller *** +*/ +#define MOTION_DELAY 0.05 + +/*----------------debugging-----------------*/ +#ifdef __GNUG__ + #ifdef DEBUG + #define Debug(l, f, args...) { if(l<=drvPC6Kdebug) printf(f,## args); } + #else + #define Debug(l, f, args...) + #endif +#else + #define Debug() +#endif +volatile int drvPC6Kdebug = 0; +extern "C" {epicsExportAddress(int, drvPC6Kdebug);} + +/* --- Local data. --- */ +int PC6K_num_cards = 0; + +/* Local data required for every driver; see "motordrvComCode.h" */ +#include "motordrvComCode.h" + +/* This is a temporary fix to introduce a delayed reading of the motor + * position after a move completes + */ +volatile double drvPC6KReadbackDelay = 0.; + +/* NOTICE !!!! Command order must match drvPC6K.h/PC6K_query_types !!!! */ +static struct { + char *cmnd; + int cmndLen; +} queryOps[]= {{CMD_STATUS, 0}, {CMD_POS, 0}, {CMD_EA_POS, 0}, {CMD_VEL, 0}, {CMD_DRIVE, 0}}; + +#define QUERY_CNT PC6K_QUERY_CNT + + +/*----------------functions-----------------*/ +static int recv_mess(int card, char *com, int flag); +static RTN_STATUS send_mess(int card, char const *, char *name); +static int send_recv_mess(int card, char const *send_com, char *recv_com); +static int send_recv_mess(int card, char const *send_com, char *recv_com, + char const *temp_eos); +static int set_status(int card, int signal); +static long report(int level); +static long init(); +static int motor_init(); +static void query_done(int, int, struct mess_node *); + +/*----------------functions-----------------*/ + +struct driver_table PC6K_access = +{ + motor_init, + motor_send, + motor_free, + motor_card_info, + motor_axis_info, + &mess_queue, + &queue_lock, + &free_list, + &freelist_lock, + &motor_sem, + &motor_state, + &total_cards, + &any_motor_in_motion, + send_mess, + recv_mess, + set_status, + query_done, + NULL, + &initialized, + NULL +}; + +struct +{ + long number; +#ifdef __cplusplus + long (*report) (int); + long (*init) (void); +#else + DRVSUPFUN report; + DRVSUPFUN init; +#endif +} drvPC6K = {2, report, init}; + +extern "C" {epicsExportAddress(drvet, drvPC6K);} + +static struct thread_args targs = {SCAN_RATE, &PC6K_access, MOTION_DELAY}; + +/********************************************************* + * Print out driver status report + *********************************************************/ +static long report(int level) +{ + int card; + + if (PC6K_num_cards <=0) + printf(" No PC6K controllers configured.\n"); + else + { + for (card = 0; card < PC6K_num_cards; card++) + { + struct controller *brdptr = motor_state[card]; + + if (brdptr == NULL) + printf(" PC6K controller %d connection failed.\n", card); + else + { + struct PC6KController *cntrl; + + cntrl = (struct PC6KController *) brdptr->DevicePrivate; + printf(" PC6K controller %d, port=%s, address=%d, id: %s \n", + card, cntrl->asyn_port, cntrl->asyn_address, + brdptr->ident); + } + } + } + return(OK); +} + + +static long init() +{ + /* + * We cannot call motor_init() here, because that function can do GPIB I/O, + * and hence requires that the drvGPIB have already been initialized. + * That cannot be guaranteed, so we need to call motor_init from device + * support + */ + /* Check for setup */ + if (PC6K_num_cards <= 0) + { + Debug(1, "init(): PC6K driver disabled. PC6KSetup() missing from startup script.\n"); + } + return((long) 0); +} + + +static void query_done(int card, int axis, struct mess_node *nodeptr) +{ +} + +/********************************************************* + * Read the status and position of all motors on a card + * start_status(int card) + * if card == -1 then start all cards + *********************************************************/ +// static void start_status(int card) +//{ +//} + +/************************************************************** + * Parse status and position strings for a card and signal + * set_status() + ************************************************************/ + +static int set_status(int card, int signal) +{ + struct PC6KController *cntrl; + struct mess_node *nodeptr; + register struct mess_info *motor_info; + char send_buff[80]; + char *strstrRtn[QUERY_CNT]; + char *recvStr; + double vel; + int rtn_state; + int recvCnt; + int motorData; + int motor; + unsigned int qindex; + bool recvRetry, recvNext; + bool plusdir, ls_active = false; + msta_field status; + + cntrl = (struct PC6KController *) motor_state[card]->DevicePrivate; + motor_info = &(motor_state[card]->motor_info[signal]); + status.All = motor_info->status.All; + motor = signal+1; + + /* LOOP: send all status queries and check for valid response + * EXIT LOOP: if communication timeout or invalid response + * but allow one retry; + */ + qindex = 0; + recvRetry = recvNext = false; + do + { + strstrRtn[qindex] = NULL; + sprintf(send_buff, "%d%s", motor, queryOps[qindex].cmnd); + if ((recvCnt = send_recv_mess(card, send_buff, cntrl->recv_string[qindex])) && + (cntrl->recv_string[qindex][0] == REPLY_CHAR)) + { + // Index into return string (add 1 to command length to account for REPLY_CHAR) + strstrRtn[qindex] = &cntrl->recv_string[qindex][strlen(send_buff)+1]; + recvRetry = false; + recvNext = (++qindex >= QUERY_CNT) ? false : true; + } + else + { + recvNext = false; + recvRetry = (recvRetry ? false : true); + } + } while (recvRetry || recvNext); + + + /* Check for normal look termination - all queries successful */ + if (qindex >= QUERY_CNT) + cntrl->status = NORMAL; + else + { + if (cntrl->status == NORMAL) + { + epicsThreadSleep(MOTION_DELAY); + cntrl->status = RETRY; + } + else + cntrl->status = COMM_ERR; + } + + if (cntrl->status != NORMAL) + { + if (cntrl->status == COMM_ERR) + { + status.Bits.CNTRL_COMM_ERR = 1; + status.Bits.RA_PROBLEM = 1; + rtn_state = 1; + goto exit; + } + else + { + rtn_state = 0; + goto exit; + } + } + else + status.Bits.CNTRL_COMM_ERR = 0; + + nodeptr = motor_info->motor_motion; + + /* + * Parse the status/fault string + */ + recvStr = strstrRtn[QSTATUS]; + + Debug(5, "set_status(): status = %s\n", recvStr); + + status.Bits.RA_DIRECTION = (recvStr[TAS_NEG] == '0') ? 1 : 0; + + status.Bits.RA_HOME = (recvStr[TAS_HOME] == '1') ? 1 : 0; + + plusdir = (status.Bits.RA_DIRECTION) ? true : false; + + status.Bits.RA_DONE = (recvStr[TAS_INMOTION] == '0') ? 1 : 0; + + /* Set Travel limit switch status bits. */ + if ((recvStr[TAS_HPLUSTL] == '0' && recvStr[TAS_SPLUSTL] == '0') || + status.Bits.RA_HOME) + status.Bits.RA_PLUS_LS = 0; + else + { + status.Bits.RA_PLUS_LS = 1; + if (plusdir == true) + ls_active = true; + } + + if ((recvStr[TAS_HMINUSTL] == '0' && recvStr[TAS_SMINUSTL] == '0') || + status.Bits.RA_HOME) + status.Bits.RA_MINUS_LS = 0; + else + { + status.Bits.RA_MINUS_LS = 1; + if (plusdir == false) + ls_active = true; + } + + + /* Position maintence enabled */ + recvStr = strstrRtn[QDRIVE]; + status.Bits.EA_POSITION = (*recvStr == '1') ? 1: 0; + + /* encoder status */ + status.Bits.EA_SLIP = 0; + status.Bits.EA_SLIP_STALL = 0; + status.Bits.EA_HOME = 0; + + /* + * Parse motor position + */ + recvStr = strstrRtn[QPOS]; + motorData = (int) atof(recvStr); + + if (motorData == motor_info->position) + { + if (nodeptr != 0) /* Increment counter only if motor is moving. */ + motor_info->no_motion_count++; + } + else + { + motor_info->position = motorData; + if (motor_state[card]->motor_info[signal].encoder_present == YES) + { + recvStr = strstrRtn[QEA_POS]; + motor_info->encoder_position = atoi(recvStr); + } + else + motor_info->encoder_position = 0; + + motor_info->no_motion_count = 0; + } + + status.Bits.RA_PROBLEM = 0; + + /* Parse motor velocity? */ + /* NEEDS WORK */ + + recvStr = strstrRtn[QVEL]; + vel = atof(recvStr); + motor_info->velocity = (int)vel; + + rtn_state = (!motor_info->no_motion_count || ls_active == true || + status.Bits.RA_DONE | status.Bits.RA_PROBLEM) ? 1 : 0; + + /* Test for post-move string. */ + if ((status.Bits.RA_DONE || ls_active == true) && nodeptr != 0 && + nodeptr->postmsgptr != 0) + { + strncpy(send_buff, nodeptr->postmsgptr, 80); + send_mess(card, send_buff, (char) NULL); + nodeptr->postmsgptr = NULL; + } + +exit: + motor_info->status.All = status.All; + return(rtn_state); +} + + +/*****************************************************/ +/* send_receive a message to the PC6K board */ +/* send_recv_mess() */ +/*****************************************************/ +static int send_recv_mess(int card, char const *send_com, char *recv_com) +{ + return(send_recv_mess(card, send_com, recv_com, NULL)); +} + +static int send_recv_mess(int card, char const *send_com, char *recv_com, + const char *temp_eos) +{ + struct PC6KController *cntrl; + int size; + size_t nwrite; + size_t nread = 0; + double timeout = 0.; + asynStatus status; + int eomReason; + + + size = strlen(send_com); + recv_com[0] = '\0'; + + if (size > MAX_MSG_SIZE) + { + errlogMessage("drvPC6K.c:send_recv_mess(); message size violation.\n"); + return(ERROR); + } + else if (size == 0) /* Normal exit on empty input message. */ + return(OK); + + if (!motor_state[card]) + { + errlogPrintf("drvPC6K.c:send_recv_mess() - invalid card #%d\n", card); + return(ERROR); + } + Debug(2, "send_recv_mess(): message = %s\n", send_com); + + cntrl = (struct PC6KController *) motor_state[card]->DevicePrivate; + + /* Enable temporary changes to EOS - ie: program creation "-" */ + if (temp_eos != NULL && strlen(temp_eos)) + pasynOctetSyncIO->setInputEos(cntrl->pasynUser,temp_eos,strlen(temp_eos)); + + timeout = TIMEOUT; + /* flush any junk at input port - should not be any data available */ + // pasynOctetSyncIO->flush(cntrl->pasynUser); + + /* Perform atomic write/read operation */ + status = pasynOctetSyncIO->writeRead(cntrl->pasynUser, send_com, strlen(send_com), + recv_com, PC6K_MSG_SIZE, + TIMEOUT, &nwrite, &nread, &eomReason); + + if ((status != asynSuccess) || (nread <= 0)) + { + recv_com[0] = '\0'; + nread = 0; + } + + Debug(2, "send_recv_mess(): recv message = \"%s\"\n", recv_com); + + /* Return to standard EOS */ + if (temp_eos != NULL && strlen(temp_eos)) + pasynOctetSyncIO->setInputEos(cntrl->pasynUser, + PC6K_IN_EOS,strlen(PC6K_IN_EOS)); + + return(nread); +} +/*****************************************************/ +/* send a message to the PC6K board */ +/* send_mess() */ +/*****************************************************/ +static RTN_STATUS send_mess(int card, char const *com, char *name) +{ + struct PC6KController *cntrl; + int size; + size_t nwrite; + + size = strlen(com); + + if (size > MAX_MSG_SIZE) + { + errlogMessage("drvPC6K.c:send_mess(); message size violation.\n"); + return(ERROR); + } + else if (size == 0) /* Normal exit on empty input message. */ + return(OK); + + if (!motor_state[card]) + { + errlogPrintf("drvPC6K.c:send_mess() - invalid card #%d\n", card); + return(ERROR); + } + + if (name != NULL) + { + errlogPrintf("drvPC6K.c:send_mess() - invalid argument = %s\n", name); + return(ERROR); + } + + Debug(2, "send_mess(): message = %s\n", com); + + cntrl = (struct PC6KController *) motor_state[card]->DevicePrivate; + + /* flush any junk at input port - should not be any data available */ + // pasynOctetSyncIO->flush(cntrl->pasynUser); + + pasynOctetSyncIO->write(cntrl->pasynUser, com, strlen(com), + TIMEOUT, &nwrite); + + return(OK); +} + + +/* + * FUNCTION... recv_mess(int card, char *com, int flag) + * + * INPUT ARGUMENTS... + * card - controller card # (0,1,...). + * *com - caller's response buffer. + * flag | FLUSH = this flag is ignored - the receive buffer is flushed + * on every write (see write_mess()) + * LOGIC... + * IF controller card does not exist. + * ERROR RETURN. + * ENDIF + * NORMAL RETURN. + */ + +static int recv_mess(int card, char *com, int flag) +{ + struct PC6KController *cntrl; + double timeout = 0.; + size_t nread = 0; + asynStatus status; + int eomReason; + + /* Check that card exists */ + if (!motor_state[card]) + return(ERROR); + + cntrl = (struct PC6KController *) motor_state[card]->DevicePrivate; + + timeout = TIMEOUT; + + status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE, + timeout, &nread, &eomReason); + + if (nread > 0) + Debug(2, "recv_mess(): message = \"%s\"\n", com); + + if ((status != asynSuccess) || (nread <= 0)) + { + com[0] = '\0'; + nread = 0; + } + + return(nread); +} + + +/*****************************************************/ +/* Setup system configuration */ +/* PC6KSetup() */ +/*****************************************************/ +RTN_STATUS +PC6KSetup(int num_cards, /* maximum number of controllers in system. */ + int scan_rate) /* polling rate - 1/60 sec units. */ +{ + int itera; + + if (num_cards < 1 || num_cards > PC6K_NUM_CARDS) + PC6K_num_cards = PC6K_NUM_CARDS; + else + PC6K_num_cards = num_cards; + + /* Set motor polling task rate */ + if (scan_rate >= 1 && scan_rate <= 60) + targs.motor_scan_rate = scan_rate; + else + targs.motor_scan_rate = SCAN_RATE; + + /* + * Allocate space for motor_state structures. Note this must be done + * before PC6KConfig is called, so it cannot be done in motor_init() + * This means that we must allocate space for a card without knowing + * if it really exists, which is not a serious problem + */ + motor_state = (struct controller **) malloc(PC6K_num_cards * + sizeof(struct controller *)); + + for (itera = 0; itera < PC6K_num_cards; itera++) + motor_state[itera] = (struct controller *) NULL; + + return(OK); +} + + +/*****************************************************/ +/* Configure a controller */ +/* PC6KConfig() */ +/*****************************************************/ +RTN_STATUS +PC6KConfig(int card, /* card being configured */ + const char *name) /* asyn port name */ +{ + struct PC6KController *cntrl; + + if (card < 0 || card >= PC6K_num_cards) + return(ERROR); + + motor_state[card] = (struct controller *) malloc(sizeof(struct controller)); + motor_state[card]->DevicePrivate = malloc(sizeof(struct PC6KController)); + cntrl = (struct PC6KController *) motor_state[card]->DevicePrivate; + + strcpy(cntrl->asyn_port, name); + return(OK); +} + + +/*****************************************************/ +/* Upload file to Controller */ +/* PC6KConfig() */ +/*****************************************************/ + +RTN_STATUS +PC6KUpLoad(int card, /* Controller Number */ + const char *file, /* full path to upload file */ + const char *progName) /* PC6K program name - NULL=execute */ +{ + FILE *fd; + char nextLine[BUFF_SIZE]; + // char replyBuff[BUFF_SIZE]; + // char eos_str[] = "-"; + // char *eos_ptr = NULL; + // int recvCnt; + int i, lineLen; + + if (card < 0 || card >= total_cards) + { + printf("{PC6KUpLoad: Controller does not exist - %d\n",card); + return(ERROR); + } + + if (motor_state[card] == NULL) + { + printf("PC6KUpLoad: Controller is not configured - %d\n",card); + return(ERROR); + } + + + if ((fd=fopen(file, "r")) == NULL) + { + printf("PC6KUpLoad: File does not exist - %s\n",file); + return(ERROR); + } + + if (progName && strlen(progName)) + { + /* Copy file into PC6K Program */ + sprintf(nextLine, "DEL %s", progName); + // recvCnt = send_recv_mess(card, nextLine, replyBuff); + send_mess(card, nextLine, (char) NULL); + // eos_ptr = eos_str; + sprintf(nextLine, "DEF %s", progName); + // recvCnt = send_recv_mess(card, nextLine, replyBuff, eos_ptr); + // recvCnt = send_recv_mess(card, nextLine, replyBuff); + send_mess(card, nextLine, (char) NULL); + } + + while (fgets(nextLine, BUFF_SIZE, fd) != NULL) + { + /* Clear control characters */ + for (i = 0, lineLen = strlen(nextLine); i < lineLen; i++) + if (!isprint(nextLine[i])) + nextLine[i] = ' '; + + // recvCnt = send_recv_mess(card, nextLine, replyBuff, eos_ptr); + // recvCnt = send_recv_mess(card, nextLine, replyBuff); + send_mess(card, nextLine, (char) NULL); + } + + fclose(fd); + + if (progName && strlen(progName)) + /* End PC6K Program */ + // recvCnt = send_recv_mess(card, "END", replyBuff); + send_mess(card, "END", (char) NULL); + + return(OK); +} + + + +/*****************************************************/ +/* initialize all software and hardware */ +/* This is called from the initialization routine in */ +/* device support. */ +/* motor_init() */ +/*****************************************************/ +static int motor_init() +{ + struct controller *brdptr; + struct PC6KController *cntrl; + int card_index, motor_index; + char buff[BUFF_SIZE]; + char send_buff[80]; + int total_axis = 0; + int recvCnt, retryCnt; + int digits; + unsigned int i; + bool nextAxis; + bool cardFound = false; + asynStatus success_rtn; + + initialized = true; /* Indicate that driver is initialized. */ + + /* Check for setup */ + if (PC6K_num_cards <= 0) + return(ERROR); + + /* Initialize command definition array used during set_status() */ + for (i=0; i < QUERY_CNT; i++) + queryOps[i].cmndLen = strlen(queryOps[i].cmnd); + + for (card_index = 0; card_index < PC6K_num_cards; card_index++) + { + if (!motor_state[card_index]) + continue; + + brdptr = motor_state[card_index]; + brdptr->cmnd_response = true; + total_cards = card_index + 1; + cntrl = (struct PC6KController *) brdptr->DevicePrivate; + + /* Initialize communications channel */ + success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port, + cntrl->asyn_address, &cntrl->pasynUser, NULL); + + if (success_rtn == asynSuccess) + { + + /* Set command End-of-string */ + pasynOctetSyncIO->setInputEos(cntrl->pasynUser, + PC6K_IN_EOS,strlen(PC6K_IN_EOS)); + pasynOctetSyncIO->setOutputEos(cntrl->pasynUser, + PC6K_OUT_EOS,strlen(PC6K_OUT_EOS)); + + /* Send a message to the board, see if it exists */ + retryCnt = 0; + + do + { + /* Return value is length of received string */ + recvCnt = send_recv_mess(card_index, GET_IDENT, buff); + /* Check for valid response -- if not retry */ + if ((recvCnt > 0) && strstr(buff, GET_IDENT) && strstr(buff,"6K")) + cardFound = true; + } while(!cardFound && ++retryCnt < 3); + } + + if (cardFound) + { + + strcpy(brdptr->ident, buff); /* Save Controller ID */ + + send_recv_mess(card_index, CMD_ECHO, buff); /* Turn off echo */ + + brdptr->localaddr = (char *) NULL; + brdptr->motor_in_motion = 0; + /* Stop all motors */ + send_recv_mess(card_index, STOP_ALL, buff); + // All stop requires a delay before the controller starts responding + // again - handshake on some command + retryCnt = 0; + do { + recvCnt = send_recv_mess(card_index, CMD_DRIVE, buff); + if (recvCnt && !strstr(buff, CMD_DRIVE)) + recvCnt = 0; + } while (!recvCnt && ++retryCnt < 3); + + + /* send_mess(card_index, COMEXEC_ENA, (char) NULL); */ /* Enable continuous commands */ + send_recv_mess(card_index, COMEXEC_ENA, buff); /* Enable continuous commands */ + // send_recv_mess(card_index, CMD_SCALE, buff); /* Enable scaling - unary */ + + /* The find how many axes this controller has */ + total_axis = 0; + + do { + brdptr->motor_info[total_axis].motor_motion = NULL; + + sprintf(send_buff, "%d%s", total_axis+1, CMD_POS); + recvCnt = send_recv_mess(card_index, send_buff, buff); + nextAxis = (recvCnt > 0 && (buff[0] == REPLY_CHAR)) ? true : false; + if (nextAxis) + total_axis++; + } + while (nextAxis); + + brdptr->total_axis = total_axis; + + for (motor_index = 0; motor_index < total_axis; motor_index++) + { + struct mess_info *motor_info = &brdptr->motor_info[motor_index]; + + motor_info->status.All = 0; + motor_info->no_motion_count = 0; + motor_info->encoder_position = 0; + motor_info->position = 0; + + + /* Encoder Enable - both STEPPER and DC motors have encoder support */ + motor_info->encoder_present = YES; + motor_info->status.Bits.EA_PRESENT = 1; + motor_info->pid_present = YES; + motor_info->status.Bits.GAIN_SUPPORT = 1; + + /* Set unary scaling for Position and Velocities - program in counts */ + // sprintf(send_buff, "%d%s", motor_index+1, CMD_SCLD); + // send_recv_mess(card_index, send_buff, buff); + // sprintf(send_buff, "%d%s", motor_index+1, CMD_SCLV); + // send_recv_mess(card_index, send_buff, buff); + // sprintf(send_buff, "%d%s", motor_index+1, CMD_SCLA); + // send_recv_mess(card_index, send_buff, buff); + + /* Determine if motor type = servo */ + sprintf(send_buff, "%d%s", motor_index+1, CMD_AXSDEF); + if (send_recv_mess(card_index, send_buff, buff) > 0 && + (buff[0] == REPLY_CHAR) && + (buff[strlen(send_buff)+1] == '1')) + cntrl->type[motor_index] = DC; + else + cntrl->type[motor_index] = STEPPER; + + + /* Determin drive resolution */ + if (cntrl->type[motor_index] == DC) + sprintf(send_buff, "%d%s", motor_index+1, CMD_ERES); + else + sprintf(send_buff, "%d%s", motor_index+1, CMD_DRES); + + if (send_recv_mess(card_index, send_buff, buff) > 0 && (buff[0] == REPLY_CHAR)) + cntrl->drive_resolution[motor_index] = 1.0 / atof(&buff[strlen(send_buff)+1]); + + digits = (int) -log10(cntrl->drive_resolution[motor_index]) + 2; + if (digits < 1) + digits = 1; + cntrl->res_decpts[motor_index] = digits; + + /* Determine low limit */ + sprintf(send_buff, "%d%s", motor_index+1, CMD_LOWLS); + if (send_recv_mess(card_index, send_buff, buff) > 0 && (buff[0] == REPLY_CHAR)) + motor_info->low_limit = atof(&buff[strlen(send_buff)+1]); + + /* Determine high limit */ + sprintf(send_buff, "%d%s", motor_index+1, CMD_HIGHLS); + if (send_recv_mess(card_index, send_buff, buff) > 0 && (buff[0] == REPLY_CHAR)) + motor_info->high_limit = atof(&buff[strlen(send_buff)+1]); + + + set_status(card_index, motor_index); /* Read status of each motor */ + } + } + else + motor_state[card_index] = (struct controller *) NULL; + } + + any_motor_in_motion = 0; + + mess_queue.head = (struct mess_node *) NULL; + mess_queue.tail = (struct mess_node *) NULL; + + free_list.head = (struct mess_node *) NULL; + free_list.tail = (struct mess_node *) NULL; + + // epicsThreadCreate((char *) "PC6K_motor", 64, 5000, (EPICSTHREADFUNC) motor_task, (void *) &targs); + epicsThreadCreate((char *) "PC6K_motor", + epicsThreadPriorityMedium, + epicsThreadGetStackSize(epicsThreadStackMedium), + (EPICSTHREADFUNC) motor_task, (void *) &targs); + + + return(OK); +} + diff --git a/motorApp/PC6KSrc/drvPC6K.h b/motorApp/PC6KSrc/drvPC6K.h index 0ed2cd7d..516b482b 100755 --- a/motorApp/PC6KSrc/drvPC6K.h +++ b/motorApp/PC6KSrc/drvPC6K.h @@ -1,122 +1,125 @@ -/* -FILENAME... drvPC6K.h -USAGE... This file contains Parker Compumotor driver "include" - information that is specific to the 6K series serial controller - -Version: $Revision: 1.1 $ -Modified By: $Author: rivers $ -Last Modified: $Date: 2005-12-21 22:58:25 $ -*/ - -/* - * Original Author: Mark Rivers - * Current Author: J. Sullivan - * Date: 10/16/97 - * - * Experimental Physics and Industrial Control System (EPICS) - * - * Copyright 1991, the Regents of the University of California, - * and the University of Chicago Board of Governors. - * - * This software was produced under U.S. Government contracts: - * (W-7405-ENG-36) at the Los Alamos National Laboratory, - * and (W-31-109-ENG-38) at Argonne National Laboratory. - * - * Initial development by: - * The Controls and Automation Group (AT-8) - * Ground Test Accelerator - * Accelerator Technology Division - * Los Alamos National Laboratory - * - * Co-developed with - * The Controls and Computing Group - * Accelerator Systems Division - * Advanced Photon Source - * Argonne National Laboratory - * - * Modification Log: - * ----------------- - * .01 06-20-05 jps initialized from drvSPiiPlus.h - */ - -#ifndef INCdrvPC6Kh -#define INCdrvPC6Kh 1 - -#include "motor.h" -#include "motordrvCom.h" -#include "asynDriver.h" -#include "asynOctetSyncIO.h" - -#define PC6K_MSG_SIZE 120 - -#define PC6K_STATUS_RETRY 10 - -/* End-of-string defines */ -#define PC6K_OUT_EOS "\n" /* Command */ -#define PC6K_IN_EOS "\n" /* Reply */ - -enum PC6K_query_types {QSTATUS, QPOS, QEA_POS, QVEL, QDRIVE}; - -enum PC6K_model -{ - PC6K8, - PCGem6K -}; - -enum PC6K_motor_type -{ - UNUSED, - STEPPER, - DC -}; - -#ifndef __cplusplus -typedef enum PC6K_model PC6K_model; -typedef enum PC6K_motor_type PC6K_motor_type; -#endif - - -/* Motion Master specific data is stored in this structure. */ -struct PC6KController -{ - asynUser *pasynUser; /* For RS-232 */ - int asyn_address; /* Use for GPIB or other address with asyn */ - char asyn_port[80]; /* asyn port name */ - char recv_string[sizeof(PC6K_query_types)][PC6K_MSG_SIZE]; /* Query result strings */ - double home_preset[MAX_AXIS]; /* Controller's home preset position (XF command). */ - PC6K_model model; /* Motion Master Model. */ - PC6K_motor_type type[8]; /* For 6K8 only; Motor type array. */ - /* Controller resolution array - * Units are in [Controller EGU's / Record RAW units]. - */ - double drive_resolution[MAX_AXIS]; - int res_decpts[MAX_AXIS]; /* Drive resolution significant dec. pts. */ - - CommStatus status; /* Controller communication status. */ - int status_retry; /* This controller needs multiple retry after GO */ -}; - - -/* Motor status bits for PC6K - TAS command. -* -* cmd: <#>TAS -* rsp: *<#>TAS0000_0000_0000_0000_0000_0000_0000_0000 (32 bits) -* ^ bit 1 ^bit 32 -*/ - -/* Adjusted bit number (-1) from manual - start counting from 0) */ -#define TAS_INMOTION 0 -#define TAS_NEG 1 -#define TAS_HOME 4+1 /* Home Sucessfull */ -#define TAS_DRIVEDOWN 12+3 /* Drive Shutdown */ -#define TAS_DRIVEFAULT 13+3 /* Drive Fault Occurred */ -#define TAS_HPLUSTL 14+3 /* Hardware Plus Travel Limit */ -#define TAS_HMINUSTL 15+3 /* Hardware Minus Travel Limit */ -#define TAS_SPLUSTL 16+4 /* Software Plus Travel Limit */ -#define TAS_SMINUSTL 17+4 /* Software Minus Travel Limit */ -#define TAS_POSERROR 22+5 /* Position Error */ -#define TAS_INTARGET 23+5 /* In Target Zone */ - - -#endif /* INCdrvPC6Kh */ - +/* +FILENAME... drvPC6K.h +USAGE... This file contains Parker Compumotor driver "include" + information that is specific to the 6K series serial controller + +Version: $Revision: 1.2 $ +Modified By: $Author: sullivan $ +Last Modified: $Date: 2006-08-31 15:42:31 $ +*/ + +/* + * Original Author: Mark Rivers + * Current Author: J. Sullivan + * Date: 10/16/97 + * + * Experimental Physics and Industrial Control System (EPICS) + * + * Copyright 1991, the Regents of the University of California, + * and the University of Chicago Board of Governors. + * + * This software was produced under U.S. Government contracts: + * (W-7405-ENG-36) at the Los Alamos National Laboratory, + * and (W-31-109-ENG-38) at Argonne National Laboratory. + * + * Initial development by: + * The Controls and Automation Group (AT-8) + * Ground Test Accelerator + * Accelerator Technology Division + * Los Alamos National Laboratory + * + * Co-developed with + * The Controls and Computing Group + * Accelerator Systems Division + * Advanced Photon Source + * Argonne National Laboratory + * + * Modification Log: + * ----------------- + * .01 06-20-05 jps initialized from drvSPiiPlus.h + */ + +#ifndef INCdrvPC6Kh +#define INCdrvPC6Kh 1 + +#include "motor.h" +#include "motordrvCom.h" +#include "asynDriver.h" +#include "asynOctetSyncIO.h" + +#define PC6K_MSG_SIZE 120 + +#define PC6K_STATUS_RETRY 10 + +/* End-of-string defines */ +#define PC6K_OUT_EOS "\n" /* Command */ +// #define PC6K_IN_EOS "\n" /* Reply */ +#define PC6K_IN_EOS ">" /* Reply */ + +#define PC6K_QUERY_CNT 5 +enum PC6K_query_types {QSTATUS, QPOS, QEA_POS, QVEL, QDRIVE}; + + +enum PC6K_model +{ + PC6K8, + PCGem6K +}; + +enum PC6K_motor_type +{ + UNUSED, + STEPPER, + DC +}; + +#ifndef __cplusplus +typedef enum PC6K_model PC6K_model; +typedef enum PC6K_motor_type PC6K_motor_type; +#endif + + +/* Motion Master specific data is stored in this structure. */ +struct PC6KController +{ + asynUser *pasynUser; /* For RS-232 */ + int asyn_address; /* Use for GPIB or other address with asyn */ + char asyn_port[80]; /* asyn port name */ + char recv_string[PC6K_QUERY_CNT][PC6K_MSG_SIZE]; /* Query result strings */ + double home_preset[MAX_AXIS]; /* Controller's home preset position (XF command). */ + PC6K_model model; /* Motion Master Model. */ + PC6K_motor_type type[8]; /* For 6K8 only; Motor type array. */ + /* Controller resolution array + * Units are in [Controller EGU's / Record RAW units]. + */ + double drive_resolution[MAX_AXIS]; + int res_decpts[MAX_AXIS]; /* Drive resolution significant dec. pts. */ + + CommStatus status; /* Controller communication status. */ + int status_retry; /* This controller needs multiple retry after GO */ +}; + + +/* Motor status bits for PC6K - TAS command. +* +* cmd: <#>TAS +* rsp: *<#>TAS0000_0000_0000_0000_0000_0000_0000_0000 (32 bits) +* ^ bit 1 ^bit 32 +*/ + +/* Adjusted bit number (-1) from manual - start counting from 0) */ +#define TAS_INMOTION 0 +#define TAS_NEG 1 +#define TAS_HOME 4+1 /* Home Sucessfull */ +#define TAS_DRIVEDOWN 12+3 /* Drive Shutdown */ +#define TAS_DRIVEFAULT 13+3 /* Drive Fault Occurred */ +#define TAS_HPLUSTL 14+3 /* Hardware Plus Travel Limit */ +#define TAS_HMINUSTL 15+3 /* Hardware Minus Travel Limit */ +#define TAS_SPLUSTL 16+4 /* Software Plus Travel Limit */ +#define TAS_SMINUSTL 17+4 /* Software Minus Travel Limit */ +#define TAS_POSERROR 22+5 /* Position Error */ +#define TAS_INTARGET 23+5 /* In Target Zone */ + + +#endif /* INCdrvPC6Kh */ +