325 lines
8.8 KiB
C
325 lines
8.8 KiB
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:
|
|
/**
|
|
* It is questionable if one should not set a flag here
|
|
* and keep polling: it is not clear if this error is a
|
|
* communication problem or that the motor really
|
|
* has stopped.
|
|
*/
|
|
return status;
|
|
break;
|
|
default:
|
|
/*
|
|
this is a programming error and has to be fixed
|
|
*/
|
|
assert(0);
|
|
}
|
|
}
|
|
iRet = LLDnodePtr2Next(self);
|
|
}
|
|
return result;
|
|
}
|
|
/*---------------------------------------------------------
|
|
A special version for EIGER. I am cautious: I have problems
|
|
with this at EIGER but I do not want to propagate the fix
|
|
elsewhere even if it may be the right thing to do.
|
|
-----------------------------------------------------------*/
|
|
int MOLIEigerStatus(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:
|
|
tuktuk.running = 0;
|
|
LLDnodeDataFrom(self, &tuktuk);
|
|
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;
|
|
strlcpy(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);
|
|
}
|
|
}
|