From bc143c9b065cdd9ec3e80caab395b7424bcd67be Mon Sep 17 00:00:00 2001 From: Ron Sluiter Date: Mon, 19 May 2003 19:19:22 +0000 Subject: [PATCH] Changed to *.cc with R3.14. --- motorApp/NewportSrc/devMM4000.c | 370 -------------- motorApp/NewportSrc/drvMM4000.c | 830 -------------------------------- 2 files changed, 1200 deletions(-) delete mode 100644 motorApp/NewportSrc/devMM4000.c delete mode 100644 motorApp/NewportSrc/drvMM4000.c diff --git a/motorApp/NewportSrc/devMM4000.c b/motorApp/NewportSrc/devMM4000.c deleted file mode 100644 index e98aa5df..00000000 --- a/motorApp/NewportSrc/devMM4000.c +++ /dev/null @@ -1,370 +0,0 @@ -/* -FILENAME... devMM4000.c -USAGE... Motor record device level support for Newport MM4000. - -Version: $Revision: 1.6 $ -Modified By: $Author: sluiter $ -Last Modified: $Date: 2002-07-05 19:25:12 $ -*/ - -/* - * 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 10-20-97 mlr initialized from drvOms58 - * .02 10-30-97 mlr Replaced driver calls with gpipIO functions - * .03 10-30-98 mlr Minor code cleanup, improved formatting - * .04 02-01-99 mlr Added temporary fix to delay reading motor - * positions at the end of a move. - * .05 04-21-01 rls Added jog velocity motor command. - */ - - -#include -#include -#include -#include -#include /* jps: include for init_record wait */ -#include - -#ifdef __cplusplus -extern "C" { -#include -} -#else -#include -#endif -#include /* for sysSymTbl*/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "motorRecord.h" -#include "motor.h" -#include "motordevCom.h" -#include "drvMMCom.h" - -#define STATIC static - -/* ----------------Create the dsets for devMM4000----------------- */ -STATIC struct driver_table *drvtabptr; -STATIC long MM4000_init(int); -STATIC long MM4000_init_record(struct motorRecord *); -STATIC long MM4000_start_trans(struct motorRecord *); -STATIC long MM4000_build_trans(motor_cmnd, double *, struct motorRecord *); -STATIC long MM4000_end_trans(struct motorRecord *); - -struct motor_dset devMM4000 = -{ - {8, NULL, MM4000_init, MM4000_init_record, NULL}, - motor_update_values, - MM4000_start_trans, - MM4000_build_trans, - MM4000_end_trans -}; - - -/* --------------------------- program data --------------------- */ - -/* This table is used to define the command types */ -/* WARNING! this must match "motor_cmnd" in motor.h */ - -static int MM4000_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 **MM4000_cards; - -/* --------------------------- program data --------------------- */ - - -/* initialize device support for MM4000 stepper motor */ -STATIC long MM4000_init(int after) -{ - SYM_TYPE type; - long rtnval; - - if (after == 0) - { - rtnval = symFindByNameEPICS(sysSymTbl, "_MM4000_access", - (char **) &drvtabptr, &type); - if (rtnval != OK) - return(rtnval); - /* - IF before DB initialization. - Initialize MM4000 driver (i.e., call init()). See comment in - drvMM4000.c init(). - ENDIF - */ - (drvtabptr->init)(); - } - - rtnval = motor_init_com(after, *drvtabptr->cardcnt_ptr, drvtabptr, &MM4000_cards); - return(rtnval); -} - - -/* initialize a record instance */ -STATIC long MM4000_init_record(struct motorRecord *mr) -{ - return(motor_init_record_com(mr, *drvtabptr->cardcnt_ptr, drvtabptr, MM4000_cards)); -} - - -/* start building a transaction */ -STATIC long MM4000_start_trans(struct motorRecord *mr) -{ - return(motor_start_trans_com(mr, MM4000_cards)); -} - - -/* end building a transaction */ -STATIC long MM4000_end_trans(struct motorRecord *mr) -{ - return(motor_end_trans_com(mr, drvtabptr)); -} - - -/* add a part to the transaction */ -STATIC long MM4000_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 MMcontroller *cntrl; - char buff[110]; - int axis, card, maxdigits, size; - double dval, cntrl_units; - long rtnval; - - rtnval = OK; - buff[0] = '\0'; - dval = *parms; - - motor_call = &(trans->motor_call); - card = motor_call->card; - axis = motor_call->signal + 1; - brdptr = (*trans->tabptr->card_array)[card]; - if (brdptr == NULL) - return(rtnval = ERROR); - - cntrl = (struct MMcontroller *) brdptr->DevicePrivate; - cntrl_units = dval * cntrl->drive_resolution[axis - 1]; - maxdigits = cntrl->res_decpts[axis - 1]; - - if (MM4000_table[command] > motor_call->type) - motor_call->type = MM4000_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, "\r"); - } - - 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, "%dPA%.*f;", axis, maxdigits, cntrl_units); - break; - - case MOVE_REL: - sprintf(buff, "%dPR%.*f;", axis, maxdigits, cntrl_units); - break; - - case HOME_FOR: - case HOME_REV: - sprintf(buff, "%dOR;", axis); - break; - - case LOAD_POS: - if (cntrl->model == MM4000) - sprintf(buff, "%dSH%.*f;%dDH;%dSH%.*f", axis, maxdigits, cntrl_units, - axis, axis, maxdigits, cntrl->home_preset[axis - 1]); - break; - - case SET_VEL_BASE: - break; /* MM4000 does not use base velocity */ - - case SET_VELOCITY: - sprintf(buff, "%dVA%.*f;", axis, maxdigits, cntrl_units); - break; - - case SET_ACCEL: - /* - * The value passed is in steps/sec/sec. - * Convert to user units/sec/sec - */ - sprintf(buff, "%dAC%.*f;", axis, maxdigits, cntrl_units); - break; - - case GO: - /* - * The MM4000 starts moving immediately on move commands, GO command - * does nothing - */ - break; - - case SET_ENC_RATIO: - /* - * The MM4000 does not have the concept of encoder ratio, ignore this - * command - */ - 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, "%dST;", axis); - break; - - case JOG: - /* - * MM4000 does not have a jog command. Simulate with move absolute - * to the appropriate software limit. We can move to MM4000 soft limits. - * If the record soft limits are set tighter than the MM4000 limits - * the record will prevent JOG motion beyond its soft limits - */ - sprintf(buff, "%dVA%.*f;", axis, maxdigits, fabs(cntrl_units)); - strcat(motor_call->message, buff); - if (dval > 0.) - sprintf(buff, "%dPA%.*f;", axis, maxdigits, mr->dhlm); - else - sprintf(buff, "%dPA%.*f;", axis, maxdigits, mr->dllm); - break; - - case SET_PGAIN: - sprintf(buff, "%dKP%f;%dUF;", axis, dval, axis); - break; - - case SET_IGAIN: - sprintf(buff, "%dKI%f;%dUF;", axis, dval, axis); - break; - - case SET_DGAIN: - sprintf(buff, "%dKD%f;%dUF;", axis, dval, axis); - break; - - case ENABLE_TORQUE: - sprintf(buff, "MO;"); - break; - - case DISABL_TORQUE: - sprintf(buff, "MF;"); - break; - - case SET_HIGH_LIMIT: - motor_info = &(*trans->tabptr->card_array)[card]->motor_info[axis - 1]; - trans->state = IDLE_STATE; /* No command sent to the controller. */ - - if (cntrl_units > motor_info->high_limit) - { - mr->dhlm = motor_info->high_limit; - rtnval = ERROR; - } - break; - - case SET_LOW_LIMIT: - motor_info = &(*trans->tabptr->card_array)[card]->motor_info[axis - 1]; - trans->state = IDLE_STATE; /* No command sent to the controller. */ - - if (cntrl_units < motor_info->low_limit) - { - mr->dllm = motor_info->low_limit; - rtnval = ERROR; - } - break; - - default: - rtnval = ERROR; - } - - size = strlen(buff); - if (size > sizeof(buff) || (strlen(motor_call->message) + size) > MAX_MSG_SIZE) - logMsg((char *) "devMM4000.c:MM4000_build_trans(): buffer overflow.\n", - 0, 0, 0, 0, 0, 0); - else - strcat(motor_call->message, buff); - - return(rtnval); -} diff --git a/motorApp/NewportSrc/drvMM4000.c b/motorApp/NewportSrc/drvMM4000.c deleted file mode 100644 index 94c3ce94..00000000 --- a/motorApp/NewportSrc/drvMM4000.c +++ /dev/null @@ -1,830 +0,0 @@ -/* -FILENAME... drvMM4000.c -USAGE... Motor record driver level support for Newport MM4000. - -Version: $Revision: 1.9 $ -Modified By: $Author: sluiter $ -Last Modified: $Date: 2002-07-05 19:27:23 $ -*/ - -/* - * Original Author: Mark Rivers - * Date: 10/20/97 - * Current Author: Ron Sluiter - * - * 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 10-20-97 mlr initialized from drvOms58 - * .02 10-30-97 mlr Replaced driver calls with gpipIO functions - * .03 10-30-98 mlr Minor code cleanup, improved formatting - * .04 02-01-99 mlr Added temporary fix to delay reading motor positions at - * the end of a move. - * .05 10-13-99 rls modified for standardized motor record. - * .06 09-17-01 rls - * - created a bit-field for motor status response. - * - start_status() allows one retry after a communication error. - * - set_status() sets RA_PROBLEM along with CNTRL_COMM_ERR to terminate - * node. - */ - - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "motor.h" -#include "drvMMCom.h" -#include "gpibIO.h" -#include "serialIO.h" - -#define STATIC static - -#define READ_RESOLUTION "TU;" -#define READ_STATUS "MS;" -#define READ_POSITION "TP;" -#define STOP_ALL "ST;" -#define MOTOR_ON "MO;" -#define GET_IDENT "VE;" - -#define INPUT_TERMINATOR '\r' - -/* Status byte bits */ -#define M_AXIS_MOVING 0x01 -#define M_MOTOR_POWER 0x02 -#define M_MOTOR_DIRECTION 0x04 -#define M_PLUS_LIMIT 0x08 -#define M_MINUS_LIMIT 0x10 -#define M_HOME_SIGNAL 0x20 - -#define MM4000_NUM_CARDS 4 -#define BUFF_SIZE 100 /* Maximum length of string to/from MM4000 */ - -#define GPIB_TIMEOUT 2000 /* Command timeout in msec. */ -#define SERIAL_TIMEOUT 2000 /* Command timeout in msec. */ - -/*----------------debugging-----------------*/ -#ifdef DEBUG - #define Debug(l, f, args...) { if(l<=drvMM4000debug) printf(f,## args); } -#else - #define Debug(l, f, args...) -#endif - -/* --- Local data. --- */ -int MM4000_num_cards = 0; -volatile int drvMM4000debug = 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 drvMM4000ReadbackDelay = 0.; - - -/*----------------functions-----------------*/ -STATIC int recv_mess(int, char *, int); -STATIC int send_mess(int card, char const *com, char c); -STATIC void start_status(int card); -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 MM4000_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, - start_status, - &initialized, - NULL -}; - -struct -{ - long number; -#ifdef __cplusplus - long (*report) (int); - long (*init) (void); -#else - DRVSUPFUN report; - DRVSUPFUN init; -#endif -} drvMM4000 = {2, report, init}; - - -/********************************************************* - * Print out driver status report - *********************************************************/ -static long report(int level) -{ - int card; - - if (MM4000_num_cards <=0) - printf(" No MM4000 controllers configured.\n"); - else - { - for (card = 0; card < MM4000_num_cards; card++) - { - struct controller *brdptr = motor_state[card]; - - if (brdptr == NULL) - printf(" MM4000 controller %d connection failed.\n", card); - else - { - struct MMcontroller *cntrl; - - cntrl = (struct MMcontroller *) brdptr->DevicePrivate; - switch (cntrl->port_type) - { - case RS232_PORT: - printf(" MM4000 controller %d port type = RS-232, id: %s \n", - card, - brdptr->ident); - break; - case GPIB_PORT: - printf(" MM4000 controller %d port type = GPIB, id: %s \n", - card, - brdptr->ident); - break; - default: - printf(" MM4000 controller %d port type = Unknown, id: %s \n", - card, - brdptr->ident); - break; - } - } - } - } - return (0); -} - - -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 (MM4000_num_cards <= 0) - { - Debug(1, "init(): MM4000 driver disabled. MM4000Setup() 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) -{ - struct MMcontroller *cntrl; - int itera, status; - - if (card >= 0) - { - cntrl = (struct MMcontroller *) motor_state[card]->DevicePrivate; - send_mess(card, READ_STATUS, NULL); - status = recv_mess(card, cntrl->status_string, 1); - if (status > 0) - { - cntrl->status = NORMAL; - send_mess(card, READ_POSITION, NULL); - recv_mess(card, cntrl->position_string, 1); - } - else - { - if (cntrl->status == NORMAL) - cntrl->status = RETRY; - else - cntrl->status = COMM_ERR; - } - } - else - { - /* - * For efficiency we send messages to all cards, then read all - * responses. This minimizes the latency due to processing on each card - */ - for (itera = 0; (itera < total_cards) && motor_state[itera]; itera++) - send_mess(itera, READ_STATUS, NULL); - for (itera = 0; (itera < total_cards) && motor_state[itera]; itera++) - { - cntrl = (struct MMcontroller *) motor_state[itera]->DevicePrivate; - status = recv_mess(itera, cntrl->status_string, 1); - if (status > 0) - cntrl->status = NORMAL; - else - { - if (cntrl->status == NORMAL) - cntrl->status = RETRY; - else - cntrl->status = COMM_ERR; - } - } - for (itera = 0; (itera < total_cards) && motor_state[itera]; itera++) - send_mess(itera, READ_POSITION, NULL); - for (itera = 0; (itera < total_cards) && motor_state[itera]; itera++) - { - cntrl = (struct MMcontroller *) motor_state[itera]->DevicePrivate; - recv_mess(itera, cntrl->position_string, 1); - } - } -} - - -/************************************************************** - * Parse status and position strings for a card and signal - * set_status() - ************************************************************/ - -STATIC int set_status(int card, int signal) -{ - struct MMcontroller *cntrl; - struct mess_node *nodeptr; - register struct mess_info *motor_info; - /* Message parsing variables */ - char *p, *tok_save; - char buff[BUFF_SIZE]; - int itera, pos; - MOTOR_STATUS mstat; - int rtn_state; - double motorData; - BOOLEAN plusdir, ls_active = OFF; - - cntrl = (struct MMcontroller *) motor_state[card]->DevicePrivate; - motor_info = &(motor_state[card]->motor_info[signal]); - if (cntrl->status != NORMAL) - { - if (cntrl->status == COMM_ERR) - { - motor_info->status |= CNTRL_COMM_ERR; - motor_info->status |= RA_PROBLEM; - return(1); - } - else - return(0); - } - else - motor_info->status &= ~CNTRL_COMM_ERR; - - nodeptr = motor_info->motor_motion; - - /* - * Parse the status string - * Status string format: 1MSx,2MSy,3MSz,... where x, y and z are the status - * bytes for the motors - */ - pos = signal*5 + 3; /* Offset in status string */ - mstat.All = cntrl->status_string[pos]; - Debug(5, "set_status(): status byte = %x\n", mstat.All); - - if (mstat.Bits.direction == OFF) - motor_info->status &= ~RA_DIRECTION; - else - motor_info->status |= RA_DIRECTION; - - plusdir = (motor_info->status & RA_DIRECTION) ? ON : OFF; - - if (mstat.Bits.inmotion == OFF) - { - motor_info->status |= RA_DONE; -/* TEMPORARY FIX, Mark Rivers, 2/1/99. The MM4000 has reported that the - * motor is done moving, which means that the "jerk time" is done. However, - * the axis can still be settling. For now we put in a delay and poll the - * motor position again. This is not a good long-term solution. - */ - if (motor_info->pid_present == YES && drvMM4000ReadbackDelay != 0.) - { - taskDelay((int)(drvMM4000ReadbackDelay * sysClkRateGet())); - send_mess(card, READ_POSITION, NULL); - recv_mess(card, cntrl->position_string, 1); - } - } - else - motor_info->status &= ~RA_DONE; - - if (mstat.Bits.plustTL == OFF) - motor_info->status &= ~RA_PLUS_LS; - else - { - motor_info->status |= RA_PLUS_LS; - if (plusdir == ON) - ls_active = ON; - } - - if (mstat.Bits.minusTL == OFF) - motor_info->status &= ~RA_MINUS_LS; - else - { - motor_info->status |= RA_MINUS_LS; - if (plusdir == OFF) - ls_active = ON; - } - - if (mstat.Bits.homels == OFF) - motor_info->status &= ~RA_HOME; - else - motor_info->status |= RA_HOME; - - if (mstat.Bits.NOT_power == OFF) - motor_info->status |= EA_POSITION; - else - motor_info->status &= ~EA_POSITION; - - /* encoder status */ - motor_info->status &= ~EA_SLIP; - motor_info->status &= ~EA_SLIP_STALL; - motor_info->status &= ~EA_HOME; - - /* - * Parse motor position - * Position string format: 1TP5.012,2TP1.123,3TP-100.567,... - * Skip to substring for this motor, convert to double - */ - - strcpy(buff, cntrl->position_string); - tok_save = NULL; - p = strtok_r(buff, ",", &tok_save); - for (itera = 0; itera < signal; itera++) - p = strtok_r(NULL, ",", &tok_save); - Debug(6, "set_status(): position substring = %s\n", p); - motorData = atof(p+3) / cntrl->drive_resolution[signal]; - - if (motorData == motor_info->position) - motor_info->no_motion_count++; - else - { - motor_info->position = NINT(motorData); - if (motor_state[card]->motor_info[signal].encoder_present == YES) - motor_info->encoder_position = (epicsInt32) motorData; - else - motor_info->encoder_position = 0; - - motor_info->no_motion_count = 0; - } - - motor_info->status &= ~RA_PROBLEM; - - /* Parse motor velocity? */ - /* NEEDS WORK */ - - motor_info->velocity = 0; - - if (!(motor_info->status & RA_DIRECTION)) - motor_info->velocity *= -1; - - rtn_state = (!motor_info->no_motion_count || ls_active == ON || - (motor_info->status & (RA_DONE | RA_PROBLEM))) ? 1 : 0; - - /* Test for post-move string. */ - if ((motor_info->status & RA_DONE || ls_active == ON) && nodeptr != 0 && - nodeptr->postmsgptr != 0) - { - strcpy(buff, nodeptr->postmsgptr); - send_mess(card, buff, NULL); - nodeptr->postmsgptr = NULL; - } - - return(rtn_state); -} - - -/*****************************************************/ -/* send a message to the MM4000 board */ -/* send_mess() */ -/*****************************************************/ -STATIC int send_mess(int card, char const *com, char inchar) -{ - struct MMcontroller *cntrl; - char local_buff[BUFF_SIZE]; - - if (strlen(com) > MAX_MSG_SIZE) - { - logMsg((char *) "drvMM4000.c:send_mess(); message size violation.\n", - 0, 0, 0, 0, 0, 0); - return (-1); - } - - if (!motor_state[card]) - { - logMsg((char *) "drvMM4000.c:send_mess() - invalid card #%d\n", card, - 0, 0, 0, 0, 0); - return (-1); - } - - if (inchar != (char) NULL) - { - logMsg((char *) "drvMM4000.c:send_mess() - invalid argument = %c\n", inchar, - 0, 0, 0, 0, 0); - return (-1); - } - - /* Make a local copy of the string and add the command line terminator. */ - strcpy(local_buff, com); - strcat(local_buff, "\r"); - Debug(2, "send_mess(): message = %s\n", local_buff); - - cntrl = (struct MMcontroller *) motor_state[card]->DevicePrivate; - - switch (cntrl->port_type) - { - case GPIB_PORT: - gpibIOSend(cntrl->gpibInfo, local_buff, strlen(local_buff), GPIB_TIMEOUT); - break; - case RS232_PORT: - serialIOSend(cntrl->serialInfo, local_buff, strlen(local_buff), SERIAL_TIMEOUT); - break; - } - return (0); -} - - -/* - * FUNCTION... recv_mess(int card, char *com, int flag) - * - * INPUT ARGUMENTS... - * card - controller card # (0,1,...). - * *com - caller's response buffer. - * flag | FLUSH = flush controller's output buffer; set timeout = 0. - * | !FLUSH = retrieve response into caller's buffer; set timeout. - * - * LOGIC... - * IF controller card does not exist. - * ERROR RETURN. - * ENDIF - * NORMAL RETURN. - */ - -STATIC int recv_mess(int card, char *com, int flag) -{ - struct MMcontroller *cntrl; - int timeout = 0; - int len = 0; - - /* Check that card exists */ - if (!motor_state[card]) - return (-1); - - cntrl = (struct MMcontroller *) motor_state[card]->DevicePrivate; - - switch (cntrl->port_type) - { - case GPIB_PORT: - if (flag != FLUSH) - timeout = GPIB_TIMEOUT; - len = gpibIORecv(cntrl->gpibInfo, com, BUFF_SIZE, INPUT_TERMINATOR, - timeout); - break; - case RS232_PORT: - if (flag != FLUSH) - timeout = SERIAL_TIMEOUT; - len = serialIORecv(cntrl->serialInfo, com, BUFF_SIZE, - INPUT_TERMINATOR, timeout); - break; - } - - if (len <= 0) - { - com[0] = '\0'; - len = 0; - } - else - com[len-1] = '\0'; - - Debug(2, "recv_mess(): message = \"%s\"\n", com); - return (len); -} - - -/*****************************************************/ -/* Setup system configuration */ -/* MM4000Setup() */ -/*****************************************************/ -int MM4000Setup(int num_cards, /* maximum number of controllers in system. */ - int num_channels, /* NOT Used. */ - int scan_rate) /* polling rate - 1/60 sec units. */ -{ - int itera; - - if (num_cards < 1 || num_cards > MM4000_NUM_CARDS) - MM4000_num_cards = MM4000_NUM_CARDS; - else - MM4000_num_cards = num_cards; - - /* Set motor polling task rate */ - if (scan_rate >= 1 && scan_rate <= sysClkRateGet()) - motor_scan_rate = sysClkRateGet() / scan_rate; - else - motor_scan_rate = SCAN_RATE; - - /* - * Allocate space for motor_state structures. Note this must be done - * before MM4000Config 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(MM4000_num_cards * - sizeof(struct controller *)); - - for (itera = 0; itera < MM4000_num_cards; itera++) - motor_state[itera] = (struct controller *) NULL; - - return (0); -} - - -/*****************************************************/ -/* Configure a controller */ -/* MM4000Config() */ -/*****************************************************/ -int MM4000Config(int card, /* card being configured */ - PortType port_type, /* GPIB_PORT or RS232_PORT */ - int addr1, /* = link for GPIB or hideos_card for RS-232 */ - int addr2) /* GPIB address or hideos_task */ -{ - struct MMcontroller *cntrl; - - if (card < 0 || card >= MM4000_num_cards) - return (ERROR); - - motor_state[card] = (struct controller *) malloc(sizeof(struct controller)); - motor_state[card]->DevicePrivate = malloc(sizeof(struct MMcontroller)); - cntrl = (struct MMcontroller *) motor_state[card]->DevicePrivate; - - switch (port_type) - { - case GPIB_PORT: - cntrl->port_type = port_type; - cntrl->gpib_link = addr1; - cntrl->gpib_address = addr2; - break; - case RS232_PORT: - cntrl->port_type = port_type; - cntrl->serial_card = addr1; - strcpy(cntrl->serial_task, (char *) addr2); - break; - default: - return (ERROR); - } - return (0); -} - - - -/*****************************************************/ -/* 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 MMcontroller *cntrl; - int card_index, motor_index, arg3, arg4; - char axis_pos[BUFF_SIZE]; - char buff[BUFF_SIZE]; - char *tok_save, *pos_ptr; - int total_axis = 0; - int status, model_num, digits; - BOOLEAN errind; - - initialized = ON; /* Indicate that driver is initialized. */ - - /* Check for setup */ - if (MM4000_num_cards <= 0) - return (ERROR); - - for (card_index = 0; card_index < MM4000_num_cards; card_index++) - { - if (!motor_state[card_index]) - continue; - - brdptr = motor_state[card_index]; - brdptr->cmnd_response = OFF; - total_cards = card_index + 1; - cntrl = (struct MMcontroller *) brdptr->DevicePrivate; - - /* Initialize communications channel */ - errind = OFF; - switch (cntrl->port_type) - { - case GPIB_PORT: - cntrl->gpibInfo = gpibIOInit(cntrl->gpib_link, - cntrl->gpib_address); - if (cntrl->gpibInfo == NULL) - errind = ON; - break; - case RS232_PORT: - cntrl->serialInfo = serialIOInit(cntrl->serial_card, - cntrl->serial_task); - if (cntrl->serialInfo == NULL) - errind = ON; - break; - } - - if (errind == OFF) - { - int retry = 0; - - /* Send a message to the board, see if it exists */ - /* flush any junk at input port - should not be any data available */ - do - recv_mess(card_index, buff, FLUSH); - while (strlen(buff) != 0); - - do - { - send_mess(card_index, READ_POSITION, NULL); - status = recv_mess(card_index, axis_pos, 1); - retry++; - /* Return value is length of response string */ - } while(status == 0 && retry < 3); - } - - if (errind == OFF && status > 0) - { - brdptr->localaddr = (char *) NULL; - brdptr->motor_in_motion = 0; - send_mess(card_index, STOP_ALL, NULL); /* Stop all motors */ - send_mess(card_index, GET_IDENT, NULL); /* Read controller ID string */ - recv_mess(card_index, buff, 1); - strcpy(brdptr->ident, &buff[2]); /* Skip "VE" */ - - /* Set Motion Master model indicator. */ - pos_ptr = strstr(brdptr->ident, "MM"); - model_num = atoi(pos_ptr + 2); - if (model_num == 4000) - cntrl->model = MM4000; - else if (model_num == 4005) - cntrl->model = MM4005; - else - { - logMsg((char *) "drvMM4000.c:motor_init() - invalid model = %s\n", (int) brdptr->ident, - 0, 0, 0, 0, 0); - return (ERROR); - } - - send_mess(card_index, READ_POSITION, NULL); - recv_mess(card_index, axis_pos, 1); - - /* The return string will tell us how many axes this controller has */ - for (total_axis = 0, tok_save = NULL, pos_ptr = strtok_r(axis_pos, ",", &tok_save); - pos_ptr != 0; pos_ptr = strtok_r(NULL, ",", &tok_save), total_axis++) - brdptr->motor_info[total_axis].motor_motion = NULL; - - brdptr->total_axis = total_axis; - - start_status(card_index); - for (motor_index = 0; motor_index < total_axis; motor_index++) - { - struct mess_info *motor_info = &brdptr->motor_info[motor_index]; - int loop_state; - - motor_info->status = 0; - motor_info->no_motion_count = 0; - motor_info->encoder_position = 0; - motor_info->position = 0; - - /* Determine if encoder present based on open/closed loop mode. */ - sprintf(buff, "%dTC", motor_index + 1); - send_mess(card_index, buff, NULL); - recv_mess(card_index, buff, 1); - loop_state = atoi(&buff[3]); /* Skip first 3 characters */ - if (loop_state != 0) - { - motor_info->encoder_present = YES; - motor_info->status |= EA_PRESENT; - motor_info->pid_present = YES; - motor_info->status |= GAIN_SUPPORT; - } - - /* Determine drive resolution. */ - sprintf(buff, "%dTU", motor_index + 1); - send_mess(card_index, buff, NULL); - recv_mess(card_index, buff, 1); - cntrl->drive_resolution[motor_index] = atof(&buff[3]); - - digits = -log10(cntrl->drive_resolution[motor_index]) + 2; - if (digits < 1) - digits = 1; - cntrl->res_decpts[motor_index] = digits; - - /* Save home preset position. */ - sprintf(buff, "%dXH", motor_index + 1); - send_mess(card_index, buff, NULL); - recv_mess(card_index, buff, 1); - cntrl->home_preset[motor_index] = atof(&buff[3]); - - /* Determine low limit */ - sprintf(buff, "%dTL", motor_index + 1); - send_mess(card_index, buff, NULL); - recv_mess(card_index, buff, 1); - motor_info->low_limit = atof(&buff[3]); - - /* Determine high limit */ - sprintf(buff, "%dTR", motor_index + 1); - send_mess(card_index, buff, NULL); - recv_mess(card_index, buff, 1); - motor_info->high_limit = atof(&buff[3]); - - set_status(card_index, motor_index); /* Read status of each motor */ - } - } - else - motor_state[card_index] = (struct controller *) NULL; - } - - motor_sem = semBCreate(SEM_Q_PRIORITY, SEM_EMPTY); - any_motor_in_motion = 0; - - FASTLOCKINIT(&queue_lock); - FASTLOCK(&queue_lock); - mess_queue.head = (struct mess_node *) NULL; - mess_queue.tail = (struct mess_node *) NULL; - FASTUNLOCK(&queue_lock); - - FASTLOCKINIT(&freelist_lock); - FASTLOCK(&freelist_lock); - free_list.head = (struct mess_node *) NULL; - free_list.tail = (struct mess_node *) NULL; - FASTUNLOCK(&freelist_lock); - - if (sizeof(int) >= sizeof(char *)) - { - arg3 = (int) (&MM4000_access); - arg4 = 0; - } - else - { - arg3 = (int) ((long) &MM4000_access >> 16); - arg4 = (int) ((long) &MM4000_access & 0xFFFF); - } - taskSpawn((char *) "MM4000_motor", 64, VX_FP_TASK | VX_STDIO, 5000, motor_task, - motor_scan_rate, arg3, arg4, 0, 0, 0, 0, 0, 0, 0); - return (0); -} -