/*------------------------------------------------------------------------
  Implements a SICS motor object with a MotorDriver interface.
  The control and communications functions are implemented in a separate layer
  in instrument/dmc2280.tcl

Copyright: see file Copyright.txt

Ferdi Franceschini November 2005
-----------------------------------------------------------------------*/
#include <stdlib.h>
#include <math.h>
#include <float.h>
#include <assert.h>
#include <string.h>
#include <stdarg.h>
#include <fortify.h>
#include <sics.h>
#include <rs232controller.h>
#include <modriv.h>
#include <dynstring.h>
/*
#include "splint/splint_fortify.h"
#include "splint/splint_tclDecls.h"
#include "splint/splint_dynstring.h"
#include "splint/splint_SCinter.h"
*/

/*-----------------------------------------------------------------------
  The motor driver structure. Please note that the first set of fields has 
  be identical with the fields of AbstractModriv in  ../modriv.h 
  ------------------------------------------------------------------------*/
typedef struct __MoDriv {
  /* general motor driver interface 
     fields. _REQUIRED!
     */
  float fUpper; /* upper limit */
  float fLower; /* lower limit */
  char *name;
  int   (*GetPosition)(void *self,float *fPos);
  int   (*RunTo)(void *self, float fNewVal);
  int   (*GetStatus)(void *self);
  void  (*GetError)(void *self, int *iCode, char *buffer, int iBufLen);
  int   (*TryAndFixIt)(void *self,int iError, float fNew);
  int   (*Halt)(void *self);
  int   (*GetDriverPar)(void *self, char *name, 
      float *value);
  int    (*SetDriverPar)(void *self,SConnection *pCon,
      char *name, float newValue);
  void   (*ListDriverPar)(void *self, char *motorName,
      SConnection *pCon);
  void   (*KillPrivate)(void *self);


  /* DMC-2280 specific fields */
  SConnection *pCon;
  prs232 controller;
  int iMotor;
  int errorCode;
  int oredMsr;
  int lastValue;
  int iConfig;
  char units[256]; /* physical units for axis */
  float speed; /* physical units per second */
  float maxSpeed; /* physical units per second */
  float accel; /* physical units per second^2 */
  float maxAccel; /* physical units per second^2 */
  float decel; /* physical units per second^2 */
  float maxDecel; /* physical units per second^2 */
  char axisLabel; 
  char lastCmd[1024];
  float home; /* home position for axis, default=0 */
  int motorHome; /* motor home position in steps */
  int stepsPerX; /* steps per physical unit */
  int abs_endcoder; /* Flag = 1 if there is an abs enc */
  int absEncHome; /* Home position in counts for abs enc */
  int cntsPerX; /* absolute encoder counts per physical unit */
} DMC2280Driv, *pDMC2280Driv;
/*------------------- error codes ----------------------------------*/
#define BADADR     -1 // Unknown host/port?
#define BADBSY     -2
#define BADCMD     -3
#define BADPAR     -4 // Does SICS already check parameter types?
#define BADUNKNOWN -5
#define BADSTP     -6
#define BADEMERG   -7
#define RVRSLIM    -8
#define FWDLIM     -9 
#define RUNFAULT   -10
#define POSFAULT   -11
#define BADCUSHION -12
#define ERRORLIM   -13
#define IMPOSSIBLE_LIM_SW -14
#define BGFAIL     -15
/*--------------------------------------------------------------------*/
#define STATUSMOVING 128 /* Motor is moving */
#define STATUSERRORLIMIT 64 /* Number of errorss exceed limit */
#define STATUSOFF 32 /* Motor off */
#define STATUSFWDLIMIT 8 /* Forward limit switch active */
#define STATUSRVRSLIMIT 4 /* Reverse limit switch active */

#define INIT_STR_SIZE 256
#define STR_RESIZE_LENGTH   256
#define CMDLEN 1024
#define ERRLEN 256
#define BUFFLEN 512
#define FAILURE 0
#define SUCCESS 1
#define _REQUIRED 1
#define _OPTIONAL 0

#define HOME "home"
#define HARDLOWERLIM "hardlowerlim"
#define HARDUPPERLIM "hardupperlim"
#define UNITS "units"
#define SPEED "speed"
#define MAXSPEED "maxSpeed"
#define ACCEL "accel"
#define MAXACCEL "maxAccel"
#define DECEL "decel"
#define MAXDECEL "maxDecel"

static int DMC2280Receive(pDMC2280Driv self, /*@out@*/ char *reply);

/* Return motor speed in steps/sec */
static int motSpeed(pDMC2280Driv self, float speed) {
  int motSpeed; 
    motSpeed = abs((int)(speed * self->stepsPerX + 0.5));
    return motSpeed;
}

/* Return motor acceleration in steps/sec^2 */
static int motAccel(pDMC2280Driv self, float accel) {
  int motAccel;
  motAccel = abs((int)(accel * self->stepsPerX + 0.5));
  return motAccel;
}

/* Return motor deceleration in steps/sec^2 */
static int motDecel(pDMC2280Driv self, float decel) {
  int motDecel;
  motDecel = abs((int)(decel * self->stepsPerX + 0.5));
  return motDecel;
}

static int DMC2280ReadChar(pDMC2280Driv self, char *reply) {
  int i, status, retries=20, dataLen=1;
  for (i=0; i<retries; i++) {
    status=readRS232(self->controller, reply, &dataLen);
    switch (status) {
      case 1:
        return SUCCESS;
      case TIMEOUT:
        self->errorCode = status;
        continue;
      default:
        self->errorCode = status;
        return FAILURE;
    }
  }
  return FAILURE;
}
/*---------------------------------------------------------------------*/
/* First character returned by controller is
   '?' for an invalid command or
   ':' or space for a valid command */
static int DMC2280Send(pDMC2280Driv self, char *command) {
  char cmdValid, pError[ERRLEN], reply[256];
  char *GetEMsg = "TC 1";
  int status;

  strncpy(self->lastCmd, command, CMDLEN);
  status = writeRS232(self->controller, command, strlen(command));
  if (status != 1) {
    self->errorCode = status;
    return FAILURE;
  }

  if (FAILURE == (status = DMC2280ReadChar(self, &cmdValid))) {
    self->errorCode = status;
    return FAILURE;
  } else {
    switch (cmdValid) {
      case ':':
      case ' ':
        return SUCCESS;
      case '?':
        status = writeRS232(self->controller, GetEMsg, strlen(GetEMsg));
        if (status != 1) {
          self->errorCode = status;
          return FAILURE;
        }
        if (FAILURE == DMC2280Receive(self, reply))
          return HWFault;
        snprintf(pError, ERRLEN, "DMC2280ERROR: Bad command '%s'", command);
        SCWrite(self->pCon, reply, eError);
        self->errorCode = BADCMD;
        return FAILURE;
      default:
        self->errorCode = BADUNKNOWN;
        return FAILURE;
    }
  }
}

static int DMC2280Receive(pDMC2280Driv self, char *reply) {
  int i, status, retries=20, dataLen=255;
  for (i=0; i<retries; i++) {
    status=readRS232TillTerm(self->controller, reply, &dataLen);
    switch (status) {
      case 1:
        return dataLen;
      case TIMEOUT:
        self->errorCode = status;
        continue;
      /* TODO case INCOMPLETE: */
      default:
        self->errorCode = status;
        return FAILURE;
    }
  }
  return FAILURE;
}
/*---------------------------------------------------------------------*/
static int DMC2280GetPos(void *pData, float *fPos){
  pDMC2280Driv self = NULL;
  char reply[1024];
  char cmd[CMDLEN];
  float absEncPos, motorPos;

  reply[0]='\0';
  self = (pDMC2280Driv)pData;
  assert(self != NULL);
  if (1 == self->abs_endcoder) {
    snprintf(cmd, CMDLEN, "TP%c", self->axisLabel);
    if (FAILURE == DMC2280Send(self, cmd))
      return HWFault;
    if (FAILURE == DMC2280Receive(self, reply))
      return HWFault;
    absEncPos =(float)atof(reply);
    *fPos = (absEncPos - self->absEncHome)/self->cntsPerX + self->home;
  } else {
    snprintf(cmd, ERRLEN, "TD%c", self->axisLabel);
    if (FAILURE == DMC2280Send(self, cmd))
      return HWFault;
    if (FAILURE == DMC2280Receive(self, reply))
      return HWFault;
    motorPos =(float)atof(reply);
    *fPos = (motorPos - self->motorHome)/self->stepsPerX + self->home;
  }
  return OKOK;
}
/*----------------------------------------------------------------------*/
static int DMC2280Run(void *pData,float fValue){
  pDMC2280Driv self = NULL;
  char axis;
  char cmd[CMDLEN], SH[CMDLEN], BG[CMDLEN], absPosCmd[CMDLEN];
  int absEncHome, stepsPerX, motorHome, cntsPerX, newAbsPosn;
  float target;

  self = (pDMC2280Driv)pData;
  assert(self != NULL);
  axis=self->axisLabel;
  motorHome = self->motorHome;
  stepsPerX=self->stepsPerX;
  snprintf(SH, CMDLEN, "SH%c", axis);
  snprintf(BG, CMDLEN, "BG%c", axis);
  target = fValue - self->home;
  newAbsPosn = (int)(target * stepsPerX + motorHome + 0.5);
  snprintf(absPosCmd, CMDLEN, "PA%c=%d",axis, newAbsPosn);

  if (1 == self->abs_endcoder) {
    /* Ensure that the defined motor position matches actual position */
    absEncHome = self->absEncHome;
    cntsPerX = self->cntsPerX;
    snprintf(cmd, CMDLEN, "DP%c=(_TP%c - %d)*(%d/%d) + %d",axis,axis,absEncHome,stepsPerX,cntsPerX,motorHome);
    if (FAILURE == DMC2280Send(self, cmd)) 
      return HWFault;
    snprintf(cmd, CMDLEN, "%cQTARGET=%d", axis, (int) (target * cntsPerX + absEncHome + 0.5));
    if (FAILURE == DMC2280Send(self, cmd))
      return HWFault;
  }

  if (FAILURE == DMC2280Send(self, absPosCmd))
      return HWFault;
  if (FAILURE == DMC2280Send(self, SH))
    return HWFault;
  if (FAILURE == DMC2280Send(self, BG))
    return HWFault;
  return OKOK;
}


/*------------------------------------------------------------------------*/
static int DMC2280Status(void *pData){
  pDMC2280Driv self = NULL;
  char cmd[CMDLEN];
  int switches;
  char switchesAscii[10], reply[256];
  int moving, fwd_limit_active, rvrs_limit_active, errorlimit;
  int SERVO_LOOP_NOT_RUNNING = -1, servoLoopStatus;
  int SHOULD_FIXPOS=1, should_fixpos;

  self = (pDMC2280Driv)pData;
  assert(self != NULL);
  /* Get status of switches
   * see TS (Tell Switches) in Galil manc2xx */
  snprintf(cmd, CMDLEN, "TS%c", self->axisLabel);
  if (FAILURE == DMC2280Send(self, cmd))
    return HWFault;
  if (FAILURE == DMC2280Receive(self, switchesAscii))
    return HWFault;
  sscanf(switchesAscii, "%d", &switches);
  moving = (switches & STATUSMOVING)>0;
  fwd_limit_active = !(switches & STATUSFWDLIMIT)>0;
  rvrs_limit_active = !(switches & STATUSRVRSLIMIT)>0;
  errorlimit = (switches & STATUSERRORLIMIT)>0;

  if (fwd_limit_active && rvrs_limit_active) {
    self->errorCode = IMPOSSIBLE_LIM_SW;
    return HWFault;
  }
  if (moving) {
    self->errorCode = BADBSY;
    return HWBusy;
  } else {
    /* If motor stopped check limits and error status */
    if (fwd_limit_active) {
      self->errorCode = FWDLIM;
      return HWFault;
    } else if (rvrs_limit_active) {
      self->errorCode = RVRSLIM;
      return HWFault;
    } else if (errorlimit) {
      self->errorCode = ERRORLIM;
      return HWFault;
    }
    if (self->abs_endcoder == 1) {
      /* Make sure that the servo loop is closed by checking if
       * the CLSLOOP thread is running on the controller.*/
      if (FAILURE == DMC2280Send(self, "MG _XQ1"))
        return HWFault;
      if (FAILURE == DMC2280Receive(self, reply))
        return HWFault;
      sscanf(reply, "%d", &servoLoopStatus);
      if (servoLoopStatus == SERVO_LOOP_NOT_RUNNING) {
        /* Start subroutine on controller to close the servo loop */
        if (FAILURE == DMC2280Send(self, "XQ#CLSLOOP"))
          return HWFault;
      }
      snprintf(cmd, CMDLEN, "MG %cSHLDFIX", self->axisLabel);
      if (FAILURE == DMC2280Send(self, cmd))
        return HWFault;
      if (FAILURE == DMC2280Receive(self, reply))
        return HWFault;
      sscanf(reply, "%d", &should_fixpos);
      if (should_fixpos == SHOULD_FIXPOS) {
        snprintf(cmd, CMDLEN, "%cFIXPOS=1", self->axisLabel);
        if (FAILURE == DMC2280Send(self, cmd))
          return HWFault;
        self->errorCode=BADBSY;
        return HWBusy;
      }
    }
    return HWIdle;
  }
}
/*----------------------------------------------------------------------*/
static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){
  pDMC2280Driv self = NULL;
  self = (pDMC2280Driv)pData;
  assert(self != NULL);

  *iCode = self->errorCode;
  switch(*iCode){
    case BADADR:
      strncpy(error,"Bad address",(size_t)errLen);
      break;
    case BADBSY:
      strncpy(error,"Motor still busy",(size_t)errLen);
      break;
    case BADCMD:
      snprintf(error, (size_t)errLen, "Bad command: '%s'", self->lastCmd);
      break;
    case BADPAR:
      strncpy(error,"Bad parameter",(size_t)errLen);
      break;
    case BADUNKNOWN:
      strncpy(error,"Unknown error condition",(size_t)errLen);
      break;
    case BADSTP:
      strncpy(error,"Motor is stopped",(size_t)errLen);
      break;
    case BADEMERG:
      strncpy(error,"Emergency stop is engaged",(size_t)errLen);
      break;
    case BGFAIL:
      strncpy(error,"Begin not possible due to limit switch",(size_t)errLen);
      break;
    case RVRSLIM:
      strncpy(error,"Crashed into reverse limit switch",(size_t)errLen);
      break;
    case FWDLIM:
      strncpy(error,"Crashed into forward limit switch",(size_t)errLen);
      break;
    case RUNFAULT:
      strncpy(error,"Run fault detected",(size_t)errLen);
      break;
    case POSFAULT:
      strncpy(error,"Positioning fault detected",(size_t)errLen);
      break;
    case BADCUSHION:
      strncpy(error,"Air cushion problem",(size_t)errLen);
      break;
    case ERRORLIM:
      strncpy(error,"Axis error exceeds error limit",(size_t)errLen);
      break;
    case IMPOSSIBLE_LIM_SW:
      strncpy(error,"Both limit switches seem active, maybe the polarity is set 'active low'. You should controller the controller with CN 1,-1,-1,0", (size_t)errLen);
      break;
    default:
      /* FIXME What's the default */
      break;
  }
}
/*----------------------------------------------------------------------*/
static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){
  pDMC2280Driv self = NULL;

  self = (pDMC2280Driv)pData;
  assert(self != NULL);

  switch(iCode){
    case BADADR:
    case BADCMD:
      //case TIMEOUT:
    case BADPAR:
    case BADBSY:
      return MOTREDO;
    case RUNFAULT:
    case POSFAULT:
      return MOTREDO;
    case NOTCONNECTED:
      initRS232(self->controller);
      return MOTREDO;
  }
  return MOTFAIL;
}
/*----------------------------------------------------------------------*/
/* Emergency Halt
 * Uses maximum deceleration */
static int DMC2280Halt(void *pData){
  pDMC2280Driv self = NULL;
  char cmd[CMDLEN];

  self = (pDMC2280Driv)pData;
  assert(self != NULL);
  /* Set maximum deceleration to stop motor */
  snprintf(cmd, CMDLEN, "DC%c", motDecel(self, self->maxDecel));
  if (FAILURE == DMC2280Send(self, cmd))
    return HWFault;
  /* Stop motor */
  snprintf(cmd, CMDLEN, "ST%c", self->axisLabel);
  if (FAILURE == DMC2280Send(self, cmd))
    return HWFault;
  /* Restore deceleration */
  snprintf(cmd, CMDLEN, "DC%c", motDecel(self, self->decel));
  if (FAILURE == DMC2280Send(self, cmd))
    return HWFault;

  return 1;
}
/*--------------------------------------------------------------------*/
static int DMC2280GetPar(void *pData, char *name,
    float *fValue){
  pDMC2280Driv self = NULL;

  self = (pDMC2280Driv)pData;

  if(strcmp(name,HOME) == 0) {
    *fValue = self->home;
    return 1;
  } 
  if(strcmp(name,HARDLOWERLIM) == 0) {
    *fValue = self->fLower;
    return 1;
  }
  if(strcmp(name,HARDUPPERLIM) == 0) {
    *fValue = self->fUpper;
    return 1;
  }
  if(strcmp(name,SPEED) == 0) {
    *fValue = self->speed;
    return 1;
  }
  if(strcmp(name,MAXSPEED) == 0) {
    *fValue = self->maxSpeed;
    return 1;
  }
  if(strcmp(name,ACCEL) == 0) {
    *fValue = self->accel;
    return 1;
  }
  if(strcmp(name,MAXACCEL) == 0) {
    *fValue = self->maxAccel;
    return 1;
  }
  if(strcmp(name,DECEL) == 0) {
    *fValue = self->decel;
    return 1;
  }
  if(strcmp(name,MAXDECEL) == 0) {
    *fValue = self->maxDecel;
    return 1;
  }
  return 0;
}
/*--------------------------------------------------------------------*/
static int DMC2280SetPar(void *pData, SConnection *pCon,
    char *name, float newValue){
  pDMC2280Driv self = NULL;
  char pError[ERRLEN];
  char cmd[CMDLEN];

  self = (pDMC2280Driv)pData;

  /* Set home */
  if(strcmp(name,HOME) == 0) {
    if ( (self->fLower - newValue) > FLT_EPSILON) {
      snprintf(pError, ERRLEN,"ERROR: %s must be greater than or equal to %f", HOME, self->fLower);
      SCWrite(pCon, pError, eError);
      return 1;
    }
    if ( (newValue - self->fUpper) > FLT_EPSILON) {
      snprintf(pError, ERRLEN,"ERROR: %s must be less than or equal to %f", HOME, self->fUpper);
      SCWrite(pCon, pError, eError);
      return 1;
    }
    self->home = newValue;
    return 1;
  }

  /* Set upper limit, lower limit */
  if(strcmp(name,HARDLOWERLIM) == 0) {
    self->fLower = newValue;
    return 1;
  }
  if(strcmp(name,HARDUPPERLIM) == 0) {
    self->fUpper = newValue;
    return 1;
  }

  /* Set speed */
  if(strcmp(name,SPEED) == 0) {
    if ((0.0 - newValue) > FLT_EPSILON) {
      snprintf(pError, ERRLEN,"ERROR: %s must be greater than or equal to %f", SPEED, 0.0);
      SCWrite(pCon, pError, eError);
      return 1;
    }
    if ((newValue - self->maxSpeed ) > FLT_EPSILON) {
      snprintf(pError, ERRLEN,"ERROR: %s must be less than or equal to %f", SPEED, self->maxSpeed);
      SCWrite(pCon, pError, eError);
      return 1;
    }
    self->speed = newValue;
    snprintf(cmd,CMDLEN,"SP%c=%d", self->axisLabel, motSpeed(self, self->speed));
    if (FAILURE == DMC2280Send(self, cmd))
      return 0; /* FIXME should signal a HWFault */
    return 1;
  }

  /* Set Acceleration */
  if(strcmp(name,ACCEL) == 0) {
    if ((0.0 - newValue) > FLT_EPSILON) {
      snprintf(pError, ERRLEN,"ERROR: %s must be greater than or equal to %f", ACCEL, 0.0);
      SCWrite(pCon, pError, eError);
      return 1;
    }
    if ((newValue - self->maxAccel ) > FLT_EPSILON) {
      snprintf(pError, ERRLEN,"ERROR: %s must be less than or equal to %f", ACCEL, self->maxAccel);
      SCWrite(pCon, pError, eError);
      return 1;
    }
    self->accel = newValue;
    snprintf(cmd,CMDLEN,"AC%c=%d", self->axisLabel, motAccel(self, self->accel));
    if (FAILURE == DMC2280Send(self, cmd))
      return 0; /* FIXME should signal a HWFault */
    return 1;
  }

  /* Set Deceleration */
  if(strcmp(name,DECEL) == 0) {
    if ((0.0 - newValue) > FLT_EPSILON) {
      snprintf(pError, ERRLEN,"ERROR: %s must be greater than or equal to %f", DECEL, 0.0);
      SCWrite(pCon, pError, eError);
      return 1;
    }
    if ((newValue - self->maxDecel ) > FLT_EPSILON) {
      snprintf(pError, ERRLEN,"ERROR: %s must be less than or equal to %f", DECEL, self->maxDecel);
      SCWrite(pCon, pError, eError);
      return 1;
    }
    self->decel = newValue;
    snprintf(cmd,CMDLEN,"DC%c=%d", self->axisLabel, motDecel(self, self->decel));
    if (FAILURE == DMC2280Send(self, cmd))
      return 0; /* FIXME should signal a HWFault */
    return 1;
  }

  return 0;
}
/*--------------------------------------------------------------------*/
static void DMC2280List(void *self, char *name, SConnection *pCon){
  char buffer[BUFFLEN];

  snprintf(buffer, BUFFLEN, "%s.axis = %c\n", name, ((pDMC2280Driv)self)->axisLabel);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.home = %f\n", name, ((pDMC2280Driv)self)->home);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.units = %s\n", name, ((pDMC2280Driv)self)->units);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.speed = %f\n", name, ((pDMC2280Driv)self)->speed);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.maxSpeed = %f\n", name, ((pDMC2280Driv)self)->maxSpeed);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.accel = %f\n", name, ((pDMC2280Driv)self)->accel);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.maxAccel = %f\n", name, ((pDMC2280Driv)self)->maxAccel);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.decel = %f\n", name, ((pDMC2280Driv)self)->decel);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.maxDecel = %f\n", name, ((pDMC2280Driv)self)->maxDecel);
  SCWrite(pCon, buffer, eStatus);
  return;
}
/*---------------------------------------------------------------------*/
static void KillDMC2280(void *pData){
  pDMC2280Driv self = NULL;
  self = (pDMC2280Driv)pData;
  assert(self != NULL);
  free(self->name);
  /*@-temptrans@*/ free(self); /*@+temptrans@*/
  return;
}
static prs232 DMC2280Connect(SConnection *pCon, char *buffer, int port) {
  prs232 controller=NULL;
  char pError[ERRLEN];
  int usecTimeout = 50000; /* 50msec timeout */

  controller=createRS232(buffer,port);
  if (controller==NULL)  {
    snprintf(pError, ERRLEN,
        "ERROR: failed to create controller for %s at port %d",
        controller->pHost, controller->iPort);
    SCWrite(pCon,pError,eError);
    return NULL;
  }
  if ( initRS232(controller) != 1) {
    snprintf(pError, ERRLEN,
        "ERROR: failed to connect to %s at port %d",
        controller->pHost, controller->iPort);
    SCWrite(pCon,pError,eError);
    return NULL;
  }
  setRS232ReplyTerminator(controller,"&\r\n:");
  setRS232SendTerminator(controller,"\r\n");
  setRS232Timeout(controller, usecTimeout);
  return controller;
}

/* Get configuration parameter */
static char *getParam(SConnection *pCon, Tcl_Interp *pTcl, char *params, char *parName, int mustHave ) {
  char *pPtr=NULL, pError[ERRLEN];
  pPtr = Tcl_GetVar2(pTcl,params,parName,TCL_GLOBAL_ONLY);
  if((mustHave == _REQUIRED) && !pPtr){
    snprintf(pError, ERRLEN,"ERROR: No '%s' parameter given for dmc2280 motor", parName);
    SCWrite(pCon,pError, eError);
  }
  return pPtr;
}

/*------------------------------------------------------------------*/
/*@null@*/ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params){
  /*@keep@*/ pDMC2280Driv pNew = NULL;
  char *pPtr = NULL;
  char buffer[132];
  char pError[ERRLEN];
  char cmd[CMDLEN];
  int port;
  Tcl_Interp *interp;

  buffer[0]='\0';

  interp = InterpGetTcl(pServ->pSics);

  /*
     allocate and initialize data structure
     */
  pNew = (pDMC2280Driv)malloc(sizeof(DMC2280Driv));
  if(!pNew){
    (void) SCWrite(pCon,"ERROR: no memory to allocate motor driver",
                   eError);
    return NULL;
  }
  /* Get hostname and port from the list of named parameters */
  if ((pPtr=getParam(pCon, interp, params,"port",1)) == NULL) return NULL;
  sscanf(pPtr,"%d",&port);
  if ((pPtr=getParam(pCon, interp, params,"host",1)) == NULL) return NULL;
  strncpy(buffer,pPtr, 131);

  /* Connect to the controller */
  memset(pNew,0,sizeof(DMC2280Driv));
  pNew->controller = DMC2280Connect(pCon, buffer,port);
  if( pNew->controller == NULL ) {
    snprintf(pError, ERRLEN, "\tError occurred when creating DMC2280 motor '%s'", motor);
    SCWrite(pCon,pError,eError);
    return NULL;
  }

  /*FIXME Tell splint that there's no memory leak because pointers are being initialised here */
  /*@-mustfreeonly@*/
  pNew->name = (char *)malloc(sizeof(char)*(strlen(motor)+1));
  if (pNew->name == NULL) {
    (void) SCWrite(pCon,"ERROR: no memory to allocate motor driver",
                   eError);
    free(pNew);
    return NULL;
  }
  strcpy(pNew->name, motor);
  pNew->pCon = pCon;
  pNew->home = 0.0;
  pNew->fLower = 0.0;//(float)atof(argv[2]);
  pNew->fUpper = 100.0;//(float)atof(argv[3]);
  pNew->GetPosition = DMC2280GetPos;
  pNew->RunTo = DMC2280Run;
  pNew->GetStatus = DMC2280Status;
  pNew->GetError = DMC2280Error;
  pNew->TryAndFixIt = DMC2280Fix;
  pNew->Halt = DMC2280Halt;
  pNew->GetDriverPar = DMC2280GetPar;
  pNew->SetDriverPar = DMC2280SetPar;
  pNew->ListDriverPar = DMC2280List;
  pNew->KillPrivate = KillDMC2280;
  if ((pPtr=getParam(pCon, interp, params,UNITS,_REQUIRED)) == NULL)
    return NULL;
  sscanf(pPtr,"%s",pNew->units);
  if ((pPtr=getParam(pCon, interp, params,MAXSPEED,_REQUIRED)) == NULL)
    return NULL;
  sscanf(pPtr,"%f",&(pNew->speed));
  pNew->maxSpeed = pNew->speed;
  if ((pPtr=getParam(pCon, interp, params,MAXACCEL,_REQUIRED)) == NULL)
    return NULL;
  sscanf(pPtr,"%f",&(pNew->accel));
  pNew->maxAccel = pNew->accel;
  if ((pPtr=getParam(pCon, interp, params,MAXDECEL,_REQUIRED)) == NULL)
    return NULL;
  sscanf(pPtr,"%f",&(pNew->decel));
  pNew->maxDecel = pNew->decel;
  if ((pPtr=getParam(pCon, interp, params,"axis",_REQUIRED)) == NULL)
    return NULL;
  sscanf(pPtr,"%c",&(pNew->axisLabel));
  if ((pPtr=getParam(pCon, interp, params,"stepsPerX",_REQUIRED)) == NULL)
    return NULL;
  sscanf(pPtr,"%d",&(pNew->stepsPerX));
  if ((pPtr=getParam(pCon, interp, params,"motorHome",_OPTIONAL)) == NULL)
    pNew->motorHome=0;
  else
    sscanf(pPtr,"%d",&(pNew->motorHome));
  if ((pPtr=getParam(pCon, interp, params,"absEnc",_OPTIONAL)) == NULL)
    pNew->abs_endcoder=0;
  else {
    sscanf(pPtr,"%d",&(pNew->abs_endcoder));
    if ((pPtr=getParam(pCon, interp, params,"absEncHome",_REQUIRED)) == NULL)
      pNew->absEncHome=0;
    else
      sscanf(pPtr,"%d",&(pNew->absEncHome));
    if ((pPtr=getParam(pCon, interp, params,"cntsPerX",_REQUIRED)) == NULL)
      pNew->cntsPerX=1;
    else
      sscanf(pPtr,"%d",&(pNew->cntsPerX));
  }
  /*@+mustfreeonly@*/

    /* Set speed */
    snprintf(cmd,CMDLEN,"SP%c=%d", pNew->axisLabel, motSpeed(pNew, pNew->speed));
    if (FAILURE == DMC2280Send(pNew, cmd))
      exit(EXIT_FAILURE);
    /* Set acceleration */
    snprintf(cmd,CMDLEN,"AC%c=%d", pNew->axisLabel, motAccel(pNew, pNew->accel));
    if (FAILURE == DMC2280Send(pNew, cmd))
      exit(EXIT_FAILURE);
    /* Set deceleration */
    snprintf(cmd,CMDLEN,"DC%c=%d", pNew->axisLabel, motDecel(pNew, pNew->decel));
    if (FAILURE == DMC2280Send(pNew, cmd))
      exit(EXIT_FAILURE);
    /* TODO Initialise current position and target to get a sensible initial list output */
  return (MotorDriver *)pNew;
}