sics/motorlist.c

/*----------------------------------------------------------------------
  Support module which manages a list of motors and their target values
  when running complex movements. See accompanying tex file for
  more info.

  copyright: see file COPYRIGHT

  Mark Koennecke, September 2005
-----------------------------------------------------------------------*/
#include "motorlist.h"
#include "lld.h"
#include "motor.h"
/*---------------------------------------------------------------*/
static void *MOLIGetInterface(void *data, int iD){
      return NULL;
}
/*----------------------------------------------------------------
 This routine can return either OKOK or HWFault when thing
 go wrong. However, the return value of Halt is usually ignored!
------------------------------------------------------------------*/
static int MOLIHalt(void *data) {
    int self = 0, iRet; 
    MotControl tuktuk;
 
    self = *(int *)data;

    iRet = LLDnodePtr2First(self);
    while(iRet != 0)
    {
      LLDnodeDataTo(self,&tuktuk);
      tuktuk.pDriv->Halt(tuktuk.data);
      iRet = LLDnodePtr2Next(self);
    }
    return OKOK;
}
/*----------------------------------------------------------------
 This routine can return either 1 or 0. 1 means the position can 
 be reached, 0 NOT
 If 0, error shall contain up to errlen characters of information
 about which limit was violated
------------------------------------------------------------------*/
static int MOLICheckLimits(void *data, float val,
                         char *error, int errlen){
    int self = 0, iRet, test, retVal = 1; 
    MotControl tuktuk;
 
    self = *(int *)data;

    iRet = LLDnodePtr2First(self);
    while(iRet != 0)
    {
      LLDnodeDataTo(self,&tuktuk);
      test = tuktuk.pDriv->CheckLimits(tuktuk.data,val, error, errlen);
      if(test == 0){
      	retVal = 0;
      }
      iRet = LLDnodePtr2Next(self);
    }

    return retVal; 
}
/*----------------------------------------------------------------
 This routine can return 0 when a limit problem occurred 
 OKOK when the motor was successfully started 
 HWFault when a problem occured starting the device
 Possible errors shall be printed to pCon
 For real motors, this is supposed to try at least three times
 to start the motor in question
 val is the value to drive the motor too
------------------------------------------------------------------*/
static long MOLISetValue(void *data, SConnection *pCon, float val){
    int self = 0, iRet, test; 
    MotControl tuktuk;
 
    self = *(int *)data;

    iRet = LLDnodePtr2First(self);
    while(iRet != 0)
    {
      LLDnodeDataTo(self,&tuktuk);
      test = tuktuk.pDriv->SetValue(tuktuk.data,pCon,tuktuk.target);
      if(test != 1) {
      	return test;
      } else {
   		tuktuk.running = 1;
		LLDnodeDataFrom(self,&tuktuk);    
      }
      iRet = LLDnodePtr2Next(self);
    }
    return OKOK;
}
/*----------------------------------------------------------------
 Checks the status of a running motor. Possible return values
   HWBusy The motor is still running
   OKOK or HWIdle when the motor finished driving
   HWFault when a hardware problem ocurred
   HWPosFault when the hardware cannot reach a position
 Errors are duly to be printed to pCon
 For real motors CheckStatus again shall try hard to fix any 
 issues with the motor 
------------------------------------------------------------------*/
static int MOLICheckStatus(void *data, SConnection *pCon){
    int self = 0, iRet, status, result = HWIdle; 
    MotControl tuktuk;
 
    self = *(int *)data;

    iRet = LLDnodePtr2First(self);
    while(iRet != 0)
    {
      LLDnodeDataTo(self,&tuktuk);
      if(tuktuk.running == 1){
        status = tuktuk.pDriv->CheckStatus(tuktuk.data, pCon);
        switch(status){
        case HWIdle:
			tuktuk.running = 0;
			LLDnodeDataFrom(self,&tuktuk);
	 		break;
      	case HWBusy:
			result = HWBusy;
		break;
      	case HWFault:
      	case HWPosFault:
			return status;
			break;
      	default:
			/*
	  		this is a programming error and has to be fixed
			*/
			assert(0); 
      	}
      }
  	  iRet = LLDnodePtr2Next(self);
    }
	return result;	
}
/*----------------------------------------------------------------
 GetValue is supposed to read a motor position
 On errors, -99999999.99 is returned and messages printed to pCon
------------------------------------------------------------------*/
static float MOLIGetValue(void *data, SConnection *pCon){
    int self = 0, iRet; 
    MotControl tuktuk;
 	float value, result = 1;
 
    self = *(int *)data;

    iRet = LLDnodePtr2First(self);
    while(iRet != 0)
    {
      LLDnodeDataTo(self,&tuktuk);
      if(MotorGetSoftPosition(tuktuk.data,pCon,&value) == 1){
          tuktuk.position = value;
      } else {
          tuktuk.position = -9999999.99;
      }
	  LLDnodeDataFrom(self,&tuktuk);
      iRet = LLDnodePtr2Next(self);
    }
    return OKOK;
}
/*======================== interface functions ========================*/
pIDrivable makeMotListInterface(){
    pIDrivable pDriv = NULL;

	pDriv = CreateDrivableInterface();
	if(pDriv == NULL){
		return NULL;
	}
    pDriv->Halt = MOLIHalt;
    pDriv->CheckLimits = MOLICheckLimits;
    pDriv->SetValue = MOLISetValue;
    pDriv->CheckStatus = MOLICheckStatus;
    pDriv->GetValue = MOLIGetValue;

    return pDriv;
}
/*----------------------------------------------------------------------*/
int addMotorToList(int listHandle, char *name, float targetValue){
	pMotor pMot = NULL;
	MotControl tuktuk;
	
	pMot = FindMotor(pServ->pSics,name);
	if(pMot == NULL){
		return 0;
	}
	tuktuk.data = pMot;
	strncpy(tuktuk.name,name,79);
	tuktuk.pDriv = pMot->pDrivInt;
	tuktuk.target = targetValue;
	tuktuk.running = 0;
	tuktuk.position = -9999999.999;
	LLDnodeAppendFrom(listHandle,&tuktuk);
	return 1;
}
/*---------------------------------------------------------------------*/
int setNewMotorTarget(int listHandle, char *name, float value){
	int iRet;
	MotControl tuktuk;
	
    iRet = LLDnodePtr2First(listHandle);
    while(iRet != 0)
    {
      LLDnodeDataTo(listHandle,&tuktuk);
      if(strcmp(tuktuk.name,name) == 0){
      	tuktuk.target = value;
      	tuktuk.running = 0;
      	LLDnodeDataFrom(listHandle, &tuktuk);
      	return 1; 
      }
      iRet = LLDnodePtr2Next(listHandle);
    }
 	return 0;
}
/*-----------------------------------------------------------------*/
int getMotorFromList(int listHandle, char *name,pMotControl tuk){
	int iRet;
	MotControl tuktuk;
	
    iRet = LLDnodePtr2First(listHandle);
    while(iRet != 0)
    {
      LLDnodeDataTo(listHandle,&tuktuk);
      if(strcmp(tuktuk.name,name) == 0){
      	memcpy(tuk,&tuktuk,sizeof(MotControl));
		return 1;
      }
      iRet = LLDnodePtr2Next(listHandle);
    }
 	return 0;
}
/*-----------------------------------------------------------------*/
float getListMotorPosition(int listHandle, char *name){
	int iRet;
	MotControl tuktuk;
	
    iRet = LLDnodePtr2First(listHandle);
    while(iRet != 0)
    {
      LLDnodeDataTo(listHandle,&tuktuk);
      if(strcmp(tuktuk.name,name) == 0){
      	return tuktuk.position;
      }
      iRet = LLDnodePtr2Next(listHandle);
    }
 	return -999999.99;
}
/*--------------------------------------------------------------------*/
void printMotorList(int listHandle, SConnection *pCon){
    int iRet;
    MotControl tuktuk;
    char pBueffel[132];
 
    MOLIGetValue(&listHandle,pCon);
    
    iRet = LLDnodePtr2First(listHandle);
    while(iRet != 0)
    {
      LLDnodeDataTo(listHandle,&tuktuk);
      snprintf(pBueffel,131,"Driving %20s from %8.3f to %8.3f",
        tuktuk.name, tuktuk.position, tuktuk.target);
      SCWrite(pCon,pBueffel,eValue);
      iRet = LLDnodePtr2Next(listHandle);
    }
}