pmacAsynCoord.c

Go to the documentation of this file.
00001 
00058 #include <stddef.h>
00059 #include <stdlib.h>
00060 #include <stdarg.h>
00061 #include <math.h>
00062 #include <stdio.h>
00063 #include <string.h>
00064 #include <errno.h>
00065 
00066 #include "paramLib.h"
00067 
00068 #include "epicsFindSymbol.h"
00069 #include "epicsTime.h"
00070 #include "epicsThread.h"
00071 #include "epicsEvent.h"
00072 #include "epicsMutex.h"
00073 #include "ellLib.h"
00074 
00075 #include "drvSup.h"
00076 #include "epicsExport.h"
00077 #define DEFINE_MOTOR_PROTOTYPES 1
00078 #include "motor_interface.h"
00079 #include "asynDriver.h"
00080 #include "asynOctetSyncIO.h"
00081 #include "pmacAsynCoord.h"
00082 
00083 motorAxisDrvSET_t pmacAsynCoord = {
00084     15,
00085     motorAxisReport,            
00086     motorAxisInit,              
00087     motorAxisSetLog,            
00088     motorAxisOpen,              
00089     motorAxisClose,             
00090     motorAxisSetCallback,       
00091     motorAxisSetDouble,         
00092     motorAxisSetInteger,        
00093     motorAxisGetDouble,         
00094     motorAxisGetInteger,        
00095     motorAxisHome,              
00096     motorAxisMove,              
00097     motorAxisVelocityMove,      
00098     motorAxisStop,              
00099    motorAxisforceCallback,     
00100 };
00101 
00102 epicsExportAddress(drvet, pmacAsynCoord);
00103 
00104 typedef struct drvPmac * PMACDRV_ID;
00105 typedef struct drvPmac
00106 {
00107     PMACDRV_ID pNext;
00108     asynUser * pasynUser;
00109     int ref;
00110     int cs;
00111     AXIS_HDL axis;
00112     epicsThreadId motorThread;
00113     epicsTimeStamp now;
00114     int movesDeferred;
00115     double movingPollPeriod;
00116     double idlePollPeriod;
00117     epicsEventId pollEventId;
00118     epicsMutexId controllerMutexId;    
00119 } drvPmac_t;
00120 
00121 /* Default polling periods (in milliseconds). */ 
00122 static const int defaultMovingPollPeriod = 100;
00123 static const int defaultIdlePollPeriod = 500;
00124 
00125 typedef struct motorAxisHandle
00126 {
00127     PMACDRV_ID pDrv;
00128     int coord_system;
00129     int axis;    /*> 1-based index into the coordinate system */
00130     int    program;
00131     asynUser * pasynUser;
00132     PARAMS params;
00133     motorAxisLogFunc print;
00134     void * logParam;
00135     epicsMutexId axisMutex;
00136     int deferred_move;
00137     double stepSize;
00138 } motorAxis;
00139 
00140 const char axisName[] = {' ', 'A','B','C','U','V','W','X','Y','Z'};
00141 
00142 static PMACDRV_ID pFirstDrv = NULL;
00143 
00144 static int drvPmacLogMsg( void * param, const motorAxisLogMask_t logMask, const char *pFormat, ...);
00145 static motorAxisLogFunc drvPrint = drvPmacLogMsg;
00146 static motorAxisLogFunc drvPrintParam = NULL;
00147 
00148 #define TRACE_FLOW    motorAxisTraceFlow
00149 #define TRACE_DRIVER  motorAxisTraceIODriver
00150 #define TRACE_ERROR   motorAxisTraceError
00151 
00152 #define MAX(a,b) ((a)>(b)? (a): (b))
00153 #define MIN(a,b) ((a)<(b)? (a): (b))
00154 
00155 /* PMAC specific parameters relating to coordinate systems */
00156 #define NAXES    9
00157 #define NAME(pAxis) (axisName[(pAxis->axis)])
00158 #define CSVAR(pAxis) (pAxis->coord_system + 50)
00159 /* Use Q71 - Q79 for motor demand positions */
00160 #define DEMAND "Q7%d"
00161 /* Use Q81 - Q89 for motor readback positions */
00162 #define READBACK "Q8%d"
00163 
00164 static void motorAxisReportAxis( AXIS_HDL pAxis, int level )
00165 {
00166     printf("axis %c:\n", NAME(pAxis));
00167     if (level > 0) printf( "drvPmac->axisMutex = %p\n", pAxis->axisMutex );
00168 
00169     if (level > 1) {
00170         motorParam->dump( pAxis->params );
00171     }
00172 }
00173 
00174 static void motorAxisReport( int level )
00175 {
00176     PMACDRV_ID pDrv;
00177 
00178     for (pDrv = pFirstDrv; pDrv != NULL; pDrv = pDrv->pNext) {
00179         int i;
00180         const char *name;
00181 
00182         pasynManager->getPortName(pDrv->pasynUser, &name);
00183         printf( "Found driver for PMAC C.S. %d via %s (ref %d)\n", pDrv->cs, name, pDrv->ref );
00184         for ( i = 0; i < NAXES; i++ ) {
00185             if (level > 0) {
00186                 motorAxisReportAxis( &(pDrv->axis[i]), level );
00187             }
00188         }
00189     }
00190 }
00191 
00192 
00193 static int motorAxisInit( void )
00194 {
00195     return MOTOR_AXIS_OK;
00196 }
00197 
00198 static int motorAxisSetLog( AXIS_HDL pAxis, motorAxisLogFunc logFunc, void * param )
00199 {
00200     if (pAxis == NULL) {
00201         if (logFunc == NULL) {
00202             drvPrint=drvPmacLogMsg;
00203             drvPrintParam = NULL;
00204         } else {
00205             drvPrint=logFunc;
00206             drvPrintParam = param;
00207         }
00208     } else {
00209         if (logFunc == NULL) {
00210             pAxis->print=drvPmacLogMsg;
00211             pAxis->logParam = NULL;
00212         } else {
00213             pAxis->print=logFunc;
00214             pAxis->logParam = param;
00215         }
00216     }
00217     return MOTOR_AXIS_OK;
00218 }
00219 
00220 static AXIS_HDL motorAxisOpen( int ref, int axis, char * param )
00221 {
00222     PMACDRV_ID pDrv;
00223     AXIS_HDL pAxis = NULL;
00224 
00225     for ( pDrv=pFirstDrv; pDrv != NULL && (ref != pDrv->ref); pDrv = pDrv->pNext){}
00226 
00227     if (pDrv != NULL) {
00228         if (axis >= 0 && axis < NAXES) pAxis = &(pDrv->axis[axis]);
00229     }
00230         
00231     return pAxis;
00232 }
00233 
00234 static int motorAxisClose( AXIS_HDL pAxis )
00235 {
00236     return MOTOR_AXIS_OK;
00237 }
00238 
00239 static int motorAxisGetInteger( AXIS_HDL pAxis, motorAxisParam_t function, int * value )
00240 {
00241     if (pAxis == NULL) {
00242         return MOTOR_AXIS_ERROR;
00243     } else {
00244         switch (function) {
00245         case motorAxisDeferMoves:
00246             *value = pAxis->pDrv->movesDeferred;
00247             return MOTOR_AXIS_OK;
00248             break;
00249         default:
00250             return motorParam->getInteger( pAxis->params, (paramIndex) function, value );
00251         }
00252     }
00253 }
00254 
00255 static int motorAxisGetDouble( AXIS_HDL pAxis, motorAxisParam_t function, double * value )
00256 {
00257     if (pAxis == NULL) {
00258         return MOTOR_AXIS_ERROR;
00259     } else {
00260         switch(function) {
00261         case motorAxisDeferMoves:
00262             *value = (double)pAxis->pDrv->movesDeferred;
00263             return MOTOR_AXIS_OK;
00264             break;
00265         default:
00266             return motorParam->getDouble( pAxis->params, (paramIndex) function, value );
00267         }
00268     }
00269 }
00270 
00271 static int motorAxisSetCallback( AXIS_HDL pAxis, motorAxisCallbackFunc callback, void * param )
00272 {
00273     if (pAxis == NULL) {
00274         return MOTOR_AXIS_ERROR;
00275     } else {
00276         return motorParam->setCallback( pAxis->params, callback, param );
00277     }
00278 }
00279 
00280 static int motorAxisAsynConnect( const char * port, int addr, asynUser ** ppasynUser, char * inputEos, char * outputEos )
00281 {
00282     asynStatus status;
00283  
00284     status = pasynOctetSyncIO->connect( port, addr, ppasynUser, NULL);
00285     if (status) {
00286         drvPrint( drvPrintParam, TRACE_ERROR,
00287                   "drvPmacCreate: unable to connect to port %s\n", 
00288                   port);
00289         return MOTOR_AXIS_ERROR;
00290     }
00291 
00292     status = pasynOctetSyncIO->setInputEos(*ppasynUser, inputEos, strlen(inputEos) );
00293     if (status) {
00294         asynPrint(*ppasynUser, ASYN_TRACE_ERROR,
00295                   "drvPmacCreate: unable to set input EOS on %s: %s\n", 
00296                   port, (*ppasynUser)->errorMessage);
00297         pasynOctetSyncIO->disconnect(*ppasynUser);
00298         return MOTOR_AXIS_ERROR;
00299     }
00300 
00301     status = pasynOctetSyncIO->setOutputEos(*ppasynUser, outputEos, strlen(outputEos));
00302     if (status) {
00303         asynPrint(*ppasynUser, ASYN_TRACE_ERROR,
00304                   "drvPmacCreate: unable to set output EOS on %s: %s\n", 
00305                   port, (*ppasynUser)->errorMessage);
00306         pasynOctetSyncIO->disconnect(*ppasynUser);
00307         return MOTOR_AXIS_ERROR;
00308     }
00309 
00310     return MOTOR_AXIS_OK;
00311 }
00312 
00313 static int motorAxisWriteRead( AXIS_HDL pAxis, char * command, size_t reply_buff_size, char * response, int logGlobal )
00314 {
00315     asynStatus status;
00316     const double timeout=6.0;
00317     size_t nwrite, nread;
00318     int eomReason;
00319     asynUser * pasynUser = (logGlobal? pAxis->pDrv->pasynUser: pAxis->pasynUser);
00320 
00321     if ( !logGlobal ) {
00322         pAxis->print( pAxis->logParam, TRACE_DRIVER, "Sending to PMAC C.S. ref %d command : %s\n",pAxis->pDrv->ref, command );
00323     }
00324         
00325     status = pasynOctetSyncIO->writeRead( pasynUser,
00326                                           command, strlen(command),
00327                                           response, reply_buff_size,
00328                                           timeout,
00329                                           &nwrite, &nread, &eomReason );
00330 
00331     if ( !logGlobal && nread != 0 ) {
00332         pAxis->print( pAxis->logParam, TRACE_DRIVER, "Recvd from PMAC C.S. ref %d response: %s\n",pAxis->pDrv->ref, response );
00333     }
00334         
00335     if (status) {
00336         motorParam->setInteger( pAxis->params, motorAxisCommError, 1 );
00337         asynPrint( pasynUser,
00338                    ASYN_TRACE_ERROR,
00339                    "Read/write error to PMAC C.S. ref %d, axis %c command %s. Status=%d, Error=%s\n",
00340                    pAxis->pDrv->ref, NAME(pAxis), command,
00341                    status, pasynUser->errorMessage);
00342         return MOTOR_AXIS_ERROR;
00343     }
00344 
00345     motorParam->setInteger( pAxis->params, motorAxisCommError, 0 );
00346     return MOTOR_AXIS_OK;
00347 }
00348 
00349 static int processDeferredMoves(drvPmac_t *pDrv)
00350 {
00351     int i;
00352     int status = MOTOR_AXIS_ERROR;
00353     char command[128], response[32];
00354     
00355     /* Abort current move to make sure axes are enabled */
00356     sprintf(command, "&%dA", pDrv->cs);    
00357     status = motorAxisWriteRead(&pDrv->axis[0], command, sizeof(response), response, 0);    
00358     
00359     /* Now do the move */
00360     sprintf(command, "&%dAB%dR", pDrv->cs, pDrv->axis[0].program);
00361     status = motorAxisWriteRead(&pDrv->axis[0], command, sizeof(response), response, 0);
00362     
00363     /* Signal that we no longer need to do a deferred move */
00364     for (i = 0; i < NAXES; i++) {
00365         pDrv->axis[i].deferred_move = 0;
00366     }
00367 
00368     return status;
00369 }
00370 
00371 static int motorAxisSetDouble( AXIS_HDL pAxis, motorAxisParam_t function, double value )
00372 {
00373     int status = MOTOR_AXIS_OK;
00374 
00375     if (pAxis == NULL) {
00376         return MOTOR_AXIS_ERROR;
00377     } else {
00378         char command[64]="\0";
00379         char response[64];
00380 
00381         if (epicsMutexLock( pAxis->axisMutex ) == epicsMutexLockOK) {
00382             switch (function) {
00383             case motorAxisPosition: {
00384                 int position = (int) floor(value + 0.5);
00385                 /* Set the demand values. We don't need to do anything clever 
00386                  * here as a move command will set reasonable demand values for 
00387                  * all axis in CS */
00388                 sprintf(command, "&%d"DEMAND"=%f",pAxis->coord_system, pAxis->axis, position*(pAxis->stepSize));
00389                 pAxis->print( pAxis->logParam, TRACE_FLOW,
00390                               "Set ref %d, axis %c to position %f\n",
00391                               pAxis->pDrv->ref, NAME(pAxis), value );
00392                 break;
00393             }
00394             case motorAxisEncoderRatio: {
00395                 pAxis->print( pAxis->logParam, TRACE_FLOW,
00396                               "Cannot set PMAC ref %d, cs %d, axis %c encoder ratio (%f)\n",
00397                               pAxis->pDrv->ref, pAxis->coord_system, NAME(pAxis), value );
00398                 break;
00399             }
00400             case motorAxisLowLimit: {
00401                 pAxis->print( pAxis->logParam, TRACE_FLOW,
00402                               "Ignoring PMAC ref %d, cs %d, axis %c low limit (%f)\n",
00403                               pAxis->pDrv->ref, pAxis->coord_system, NAME(pAxis), value );
00404                 break;
00405             }
00406             case motorAxisHighLimit: {
00407                 pAxis->print( pAxis->logParam, TRACE_FLOW,
00408                               "Ignoring PMAC ref %d, cs %d, axis %c high limit (%f)\n",
00409                               pAxis->pDrv->ref, pAxis->coord_system, NAME(pAxis), value );
00410                 break;
00411             }
00412             case motorAxisPGain: {
00413                 pAxis->print( pAxis->logParam, TRACE_FLOW,
00414                               "Ignoring PMAC ref %d, cs %d, axis %c pgain (%f)\n",
00415                               pAxis->pDrv->ref, pAxis->coord_system, NAME(pAxis), value );
00416                 break;
00417             }
00418             case motorAxisIGain: {
00419                 pAxis->print( pAxis->logParam, TRACE_FLOW,
00420                               "Ignoring PMAC ref %d, cs %d, axis %c igain (%f)\n",
00421                               pAxis->pDrv->ref, pAxis->coord_system, NAME(pAxis), value );
00422                 break;
00423             }
00424             case motorAxisDGain: {
00425                 pAxis->print( pAxis->logParam, TRACE_FLOW,
00426                               "Ignoring PMAC ref %d, cs %d, axis %c dgain (%f)\n",
00427                               pAxis->pDrv->ref, pAxis->coord_system, NAME(pAxis), value );
00428                 break;
00429             }
00430             case motorAxisClosedLoop: {
00431                 pAxis->print( pAxis->logParam, TRACE_FLOW,
00432                               "Cannot set PMAC ref %d, axis %c closed loop (%s)\n",
00433                               pAxis->pDrv->ref, NAME(pAxis), (value!=0?"ON":"OFF") );
00434                 break;
00435             }
00436             case motorAxisDeferMoves: {
00437                 pAxis->print( pAxis->logParam, TRACE_FLOW,
00438                               "%sing Deferred Move flag on PMAC ref %d, cs %d\n",
00439                               value != 0.0?"Sett":"Clear", pAxis->pDrv->ref, pAxis->coord_system);
00440                 if (value == 0.0 && pAxis->pDrv->movesDeferred != 0) {
00441                     processDeferredMoves(pAxis->pDrv);
00442                 }
00443                 pAxis->pDrv->movesDeferred = (int)value;
00444                 break;
00445             }
00446             default:
00447                 status = MOTOR_AXIS_ERROR;
00448                 break;
00449             }
00450 
00451             if ( command[0] != 0 && status == MOTOR_AXIS_OK) {
00452                 status = motorAxisWriteRead( pAxis, command, sizeof(response), response, 0 );
00453             }
00454 
00455             if (status == MOTOR_AXIS_OK ) {
00456                 motorParam->setDouble( pAxis->params, function, value );
00457                 motorParam->callCallback( pAxis->params );
00458             }
00459             epicsMutexUnlock( pAxis->axisMutex );
00460         }
00461     }
00462   return status;
00463 }
00464 
00465 static int motorAxisSetInteger( AXIS_HDL pAxis, motorAxisParam_t function, int value )
00466 {
00467     int status = MOTOR_AXIS_OK;
00468 
00469     if (pAxis == NULL) {
00470         return MOTOR_AXIS_ERROR;
00471     } else {
00472         status = motorAxisSetDouble( pAxis, function, (double) value );
00473     }
00474     return status;
00475 }
00476 
00477 
00478 static int motorAxisMove( AXIS_HDL pAxis, double position, int relative, double min_velocity, double max_velocity, double acceleration )
00479 {
00480     int status = MOTOR_AXIS_ERROR;
00481     
00482     if (pAxis != NULL) {
00483         char acc_buff[32]="";
00484         char vel_buff[32]="";
00485         char buff[128];
00486         char command[256];
00487         char commandtemp[128];
00488         char response[256];
00489         double deviceUnits = 0.0;
00490 
00491         if (max_velocity != 0) {
00492             /* Isx89 = default feedrate in EGU/s */        
00493             sprintf(vel_buff, "I%d89=%f ", CSVAR(pAxis), max_velocity * pAxis->stepSize);
00494         }
00495         if (acceleration != 0) {
00496             if (max_velocity != 0) {
00497                 /* Isx87 = accel time in msec */
00498                 sprintf(acc_buff, "I%d87=%f ", CSVAR(pAxis),
00499                         (fabs(max_velocity/acceleration) * 1000.0));
00500             }
00501         }
00502 
00503         deviceUnits = position * pAxis->stepSize;
00504         sprintf( command, "&%d%s%s"DEMAND"=%.12f", pAxis->coord_system, vel_buff, acc_buff, pAxis->axis, deviceUnits );             
00505 
00506         if (pAxis->pDrv->movesDeferred) {
00507             pAxis->deferred_move = 1;
00508         }
00509         else if (pAxis->program != 0) {        
00510             /* Abort current move to make sure axes are enabled */
00511             sprintf(commandtemp, "&%dA", pAxis->coord_system);    
00512             status = motorAxisWriteRead(pAxis, commandtemp, sizeof(response), response, 0);            
00513             /* If the program specified is non-zero, add a command to run the program.
00514              * If program number is zero, then the move will have to be started by some
00515              * external process, which is a mechanism of allowing coordinated starts to
00516              * movement. */                    
00517             sprintf(buff, "B%dR", pAxis->program);
00518             strcat(command, buff);
00519         }
00520         if (epicsMutexLock( pAxis->axisMutex ) == epicsMutexLockOK) {
00521             status = motorAxisWriteRead( pAxis, command, sizeof(response), response, 0 );
00522             motorParam->setInteger( pAxis->params, motorAxisDone, 0 );
00523             motorParam->callCallback( pAxis->params );
00524             epicsMutexUnlock( pAxis->axisMutex );
00525         }
00526     /* Signal the poller task.*/
00527     epicsEventSignal(pAxis->pDrv->pollEventId);
00528     }
00529     return status;
00530 }
00531 
00532 static int motorAxisHome( AXIS_HDL pAxis, double min_velocity, double max_velocity, double acceleration, int forwards )
00533 {
00534     asynPrint(pAxis->pasynUser, ASYN_TRACE_ERROR, "motorAxisHome: not implemented for CS axes\n");
00535     return MOTOR_AXIS_ERROR;
00536 }
00537 
00538 
00539 static int motorAxisVelocityMove(  AXIS_HDL pAxis, double min_velocity, double velocity, double acceleration )
00540 {
00541     asynPrint(pAxis->pasynUser, ASYN_TRACE_ERROR, "motorAxisVelocityMove: not implemented for CS axes\n");
00542     return MOTOR_AXIS_ERROR;
00543 }
00544 
00545 
00546 static int motorAxisStop( AXIS_HDL pAxis, double acceleration )
00547 {
00548     int status = MOTOR_AXIS_ERROR;
00549 
00550     if (pAxis != NULL) {
00551         char acc_buff[32]="\0";
00552         char command[128];
00553         char response[32];
00554 
00555         sprintf( command, "&%d%sA "DEMAND"="READBACK, pAxis->coord_system, acc_buff,
00556                  pAxis->axis, pAxis->axis);
00557         pAxis->deferred_move = 0;
00558         
00559         status = motorAxisWriteRead( pAxis, command, sizeof(response), response, 0 );
00560     }
00561     return status;
00562 }
00563 
00564 static int motorAxisforceCallback(AXIS_HDL pAxis)
00565 {
00566     if (pAxis == NULL) return MOTOR_AXIS_ERROR;
00567 
00568     pAxis->print(pAxis->logParam, TRACE_FLOW, "motorAxisforceCallback: request ref %d, cs %d, axis %c status update\n",
00569                 pAxis->pDrv->ref, pAxis->coord_system, NAME(pAxis));
00570 
00571     motorParam->forceCallback(pAxis->params);
00572 
00573     return (MOTOR_AXIS_OK);
00574 }
00575 
00576 static int drvPmacGetCoordStatus(AXIS_HDL pAxis, asynUser *pasynUser,
00577                  epicsUInt32 *status)
00578 {
00579     char command[32];
00580     char response[128]="";
00581     int nvals, cmdStatus;
00582     int retval = 0;
00583 
00584     /* Read all the status for this co-ordinate system in one go */
00585     sprintf( command, "&%d??", pAxis->coord_system );
00586     cmdStatus = motorAxisWriteRead( pAxis, command, sizeof(response), response, 1 );
00587     nvals = sscanf( response, "%6x%6x%6x", &status[0], &status[1], &status[2]);
00588 
00589     if ( cmdStatus || nvals != 3) {
00590         asynPrint(pasynUser, ASYN_TRACE_ERROR,
00591                     "drvPmacAxisGetStatus: not all status values returned. Status: %d\nCommand :%s\nResponse:%s",
00592                     cmdStatus, command, response );
00593     } else {
00594         retval = 1;
00595     }
00596     return retval;
00597 }
00598 
00599 static void drvPmacGetAxesStatus( PMACDRV_ID pDrv, epicsUInt32 *status)
00600 {
00601   char command[128];
00602     char commandtemp[128];
00603     char pos_response[128];
00604     char *pos_parse = pos_response;
00605     int cmdStatus, done;
00606     double position, oldposition, error;
00607     int i;
00608     asynUser *pasynUser = pDrv->pasynUser;
00609     AXIS_HDL first_axis, last_axis, pAxis;
00610     int direction;
00611     int homeSignal;
00612 
00613     /* First lock all the axes */
00614     for (i = 0; i < NAXES; i++) {
00615         pAxis = &pDrv->axis[i];
00616         epicsMutexLock( pAxis->axisMutex );
00617     }
00618     
00619     /* As yet, there is no way to get the following error of a C.S. axis - set it to zero for now */
00620     error = 0;
00621     first_axis = &pDrv->axis[0];
00622     last_axis = &pDrv->axis[NAXES-1];
00623     
00624     /* Read all the positions for this co-ordinate system in one go */
00625     sprintf( command, "&%d", first_axis->coord_system);
00626     for (i = first_axis->axis; i <= last_axis->axis; i++) {
00627       sprintf( commandtemp, READBACK, i);
00628       strcat(command, commandtemp);
00629     }
00630 
00631     cmdStatus = motorAxisWriteRead( first_axis, command, sizeof(pos_response), pos_response, 1 );
00632 
00633     /* Get the co-ordinate system status */
00634     drvPmacGetCoordStatus(first_axis, pDrv->pasynUser, status);
00635 
00636     for (i = 0; i < NAXES; i++) {
00637         pAxis = &pDrv->axis[i];
00638 /*            int homeSignal = ((status[1] & CS_STATUS2_HOME_COMPLETE) != 0);*/
00639 /* TODO: possibly look at the aggregate of the home status of all motors in the c.s ?? */
00640         homeSignal = 0;
00641         errno = 0;
00642         position = strtod(pos_parse, &pos_parse);
00643         if (position == 0 && errno != 0) {
00644             asynPrint(pasynUser, ASYN_TRACE_ERROR,
00645                         "drvPmacAxisGetStatus: not all status values returned. Status: %d\nCommand :%s\nResponse1:%s\n",
00646                         cmdStatus, command, pos_response );
00647         }
00648 
00649         position = position / pAxis->stepSize;
00650         motorParam->getDouble(  pAxis->params, motorAxisPosition,      &oldposition ); 
00651         oldposition = oldposition * pAxis->stepSize;
00652         motorParam->setDouble(  pAxis->params, motorAxisPosition,      (position+error) );
00653         motorParam->setDouble(  pAxis->params, motorAxisEncoderPosn,   position );
00654         /* Don't set direction if velocity equals zero and was previously negative */
00655         motorParam->getInteger( pAxis->params, motorAxisDirection,     &direction );
00656         motorParam->setInteger( pAxis->params, motorAxisDirection,     ((position > oldposition) || (position == oldposition && direction)) );
00657         motorParam->setInteger( pAxis->params, motorAxisHighHardLimit, ((status[2] & CS_STATUS3_LIMIT) != 0) );
00658         motorParam->setInteger( pAxis->params, motorAxisHomeSignal,    homeSignal );
00659         motorParam->setInteger( pAxis->params, motorAxisMoving,        ((status[1] & CS_STATUS2_IN_POSITION) == 0) );
00660         if (pAxis->deferred_move) {
00661             done = 0;
00662         } else {
00663             done = ((status[0] & CS_STATUS1_RUNNING_PROG) == 0)&&((status[1] & CS_STATUS2_IN_POSITION) != 0);
00664         }
00665         motorParam->setInteger( pAxis->params, motorAxisDone,          done);            
00666         motorParam->setInteger( pAxis->params, motorAxisLowHardLimit,  ((status[2] & CS_STATUS3_LIMIT)!=0) );
00667         /*Deal with error bits*/
00668         motorParam->setInteger( pAxis->params, motorAxisFollowingError,((status[1] & CS_STATUS2_FOLLOW_ERR)!=0) );
00669         motorParam->setInteger( pAxis->params, motorAxisProblem, ((status[1] & CS_STATUS2_AMP_FAULT) != 0) );
00670         motorParam->setInteger( pAxis->params, motorAxisProblem, ((status[1] & CS_STATUS2_RUNTIME_ERR) != 0) );
00671         motorParam->callCallback( pAxis->params );           
00672     }
00673     
00674     /* Now unlock all the axes */
00675     for (i = 0; i < NAXES; i++) {
00676         pAxis = &pDrv->axis[i];
00677         epicsMutexUnlock( pAxis->axisMutex );
00678     }    
00679 }
00680 /* 
00681 static void drvPmacGetAxisInitialStatus( AXIS_HDL pAxis, asynUser * pasynUser )
00682 {
00683    char command[32];
00684     char response[128];
00685     int cmdStatus;
00686     double low_limit, high_limit, pgain, igain, dgain;
00687     int nvals;
00688 
00689     return;
00690 }
00691 */
00692 
00693 
00694 static void drvPmacTask( PMACDRV_ID pDrv )
00695 {
00696   int i = 0;
00697   int done = 0;
00698   int eventStatus = 0;
00699   float timeout = 0.0;
00700   epicsUInt32 status[3];
00701 
00702   while ( 1 ) 
00703   {
00704     /* if it's time for an idle poll or the motor is moving */
00705     drvPmacGetAxesStatus( pDrv, status);    
00706     /* Wait for an event, or a timeout. If we get an event, force an update.*/
00707     if (epicsMutexLock(pDrv->controllerMutexId) == epicsMutexLockOK) {
00708       timeout = pDrv->idlePollPeriod;
00709     }
00710     else {
00711       drvPrint(drvPrintParam, TRACE_ERROR, "drvPmacTask: Failed to get controllerMutexId lock.\n");
00712     }
00713     /* Get axis status */
00714     for ( i = 0; i < NAXES; i++ )
00715     {
00716       AXIS_HDL pAxis = &(pDrv->axis[i]);
00717       /* get the cached done status */
00718       epicsMutexLock( pAxis->axisMutex );
00719       motorParam->getInteger( pAxis->params, motorAxisDone, &done );
00720       epicsMutexUnlock( pAxis->axisMutex );
00721       if ((eventStatus == epicsEventWaitOK) || (done == 0))
00722       {
00723           /* If we got an event, then one motor is moving, so force an update for all */      
00724         timeout = pDrv->movingPollPeriod;          
00725         break;
00726       }
00727     }
00728     epicsMutexUnlock(pDrv->controllerMutexId);
00729     eventStatus = epicsEventWaitWithTimeout(pDrv->pollEventId, timeout);    
00730   }
00731 }
00732 
00733 
00738 int asynMatch(asynUser *pasynUser, char *checkport, int checkaddr)
00739 {
00740     const char *port;
00741     int addr;
00742     
00743     pasynManager->getPortName(pasynUser, &port);
00744     pasynManager->getAddr(pasynUser, &addr);
00745     if (strncmp(port, checkport, strlen(port)) != 0 || addr != checkaddr) {
00746         return 0;
00747     }
00748     return 1;
00749 }
00750 
00760 int pmacAsynCoordCreate( char *port, int addr, int cs, int ref, int program )
00761 {
00762     int i;
00763     int status = MOTOR_AXIS_OK;
00764     PMACDRV_ID pDrv;
00765     PMACDRV_ID * ppLast = &(pFirstDrv);
00766 
00767     if ((cs < 1) || (cs > 16)) {
00768         drvPrint( drvPrintParam, TRACE_ERROR, "Invalid co-ordinate system number: %d\n", cs);
00769         return MOTOR_AXIS_ERROR;
00770     }
00771 
00772     for ( pDrv = pFirstDrv;
00773           pDrv != NULL && (pDrv->ref != ref) && (!asynMatch(pDrv->pasynUser, port, addr) || pDrv->cs != cs);
00774           pDrv = pDrv->pNext )
00775     {
00776         ppLast = &(pDrv->pNext);
00777     }
00778 
00779     if ( pDrv == NULL) {
00780         drvPrint( drvPrintParam, TRACE_FLOW,
00781                "Creating PMAC co-ordinate system driver on port %s, address %d, cs: %d (ref = %d)\n",
00782                port, addr, cs, ref );
00783 
00784         pDrv = (PMACDRV_ID) calloc( 1, sizeof(drvPmac_t) );
00785 
00786         if (pDrv != NULL) {
00787             pDrv->axis = (AXIS_HDL) calloc( NAXES, sizeof( motorAxis ) );
00788 
00789             if (pDrv->axis != NULL ) {
00790                 pDrv->cs = cs;
00791                 pDrv->ref = ref;
00792         /* Set default polling rates.*/
00793         pDrv->movingPollPeriod = (double)defaultMovingPollPeriod / 1000.0;
00794         pDrv->idlePollPeriod = (double)defaultIdlePollPeriod / 1000.0;
00795         /* Create event to signal poller task with.*/
00796         pDrv->pollEventId = epicsEventMustCreate(epicsEventEmpty);
00797         /* Create mutex ID for controller.*/
00798         if ((pDrv->controllerMutexId = epicsMutexCreate()) == NULL) {
00799             drvPrint( drvPrintParam, TRACE_ERROR, "pmacAsynMotorCreate: Could not create controllerMutexId.\n");
00800         }
00801 
00802                 status = motorAxisAsynConnect( port, addr, &(pDrv->pasynUser), "\006", "\r" );
00803 
00804                 for (i=0; i<NAXES && status == MOTOR_AXIS_OK; i++ ) {
00805                     if ((pDrv->axis[i].params = motorParam->create( 0, MOTOR_AXIS_NUM_PARAMS )) != NULL &&
00806                         (pDrv->axis[i].axisMutex = epicsMutexCreate( )) != NULL ) {
00807                         pDrv->axis[i].pDrv = pDrv;
00808                         pDrv->axis[i].coord_system = cs;
00809                         pDrv->axis[i].program = program;
00810                         pDrv->axis[i].axis = i + 1;
00811                         pDrv->axis[i].logParam  = pDrv->pasynUser;
00812                         pDrv->axis[i].pasynUser = pDrv->pasynUser;
00813                         pDrv->axis[i].stepSize = 0.0001;
00814 
00815                         asynPrint( pDrv->pasynUser, ASYN_TRACE_FLOW, "Created motor for ref %d, C.S. %d, signal %d OK\n", ref, cs, i );
00816                     } else {
00817                         asynPrint(pDrv->pasynUser, ASYN_TRACE_ERROR,
00818                                   "drvPmacCreate: unable to set create axis %d on %s: insufficient memory\n", 
00819                                   i, port );
00820                         status = MOTOR_AXIS_ERROR;
00821                     }
00822                 }
00823 
00824                 if ( status == MOTOR_AXIS_ERROR ) {
00825                     for (i=0; i<NAXES; i++ ) {
00826                         if (pDrv->axis[i].params != NULL) motorParam->destroy( pDrv->axis[i].params );
00827                         if (pDrv->axis[i].axisMutex != NULL) epicsMutexDestroy( pDrv->axis[i].axisMutex );
00828                     }
00829                     free ( pDrv );
00830                 }
00831             } else {
00832                 free ( pDrv );
00833                 status = MOTOR_AXIS_ERROR;
00834             }
00835         } else {
00836             drvPrint( drvPrintParam, TRACE_ERROR,
00837                       "drvPmacCreate: unable to create driver for port %s: insufficient memory\n",
00838                       port );
00839             status = MOTOR_AXIS_ERROR;
00840         }
00841 
00842         if ( status == MOTOR_AXIS_OK ) {
00843             *ppLast = pDrv;
00844         }
00845     } else {
00846         drvPrint( drvPrintParam, TRACE_ERROR, "C.S. reference %d already exists\n", ref );
00847         status = MOTOR_AXIS_ERROR;
00848     }
00849 
00850     if (status == MOTOR_AXIS_OK) {
00851         epicsUInt32 status[3];
00852 
00853         /* Do an initial poll of all status */
00854         drvPmacGetAxesStatus( pDrv, status );
00855         
00856         pDrv->motorThread = epicsThreadCreate( "drvPmacCSThread",
00857                                                epicsThreadPriorityLow,
00858                                                epicsThreadGetStackSize(epicsThreadStackMedium),
00859                                                (EPICSTHREADFUNC) drvPmacTask, (void *) pDrv );
00860         if (pDrv->motorThread == NULL) {
00861             asynPrint(pDrv->pasynUser, ASYN_TRACE_ERROR, "Cannot start motor polling thread\n" );
00862             return MOTOR_AXIS_ERROR;
00863         }
00864     }
00865 
00866     return status;
00867 }
00868 
00869 
00870 
00876 int pmacSetDefaultCoordSteps(double defaultSteps)
00877 {
00878     int status = -1;
00879     PMACDRV_ID pDrv = NULL;
00880     AXIS_HDL pAxis = NULL;
00881     int axis = 0;
00882 
00883     if (defaultSteps <= 0.0) {
00884         drvPrint(drvPrintParam, TRACE_ERROR, "ERROR: Scaling factor has to be positive.\n" );
00885         return status;
00886     }
00887 
00888     if (pFirstDrv != NULL) {
00889         for (pDrv = pFirstDrv; pDrv != NULL; pDrv = pDrv->pNext) {
00890             if (epicsMutexLock( pDrv->controllerMutexId ) == epicsMutexLockOK) {
00891                 for (axis = 0; axis < NAXES; axis++) {
00892                     pAxis = &pDrv->axis[axis];
00893                     pAxis->stepSize = 1.0 / defaultSteps;
00894                 }
00895                 epicsMutexUnlock( pDrv->controllerMutexId );
00896                 drvPrint(drvPrintParam, TRACE_FLOW, "  Set scaling factor to %f for CS ref %d.\n", defaultSteps, pDrv->ref);
00897                 status = 0;
00898             } else {
00899                 drvPrint(drvPrintParam, TRACE_ERROR, "ERROR: Could not lock axis mutex.\n");
00900                 return status;
00901             }
00902         } 
00903     }
00904 
00905     return status;
00906 }
00907 
00918 int pmacSetCoordStepsPerUnit(int ref, int axis, double stepsPerUnit)
00919 {
00920     int status = -1;
00921     PMACDRV_ID pDrv = NULL;
00922 
00923     if (stepsPerUnit <= 0.0) {
00924         drvPrint(drvPrintParam, TRACE_ERROR, "ERROR: Scaling factor has to be positive.\n" );
00925         return status;
00926     }
00927 
00928     if ((axis < 0) | (axis >= NAXES)) {
00929         drvPrint(drvPrintParam, TRACE_ERROR, "ERROR: axis number has to be between 0 and %d.\n", NAXES-1);
00930         return status;
00931     }
00932 
00933     if (pFirstDrv != NULL) {
00934         for (pDrv = pFirstDrv; pDrv != NULL; pDrv = pDrv->pNext) {
00935             if (ref == pDrv->ref) {
00936                 if (epicsMutexLock( pDrv->controllerMutexId ) == epicsMutexLockOK) {
00937                     pDrv->axis[axis].stepSize = 1.0 / stepsPerUnit;
00938                     epicsMutexUnlock( pDrv->controllerMutexId );
00939                     status = 0;
00940                     drvPrint(drvPrintParam, TRACE_FLOW, "  Setting axis %d scaling factor to %f.\n", axis, stepsPerUnit);
00941                 } else {
00942                     drvPrint(drvPrintParam, TRACE_ERROR, "ERROR: Could not lock axis mutex.\n");
00943                     return status;
00944                 }
00945             }
00946         }
00947     }
00948 
00949     return status;
00950 }
00951 
00952 
00953 static int drvPmacLogMsg( void * param, const motorAxisLogMask_t mask, const char *pFormat, ...)
00954 {
00955     va_list    pvar;
00956     int        nchar=0;
00957     asynUser *  pasynUser =  (asynUser *) param;
00958     int         reason = (int) mask;
00959 
00960     if ( pasynUser == NULL ) {
00961         va_start(pvar, pFormat);
00962         vprintf( pFormat, pvar );
00963         va_end (pvar);
00964     } else if ( pasynTrace->getTraceMask(pasynUser) & reason ) {
00965         va_start(pvar, pFormat);
00966         nchar = pasynTrace->vprint( pasynUser, reason, pFormat, pvar );
00967         va_end (pvar);
00968     }
00969 
00970     return(nchar);
00971 }
00972 
00973 
00981 int pmacSetCoordMovingPollPeriod(int ref, int movingPollPeriod)
00982 {
00983     int status = 1;
00984     PMACDRV_ID pDrv = NULL;
00985 
00986     if (pFirstDrv != NULL) {
00987         for (pDrv = pFirstDrv; pDrv != NULL; pDrv = pDrv->pNext) {
00988             drvPrint(drvPrintParam, TRACE_FLOW, "** Setting moving poll period of %dms on ref %d\n", movingPollPeriod, pDrv->ref);
00989             if (ref == pDrv->ref) {
00990                 if (epicsMutexLock( pDrv->controllerMutexId ) == epicsMutexLockOK) {
00991                     pDrv->movingPollPeriod = (double)movingPollPeriod / 1000.0;
00992                     epicsMutexUnlock( pDrv->controllerMutexId );
00993                     epicsEventSignal( pDrv->pollEventId );
00994                     status = 0;
00995                 } else {
00996                     drvPrint(drvPrintParam, TRACE_ERROR, "pmacSetCoordMovingPollPeriod: could not access pDrv to set polling period.\n" );
00997                 } 
00998             }
00999         }
01000     }
01001 
01002     return status;
01003 }
01004 
01005 
01013 int pmacSetCoordIdlePollPeriod(int ref, int idlePollPeriod)
01014 {
01015     int status = 1;
01016     PMACDRV_ID pDrv = NULL;
01017 
01018     if (pFirstDrv != NULL) {
01019         for (pDrv = pFirstDrv; pDrv!=NULL; pDrv = pDrv->pNext) {
01020             drvPrint(drvPrintParam, TRACE_FLOW, "** Setting idle poll period of %dms on ref %d\n", idlePollPeriod, pDrv->ref);
01021             if (ref == pDrv->ref) {
01022                 if (epicsMutexLock( pDrv->controllerMutexId ) == epicsMutexLockOK) {
01023                     pDrv->idlePollPeriod = (double)idlePollPeriod / 1000.0;
01024                     epicsMutexUnlock( pDrv->controllerMutexId );
01025                     epicsEventSignal( pDrv->pollEventId );
01026                     status = 0;
01027                 } else {
01028                     drvPrint(drvPrintParam, TRACE_ERROR, "pmacSetMovingPollPeriod: could not access pDrv to set polling period.\n" );
01029                 }
01030             }
01031         }
01032     }
01033 
01034     return status;
01035 }
 All Classes Files Functions Variables Typedefs Defines