diff --git a/motorApp/NewportSrc/devXPSC8.cc b/motorApp/NewportSrc/devXPSC8.cc index 35108708..ed4a1f36 100755 --- a/motorApp/NewportSrc/devXPSC8.cc +++ b/motorApp/NewportSrc/devXPSC8.cc @@ -11,8 +11,8 @@ * * 1) The use of multiaxis groups has been enabled with the addition of * variables to the drvXPSC8.cc: XPSC8Name_config function used in the st.cmd. - * 2) A cue feature has now been added so that if a command is sent while - * a group is busy it is cued until the group becomes static. + * 2) A queue feature has now been added so that if a command is sent while + * a group is busy it is queued until the group becomes static. * 3) The driver waits for a time specified in drvXPSC8.h: XPSC8_QUE_PAUSE when * a command is issued. It then performs all the motions specified in the lapsed * time as a single syncronised motion. The pause is performed in drvXPSC8.cc: @@ -48,14 +48,14 @@ extern struct driver_table XPSC8_access; #define DEBUG #ifdef __GNUG__ - #ifdef DEBUG - volatile int devXPSC8Debug = 3; - #define Debug(L, FMT, V...) { if(L <= devXPSC8Debug) \ - { printf("%s(%d):",__FILE__,__LINE__); \ - printf(FMT,##V); } } - epicsExportAddress(int, devXPSC8Debug); + #ifdef DEBUG + volatile int devXPSC8Debug = 0; + #define Debug(L, FMT, V...) { if(L <= devXPSC8Debug) \ + { printf("%s(%d):",__FILE__,__LINE__); \ + printf(FMT,##V); } } + epicsExportAddress(int, devXPSC8Debug); #else - #define Debug(L, FMT, V...) + #define Debug(L, FMT, V...) #endif #else #define Debug() @@ -192,16 +192,16 @@ STATIC RTN_STATUS XPSC8_end_trans(struct motorRecord *mr) /* add a part to the transaction */ -STATIC RTN_STATUS XPSC8_build_trans(motor_cmnd command,\ - double *parms, \ - struct motorRecord *mr) +STATIC RTN_STATUS XPSC8_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 XPSC8controller *control; struct XPSC8axis *cntrl; - struct XPSC8group *groupcntrl; /*XPS group specific data */ + struct XPSC8group *groupcntrl; /*XPS group specific data */ double dval=0.0,resolution,steps; int ival=0; RTN_STATUS rtnval=OK; @@ -234,13 +234,13 @@ STATIC RTN_STATUS XPSC8_build_trans(motor_cmnd command,\ groupnumber = cntrl->groupnumber; groupcntrl = (struct XPSC8group *)&control->group[groupnumber]; - groupsize = groupcntrl->groupsize; /* Number of motors in group */ + groupsize = groupcntrl->groupsize; /* Number of motors in group */ - cntrl->resolution = mr->mres; /* Read in the motor resolution */ + cntrl->resolution = mr->mres; /* Read in the motor resolution */ resolution = cntrl->resolution; - steps = resolution * dval; /* This could be a position or velocity */ + steps = resolution * dval; /* This could be a position or velocity */ -/* mr->dllm = cntrl->minlimit;*/ /* set the epics limits to the XPS limits */ +/* mr->dllm = cntrl->minlimit;*/ /* set the epics limits to the XPS limits */ /* mr->dhlm = cntrl->maxlimit; */ Debug(10, "XPSC8_build_trans: card=%d, signal=%d, command=%d, ival=%d"\ @@ -268,57 +268,56 @@ STATIC RTN_STATUS XPSC8_build_trans(motor_cmnd command,\ &cntrl->groupstatus); if (status != 0) printf("BuildTrans Error performing GroupStatusGet status=%d\n", status); - - groupstatus = cntrl->groupstatus; /* Update groupstatus */ - + + groupstatus = cntrl->groupstatus; /* Update groupstatus */ + switch (command) { case MOVE_ABS:/* command 0*/ - - - if ((groupstatus < 10) || (groupstatus == 47)) { + if ((groupstatus < 10) || (groupstatus == 47)) { /* ie not initialized state or Jogging!*/ - break;} - - /* If there is no cue, update the cue array to make sure you don't move */ - /* the wrong motors */ - if ((groupcntrl->cuesize == 0) && (groupstatus > 9 && groupstatus < 20)){ - status = GroupPositionCurrentGet(cntrl->devpollsocket, - cntrl->groupname, - groupsize, - groupcntrl->positionarray); /* Array! */ + break; + } - if (status != 0) { - printf(" Error performing GroupPositionCurrentGet\n"); - } - } + /* If there is no queue, update the queue array to make sure you don't move */ + /* the wrong motors */ + if ((groupcntrl->queuesize == 0) && (groupstatus > 9 && groupstatus < 20)){ + status = GroupPositionCurrentGet(cntrl->devpollsocket, + cntrl->groupname, + groupsize, + groupcntrl->positionarray); /* Array! */ - - /* Always add the move to the cue */ - groupcntrl->positionarray[axisingroup] = steps; - ++groupcntrl->cuesize; /* Add 1 to cue total */ - if (groupcntrl->cuesize == 1) - groupcntrl->cueflag = 1;/* If first call set flag */ - if (groupcntrl->cuesize > 1) - Debug(2,"******Adding move to an existing cue***\n"); - - /* The communication and looping has been moved to drvXPSC8 send_mess */ - + if (status != 0) { + printf(" Error performing GroupPositionCurrentGet\n"); + } + } + + /* Always add the move to the queue */ + groupcntrl->positionarray[axisingroup] = steps; + ++groupcntrl->queuesize; /* Add 1 to queue total */ + if (groupcntrl->queuesize == 1) + groupcntrl->queueflag = 1;/* If first call set flag */ + if (groupcntrl->queuesize > 1) + Debug(2,"******Adding move to an existing queue***\n"); + + /* The communication and looping has been moved to drvXPSC8 send_mess */ + break; case MOVE_REL:/*1*/ - /* The motor record seems to impliment the relative move by - calculating the new position and calling MOVE_ABS */ + /* The motor record seems to impliment the relative move by + calculating the new position and calling MOVE_ABS */ case HOME_FOR: case HOME_REV: /*3*/ - if (cntrl->groupstatus == 43 ) { /* Allready Homing */ - break;} - - /* If motion has been killed the group will need to be initialized*/ + if (cntrl->groupstatus == 43 ) { /* Already Homing */ + break; + } + + /* If motion has been killed the group will need to be initialized*/ /* and homed before the motors can be driven again */ - if (cntrl->groupstatus < 10 ) { + if (cntrl->groupstatus < 10 ) { /* ie not initialized state!*/ status = GroupInitialize(cntrl->devpollsocket,cntrl->groupname); if (status != 0) { @@ -332,9 +331,9 @@ STATIC RTN_STATUS XPSC8_build_trans(motor_cmnd command,\ goto home_rev_end; } - - /* Else kill all the motions an home */ - + + /* Else kill all the motions an home */ + status = GroupKill(cntrl->devpollsocket,cntrl->groupname); if (status != 0) { printf(" Error performing GroupKill\n"); @@ -350,21 +349,21 @@ STATIC RTN_STATUS XPSC8_build_trans(motor_cmnd command,\ printf(" Error performing GroupHomeSearch\n"); } Debug(2, "XPSC8_build_trans:******* Perform GroupHomeSearch\n"); - Debug(2, "XPSC8_build_trans:groupsize=%i\n",groupsize); + Debug(2, "XPSC8_build_trans:groupsize=%i\n",groupsize); - -home_rev_end: /* Used for goto statment above */ - /* When a group is homed the drive cue is emptied */ - - groupcntrl->cuesize = 0; /* Reset cue */ - groupcntrl->cueflag = 0; - +home_rev_end: /* Used for goto statment above */ + + /* When a group is homed the drive queue is emptied */ + + groupcntrl->queuesize = 0; /* Reset queue */ + groupcntrl->queueflag = 0; + break; case LOAD_POS:/* 4*/ /* command 4 Not used*/ - + break; case SET_VEL_BASE: @@ -381,8 +380,8 @@ home_rev_end: /* Used for goto statment above */ cntrl->maxjerktime); if (status != 0) { printf(" Error performing PositionerSGammaParameters Set Vel\n"); - printf(" steps=%f resoulution=%f DVAL=%f\n",steps,resolution,dval); - } + printf(" steps=%f resoulution=%f DVAL=%f\n",steps,resolution,dval); + } else cntrl->velocity = steps; break; @@ -394,20 +393,20 @@ home_rev_end: /* Used for goto statment above */ cntrl->maxjerktime); if (status != 0) { printf(" Error performing PositionerSGammaParameters Set Accel %i\n", - status); - printf(" steps=%f resoulution=%f DVAL=%f\n",steps,resolution,dval); - if (status == -17) - printf("devXPSC8 BuildTrans: One of the parameters was out of range!"); - } + status); + printf(" steps=%f resoulution=%f DVAL=%f\n",steps,resolution,dval); + if (status == -17) + printf("devXPSC8 BuildTrans: One of the parameters was out of range!"); + } else cntrl->accel = steps; break; case GO: /* 8 */ if (groupstatus == 20) { /* If disabled then enable */ - status = GroupMotionEnable(cntrl->devpollsocket,cntrl->groupname); - if (status != 0) + status = GroupMotionEnable(cntrl->devpollsocket,cntrl->groupname); + if (status != 0) printf(" Error performing GroupMotionEnable %i\n",status); - } + } break; case SET_ENC_RATIO: /* These must be set in the Stages.ini file */ @@ -418,28 +417,29 @@ home_rev_end: /* Used for goto statment above */ case STOP_AXIS: /* 11 */ /* The whole group must stop, not just 1 axis */ - /* Update status to see if the group is moving */ - - if (cntrl->groupstatus > 42) { + /* Update status to see if the group is moving */ + + if (cntrl->groupstatus > 42) { /* Then the group is moving! */ status = GroupMoveAbort(cntrl->devpollsocket,cntrl->groupname); if (status != 0) { - printf(" Error performing GroupMoveAbort = %i %s(\n",\ - status,cntrl->groupname); - - } - /* When a group is stopped the drive cue is emptied and reset*/ - - status = GroupPositionCurrentGet(cntrl->devpollsocket, - cntrl->groupname, - groupsize, - groupcntrl->positionarray); - if (status != 0) { - printf(" Error performing GroupPositionCurrentGet\n"); - } - } - groupcntrl->cuesize = 0; /* Reset cue */ - groupcntrl->cueflag = 0; + printf(" Error performing GroupMoveAbort = %i %s(\n",\ + status,cntrl->groupname); + + } + + /* When a group is stopped the drive queue is emptied and reset*/ + + status = GroupPositionCurrentGet(cntrl->devpollsocket, + cntrl->groupname, + groupsize, + groupcntrl->positionarray); + if (status != 0) { + printf(" Error performing GroupPositionCurrentGet\n"); + } + } + groupcntrl->queuesize = 0; /* Reset queue */ + groupcntrl->queueflag = 0; break; case JOG: diff --git a/motorApp/NewportSrc/drvXPSC8.cc b/motorApp/NewportSrc/drvXPSC8.cc index 01e38fab..1549e01d 100755 --- a/motorApp/NewportSrc/drvXPSC8.cc +++ b/motorApp/NewportSrc/drvXPSC8.cc @@ -37,22 +37,21 @@ #define DEBUG #ifdef __GNUG__ - #ifdef DEBUG - #define Debug(l, f, args...) { if(l<=drvXPSC8Debug) printf(f,## args); } + #ifdef DEBUG + #define Debug(l, f, args...) { if(l<=drvXPSC8Debug) printf(f,## args); } #else - #define Debug(l, f, args...) + #define Debug(l, f, args...) #endif #else #define Debug() #endif -volatile int drvXPSC8Debug = 6; +volatile int drvXPSC8Debug = 0; /* --- Local data. --- */ int XPSC8_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, const char *, char *); @@ -109,6 +108,7 @@ extern "C" {epicsExportAddress(drvet, drvXPSC8);} /* I don't know where the XPSC8_access is set?*/ STATIC struct thread_args targs = {SCAN_RATE, &XPSC8_access, 0.0}; + /********************************************************* * Print out driver status report *********************************************************/ @@ -142,11 +142,13 @@ static long init() if (XPSC8_num_cards <= 0) { Debug(10, "init() XPSC8etup() missing from startup script.%i\n", - XPSC8_num_cards); + XPSC8_num_cards); } return ((long) 0); } + + STATIC void query_done(int card, int axis, struct mess_node *nodeptr) { } @@ -171,6 +173,7 @@ STATIC void start_status(int card) } } + /***************************************************************** * Read in the motion parameters such as velocity, Accel, Position etc *****************************************************************/ @@ -187,11 +190,11 @@ STATIC void readXPSC8Status(int card) register struct mess_info *motor_info; int groupsize, groupnumber; /* To be read in from struct */ - struct XPSC8group *groupcntrl; /*XPS group specific data */ + struct XPSC8group *groupcntrl; /*XPS group specific data */ double position[XPSC8_NUM_CHANNELS]; /* To temp store position array */ int axisingroup; - int loop; /* Variables used when the XPS returns accel=zero */ - int max_loop = 10; /* This creates an error when we set the vel/accel */ + int loop; /* Variables used when the XPS returns accel=zero */ + int max_loop = 10; /* This creates an error when we set the vel/accel */ msta_field statusflags; status = 0; @@ -201,7 +204,7 @@ STATIC void readXPSC8Status(int card) Debug(10, "XPSC8:readXPSC8Status card=%d num_axes=%d\n",card,XPSC8_num_axes); epicsMutexLock(control->XPSC8Lock); - /*epicsThreadSleep(10);*/ + /*epicsThreadSleep(10);*/ for (i=0; iDevicePrivate; cntrl = (struct XPSC8axis *)&control->axis[i]; - axisingroup = cntrl->axisingroup; - if ((axisingroup < 0) || (axisingroup > 7) ) { + axisingroup = cntrl->axisingroup; + if ((axisingroup < 0) || (axisingroup > 7) ) { printf(" Error Axis In Group Out Of Range status=%d\n", status); statuserror =1; } - groupnumber = cntrl->groupnumber; - if ((groupnumber < 0) || (groupnumber > 7) ) { + groupnumber = cntrl->groupnumber; + if ((groupnumber < 0) || (groupnumber > 7) ) { printf(" Error Group Number Out Of Range status=%d\n", status); statuserror =1; } - groupcntrl = (struct XPSC8group *)&control->group[groupnumber]; + groupcntrl = (struct XPSC8group *)&control->group[groupnumber]; - groupsize = groupcntrl->groupsize; - if ((groupsize < 1) || (groupsize > 8) ) { + groupsize = groupcntrl->groupsize; + if ((groupsize < 1) || (groupsize > 8) ) { printf(" Error Group Size Out Of Range status=%d\n", status); statuserror =1; } @@ -246,10 +249,10 @@ STATIC void readXPSC8Status(int card) printf(" ReadStatus Error performing GroupStatusGet status=%d\n", status); statuserror =1; } - - /* This do loop is required because some times the XPS returns accel=zero*/ - loop = 0; - do { + + /* This do loop is required because some times the XPS returns accel=zero*/ + loop = 0; + do { status = PositionerSGammaParametersGet(cntrl->pollsocket, cntrl->positionername, &cntrl->velocity, @@ -257,19 +260,19 @@ STATIC void readXPSC8Status(int card) &cntrl->minjerktime, &cntrl->maxjerktime); loop++; - } - while ((cntrl->accel < 0.00001) && (loop < max_loop)); - - if (status != 0) { + } + while ((cntrl->accel < 0.00001) && (loop < max_loop)); + + if (status != 0) { printf(" Error performing PositionerSGammaParametersGet\n"); statuserror =1; } - /* Is the XPS sending the wrong accel and vel data? */ - if (cntrl->accel <= 0.00001 ) - printf("drvXPSC8 Error PositionerSGammaParametersGet accel=%f vel=%f loop=%i\n", - cntrl->accel,cntrl->velocity,loop); + /* Is the XPS sending the wrong accel and vel data? */ + if (cntrl->accel <= 0.00001 ) + printf("drvXPSC8 Error PositionerSGammaParametersGet accel=%f vel=%f loop=%i\n", + cntrl->accel,cntrl->velocity,loop); -/* Not implimented due to lack of epics motor commands +/* Not implimented due to lack of epics motor commands status = GroupJogParametersGet(cntrl->pollsocket, cntrl->groupname, positioner, @@ -289,8 +292,8 @@ STATIC void readXPSC8Status(int card) printf(" Error performing GroupPositionCurrentGet\n"); statuserror =1; } else { - /* Place the position data into the axis structure */ - cntrl->currentposition = position[axisingroup];} + /* Place the position data into the axis structure */ + cntrl->currentposition = position[axisingroup];} status = GroupPositionTargetGet(cntrl->pollsocket, @@ -301,8 +304,8 @@ STATIC void readXPSC8Status(int card) printf(" Error performing GroupPositionTargetGet\n"); statuserror =1; } else { - /* Place the position data into the axis structure */ - cntrl->targetposition = position[axisingroup];} + /* Place the position data into the axis structure */ + cntrl->targetposition = position[axisingroup];} status = GroupPositionSetpointGet(cntrl->pollsocket, @@ -313,8 +316,8 @@ STATIC void readXPSC8Status(int card) printf(" Error performing GroupPositionSetpointGet\n"); statuserror =1; } else { - /* Place the position data into the axis structure */ - cntrl->setpointposition = position[axisingroup];} + /* Place the position data into the axis structure */ + cntrl->setpointposition = position[axisingroup];} status = PositionerErrorGet(cntrl->pollsocket, cntrl->positionername, @@ -349,6 +352,7 @@ STATIC void readXPSC8Status(int card) epicsMutexUnlock(control->XPSC8Lock); } + /************************************************************** * Parse status and position for a card and signal * set_status() @@ -409,13 +413,13 @@ STATIC int set_status(int card, int signal) /* These states mean it is moving/homeing/jogging etc*/ cntrl->moving = 1; - if (target >= pos) { - status.Bits.RA_DIRECTION = 1; - Debug(2, "Set_status: Positive Direction\n"); - } else { - status.Bits.RA_DIRECTION = 0; - Debug(2, "Set_status: Negative Direction\n"); - } + if (target >= pos) { + status.Bits.RA_DIRECTION = 1; + Debug(2, "Set_status: Positive Direction\n"); + } else { + status.Bits.RA_DIRECTION = 0; + Debug(2, "Set_status: Negative Direction\n"); + } } @@ -464,9 +468,9 @@ STATIC int set_status(int card, int signal) if ((groupstatus >= 0 && groupstatus < 10) || (groupstatus >= 20 && groupstatus < 43)) { /* Set the Hard limits To show that it is unable to move. This means when you - home the motor record will only let you home away from the current shown - limit */ - + home the motor record will only let you home away from the current shown + limit */ + status.Bits.RA_MINUS_LS=1; status.Bits.RA_PLUS_LS=1; status.Bits.RA_PROBLEM=1; /*This variable is to do with polling*/ @@ -491,12 +495,12 @@ STATIC int set_status(int card, int signal) motor_info->status.All = status.All; Debug(12, "status.Bits.RA_PROBLEM: %i status.Bits.RA_DONE: %i", - status.Bits.RA_PROBLEM,status.Bits.RA_DONE); + status.Bits.RA_PROBLEM,status.Bits.RA_DONE); return(rtn_state); } - + /*****************************************************/ /* send a message to the XPS board */ /* send_mess() */ @@ -505,7 +509,7 @@ STATIC RTN_STATUS send_mess(int card, char const *com, char *c) { struct XPSC8controller *control; struct XPSC8axis *cntrl; - struct XPSC8group *groupcntrl; /*XPS group specific data */ + struct XPSC8group *groupcntrl; /*XPS group specific data */ int groupnumber, groupsize; int axisingroup, groupstatus; int status; @@ -515,14 +519,14 @@ STATIC RTN_STATUS send_mess(int card, char const *com, char *c) if (motor_state[0] == NULL) return(ERROR); - + int signal = -1; /* This is incremented up to 0 further down */ control = (struct XPSC8controller *) motor_state[card]->DevicePrivate; epicsMutexLock(control->XPSC8Lock); /*control = (struct XPSC8controller *) brdptr->DevicePrivate;*/ - /* Loop until you find the axis/group with a move cued up! */ + /* Loop until you find the axis/group with a move queued up! */ do { ++signal; @@ -531,109 +535,107 @@ STATIC RTN_STATUS send_mess(int card, char const *com, char *c) axisingroup = cntrl->axisingroup; /* Pull in group info */ groupnumber = cntrl->groupnumber; groupcntrl = (struct XPSC8group *)&control->group[groupnumber]; - groupsize = groupcntrl->groupsize; /* Number of motors in group */ + groupsize = groupcntrl->groupsize; /* Number of motors in group */ } - while ((groupcntrl->cuesize == 0) && (signal < (XPSC8_NUM_CHANNELS-1))); + while ((groupcntrl->queuesize == 0) && (signal < (XPSC8_NUM_CHANNELS-1))); - Debug(5,"Send_mess After do loop axisingp=%d, groupstatus=%i socket=%i cue=%i flag=%i \n", - axisingroup,groupstatus,cntrl->socket,groupcntrl->cuesize,groupcntrl->cueflag); + Debug(5,"Send_mess After do loop axisingp=%d, groupstatus=%i socket=%i queue=%i flag=%i \n", + axisingroup,groupstatus,cntrl->socket,groupcntrl->queuesize,groupcntrl->queueflag); - if((signal == (XPSC8_NUM_CHANNELS-1)) && (groupcntrl->cuesize == 0)) - goto send_mess_end; /*No moves to perform */ + if((signal == (XPSC8_NUM_CHANNELS-1)) && (groupcntrl->queuesize == 0)) + goto send_mess_end; /*No moves to perform */ status = GroupStatusGet(cntrl->pollsocket, cntrl->groupname, &cntrl->groupstatus); /* Update Status */ groupstatus = cntrl->groupstatus; - + if (status != 0) printf(" SendMess Error performing GroupStatusGet status=%d\n", status); - if (groupstatus > 9 && groupstatus < 20){ /* Ready from move */ + if (groupstatus > 9 && groupstatus < 20){ /* Ready from move */ - if (groupcntrl->cueflag == 1) /* First motor called */ - { - - epicsMutexUnlock(control->XPSC8Lock);/* Free up for the other motors*/ - epicsThreadSleep(XPSC8_QUE_PAUSE_READY); /* Wait for other motors */ - epicsMutexLock(control->XPSC8Lock); - groupcntrl->cuesize = 0; /* Reset cue */ - groupcntrl->cueflag = 0; + if (groupcntrl->queueflag == 1) { /* First motor called */ + epicsMutexUnlock(control->XPSC8Lock);/* Free up for the other motors*/ + epicsThreadSleep(XPSC8_QUE_PAUSE_READY); /* Wait for other motors */ + epicsMutexLock(control->XPSC8Lock); + groupcntrl->queuesize = 0; /* Reset queue */ + groupcntrl->queueflag = 0; - status = GroupMoveAbsolute(cntrl->socket, - cntrl->groupname, - groupsize, - groupcntrl->positionarray); /*Pointer to array*/ - - if (status != 0 && status != -27) - printf(" Error performing GroupMoveAbsolute %i\n",status); - /* Error -27 is caused when the motor record changes dir i.e. - when it aborts a move!*/ + status = GroupMoveAbsolute(cntrl->socket, + cntrl->groupname, + groupsize, + groupcntrl->positionarray); /*Pointer to array*/ - Debug(5,"Send_mess After move in Send_mess axisingp=%d, groupstatus=%i socket=%i psocket=%i\n", - cntrl->axisingroup,groupstatus,cntrl->socket,cntrl->pollsocket); - goto send_mess_end; - } + if (status != 0 && status != -27) + printf(" Error performing GroupMoveAbsolute %i\n",status); + /* Error -27 is caused when the motor record changes dir i.e. + * when it aborts a move!*/ + + Debug(5,"Send_mess After move in Send_mess axisingp=%d, groupstatus=%i socket=%i psocket=%i\n", + cntrl->axisingroup,groupstatus,cntrl->socket,cntrl->pollsocket); + goto send_mess_end; + } } - if (groupstatus > 42 && groupstatus < 47){ /* Moveing/Homing*/ - if (groupcntrl->cuesize > 1) - Debug(1,"Send_mess**Added move to the cue group busy ie xps moving***\n"); - if (groupcntrl->cueflag == 1){ /* First motor called */ - groupcntrl->cueflag = 0; - waitcount = 0; - while((groupstatus > 42 && groupstatus < 49) && - (waitcount <= XPSC8_MAX_WAIT)){ - epicsMutexUnlock(control->XPSC8Lock);/* Free up for the others*/ - epicsThreadSleep(XPSC8_QUE_PAUSE_MOVING); /* Wait for end of motion */ - epicsMutexLock(control->XPSC8Lock); - ++waitcount; - status = GroupStatusGet(cntrl->pollsocket, cntrl->groupname, - &cntrl->groupstatus); - if (status != 0) - printf("Error performing GroupStatusGet Bottom Send_mess status=%d\n", status); - - groupstatus = cntrl->groupstatus; /* Update groupstatus */ + if (groupstatus > 42 && groupstatus < 47){ /* Moveing/Homing*/ + if (groupcntrl->queuesize > 1) + Debug(1,"Send_mess**Added move to the queue group busy ie xps moving***\n"); + if (groupcntrl->queueflag == 1) { /* First motor called */ + groupcntrl->queueflag = 0; + waitcount = 0; + while((groupstatus > 42 && groupstatus < 49) && + (waitcount <= XPSC8_MAX_WAIT)){ + epicsMutexUnlock(control->XPSC8Lock);/* Free up for the others*/ + epicsThreadSleep(XPSC8_QUE_PAUSE_MOVING); /* Wait for end of motion */ + epicsMutexLock(control->XPSC8Lock); + ++waitcount; + status = GroupStatusGet(cntrl->pollsocket, cntrl->groupname, + &cntrl->groupstatus); + if (status != 0) + printf("Error performing GroupStatusGet Bottom Send_mess status=%d\n", status); - } - - if (waitcount >= XPSC8_MAX_WAIT){ - printf(" Error Motor Cue timed out \n"); - groupcntrl->cuesize = 0; /* Reset cue */ - groupcntrl->cueflag = 0; - /* Re set the cue array to current positions */ - status = GroupPositionCurrentGet(cntrl->pollsocket, - cntrl->groupname, - groupsize, - groupcntrl->positionarray); - if (status != 0) - printf(" Error performing GroupPositionCurrentGet\n"); - goto send_mess_end; - } - - if (groupstatus > 9 && groupstatus < 20){ /* Ready from move */ - groupcntrl->cuesize = 0; /* Reset cue */ - groupcntrl->cueflag = 0; - status = GroupMoveAbsolute(cntrl->socket, - cntrl->groupname, - groupsize, - groupcntrl->positionarray); /*Pointer to array*/ - - /* Error -27 is caused when the motor record changes dir i.e. - when it aborts a move!*/ - if (status != 0 && status != -27 ) - printf(" Error performing GroupMoveAbsolute %i\n",status); - - - } - } - } + groupstatus = cntrl->groupstatus; /* Update groupstatus */ + + } + + if (waitcount >= XPSC8_MAX_WAIT) { + printf(" Error Motor Queue timed out \n"); + groupcntrl->queuesize = 0; /* Reset queue */ + groupcntrl->queueflag = 0; + /* Reset the queue array to current positions */ + status = GroupPositionCurrentGet(cntrl->pollsocket, + cntrl->groupname, + groupsize, + groupcntrl->positionarray); + if (status != 0) + printf(" Error performing GroupPositionCurrentGet\n"); + goto send_mess_end; + } + + if (groupstatus > 9 && groupstatus < 20) { /* Ready from move */ + groupcntrl->queuesize = 0; /* Reset queue */ + groupcntrl->queueflag = 0; + status = GroupMoveAbsolute(cntrl->socket, + cntrl->groupname, + groupsize, + groupcntrl->positionarray); /*Pointer to array*/ + + /* Error -27 is caused when the motor record changes dir i.e. + * when it aborts a move!*/ + if (status != 0 && status != -27 ) + printf(" Error performing GroupMoveAbsolute %i\n",status); + + } + } + } send_mess_end: - epicsMutexUnlock(control->XPSC8Lock); + epicsMutexUnlock(control->XPSC8Lock); return (OK); } + /*****************************************************/ /* receive a message from the XPS board */ /* recv_mess() */ @@ -647,7 +649,7 @@ STATIC int recv_mess(int card, char *com, int flag) } - + /*****************************************************/ /* Setup system configuration */ /* XPSC8Setup() */ @@ -682,6 +684,7 @@ RTN_STATUS XPSC8Setup(int num_cards, /* number of controllers in system. */ } + /*****************************************************/ /* Configure a Newport XPS controller one command */ /* For each controller */ @@ -689,8 +692,8 @@ RTN_STATUS XPSC8Setup(int num_cards, /* number of controllers in system. */ /*****************************************************/ RTN_STATUS XPSC8Config(int card, /* Controller number */ const char *ip, /* XPS IP address*/ - int port, /* This may be 5001 */ - int totalaxes) /* Number of axis/positioners used*/ + int port, /* This may be 5001 */ + int totalaxes) /* Number of axis/positioners used*/ { struct XPSC8controller *control; @@ -730,8 +733,8 @@ RTN_STATUS XPSC8Config(int card, /* Controller number */ printf(" Error TCP_ConnectToServer for pollsocket\n"); statuserror =1; } - - + + for (axis=0; axispollsocket = pollsocket; - cntrl->devpollsocket = devpollsocket; + cntrl->devpollsocket = devpollsocket; cntrl->socket = socket; cntrl->ip = epicsStrDup(ip); @@ -764,23 +767,24 @@ RTN_STATUS XPSC8Config(int card, /* Controller number */ return (OK); } + /*********************************************************/ /* Pass the group and positioner names */ /* Call this for each axis */ /* */ /*********************************************************/ -RTN_STATUS XPSC8NameConfig(int card, /*specify which controller 0-up*/ - int axis, /*axis number 0-7*/ - int groupnumber, /* 0-7*/ - int groupsize, /* 1-8*/ - int axisingroup, /* eg 4th axis in group 2 */ - const char *gpname, /*group name e.g. Diffractometer */ - const char *posname) /*positioner name e.g. Diffractometer.Phi*/ +RTN_STATUS XPSC8NameConfig(int card, /* specify which controller 0-up*/ + int axis, /* axis number 0-7*/ + int groupnumber, /* 0-7*/ + int groupsize, /* 1-8*/ + int axisingroup, /* eg 4th axis in group 2 */ + const char *gpname, /* group name e.g. Diffractometer */ + const char *posname) /* positioner name e.g. Diffractometer.Phi*/ { struct XPSC8controller *control; struct XPSC8axis *cntrl; - struct XPSC8group *groupcntrl; /*XPS group specific data */ + struct XPSC8group *groupcntrl; /*XPS group specific data */ Debug(10, "XPSC8NameConfig: card=%d axis=%d, group=%s, positioner=%s\n", card, axis, gpname, posname); @@ -798,6 +802,7 @@ RTN_STATUS XPSC8NameConfig(int card, /*specify which controller 0-up*/ return (OK); } + /*****************************************************/ /* initialize all software and hardware */ /* This is called from the init() */ @@ -809,10 +814,10 @@ STATIC int motor_init() struct controller *brdptr; struct XPSC8controller *control; struct XPSC8axis *cntrl; - struct XPSC8group *groupcntrl; /*XPS group specific data */ + struct XPSC8group *groupcntrl; /*XPS group specific data */ int card_index, motor_index; int status = 0, totalaxes; - int i,counter; /* Used in for loops */ + int i,counter; /* Used in for loops */ bool errind; initialized = true; /* Indicate that driver is initialized. */ @@ -840,28 +845,28 @@ STATIC int motor_init() /* Just to test status, 0 means the call worked */ counter = 0; - /* wait incase the socket is busy */ - while (status < 0 && counter < (TIMEOUT*10)) { - status = GroupStatusGet(cntrl->pollsocket,cntrl->groupname, - &cntrl->groupstatus); - counter++; - epicsThreadSleep(0.1); - } - + /* wait incase the socket is busy */ + while (status < 0 && counter < (TIMEOUT*10)) { + status = GroupStatusGet(cntrl->pollsocket,cntrl->groupname, + &cntrl->groupstatus); + counter++; + epicsThreadSleep(0.1); + } + if (status !=0) errind = true; Debug(10, "XPSC8:motor_init: card_index=%d, errind=%d\n", card_index, errind); - Debug(10, "XPSC8:motor_init: psocket %i, gpname %s, gpstatus %i status %i\n", - cntrl->pollsocket,cntrl->groupname,cntrl->groupstatus,status); - - /* Set the XPSC8group struct variables to zero */ - - for(i=0 ; i< XPSC8_NUM_CHANNELS ; ++i){ - groupcntrl = (struct XPSC8group *)&control->group[i]; - groupcntrl->cuesize = 0; - groupcntrl->cueflag = 0; - } - + Debug(10, "XPSC8:motor_init: psocket %i, gpname %s, gpstatus %i status %i\n", + cntrl->pollsocket,cntrl->groupname,cntrl->groupstatus,status); + + /* Set the XPSC8group struct variables to zero */ + + for (i=0 ; i< XPSC8_NUM_CHANNELS ; ++i) { + groupcntrl = (struct XPSC8group *)&control->group[i]; + groupcntrl->queuesize = 0; + groupcntrl->queueflag = 0; + } + if (errind == false) { brdptr->localaddr = (char *) NULL; @@ -901,8 +906,9 @@ STATIC int motor_init() free_list.tail = (struct mess_node *) NULL; epicsThreadCreate((char *) "XPSC8_motor", epicsThreadPriorityMedium, - epicsThreadGetStackSize(epicsThreadStackMedium), + epicsThreadGetStackSize(epicsThreadStackMedium), (EPICSTHREADFUNC) motor_task, (void *) &targs); return (OK); } + diff --git a/motorApp/NewportSrc/drvXPSC8.h b/motorApp/NewportSrc/drvXPSC8.h index 76b11112..48fa7b43 100755 --- a/motorApp/NewportSrc/drvXPSC8.h +++ b/motorApp/NewportSrc/drvXPSC8.h @@ -22,7 +22,7 @@ performing a move from stationary*/ #define XPSC8_QUE_PAUSE_MOVING 0.1 /* Poll time the driver waits to check if the motor is still moving after a - cue has been started*/ + queue has been started*/ #define TIMEOUT 0.5 /* TCP/IP timeout */ #define NOTREF 42 @@ -59,10 +59,10 @@ struct XPSC8axis }; struct XPSC8group{ /* This structure is for each group not axis */ - double positionarray[XPSC8_NUM_CHANNELS]; /* Used to store cued moves */ + double positionarray[XPSC8_NUM_CHANNELS]; /* Used to store queued moves */ int groupsize; /* (1-8) Number of members of a group */ - int cuesize; /* Number of axes in the cue */ - int cueflag; /* 1 = move/wait loop needs to be started */ + int queuesize; /* Number of axes in the queue */ + int queueflag; /* 1 = move/wait loop needs to be started */ }; struct XPSC8controller{