diff --git a/motorApp/NewportSrc/drvPM500.cc b/motorApp/NewportSrc/drvPM500.cc index 4872cc03..57e23be2 100644 --- a/motorApp/NewportSrc/drvPM500.cc +++ b/motorApp/NewportSrc/drvPM500.cc @@ -2,15 +2,15 @@ FILENAME... drvPM500.cc USAGE... Motor record driver level support for Newport PM500. -Version: $Revision: 1.19 $ +Version: $Revision: 1.20 $ Modified By: $Author: sluiter $ -Last Modified: $Date: 2007-01-17 15:34:46 $ +Last Modified: $Date: 2007-01-17 15:39:43 $ */ /* Device Driver Support routines for PM500 motor controller */ /* * Original Author: Mark Rivers - * Current Author: Ron Sluiter + * Current Author: Ron Sluiter * * Experimental Physics and Industrial Control System (EPICS) * @@ -84,9 +84,9 @@ Last Modified: $Date: 2007-01-17 15:34:46 $ /*----------------debugging-----------------*/ #ifdef __GNUG__ #ifdef DEBUG - #define Debug(l, f, args...) { if(l<=drvPM500debug) printf(f,## args); } + #define Debug(l, f, args...) { if(l<=drvPM500debug) printf(f,## args); } #else - #define Debug(l, f, args...) + #define Debug(l, f, args...) #endif #else #define Debug() @@ -97,11 +97,12 @@ extern "C" {epicsExportAddress(int, drvPM500debug);} /* --- Local data. --- */ int PM500_num_cards = 0; static char *PM500_axis_names[] = {"X", "Y", "Z", "A", "B", "C", "D", "E", "F", - "G", "H", "I"}; + "G", "H", "I"}; /* Local data required for every driver; see "motordrvComCode.h" */ #include "motordrvComCode.h" + /*----------------functions-----------------*/ static int recv_mess(int, char *, int); static RTN_STATUS send_mess(int, char const *, char *); @@ -161,42 +162,42 @@ static long report(int level) int card; if (PM500_num_cards <=0) - printf(" No PM500 controllers configured.\n"); + printf(" No PM500 controllers configured.\n"); else { - for (card = 0; card < PM500_num_cards; card++) - { - struct controller *brdptr = motor_state[card]; - - if (brdptr == NULL) - printf(" PM500 controller #%d connection failed.\n", card); - else + for (card = 0; card < PM500_num_cards; card++) { - struct MMcontroller *cntrl; + struct controller *brdptr = motor_state[card]; - cntrl = (struct MMcontroller *) brdptr->DevicePrivate; - printf(" PM500 controller %d port=%s, address=%d, id: %s \n", - card, cntrl->asyn_port, cntrl->asyn_address, - brdptr->ident); + if (brdptr == NULL) + printf(" PM500 controller #%d connection failed.\n", card); + else + { + struct MMcontroller *cntrl; + + cntrl = (struct MMcontroller *) brdptr->DevicePrivate; + printf(" PM500 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 - */ + /* + * 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 (PM500_num_cards <= 0) { - Debug(1, "init(): PM500 driver disabled. PM500Setup() missing from startup script.\n"); + Debug(1, "init(): PM500 driver disabled. PM500Setup() missing from startup script.\n"); } return((long) 0); } @@ -237,25 +238,25 @@ static int set_status(int card, int signal) rtnval = recv_mess(card, response, 1); if (rtnval > 0) { - cntrl->status = NORMAL; - status.Bits.CNTRL_COMM_ERR = 0; + cntrl->status = NORMAL; + status.Bits.CNTRL_COMM_ERR = 0; } else { - if (cntrl->status == NORMAL) - { - cntrl->status = RETRY; - rtn_state = 0; - goto exit; - } - else - { - cntrl->status = COMM_ERR; - status.Bits.CNTRL_COMM_ERR = 1; - status.Bits.RA_PROBLEM = 1; - rtn_state = 1; - goto exit; - } + if (cntrl->status == NORMAL) + { + cntrl->status = RETRY; + rtn_state = 0; + goto exit; + } + else + { + cntrl->status = COMM_ERR; + status.Bits.CNTRL_COMM_ERR = 1; + status.Bits.RA_PROBLEM = 1; + rtn_state = 1; + goto exit; + } } status_char = response[1]; @@ -268,17 +269,17 @@ static int set_status(int card, int signal) if (status_char == 'L') { - ls_active = true; - if (dir_char == '+') - status.Bits.RA_PLUS_LS = 1; - else - status.Bits.RA_MINUS_LS = 1; + ls_active = true; + if (dir_char == '+') + status.Bits.RA_PLUS_LS = 1; + else + status.Bits.RA_MINUS_LS = 1; } else { - ls_active = false; - status.Bits.RA_PLUS_LS = 0; - status.Bits.RA_MINUS_LS = 0; + ls_active = false; + status.Bits.RA_PLUS_LS = 0; + status.Bits.RA_MINUS_LS = 0; } status.Bits.RA_HOME = 0; @@ -297,18 +298,18 @@ static int set_status(int card, int signal) if (motorData == motor_info->position) { - if (nodeptr != 0) /* Increment counter only if motor is moving. */ - motor_info->no_motion_count++; + if (nodeptr != 0) /* Increment counter only if motor is moving. */ + 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->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->no_motion_count = 0; } status.Bits.RA_PROBLEM = 0; @@ -319,19 +320,19 @@ static int set_status(int card, int signal) motor_info->velocity = 0; if (!status.Bits.RA_DIRECTION) - motor_info->velocity *= -1; + motor_info->velocity *= -1; rtn_state = (!motor_info->no_motion_count || ls_active == true || - status.Bits.RA_DONE | status.Bits.RA_PROBLEM) ? 1 : 0; + 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) + nodeptr->postmsgptr != 0) { - strcpy(buff, nodeptr->postmsgptr); - strcat(buff, "\r"); - send_mess(card, buff, (char) NULL); - nodeptr->postmsgptr = NULL; + strcpy(buff, nodeptr->postmsgptr); + strcat(buff, "\r"); + send_mess(card, buff, (char) NULL); + nodeptr->postmsgptr = NULL; } exit: @@ -354,16 +355,16 @@ static RTN_STATUS send_mess(int card, char const *com, char *name) if (size > MAX_MSG_SIZE) { - errlogMessage("drvPM500.c:send_mess(); message size violation.\n"); - return(ERROR); + errlogMessage("drvPM500.c:send_mess(); message size violation.\n"); + return(ERROR); } else if (size == 0) /* Normal exit on empty input message. */ - return(OK); + return(OK); if (!motor_state[card]) { - errlogPrintf("drvPM500.c:send_mess() - invalid card #%d\n", card); - return(ERROR); + errlogPrintf("drvPM500.c:send_mess() - invalid card #%d\n", card); + return(ERROR); } Debug(2, "send_mess(): message = %s\n", com); @@ -403,13 +404,14 @@ static int recv_mess(int card, char *com, int flag) /* Check that card exists */ if (!motor_state[card]) - return(ERROR); + return(ERROR); cntrl = (struct MMcontroller *) motor_state[card]->DevicePrivate; - if (flag != FLUSH) { + if (flag != FLUSH) + { flush=0; - timeout = SERIAL_TIMEOUT; + timeout = SERIAL_TIMEOUT; } if (flush) status = pasynOctetSyncIO->flush(cntrl->pasynUser); status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE, @@ -417,14 +419,14 @@ static int recv_mess(int card, char *com, int flag) if ((status != asynSuccess) || (nread <= 0)) { - com[0] = '\0'; - nread = 0; + com[0] = '\0'; + nread = 0; } else { - /* Test for "system error" response. */ - if (strncmp(com, "SE", 2) == 0) - errlogMessage("recv_mess(): PM500 system error.\n"); + /* Test for "system error" response. */ + if (strncmp(com, "SE", 2) == 0) + errlogMessage("recv_mess(): PM500 system error.\n"); } Debug(2, "recv_mess(): message = \"%s\"\n", com); @@ -438,32 +440,32 @@ static int recv_mess(int card, char *com, int flag) /*****************************************************/ RTN_STATUS PM500Setup(int num_cards, /* maximum number of controllers in system. */ - int scan_rate) /* polling rate - 1/60 sec units. */ + int scan_rate) /* polling rate - 1/60 sec units. */ { int itera; if (num_cards < 1 || num_cards > PM500_NUM_CARDS) - PM500_num_cards = PM500_NUM_CARDS; + PM500_num_cards = PM500_NUM_CARDS; else - PM500_num_cards = num_cards; + PM500_num_cards = num_cards; /* Set motor polling task rate */ if (scan_rate >= 1 && scan_rate <= 60) - targs.motor_scan_rate = scan_rate; + targs.motor_scan_rate = scan_rate; else - targs.motor_scan_rate = SCAN_RATE; + targs.motor_scan_rate = SCAN_RATE; - /* - * Allocate space for motor_state structures. Note this must be done - * before PM500Config 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 - */ + /* + * Allocate space for motor_state structures. Note this must be done + * before PM500Config 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(PM500_num_cards * - sizeof(struct controller *)); + sizeof(struct controller *)); for (itera = 0; itera < PM500_num_cards; itera++) - motor_state[itera] = (struct controller *) NULL; + motor_state[itera] = (struct controller *) NULL; return(OK); } @@ -481,7 +483,7 @@ PM500Config(int card, /* card being configured */ struct MMcontroller *cntrl; if (card < 0 || card >= PM500_num_cards) - return (ERROR); + return(ERROR); motor_state[card] = (struct controller *) malloc(sizeof(struct controller)); motor_state[card]->DevicePrivate = malloc(sizeof(struct MMcontroller)); @@ -514,78 +516,78 @@ static int motor_init() /* Check for setup */ if (PM500_num_cards <= 0) - return(ERROR); + return(ERROR); for (card_index = 0; card_index < PM500_num_cards; card_index++) { - if (!motor_state[card_index]) - continue; + if (!motor_state[card_index]) + continue; - brdptr = motor_state[card_index]; - brdptr->cmnd_response = true; - total_cards = card_index + 1; - cntrl = (struct MMcontroller *) brdptr->DevicePrivate; + brdptr = motor_state[card_index]; + brdptr->cmnd_response = true; + total_cards = card_index + 1; + cntrl = (struct MMcontroller *) brdptr->DevicePrivate; - /* Initialize communications channel */ - success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port, - cntrl->asyn_address, &cntrl->pasynUser, NULL); + /* Initialize communications channel */ + success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port, + cntrl->asyn_address, &cntrl->pasynUser, NULL); - if (success_rtn == asynSuccess) - { - /* flush any junk at input port - should not be any data available */ + if (success_rtn == asynSuccess) + { + /* flush any junk at input port - should not be any data available */ pasynOctetSyncIO->flush(cntrl->pasynUser); - /* Send a SCUM 1 command to put device in this mode. */ - send_mess(card_index, "SCUM 1", (char) NULL); - recv_mess(card_index, buff, 1); + /* Send a SCUM 1 command to put device in this mode. */ + send_mess(card_index, "SCUM 1", (char) NULL); + recv_mess(card_index, buff, 1); - /* Set up basic controller parameters - * "ENAINT $AF" means the following: - * Bit 0=1, only affected axis halts on limit - * Bit 1=1, No message when moving axis beyond limit - * Bit 2=1, No query echo, prepends status character to axis. - * Bit 3=1, NO status character inserted in responses. - * Bit 4=0, No acknowledgement when command is received - * Bit 5=1, Disable sign-on message at power-up - * Bit 6=0, No echo - * Bit 7=1, CR terminator only on commands and responses - * Bit 8=0, CR terminator only on commands and responses - * Bit 9=0, No EOI sent - * Bit 10=0, CR terminator only on commands and responses - * Bit 11=0, CR terminator only on commands and responses - * Bit 12=0, Decimal number format - * Bit 13=0, Eearly serial poll mapping - * Bit 14=0, No SRQ assertion - */ - send_mess(card_index, "SENAINT $AF", (char) NULL); - recv_mess(card_index, buff, 1); + /* Set up basic controller parameters + * "ENAINT $AF" means the following: + * Bit 0=1, only affected axis halts on limit + * Bit 1=1, No message when moving axis beyond limit + * Bit 2=1, No query echo, prepends status character to axis. + * Bit 3=1, NO status character inserted in responses. + * Bit 4=0, No acknowledgement when command is received + * Bit 5=1, Disable sign-on message at power-up + * Bit 6=0, No echo + * Bit 7=1, CR terminator only on commands and responses + * Bit 8=0, CR terminator only on commands and responses + * Bit 9=0, No EOI sent + * Bit 10=0, CR terminator only on commands and responses + * Bit 11=0, CR terminator only on commands and responses + * Bit 12=0, Decimal number format + * Bit 13=0, Eearly serial poll mapping + * Bit 14=0, No SRQ assertion + */ + send_mess(card_index, "SENAINT $AF", (char) NULL); + recv_mess(card_index, buff, 1); - /* Send a message and read response from controller to see if - * it exists */ - send_mess(card_index, GET_IDENT, (char) NULL); - status = recv_mess(card_index, buff, 1); - /* Return value is length of response string */ - } + /* Send a message and read response from controller to see if + * it exists */ + send_mess(card_index, GET_IDENT, (char) NULL); + status = recv_mess(card_index, buff, 1); + /* Return value is length of response string */ + } - if (success_rtn == asynSuccess && status > 0) - { - brdptr->localaddr = (char *) NULL; - brdptr->motor_in_motion = 0; - send_mess(card_index, GET_IDENT, (char) NULL); /* Read controller ID string */ - recv_mess(card_index, buff, 1); - strncpy(brdptr->ident, &buff[2], 50); /* Skip "XD" */ + if (success_rtn == asynSuccess && status > 0) + { + brdptr->localaddr = (char *) NULL; + brdptr->motor_in_motion = 0; + send_mess(card_index, GET_IDENT, (char) NULL); /* Read controller ID string */ + recv_mess(card_index, buff, 1); + strncpy(brdptr->ident, &buff[2], 50); /* Skip "XD" */ /* Figure out how many axes this controller has. * Do this by querying status of each axis in order */ for (total_axis = 0; total_axis < PM500_NUM_CHANNELS; total_axis++) - { - int axis_name = (int) *PM500_axis_names[total_axis]; - brdptr->motor_info[total_axis].motor_motion = NULL; - sprintf(buff, "%cSTAT?", axis_name); + { + int axis_name = (int) *PM500_axis_names[total_axis]; + brdptr->motor_info[total_axis].motor_motion = NULL; + sprintf(buff, "%cSTAT?", axis_name); send_mess(card_index, buff, (char) NULL); recv_mess(card_index, buff, 1); if (buff[1] == 'E') - break; + break; /* Determine axis type and resolution. * This is a real pain, since the only way to get this * information is to ask for the axis firmware and use a @@ -598,72 +600,72 @@ static int motor_init() */ } - brdptr->total_axis = total_axis; + 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]; - char *firmware, *axis_name = PM500_axis_names[motor_index]; - double res = 0.0; + for (motor_index = 0; motor_index < total_axis; motor_index++) + { + struct mess_info *motor_info = &brdptr->motor_info[motor_index]; + char *firmware, *axis_name = PM500_axis_names[motor_index]; + double res = 0.0; sprintf(buff, "%sCONFIG?", axis_name); - send_mess(card_index, buff, (char) NULL); - recv_mess(card_index, buff, 1); + send_mess(card_index, buff, (char) NULL); + recv_mess(card_index, buff, 1); firmware = &buff[8]; - Debug(3, "motor_init: firmware = %s\n", firmware); - if (!strcmp(firmware, "302")) - { - /* 50 nm translator */ - res = .01; - Debug(3, "motor_init: axis %d is a 50 nm translator\n", motor_index); - } - else if (!strcmp(firmware, "309")) - { - /* 25 nm translator */ - res = .01; - Debug(3, "motor_init: axis %d is a 25 nm translator\n", motor_index); - } - else if (!strcmp(firmware, "300")) - { - /* ????? translator ?????? */ - res = .01; - Debug(3, "motor_init: axis %d is a ?????? translator\n", motor_index); - } - else if (!strcmp(firmware, "XXX")) - { - /* Rotator */ - res = .01; - } + Debug(3, "motor_init: firmware = %s\n", firmware); + if (!strcmp(firmware, "302")) + { + /* 50 nm translator */ + res = .01; + Debug(3, "motor_init: axis %d is a 50 nm translator\n", motor_index); + } + else if (!strcmp(firmware, "309")) + { + /* 25 nm translator */ + res = .01; + Debug(3, "motor_init: axis %d is a 25 nm translator\n", motor_index); + } + else if (!strcmp(firmware, "300")) + { + /* ????? translator ?????? */ + res = .01; + Debug(3, "motor_init: axis %d is a ?????? translator\n", motor_index); + } + else if (!strcmp(firmware, "XXX")) + { + /* Rotator */ + res = .01; + } /* Set drive resolution. */ - cntrl->drive_resolution[motor_index] = res; + cntrl->drive_resolution[motor_index] = res; - digits = (int) -log10(cntrl->drive_resolution[motor_index]) + 2; - if (digits < 1) - digits = 1; - cntrl->res_decpts[motor_index] = digits; + digits = (int) -log10(cntrl->drive_resolution[motor_index]) + 2; + if (digits < 1) + digits = 1; + cntrl->res_decpts[motor_index] = digits; - /* PM500 only supports DC motors. */ - motor_info->encoder_present = YES; - motor_info->status.Bits.EA_PRESENT = 1; - motor_info->pid_present = YES; - motor_info->status.Bits.GAIN_SUPPORT = 1; + /* PM500 only supports DC motors. */ + motor_info->encoder_present = YES; + motor_info->status.Bits.EA_PRESENT = 1; + motor_info->pid_present = YES; + motor_info->status.Bits.GAIN_SUPPORT = 1; - motor_info->status.All = 0; - motor_info->no_motion_count = 0; - motor_info->encoder_position = 0; - motor_info->position = 0; + motor_info->status.All = 0; + motor_info->no_motion_count = 0; + motor_info->encoder_position = 0; + motor_info->position = 0; - motor_info->encoder_present = NO; - motor_info->pid_present = NO; + motor_info->encoder_present = NO; + motor_info->pid_present = NO; - cntrl->home_preset[motor_index] = 0; + cntrl->home_preset[motor_index] = 0; - set_status(card_index, motor_index); /* Read status of each motor */ + set_status(card_index, motor_index); /* Read status of each motor */ + } } - } - else - motor_state[card_index] = (struct controller *) NULL; + else + motor_state[card_index] = (struct controller *) NULL; } any_motor_in_motion = 0; @@ -675,8 +677,8 @@ static int motor_init() free_list.tail = (struct mess_node *) NULL; epicsThreadCreate((char *) "PM500_motor", epicsThreadPriorityMedium, - epicsThreadGetStackSize(epicsThreadStackMedium), - (EPICSTHREADFUNC) motor_task, (void *) &targs); + epicsThreadGetStackSize(epicsThreadStackMedium), + (EPICSTHREADFUNC) motor_task, (void *) &targs); return(OK); }