From d36ecc4af17655cf5d02a56da6df1cf6d18fc7cd Mon Sep 17 00:00:00 2001 From: Ron Sluiter Date: Fri, 14 Feb 2003 15:15:29 +0000 Subject: [PATCH] Original. --- motorApp/MXmotorSrc/MXRegister.cc | 58 +++++ motorApp/MXmotorSrc/MXmotor.h | 62 ++++++ motorApp/MXmotorSrc/Makefile | 25 +++ motorApp/MXmotorSrc/devMXmotor.cc | 206 ++++++++++++++++++ motorApp/MXmotorSrc/devMXmotor.dbd | 5 + motorApp/MXmotorSrc/drvMXmotor.cc | 331 +++++++++++++++++++++++++++++ 6 files changed, 687 insertions(+) create mode 100644 motorApp/MXmotorSrc/MXRegister.cc create mode 100644 motorApp/MXmotorSrc/MXmotor.h create mode 100644 motorApp/MXmotorSrc/Makefile create mode 100644 motorApp/MXmotorSrc/devMXmotor.cc create mode 100644 motorApp/MXmotorSrc/devMXmotor.dbd create mode 100644 motorApp/MXmotorSrc/drvMXmotor.cc diff --git a/motorApp/MXmotorSrc/MXRegister.cc b/motorApp/MXmotorSrc/MXRegister.cc new file mode 100644 index 00000000..0946f921 --- /dev/null +++ b/motorApp/MXmotorSrc/MXRegister.cc @@ -0,0 +1,58 @@ +/* +FILENAME... MXRegister.cc +USAGE... Register MX motor device driver shell commands. + +Version: $Revision: 1.1 $ +Modified By: $Author: sluiter $ +Last Modified: $Date: 2003-02-14 15:15:26 $ +*/ + +/***************************************************************** + 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 +#include +#include + +#include + +#include "MXmotor.h" + +extern "C" +{ + +// MXmotorSetup +static const iocshArg setupArg0 = {"Max. motor #", iocshArgInt}; +static const iocshArg setupArg1 = {"MX data file", iocshArgString}; +static const iocshArg setupArg2 = {"Polling rate", iocshArgInt}; +static const iocshArg * const mxmotorArgs[3] = {&setupArg0, &setupArg1, &setupArg2}; +static const iocshFuncDef setupFuncDef = {"MXmotorSetup", 3, mxmotorArgs}; +static void setupCallFunc(const iocshArgBuf *args) +{ + MXmotorSetup(args[0].ival, args[1].sval, args[2].ival); +} + + +static void mxmotorRegister(void) +{ + iocshRegister(&setupFuncDef, setupCallFunc); +} + +class MXmotorRegister +{ +public: + MXmotorRegister() {mxmotorRegister();} +}; + +static MXmotorRegister MXmotorRegisterObj; + +} // extern "C" diff --git a/motorApp/MXmotorSrc/MXmotor.h b/motorApp/MXmotorSrc/MXmotor.h new file mode 100644 index 00000000..7a5402c4 --- /dev/null +++ b/motorApp/MXmotorSrc/MXmotor.h @@ -0,0 +1,62 @@ +/* +FILENAME... MXmotor.h +USAGE... This file contains "include" information that is specific to + MX motor device driver support. + +Version: $Revision: 1.1 $ +Modified By: $Author: sluiter $ +Last Modified: $Date: 2003-02-14 15:15:27 $ +*/ + +/* + * Original Author: Ron Sluiter + * Date: 11/12/02 + * + * 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: + * ----------------- + */ + +#ifndef INCMXmotorh +#define INCMXmotorh 1 + +#include "motor.h" +extern "C" +{ +#include +#include +} + +struct MXcontroller +{ + CALLBACK_VALUE callback_flag; + MX_RECORD *MXmotor_record; +}; + +RTN_STATUS MXmotorSetup(int, char const *, int); +extern MX_RECORD *MXmotor_record_list; + +#endif /* INCMXmotorh */ + diff --git a/motorApp/MXmotorSrc/Makefile b/motorApp/MXmotorSrc/Makefile new file mode 100644 index 00000000..547b28ee --- /dev/null +++ b/motorApp/MXmotorSrc/Makefile @@ -0,0 +1,25 @@ +# Makefile +TOP = ../.. +include $(TOP)/configure/CONFIG +#---------------------------------------- +# ADD MACRO DEFINITIONS AFTER THIS LINE + +# The following are used for debugging messages. +USR_CFLAGS += -DDEBUG +USR_CXXFLAGS += -DDEBUG + +DBD += devMXmotor.dbd + +LIBRARY_IOC = MXmotor + +# The following is required for the Soft Channel (i.e., MotorSoft) device driver. +SRCS_DEFAULT += devMXmotor.cc drvMXmotor.cc +SRCS_vxWorks += -nil- + +OBJS_DEFAULT += MXRegister +OBJS_vxWorks += -nil- + +include $(TOP)/configure/RULES +#---------------------------------------- +# ADD RULES AFTER THIS LINE + diff --git a/motorApp/MXmotorSrc/devMXmotor.cc b/motorApp/MXmotorSrc/devMXmotor.cc new file mode 100644 index 00000000..38f4de0a --- /dev/null +++ b/motorApp/MXmotorSrc/devMXmotor.cc @@ -0,0 +1,206 @@ +/* +FILENAME... devMXmotor.cc +USAGE... Motor record device level support for MX device driver. + +Version: $Revision: 1.1 $ +Modified By: $Author: sluiter $ +Last Modified: $Date: 2003-02-14 15:15:28 $ +*/ + +/* + * Original Author: Ron Sluiter + * Date: 06/15/99 + * + * Experimental Physics and Industrial Control System (EPICS) + * + * Copyright 1991, the University of Chicago Board of Governors. + * + * This software was produced under U.S. Government contract + * W-31-109-ENG-38 at Argonne National Laboratory. + * + * Beamline Controls & Data Acquisition Group + * Experimental Facilities Division + * Advanced Photon Source + * Argonne National Laboratory + * + * Modification Log: + * ----------------- + */ + +#include + +#include "motorRecord.h" +#include "motor.h" +#include "motordevCom.h" + +#include "MXmotor.h" + +extern int MXmotor_num_cards; +extern struct driver_table MXmotor_access; + +/* ----------------Create the dsets for devMXmotor----------------- */ +static long MXmotor_init(void *); +static long MXmotor_init_record(void *); +static long MXmotor_start_trans(struct motorRecord *); +static RTN_STATUS MXmotor_build(motor_cmnd, double *, struct motorRecord *); +static RTN_STATUS MXmotor_end_trans(struct motorRecord *); + +struct motor_dset devMXmotor = +{ + {8, NULL, MXmotor_init, MXmotor_init_record, NULL}, + motor_update_values, + MXmotor_start_trans, + MXmotor_build, + MXmotor_end_trans +}; + + +/* This table is used to define the command types */ +/* WARNING! this must match "motor_cmnd" in motor.h */ + +static msg_types MXmotor_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 **MXmotor_cards; +static const char errmsg[] = {"\n\n!!!ERROR!!! - MX driver uninitialized.\n"}; + +/* initialize device support for MX motor */ +static long MXmotor_init(void *after) +{ + int before_after = (int) after; + long rtnval = 0; + + if (*(MXmotor_access.init_indicator) == NO) + { + errlogSevPrintf(errlogMinor, "%s", errmsg); + return(ERROR); + } + else + return(motor_init_com(before_after, MXmotor_num_cards, &MXmotor_access, + &MXmotor_cards)); +} + + +static long MXmotor_init_record(void *arg) +{ + struct motorRecord *mr = (struct motorRecord *) arg; + struct motor_trans *trans; + struct controller *brdptr; + struct MXcontroller *cntrl; + MX_RECORD *motor_record; + long rtnval; + + motor_record = mx_get_record(MXmotor_record_list, mr->name); + if (motor_record == NULL) + { + printf("Motor '%s' does not exist.\n", mr->name); + return(ERROR); + } + + rtnval = motor_init_record_com(mr, MXmotor_num_cards, &MXmotor_access, MXmotor_cards); + + trans = (struct motor_trans *) mr->dpvt; + brdptr = (*trans->tabptr->card_array)[mr->card]; + if (brdptr == NULL) + return(rtnval = ERROR); + cntrl = (struct MXcontroller *) brdptr->DevicePrivate; + cntrl->MXmotor_record = motor_record; + + return(rtnval); +} + + +static long MXmotor_start_trans(struct motorRecord *mr) +{ + long rtnval; + return(rtnval); +} + + +static RTN_STATUS MXmotor_build(motor_cmnd command, double *parms, struct motorRecord *mr) +{ + struct motor_trans *trans = (struct motor_trans *) mr->dpvt; + struct mess_node *motor_call; + int size; + char buff[110]; + RTN_STATUS rtnval = OK; + bool send = true; /* Default to send motor command. */ + + struct controller *brdptr; + struct MXcontroller *cntrl; + float fparm; + mx_status_type mx_status; + + brdptr = (*trans->tabptr->card_array)[mr->card]; + if (brdptr == NULL) + return(rtnval = ERROR); + cntrl = (struct MXcontroller *) brdptr->DevicePrivate; + + buff[0] = '\0'; + + motor_start_trans_com(mr, MXmotor_cards); + + motor_call = &(trans->motor_call); + + if (MXmotor_table[command] > motor_call->type) + motor_call->type = MXmotor_table[command]; + + switch (command) + { + case MOVE_ABS: + sprintf(buff, "%d %f", command, *parms); + fparm = *parms; + mx_status = mx_motor_raw_move_absolute(cntrl->MXmotor_record, fparm); + break; + + case MOVE_REL: + sprintf(buff, "%d %f", command, *parms); + break; + + default: + send = false; + rtnval = ERROR; + } + + size = strlen(buff); + if (send == false) + return(rtnval); + else if (size > sizeof(buff) || (strlen(motor_call->message) + size) > MAX_MSG_SIZE) + printf("MXmotor_build(): buffer overflow.\n"); + else + { + strcat(motor_call->message, buff); + motor_end_trans_com(mr, &MXmotor_access); + } + return(rtnval); +} + + +static RTN_STATUS MXmotor_end_trans(struct motorRecord *mr) +{ + RTN_STATUS rtnval = OK; + return(rtnval); +} + diff --git a/motorApp/MXmotorSrc/devMXmotor.dbd b/motorApp/MXmotorSrc/devMXmotor.dbd new file mode 100644 index 00000000..7f868fab --- /dev/null +++ b/motorApp/MXmotorSrc/devMXmotor.dbd @@ -0,0 +1,5 @@ +# Soft Channel driver support. +device(motor,VME_IO,devMXmotor,"MXmotor") +driver(drvMXmotor) + + diff --git a/motorApp/MXmotorSrc/drvMXmotor.cc b/motorApp/MXmotorSrc/drvMXmotor.cc new file mode 100644 index 00000000..0d07e725 --- /dev/null +++ b/motorApp/MXmotorSrc/drvMXmotor.cc @@ -0,0 +1,331 @@ +/* +FILENAME... drvMXmotor.cc +USAGE... Motor record driver level support for MX device driver. + +Version: $Revision: 1.1 $ +Modified By: $Author: sluiter $ +Last Modified: $Date: 2003-02-14 15:15:29 $ +*/ + +/* + * Original Author: Ron Sluiter + * Date: 06/15/99 + * + * Experimental Physics and Industrial Control System (EPICS) + * + * Copyright 1991, the University of Chicago Board of Governors. + * + * This software was produced under U.S. Government contract + * W-31-109-ENG-38 at Argonne National Laboratory. + * + * Beamline Controls & Data Acquisition Group + * Experimental Facilities Division + * Advanced Photon Source + * Argonne National Laboratory + * + * Modification Log: + * ----------------- + */ + +#include + +#include "motor.h" +#include "motordrvCom.h" +#include "MXmotor.h" + +/*----------------debugging-----------------*/ +#ifdef __GNUG__ + #ifdef DEBUG + volatile int drvMXmotordebug = 0; + #define Debug(l, f, args...) { if(l<=drvMXmotordebug) printf(f,## args); } + #else + #define Debug(l, f, args...) + #endif +#else + #define Debug() +#endif + +/* Global data. */ +int MXmotor_num_cards = 0; +MX_RECORD *MXmotor_record_list; + +/* Local data required for every driver; see "motordrvComCode.h" */ +#include "motordrvComCode.h" + +/* Common local function declarations. */ + +static long report(int level); +static long init(); +static void query_done(int, int, struct mess_node *); +static int set_status(int card, int signal); +static RTN_STATUS send_mess(int card, char const *com, char c); +static int recv_mess(int, char *, int); +static int motor_init(); + +struct driver_table MXmotor_access = +{ + NULL, + 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; + long (*report) (int); + long (*init) (void); +} drvMXmotor = {2, report, init}; + +static struct thread_args targs = {SCAN_RATE, &MXmotor_access}; + +/*----------------functions-----------------*/ + +static long report(int level) +{ + int card; + + if (MXmotor_num_cards <= 0) + printf(" No MX motors configured.\n"); + else + { + for (card = 0; card < MXmotor_num_cards; card++) + { + struct controller *brdptr = motor_state[card]; + + if (brdptr == NULL) + printf(" MX motor #%d not found.\n", card); + else + printf(" MX motor #%d\n", card); + } + } + return (0); +} + +static long init() +{ + initialized = true; /* Indicate that driver is initialized. */ + (void) motor_init(); + return ((long) 0); +} + + +static void query_done(int, int, struct mess_node *) +{ +} + + +RTN_STATUS MXmotorSetup(int max_motors, /* Maximum number of motors. */ + char const *filename, /* MX data file. */ + int scan_rate) /* polling rate - 1/60 sec units */ +{ + RTN_STATUS rtnval = OK; + mx_status_type mx_status; + + if (max_motors > 1000) + MXmotor_num_cards = 1000; + else if (max_motors < 0) + MXmotor_num_cards = 0; + else + MXmotor_num_cards = max_motors; + + mx_status = mx_setup_database(&MXmotor_record_list, (char *) filename); + if (mx_status.code != MXE_SUCCESS) + { + printf("MXmotorSetup: database setup error.\n"); + return(ERROR); + } + + /* 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; + + return(rtnval); +} + + +/* Initialize all MX. */ +static int motor_init() +{ + struct controller *pmotorState; + long status; + int card_index, motor_index; + char axis_pos[50], encoder_pos[50]; + int total_encoders = 0, total_axis = 0; + + /* Check for setup */ + if (MXmotor_num_cards <= 0) + { + Debug(1, "motor_init: *OMS driver disabled* \n omsSetup() is missing from startup script.\n"); + return (ERROR); + } + + /* allocate space for total number of motors */ + motor_state = (struct controller **) malloc(MXmotor_num_cards * + sizeof(struct controller *)); + + /* allocate structure space for each motor present */ + + total_cards = MXmotor_num_cards; + + for (card_index = 0; card_index < MXmotor_num_cards; card_index++) + { + Debug(2, "motor_init: card %d\n", card_index); + + motor_state[card_index] = (struct controller *) malloc(sizeof(struct controller)); + motor_state[card_index]->DevicePrivate = malloc(sizeof(struct MXcontroller)); + pmotorState = motor_state[card_index]; + strcpy(pmotorState->ident, "MXmotor"); + pmotorState->localaddr = (char *) 0; + pmotorState->motor_in_motion = 0; + pmotorState->cmnd_response = false; + + pmotorState->irqdata = (struct irqdatastr *) NULL; + + Debug(3, "Total axis = %d\n", total_axis); + pmotorState->total_axis = total_axis = 1; + + for (total_encoders = 0, motor_index = 0; motor_index < total_axis; motor_index++) + pmotorState->motor_info[motor_index].encoder_present = NO; + + for (motor_index = 0; motor_index < total_axis; motor_index++) + { + pmotorState->motor_info[motor_index].motor_motion = NULL; + pmotorState->motor_info[motor_index].status = 0; + pmotorState->motor_info[motor_index].no_motion_count = 0; + pmotorState->motor_info[motor_index].encoder_position = 0; + pmotorState->motor_info[motor_index].position = 0; + + if (pmotorState->motor_info[motor_index].encoder_present == YES) + pmotorState->motor_info[motor_index].status |= EA_PRESENT; + } + } + + 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; + + Debug(3, "Motors initialized\n"); + + epicsThreadCreate((const char *) "MX_motor", 64, 5000, (EPICSTHREADFUNC) motor_task, (void *) &targs); + + Debug(3, "Started motor_task\n"); + return (0); +} + + +static int set_status(int card, int signal) +{ + struct mess_info *motor_info; + + struct controller *brdptr; + struct MXcontroller *cntrl; + mx_status_type mx_status; + + int rtn_state; + bool plusdir, ls_active = false; + unsigned long status; + double motorData; + + brdptr = motor_state[card]; + motor_info = &(brdptr->motor_info[signal]); + cntrl = (struct MXcontroller *) brdptr->DevicePrivate; + + mx_status = mx_motor_get_status(cntrl->MXmotor_record, &status); + if (mx_status.code != MXE_SUCCESS) + return(rtn_state = ERROR); + + if (status & MXSF_MTR_IS_BUSY ) + motor_info->status &= ~RA_DONE; + else + motor_info->status |= RA_DONE; + + mx_status = mx_motor_get_position(cntrl->MXmotor_record, &motorData); + if (mx_status.code != MXE_SUCCESS) + return(rtn_state = ERROR); + + if (motorData == motor_info->position) + motor_info->no_motion_count++; + else + { + epicsInt32 newposition; + + newposition = NINT(motorData); + if (newposition >= motor_info->position) + motor_info->status |= RA_DIRECTION; + else + motor_info->status &= ~RA_DIRECTION; + motor_info->position = newposition; + motor_info->no_motion_count = 0; + } + + plusdir = (motor_info->status & RA_DIRECTION) ? true : false; + + rtn_state = (!motor_info->no_motion_count || ls_active == true || + (motor_info->status & (RA_DONE | RA_PROBLEM))) ? 1 : 0; + + return(rtn_state); +} + +static RTN_STATUS send_mess(int card, char const *com, char inchar) +{ + struct controller *brdptr; + struct MXcontroller *cntrl; + motor_cmnd command; + mx_status_type mx_status; + char *cmndptr, *posptr; + float position; + + brdptr = motor_state[card]; + cntrl = (struct MXcontroller *) brdptr->DevicePrivate; + + posptr = NULL; + cmndptr = strtok_r((char *) com, " ", &posptr); + command = (motor_cmnd) atoi(cmndptr); + position = atof(posptr); + + switch (command) + { + case MOVE_ABS: + mx_status = mx_motor_raw_move_absolute(cntrl->MXmotor_record, position); + break; + + default: + break; + } + + if (mx_status.code != MXE_SUCCESS) + return(ERROR); + + return(OK); +} + + +static int recv_mess(int card, char *com, int amount) +{ + return (0); +} + +