Change from RS232Controller to MultiChan
r1711 | dcl | 2007-03-23 18:51:15 +1100 (Fri, 23 Mar 2007) | 2 lines
This commit is contained in:
@ -22,7 +22,7 @@
|
||||
#include <sys/time.h>
|
||||
#include <fortify.h>
|
||||
#include <sics.h>
|
||||
#include <rs232controller.h>
|
||||
#include <multichan.h>
|
||||
#include <nwatch.h>
|
||||
#include <modriv.h>
|
||||
#include <motor.h>
|
||||
@ -67,7 +67,7 @@ typedef struct __MoDriv {
|
||||
|
||||
|
||||
/* DMC-2280 specific fields */
|
||||
prs232 controller;
|
||||
pMultiChan mcc;
|
||||
pMotor pMot; /**< Points to logical motor object */
|
||||
int errorCode;
|
||||
char *errorMsg; /**< Points to memory for error messages */
|
||||
@ -165,6 +165,22 @@ static int DMC2280Halt(void *pData);
|
||||
static int DMC2280SetPar(void *pData, SConnection *pCon,
|
||||
char *name, float newValue);
|
||||
|
||||
typedef struct __command Command, *pCommand;
|
||||
typedef int (*CommandCallback)(pCommand pCmd);
|
||||
|
||||
struct __command {
|
||||
pMultiChan unit;
|
||||
int cstate;
|
||||
int lstate;
|
||||
char* out_buf;
|
||||
int out_len;
|
||||
int out_idx;
|
||||
char* inp_buf;
|
||||
int inp_len;
|
||||
int inp_idx;
|
||||
CommandCallback func;
|
||||
void* cntx;
|
||||
};
|
||||
|
||||
/** \brief Convert axis speed in physical units to
|
||||
* motor speed in steps/sec.
|
||||
@ -202,66 +218,242 @@ static int motDecel(pDMC2280Driv self, float axisDecel) {
|
||||
return decel;
|
||||
}
|
||||
|
||||
/** \brief Reads a single character from the DMC2280 controller.
|
||||
*
|
||||
* On failure it sets the errorCode field in the motor's data structure
|
||||
* \param self (rw) provides access to the motor's data structure
|
||||
* \param *reply (w) the character read from the controller
|
||||
* \return
|
||||
* - SUCCESS
|
||||
* - FAILURE
|
||||
* \see SUCCESS FAILURE
|
||||
*/
|
||||
static int DMC2280ReadChar(pDMC2280Driv self, /*@out@*/char *reply) {
|
||||
int i, status, retries=1, dataLen=1;
|
||||
reply[0] = '\0';
|
||||
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;
|
||||
static int DMC_Tx(void* ctx)
|
||||
{
|
||||
int iRet = 1;
|
||||
pCommand myCmd = (pCommand) ctx;
|
||||
|
||||
if (myCmd) {
|
||||
iRet = MultiChanWrite(myCmd->unit, myCmd->out_buf, myCmd->out_len);
|
||||
/* TODO handle errors */
|
||||
if (iRet < 0) { /* TODO: EOF */
|
||||
/*
|
||||
iRet = MultiChanReconnect(myCmd->unit);
|
||||
if (iRet == 0)
|
||||
*/
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
return FAILURE;
|
||||
return 1;
|
||||
}
|
||||
|
||||
/** \brief Gets output from the DMC2280, the abstract motor code should
|
||||
* handle retries if the request times out.
|
||||
*
|
||||
* Note: The timeout for readRS232TillTerm is set by DMC2280Connect
|
||||
* \param self (rw) provides access to the motor's data structure
|
||||
* \param *reply (w) the data from the DMC2280.
|
||||
* \return
|
||||
* - SUCCESS
|
||||
* - FAILURE
|
||||
* \see SUCCESS FAILURE
|
||||
static int DMC_Rx(void* ctx, int rxchar) {
|
||||
int iRet = 1;
|
||||
pCommand myCmd = (pCommand) ctx;
|
||||
|
||||
if (rxchar == MCC_TIMEOUT) {
|
||||
/* handle command timeout */
|
||||
myCmd->inp_idx = MCC_TIMEOUT;
|
||||
if (myCmd->func)
|
||||
iRet = myCmd->func(myCmd);
|
||||
free(myCmd->out_buf);
|
||||
free(myCmd->inp_buf);
|
||||
free(myCmd);
|
||||
return MCC_POP_CMD;
|
||||
}
|
||||
|
||||
switch (myCmd->cstate) {
|
||||
case 0: /* first character */
|
||||
if (rxchar == ':') {
|
||||
/* normal prompt */
|
||||
myCmd->cstate = 99;
|
||||
}
|
||||
else if (rxchar == '?') {
|
||||
/* error prompt, send TC1 ahead of any queued commands */
|
||||
iRet = MultiChanWrite(myCmd->unit, "TC1\r\n", 5);
|
||||
myCmd->cstate = 1;
|
||||
}
|
||||
else {
|
||||
/* normal data */
|
||||
myCmd->cstate = 1;
|
||||
}
|
||||
/* note fallthrough */
|
||||
case 1: /* receiving reply */
|
||||
if (myCmd->inp_idx < myCmd->inp_len)
|
||||
myCmd->inp_buf[myCmd->inp_idx++] = rxchar;
|
||||
if (rxchar == 0x0D)
|
||||
myCmd->cstate = 2;
|
||||
break;
|
||||
case 2: /* received CR and looking for LF */
|
||||
if (myCmd->inp_idx < myCmd->inp_len)
|
||||
myCmd->inp_buf[myCmd->inp_idx++] = rxchar;
|
||||
if (rxchar == 0x0A) {
|
||||
/* end of line */
|
||||
/*
|
||||
myCmd->inp_idx -= 2;
|
||||
myCmd->inp_buf[myCmd->inp_idx++] = rxchar;
|
||||
*/
|
||||
static int DMC2280Receive(pDMC2280Driv self, char *reply) {
|
||||
int i, status, retries=1, dataLen=255;
|
||||
reply[0] = '\0';
|
||||
for (i=0; i<retries; i++) {
|
||||
status=readRS232TillTerm(self->controller, reply, &dataLen);
|
||||
switch (status) {
|
||||
case 1:
|
||||
if (self->debug)
|
||||
SICSLogWrite(reply, eStatus);
|
||||
return dataLen;
|
||||
case TIMEOUT:
|
||||
self->errorCode = status;
|
||||
continue;
|
||||
/* TODO case INCOMPLETE: */
|
||||
myCmd->cstate = 0;
|
||||
}
|
||||
else
|
||||
myCmd->cstate = 1;
|
||||
break;
|
||||
}
|
||||
if (myCmd->cstate == 99) {
|
||||
myCmd->inp_buf[myCmd->inp_idx] = '\0';
|
||||
if (strncmp(myCmd->inp_buf, myCmd->out_buf, myCmd->out_len) == 0) {
|
||||
int i;
|
||||
SICSLogWrite("Line echo detected", eStatus);
|
||||
for (i = myCmd->out_len; i <= myCmd->inp_idx; ++i) {
|
||||
myCmd->inp_buf[i - myCmd->out_len] = myCmd->inp_buf[i];
|
||||
}
|
||||
}
|
||||
if (myCmd->func)
|
||||
iRet = myCmd->func(myCmd);
|
||||
else
|
||||
iRet = 0;
|
||||
myCmd->cstate = 0;
|
||||
myCmd->inp_idx = 0;
|
||||
}
|
||||
if (iRet == 0) { /* end of command */
|
||||
free(myCmd->out_buf);
|
||||
free(myCmd->inp_buf);
|
||||
free(myCmd);
|
||||
return MCC_POP_CMD;
|
||||
}
|
||||
return iRet;
|
||||
}
|
||||
|
||||
static int DMC_SendCmd(pMultiChan unit,
|
||||
char* command, int cmd_len,
|
||||
CommandCallback callback, void* context, int rsp_len)
|
||||
{
|
||||
pCommand myCmd = NULL;
|
||||
|
||||
assert(unit);
|
||||
myCmd = (pCommand) malloc(sizeof(Command));
|
||||
assert(myCmd);
|
||||
memset(myCmd, 0, sizeof(Command));
|
||||
myCmd->out_buf = (char*) malloc(cmd_len + 5);
|
||||
memcpy(myCmd->out_buf, command, cmd_len);
|
||||
myCmd->out_len = cmd_len;
|
||||
if (myCmd->out_len < 2 ||
|
||||
myCmd->out_buf[myCmd->out_len - 1] != 0x0A ||
|
||||
myCmd->out_buf[myCmd->out_len - 2] != 0x0D) {
|
||||
myCmd->out_buf[myCmd->out_len++] = 0x0D;
|
||||
myCmd->out_buf[myCmd->out_len++] = 0x0A;
|
||||
}
|
||||
myCmd->out_buf[myCmd->out_len] = '\0';
|
||||
myCmd->func = callback;
|
||||
myCmd->cntx = context;
|
||||
if (rsp_len == 0)
|
||||
myCmd->inp_buf = NULL;
|
||||
else {
|
||||
myCmd->inp_buf = malloc(rsp_len + 1);
|
||||
memset(myCmd->inp_buf, 0, rsp_len + 1);
|
||||
}
|
||||
myCmd->inp_len = rsp_len;
|
||||
myCmd->unit = unit;
|
||||
return MultiChanEnque(unit, myCmd, DMC_Tx, DMC_Rx);
|
||||
}
|
||||
|
||||
static void DMC_Notify(void* context, int event)
|
||||
{
|
||||
pDMC2280Driv self = (pDMC2280Driv) context;
|
||||
char line[132];
|
||||
|
||||
switch (event) {
|
||||
case MCC_DISCONNECT:
|
||||
snprintf(line, 132, "Disconnect on Motor '%s'", self->name);
|
||||
SICSLogWrite(line, eStatus);
|
||||
/* TODO: disconnect */
|
||||
break;
|
||||
case MCC_RECONNECT:
|
||||
snprintf(line, 132, "Reconnect on Motor '%s'", self->name);
|
||||
SICSLogWrite(line, eStatus);
|
||||
/* TODO: reconnect */
|
||||
break;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
typedef struct txn_s {
|
||||
char* transReply;
|
||||
int transWait;
|
||||
} TXN, *pTXN;
|
||||
|
||||
/**
|
||||
* \brief SendCallback is the callback for the general command.
|
||||
*/
|
||||
static int SendCallback(pCommand pCmd)
|
||||
{
|
||||
char* cmnd = pCmd->out_buf;
|
||||
char* resp = pCmd->inp_buf;
|
||||
int resp_len = pCmd->inp_idx;
|
||||
pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx;
|
||||
|
||||
if (resp_len > 0) {
|
||||
switch (resp[0]) {
|
||||
case ':':
|
||||
case ' ':
|
||||
if (self->debug) {
|
||||
SICSLogWrite(cmnd, eStatus);
|
||||
SICSLogWrite(resp, eStatus);
|
||||
}
|
||||
break;
|
||||
case '?':
|
||||
strncpy(self->lastCmd, pCmd->out_buf, CMDLEN);
|
||||
strncpy(self->dmc2280Error, &resp[1], CMDLEN);
|
||||
SICSLogWrite(cmnd, eError);
|
||||
SICSLogWrite(resp, eError);
|
||||
self->errorCode = BADCMD;
|
||||
break;
|
||||
default:
|
||||
self->errorCode = status;
|
||||
return FAILURE;
|
||||
SICSLogWrite(cmnd, eError);
|
||||
SICSLogWrite(resp, eError);
|
||||
self->errorCode = BADUNKNOWN;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return FAILURE;
|
||||
else {
|
||||
/* TODO: timeout */
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief TransCallback is the callback for the general command transaction.
|
||||
*/
|
||||
static int TransCallback(pCommand pCmd) {
|
||||
char* resp = pCmd->inp_buf;
|
||||
int resp_len = pCmd->inp_idx;
|
||||
pTXN self = (pTXN) pCmd->cntx;
|
||||
|
||||
if (resp_len < 0) {
|
||||
self->transReply[0] = '\0';
|
||||
self->transWait = -1;
|
||||
}
|
||||
else {
|
||||
memcpy(self->transReply, resp, resp_len);
|
||||
self->transReply[resp_len] = '\0';
|
||||
self->transWait = 0;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*------------------------------------------------------------------------*/
|
||||
static int DMC_transact(pDMC2280Driv self, void *send, int sendLen,
|
||||
void *reply, int replyLen)
|
||||
{
|
||||
TXN txn;
|
||||
assert(self);
|
||||
txn.transReply = reply;
|
||||
txn.transWait = 1;
|
||||
DMC_SendCmd(self->mcc,
|
||||
send, sendLen,
|
||||
TransCallback, &txn, replyLen);
|
||||
while (txn.transWait == 1)
|
||||
TaskYield(pServ->pTasker);
|
||||
if (txn.transWait < 0)
|
||||
return txn.transWait;
|
||||
return 1;
|
||||
}
|
||||
|
||||
static int DMC2280Queue(pDMC2280Driv self, char *cmd, CommandCallback cb) {
|
||||
if (cb == NULL)
|
||||
cb = SendCallback;
|
||||
return DMC_SendCmd(self->mcc, cmd, strlen(cmd), cb, self, CMDLEN);
|
||||
}
|
||||
|
||||
/** \brief Sends a DMC2280 command to the motor controller.
|
||||
@ -283,53 +475,7 @@ static int DMC2280Receive(pDMC2280Driv self, char *reply) {
|
||||
' ' for a valid command with a response (':' follows)
|
||||
*/
|
||||
static int DMC2280Send(pDMC2280Driv self, char *command) {
|
||||
char cmdValid, reply[256];
|
||||
char *GetEMsg = "TC 1";
|
||||
int status;
|
||||
|
||||
if ((self->lastCmd) != command) {
|
||||
/*@-mayaliasunique@ this won't happen unless they overlap */
|
||||
strncpy(self->lastCmd, command, CMDLEN);
|
||||
/*@+mayaliasunique@*/
|
||||
}
|
||||
|
||||
if (self->debug)
|
||||
SICSLogWrite(command, eStatus);
|
||||
|
||||
/*@+matchanyintegral@ let size_t from strlen match int */
|
||||
status = writeRS232(self->controller, command, strlen(command));
|
||||
/*@-matchanyintegral@*/
|
||||
if (status != 1) {
|
||||
self->errorCode = status;
|
||||
return FAILURE;
|
||||
}
|
||||
|
||||
if (FAILURE == (status = DMC2280ReadChar(self, &cmdValid))) {
|
||||
return FAILURE;
|
||||
} else {
|
||||
switch (cmdValid) {
|
||||
case ':':
|
||||
case ' ':
|
||||
return SUCCESS;
|
||||
case '?':
|
||||
/*@+matchanyintegral@ let size_t from strlen match int */
|
||||
status = writeRS232(self->controller, GetEMsg, strlen(GetEMsg));
|
||||
/*@-matchanyintegral@*/
|
||||
if (status != 1) {
|
||||
self->errorCode = status;
|
||||
return FAILURE;
|
||||
}
|
||||
if (FAILURE == DMC2280Receive(self, reply))
|
||||
return HWFault;
|
||||
strncpy(self->dmc2280Error, reply, CMDLEN);
|
||||
SICSLogWrite(reply, eError);
|
||||
self->errorCode = BADCMD;
|
||||
return FAILURE;
|
||||
default:
|
||||
self->errorCode = BADUNKNOWN;
|
||||
return FAILURE;
|
||||
}
|
||||
}
|
||||
return DMC2280Queue(self, command, SendCallback);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -343,10 +489,46 @@ static int DMC2280Send(pDMC2280Driv self, char *command) {
|
||||
static int DMC2280SendReceive(pDMC2280Driv self, char *cmd, char* reply) {
|
||||
int status;
|
||||
|
||||
if (FAILURE == DMC2280Send(self, cmd))
|
||||
return FAILURE;
|
||||
if (FAILURE == DMC2280Receive(self, reply))
|
||||
status = DMC_transact(self, cmd, strlen(cmd), reply, CMDLEN);
|
||||
|
||||
if (status != 1) {
|
||||
if (self->debug)
|
||||
SICSLogWrite(cmd, eStatus);
|
||||
if (status == -1)
|
||||
self->errorCode = MOTCMDTMO;
|
||||
else
|
||||
self->errorCode = BADUNKNOWN;
|
||||
return FAILURE;
|
||||
}
|
||||
|
||||
switch (reply[0]) {
|
||||
case ':':
|
||||
if (self->debug) {
|
||||
SICSLogWrite(cmd, eStatus);
|
||||
SICSLogWrite(reply, eStatus);
|
||||
}
|
||||
return SUCCESS;
|
||||
case ' ':
|
||||
if (self->debug) {
|
||||
SICSLogWrite(cmd, eStatus);
|
||||
SICSLogWrite(reply, eStatus);
|
||||
}
|
||||
return SUCCESS;
|
||||
case '?':
|
||||
strncpy(self->lastCmd, cmd, CMDLEN);
|
||||
strncpy(self->dmc2280Error, &reply[1], CMDLEN);
|
||||
SICSLogWrite(cmd, eError);
|
||||
SICSLogWrite(reply, eError);
|
||||
self->errorCode = BADCMD;
|
||||
return FAILURE;
|
||||
default:
|
||||
strncpy(self->lastCmd, cmd, CMDLEN);
|
||||
strncpy(self->dmc2280Error, &reply[0], CMDLEN);
|
||||
SICSLogWrite(cmd, eError);
|
||||
SICSLogWrite(reply, eError);
|
||||
self->errorCode = BADUNKNOWN;
|
||||
return FAILURE;
|
||||
}
|
||||
return OKOK;
|
||||
}
|
||||
|
||||
@ -561,14 +743,43 @@ static int DMC2280RunCommon(pDMC2280Driv self,float fValue){
|
||||
return OKOK;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief process the airpad status response
|
||||
*/
|
||||
static int airpad_callback(pCommand pCmd) {
|
||||
char* resp = pCmd->inp_buf;
|
||||
int resp_len = pCmd->inp_idx;
|
||||
pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx;
|
||||
|
||||
if (resp_len > 0) {
|
||||
float fReply;
|
||||
if (self->debug)
|
||||
SICSLogWrite(resp, eStatus);
|
||||
fReply = (float) atof(resp);
|
||||
if (self->airpad_state == AIRPADS_RAISE && fReply > 0) {
|
||||
int iRet;
|
||||
self->airpad_state = AIRPADS_UP;
|
||||
iRet = DMC2280RunCommon(self, self->fTarget);
|
||||
if (iRet != OKOK)
|
||||
self->errorCode = BADCUSHION;
|
||||
return 0;
|
||||
}
|
||||
if (self->airpad_state == AIRPADS_LOWER && fReply == 0) {
|
||||
self->airpad_state = AIRPADS_DOWN;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
else {
|
||||
/* TODO: timeout */
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief request the airpad status periodically
|
||||
*/
|
||||
static int airpad_timeout(void* ctx, int mode) {
|
||||
pDMC2280Driv self = (pDMC2280Driv) ctx;
|
||||
char reply[CMDLEN];
|
||||
float fReply;
|
||||
|
||||
self->airpad_timer = NULL;
|
||||
|
||||
@ -576,29 +787,6 @@ static int airpad_timeout(void* ctx, int mode) {
|
||||
self->airpad_state == AIRPADS_DOWN)
|
||||
return 0;
|
||||
|
||||
if (self->airpad_counter <= 0) {
|
||||
self->errorCode = BADCUSHION;
|
||||
self->airpad_state = AIRPADS_DOWN;
|
||||
return 0;
|
||||
}
|
||||
if (FAILURE == DMC2280SendReceive(self, "MG APDONE", reply)) {
|
||||
self->errorCode = BADCUSHION;
|
||||
self->airpad_state = AIRPADS_DOWN;
|
||||
return 0;
|
||||
}
|
||||
fReply = (float) atof(reply);
|
||||
if (self->airpad_state == AIRPADS_RAISE && fReply > 0) {
|
||||
int iRet;
|
||||
self->airpad_state = AIRPADS_UP;
|
||||
iRet = DMC2280RunCommon(self, self->fTarget);
|
||||
if (iRet != OKOK)
|
||||
self->errorCode = BADCUSHION;
|
||||
return 0;
|
||||
}
|
||||
if (self->airpad_state == AIRPADS_LOWER && fReply == 0) {
|
||||
self->airpad_state = AIRPADS_DOWN;
|
||||
return 0;
|
||||
}
|
||||
if (self->airpad_counter <= 0) {
|
||||
self->errorCode = BADCUSHION;
|
||||
self->airpad_state = AIRPADS_DOWN;
|
||||
@ -607,6 +795,12 @@ static int airpad_timeout(void* ctx, int mode) {
|
||||
--self->airpad_counter;
|
||||
NetWatchRegisterTimer(&self->airpad_timer, 1000,
|
||||
airpad_timeout, self);
|
||||
|
||||
if (FAILURE == DMC2280Queue(self, "MG APDONE", airpad_callback)) {
|
||||
self->errorCode = BADCUSHION;
|
||||
self->airpad_state = AIRPADS_DOWN;
|
||||
return 0;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -707,7 +901,7 @@ static int DMC2280Run(void *pData,float fValue){
|
||||
self->time_settle_done.tv_sec = 0;
|
||||
|
||||
/*
|
||||
* Note: this will read the current position
|
||||
* Note: this will read the current position which will block
|
||||
*/
|
||||
do {
|
||||
#if 1
|
||||
@ -1001,12 +1195,14 @@ static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){
|
||||
}
|
||||
*iCode = self->errorCode;
|
||||
switch(*iCode){
|
||||
#ifdef HAS_RS232
|
||||
case NOTCONNECTED:
|
||||
case TIMEOUT:
|
||||
case BADSEND:
|
||||
case BADMEMORY:
|
||||
case INCOMPLETE:
|
||||
getRS232Error(*iCode, error, errLen);
|
||||
strncpy(error, "General Error(TODO)", (size_t)errLen); /* TODO */
|
||||
break;
|
||||
#endif
|
||||
case MOTCMDTMO:
|
||||
strncpy(error, "Command Timeout", (size_t)errLen);
|
||||
break;
|
||||
@ -1090,7 +1286,6 @@ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){
|
||||
case BADADR:
|
||||
return MOTFAIL;
|
||||
case BADCMD:
|
||||
//case TIMEOUT:
|
||||
case BADPAR:
|
||||
case BLOCKED:
|
||||
case MOTIONCONTROLOFF:
|
||||
@ -1098,13 +1293,14 @@ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){
|
||||
case MOTCMDTMO:
|
||||
return MOTFAIL;
|
||||
case POSFAULT:
|
||||
#if HAS_RS232
|
||||
case BADSEND:
|
||||
case TIMEOUT:
|
||||
case BADMEMORY: /* Won't happen if MonConnect sets the send terminator */
|
||||
case INCOMPLETE:
|
||||
return MOTREDO;
|
||||
case NOTCONNECTED:
|
||||
initRS232(self->controller);
|
||||
#endif
|
||||
return MOTREDO;
|
||||
break;
|
||||
default:
|
||||
@ -1564,45 +1760,15 @@ static void KillDMC2280(/*@only@*/void *pData){
|
||||
free(self->errorMsg);
|
||||
self->errorMsg = NULL;
|
||||
}
|
||||
if (self->mcc) {
|
||||
MultiChanDestroy(self->mcc);
|
||||
self->mcc = NULL;
|
||||
}
|
||||
/* Not required as performed in caller
|
||||
* free(self);
|
||||
*/
|
||||
return;
|
||||
}
|
||||
/*@only@*/ prs232 createRS232(char *host, int iPort);
|
||||
/** \brief Open a connection to the motor controller
|
||||
* \param *pCon (r) connection object.
|
||||
* \param *host (r) DMC2280 host address or name.
|
||||
* \param port DMC2280 port number
|
||||
* \return controller structure
|
||||
*/
|
||||
/*@null@*/ /*@only@*/ static prs232 DMC2280Connect(/*@dependent@*/SConnection *pCon, char *host, int port) {
|
||||
prs232 controller=NULL;
|
||||
char pError[ERRLEN];
|
||||
int msecTimeout = 5000;
|
||||
|
||||
controller=createRS232(host,port);
|
||||
if (controller==NULL) {
|
||||
snprintf(pError, ERRLEN,
|
||||
"ERROR: failed to create controller for %s at port %d",
|
||||
host, port);
|
||||
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);
|
||||
KillRS232(controller);
|
||||
return NULL;
|
||||
}
|
||||
setRS232ReplyTerminator(controller,"&\r\n:");
|
||||
setRS232SendTerminator(controller,"\r\n");
|
||||
setRS232Timeout(controller, msecTimeout);
|
||||
return controller;
|
||||
}
|
||||
|
||||
|
||||
/** \brief Create a driver for the DMC2280 Galil controller.
|
||||
*
|
||||
@ -1632,7 +1798,6 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
|
||||
char buffer[132];
|
||||
char pError[ERRLEN];
|
||||
char cmd[CMDLEN];
|
||||
int port;
|
||||
Tcl_Interp *interp;
|
||||
|
||||
buffer[0]='\0';
|
||||
@ -1644,35 +1809,44 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
|
||||
*/
|
||||
pNew = (pDMC2280Driv)malloc(sizeof(DMC2280Driv));
|
||||
if(NULL == pNew){
|
||||
(void) SCWrite(pCon,"ERROR: no memory to allocate motor driver",
|
||||
eError);
|
||||
snprintf(pError, ERRLEN, "ERROR: no memory when creating DMC2280 motor '%s'", motor);
|
||||
SCWrite(pCon, pError, eError);
|
||||
return NULL;
|
||||
}
|
||||
memset(pNew, 0, sizeof(DMC2280Driv));
|
||||
pNew->controller = NULL;
|
||||
pNew->name = NULL;
|
||||
pNew->errorCode = 0;
|
||||
pNew->errorMsg = NULL;
|
||||
pNew->lastCmd[0] = '\0';
|
||||
pNew->dmc2280Error[0] = '\0';
|
||||
pNew->absEncHome = 0;
|
||||
pNew->cntsPerX = 0;
|
||||
/* Get hostname and port from the list of named parameters */
|
||||
if ((pPtr=getParam(pCon, interp, params,"port",1)) == NULL) {
|
||||
KillDMC2280(pNew);
|
||||
return NULL;
|
||||
}
|
||||
if (sscanf(pPtr,"%d",&port)==0)
|
||||
port = getPortNum(pCon, pPtr);
|
||||
if ((pPtr=getParam(pCon, interp, params,"host",1)) == NULL) {
|
||||
KillDMC2280(pNew);
|
||||
return NULL;
|
||||
}
|
||||
strncpy(buffer,pPtr, 131);
|
||||
|
||||
/* Connect to the controller */
|
||||
pNew->controller = DMC2280Connect(pCon, buffer,port);
|
||||
if( pNew->controller == NULL ) {
|
||||
/* Get multichan from the list of named parameters */
|
||||
if ((pPtr=getParam(pCon, interp, params, "multichan", _OPTIONAL)) != NULL) {
|
||||
/* MultiChan */
|
||||
if (!MultiChanCreate(pPtr, &pNew->mcc)) {
|
||||
snprintf(pError, ERRLEN, "Cannot find MultiChan '%s' when creating DMC2280 motor '%s'",
|
||||
pPtr, motor);
|
||||
SCWrite(pCon,pError,eError);
|
||||
KillDMC2280(pNew);
|
||||
return NULL;
|
||||
}
|
||||
MultiChanSetNotify(pNew->mcc, pNew, DMC_Notify);
|
||||
}
|
||||
else if ((pPtr=getParam(pCon, interp, params, "host", _OPTIONAL)) != NULL) {
|
||||
char* host = pPtr;
|
||||
if ((pPtr=getParam(pCon, interp, params,"port",_REQUIRED)) == NULL) {
|
||||
snprintf(pError, ERRLEN, "\tError occurred when creating DMC2280 motor '%s'", motor);
|
||||
SCWrite(pCon,pError,eError);
|
||||
KillDMC2280(pNew);
|
||||
return NULL;
|
||||
}
|
||||
/* MultiChan */
|
||||
if (!MultiChanCreateHost(host, pPtr, &pNew->mcc)) {
|
||||
snprintf(pError, ERRLEN,
|
||||
"Cannot create MultiChan '%s:%s' for DMC2280 motor '%s'",
|
||||
host, pPtr, motor);
|
||||
SCWrite(pCon,pError,eError);
|
||||
KillDMC2280(pNew);
|
||||
return NULL;
|
||||
}
|
||||
MultiChanSetNotify(pNew->mcc, pNew, DMC_Notify);
|
||||
}
|
||||
else {
|
||||
snprintf(pError, ERRLEN, "\tError occurred when creating DMC2280 motor '%s'", motor);
|
||||
SCWrite(pCon,pError,eError);
|
||||
KillDMC2280(pNew);
|
||||
|
Reference in New Issue
Block a user