diff --git a/motorApp/NewportSrc/drvESP300.cc b/motorApp/NewportSrc/drvESP300.cc index 52681590..ed4fea66 100644 --- a/motorApp/NewportSrc/drvESP300.cc +++ b/motorApp/NewportSrc/drvESP300.cc @@ -2,15 +2,15 @@ FILENAME... drvESP300.cc USAGE... Motor record driver level support for Newport ESP300/100. -Version: $Revision: 1.21 $ +Version: $Revision: 1.22 $ Modified By: $Author: sluiter $ -Last Modified: $Date: 2005-10-20 19:43:37 $ +Last Modified: $Date: 2006-04-24 19:52:36 $ */ /* * Original Author: Ron Sluiter * Date: 02/19/03 - * Current Author: Ron Sluiter + * Current Author: Ron Sluiter * * Experimental Physics and Industrial Control System (EPICS) * @@ -75,9 +75,9 @@ Last Modified: $Date: 2005-10-20 19:43:37 $ /*----------------debugging-----------------*/ #ifdef __GNUG__ #ifdef DEBUG - #define Debug(l, f, args...) { if(l<=drvESP300debug) printf(f,## args); } + #define Debug(l, f, args...) { if(l<=drvESP300debug) printf(f,## args); } #else - #define Debug(l, f, args...) + #define Debug(l, f, args...) #endif #else #define Debug() @@ -90,6 +90,7 @@ int ESP300_num_cards = 0; /* 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 *); @@ -149,42 +150,42 @@ static long report(int level) int card; if (ESP300_num_cards <=0) - printf(" No ESP300 controllers configured.\n"); + printf(" No ESP300 controllers configured.\n"); else { - for (card = 0; card < ESP300_num_cards; card++) - { - struct controller *brdptr = motor_state[card]; - - if (brdptr == NULL) - printf(" ESP300 controller %d connection failed.\n", card); - else + for (card = 0; card < ESP300_num_cards; card++) { - struct MMcontroller *cntrl; + struct controller *brdptr = motor_state[card]; - cntrl = (struct MMcontroller *) brdptr->DevicePrivate; - printf(" ESP300 controller %d port=%s, address=%d, id: %s \n", - card, cntrl->asyn_port, cntrl->asyn_address, - brdptr->ident); + if (brdptr == NULL) + printf(" ESP300 controller %d connection failed.\n", card); + else + { + struct MMcontroller *cntrl; + + cntrl = (struct MMcontroller *) brdptr->DevicePrivate; + printf(" ESP300 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 (ESP300_num_cards <= 0) { - Debug(1, "init(): ESP300 driver disabled. ESP300Setup() missing from startup script.\n"); + Debug(1, "init(): ESP300 driver disabled. ESP300Setup() missing from startup script.\n"); } return((long) 0); } @@ -225,25 +226,25 @@ static int set_status(int card, int signal) if (charcnt == 1 && (inbuff[0] == '0' || inbuff[0] == '1')) { - 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.Bits.RA_DONE = (inbuff[0] == '1') ? 1 : 0; @@ -257,17 +258,17 @@ 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 { - epicsInt32 newposition; + epicsInt32 newposition; - newposition = NINT(motorData); - status.Bits.RA_DIRECTION = (newposition >= motor_info->position) ? 1 : 0; - motor_info->position = newposition; - motor_info->no_motion_count = 0; + newposition = NINT(motorData); + status.Bits.RA_DIRECTION = (newposition >= motor_info->position) ? 1 : 0; + motor_info->position = newposition; + motor_info->no_motion_count = 0; } plusdir = (status.Bits.RA_DIRECTION) ? true : false; @@ -279,29 +280,29 @@ static int set_status(int card, int signal) cptr = strchr(inbuff, 'H'); if (cptr == NULL) { - Debug(2, "set_status(): PH error = %s\n", inbuff); - rtn_state = 1; - goto exit; + Debug(2, "set_status(): PH error = %s\n", inbuff); + rtn_state = 1; + goto exit; } mstatus = strtol(inbuff, &cptr, 16); /* Set Travel limit switch status bits. */ if (((mstatus >> signal) & 0x01) == false) - status.Bits.RA_PLUS_LS = 0; + status.Bits.RA_PLUS_LS = 0; else { - status.Bits.RA_PLUS_LS = 1; - if (plusdir == true) - ls_active = true; + status.Bits.RA_PLUS_LS = 1; + if (plusdir == true) + ls_active = true; } if (((mstatus >> (signal + 8)) & 0x01) == false) - status.Bits.RA_MINUS_LS = 0; + status.Bits.RA_MINUS_LS = 0; else { - status.Bits.RA_MINUS_LS = 1; - if (plusdir == false) - ls_active = true; + status.Bits.RA_MINUS_LS = 1; + if (plusdir == false) + ls_active = true; } /* Set home switch status. */ @@ -331,23 +332,23 @@ 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(outbuff, nodeptr->postmsgptr); - send_mess(card, outbuff, (char) NULL); - nodeptr->postmsgptr = NULL; + strcpy(outbuff, nodeptr->postmsgptr); + send_mess(card, outbuff, (char) NULL); + nodeptr->postmsgptr = NULL; } exit: motor_info->status.All = status.All; - return (rtn_state); + return(rtn_state); } @@ -365,22 +366,22 @@ static RTN_STATUS send_mess(int card, char const *com, char *name) if (size > MAX_MSG_SIZE) { - errlogMessage("drvESP300:send_mess(); message size violation.\n"); - return(ERROR); + errlogMessage("drvESP300: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("drvESP300:send_mess() - invalid card #%d\n", card); - return(ERROR); + errlogPrintf("drvESP300:send_mess() - invalid card #%d\n", card); + return(ERROR); } if (name != NULL) { - errlogPrintf("drvESP300:send_mess() - invalid argument = %s\n", name); - return(ERROR); + errlogPrintf("drvESP300:send_mess() - invalid argument = %s\n", name); + return(ERROR); } Debug(2, "send_mess(): message = %s\n", com); @@ -447,33 +448,37 @@ 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); + if (flush) + status = pasynOctetSyncIO->flush(cntrl->pasynUser); status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE, timeout, &nread, &eomReason); if (nread > 3 && com[0] == 'E') { - long error; + long error; - error = strtol(&com[1], NULL, 0); - if (error >= 35 && error <= 42) { - if (flush) status = pasynOctetSyncIO->flush(cntrl->pasynUser); - status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE, + error = strtol(&com[1], NULL, 0); + if (error >= 35 && error <= 42) + { + if (flush) + status = pasynOctetSyncIO->flush(cntrl->pasynUser); + status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE, timeout, &nread, &eomReason); } } if ((status != asynSuccess) || (nread <= 0)) { - com[0] = '\0'; - nread = 0; + com[0] = '\0'; + nread = 0; } Debug(2, "recv_mess(): message = \"%s\"\n", com); @@ -487,32 +492,32 @@ static int recv_mess(int card, char *com, int flag) /*****************************************************/ RTN_STATUS ESP300Setup(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 > ESP300_NUM_CARDS) - ESP300_num_cards = ESP300_NUM_CARDS; + ESP300_num_cards = ESP300_NUM_CARDS; else - ESP300_num_cards = num_cards; + ESP300_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 ESP300Config 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 ESP300Config 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(ESP300_num_cards * - sizeof(struct controller *)); + sizeof(struct controller *)); for (itera = 0; itera < ESP300_num_cards; itera++) - motor_state[itera] = (struct controller *) NULL; + motor_state[itera] = (struct controller *) NULL; return(OK); } @@ -524,13 +529,13 @@ ESP300Setup(int num_cards, /* maximum number of controllers in system. */ /*****************************************************/ RTN_STATUS ESP300Config(int card, /* card being configured */ - const char *name, /* asyn port name */ - int address) /* asyn address (GPIB) */ + const char *name, /* asyn port name */ + int address) /* asyn address (GPIB) */ { struct MMcontroller *cntrl; if (card < 0 || card >= ESP300_num_cards) - return (ERROR); + return(ERROR); motor_state[card] = (struct controller *) malloc(sizeof(struct controller)); motor_state[card]->DevicePrivate = malloc(sizeof(struct MMcontroller)); @@ -565,96 +570,96 @@ static int motor_init() /* Check for setup */ if (ESP300_num_cards <= 0) - return(ERROR); + return(ERROR); for (card_index = 0; card_index < ESP300_num_cards; card_index++) { - if (!motor_state[card_index]) - continue; + if (!motor_state[card_index]) + continue; - brdptr = motor_state[card_index]; - brdptr->cmnd_response = false; - total_cards = card_index + 1; - cntrl = (struct MMcontroller *) brdptr->DevicePrivate; + brdptr = motor_state[card_index]; + brdptr->cmnd_response = false; + 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); - pasynOctetSyncIO->setOutputEos(cntrl->pasynUser, output_terminator, - strlen(output_terminator)); - pasynOctetSyncIO->setInputEos(cntrl->pasynUser, input_terminator, - strlen(input_terminator)); + /* Initialize communications channel */ + success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port, + cntrl->asyn_address, &cntrl->pasynUser, NULL); + pasynOctetSyncIO->setOutputEos(cntrl->pasynUser, output_terminator, + strlen(output_terminator)); + pasynOctetSyncIO->setInputEos(cntrl->pasynUser, input_terminator, + strlen(input_terminator)); - if (success_rtn == asynSuccess) - { - int retry = 0; + if (success_rtn == asynSuccess) + { + 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 */ + /* Send a message to the board, see if it exists */ + /* flush any junk at input port - should not be any data available */ pasynOctetSyncIO->flush(cntrl->pasynUser); - do - { - send_mess(card_index, GET_IDENT, (char) NULL); - status = recv_mess(card_index, buff, 1); - retry++; - /* Return value is length of response string */ - } while (status == 0 && retry < 3); - } - - if (success_rtn == asynSuccess && status > 0) - { - brdptr->localaddr = (char *) NULL; - brdptr->motor_in_motion = 0; - strcpy(brdptr->ident, &buff[1]); /* Skip "\n" */ - - send_mess(card_index, "ZU", (char) NULL); - recv_mess(card_index, buff, 1); - total_axis = buff[0] >> 4; - if (total_axis > 4) - { - Debug(2, "motor_init(): ZU = %s\n", buff); - total_axis = 4; + do + { + send_mess(card_index, GET_IDENT, (char) NULL); + status = recv_mess(card_index, buff, 1); + retry++; + /* Return value is length of response string */ + } while (status == 0 && retry < 3); } - for (motor_index = 0; motor_index < total_axis; motor_index++) + if (success_rtn == asynSuccess && status > 0) { - sprintf(buff, STOP_AXIS, motor_index + 1); /* Stop motor */ - send_mess(card_index, buff, (char) NULL); - /* Initialize. */ - brdptr->motor_info[motor_index].motor_motion = NULL; + brdptr->localaddr = (char *) NULL; + brdptr->motor_in_motion = 0; + strcpy(brdptr->ident, &buff[1]); /* Skip "\n" */ + + send_mess(card_index, "ZU", (char) NULL); + recv_mess(card_index, buff, 1); + total_axis = buff[0] >> 4; + if (total_axis > 4) + { + Debug(2, "motor_init(): ZU = %s\n", buff); + total_axis = 4; + } + + for (motor_index = 0; motor_index < total_axis; motor_index++) + { + sprintf(buff, STOP_AXIS, motor_index + 1); /* Stop motor */ + send_mess(card_index, buff, (char) NULL); + /* Initialize. */ + brdptr->motor_info[motor_index].motor_motion = NULL; + } + + 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]; + + /* Set axis resolution. */ + sprintf(buff, "%.2dSU?", motor_index + 1); + send_mess(card_index, buff, 0); + recv_mess(card_index, buff, 1); + cntrl->drive_resolution[motor_index] = atof(&buff[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 = YES; + + if (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_status(card_index, motor_index); /* Read status of each motor */ + } } - - 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]; - - /* Set axis resolution. */ - sprintf(buff, "%.2dSU?", motor_index + 1); - send_mess(card_index, buff, 0); - recv_mess(card_index, buff, 1); - cntrl->drive_resolution[motor_index] = atof(&buff[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 = YES; - - if (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_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; @@ -666,8 +671,8 @@ static int motor_init() free_list.tail = (struct mess_node *) NULL; epicsThreadCreate((char *) "ESP300_motor", epicsThreadPriorityMedium, - epicsThreadGetStackSize(epicsThreadStackMedium), - (EPICSTHREADFUNC) motor_task, (void *) &targs); + epicsThreadGetStackSize(epicsThreadStackMedium), + (EPICSTHREADFUNC) motor_task, (void *) &targs); return(OK); }