linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

- Adapted indenation to new agreed upon system

Browse Source 
- Added support for second generation scriptcontext based counter
This commit is contained in:
koennecke
2009-02-13 09:00:03 +00:00
parent a3dcad2bfa
commit 91d4af0541
405 changed files with 88101 additions and 88173 deletions
Download Patch File Download Diff File Expand all files Collapse all files

383
motorlist.c
View File

@ -11,54 +11,57 @@
#include "lld.h"
#include "motor.h"
/*---------------------------------------------------------------*/
static void *MOLIGetInterface(void *data, int iD){
return NULL;
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;
static int MOLIHalt(void *data)
{
int self = 0, iRet;
MotControl tuktuk;
iRet = LLDnodePtr2First(self);
while(iRet != 0)
{
LLDnodeDataTo(self,&tuktuk);
tuktuk.pDriv->Halt(tuktuk.data);
iRet = LLDnodePtr2Next(self);
}
return OKOK;
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;
static int MOLICheckLimits(void *data, float val, char *error, int errlen)
{
int self = 0, iRet, test, retVal = 1;
MotControl tuktuk;
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);
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;
return retVal;
}
/*----------------------------------------------------------------
This routine can return 0 when a limit problem occurred
OKOK when the motor was successfully started
@ -68,27 +71,28 @@ static int MOLICheckLimits(void *data, float val,
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;
static long MOLISetValue(void *data, SConnection * pCon, float val)
{
int self = 0, iRet, test;
MotControl tuktuk;
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);
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);
}
return OKOK;
iRet = LLDnodePtr2Next(self);
}
return OKOK;
}
/*----------------------------------------------------------------
Checks the status of a running motor. Possible return values
HWBusy The motor is still running
@ -99,167 +103,176 @@ static long MOLISetValue(void *data, SConnection *pCon, float val){
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;
static int MOLICheckStatus(void *data, SConnection * pCon)
{
int self = 0, iRet, status, result = HWIdle;
MotControl tuktuk;
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);
}
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;
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;
static float MOLIGetValue(void *data, SConnection * pCon)
{
int self = 0, iRet;
MotControl tuktuk;
float value, result = 1;
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);
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;
}
return OKOK;
LLDnodeDataFrom(self, &tuktuk);
iRet = LLDnodePtr2Next(self);
}
return OKOK;
}
/*======================== interface functions ========================*/
pIDrivable makeMotListInterface(){
pIDrivable pDriv = NULL;
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;
pDriv = CreateDrivableInterface();
if (pDriv == NULL) {
return NULL;
}
pDriv->Halt = MOLIHalt;
pDriv->CheckLimits = MOLICheckLimits;
pDriv->SetValue = MOLISetValue;
pDriv->CheckStatus = MOLICheckStatus;
pDriv->GetValue = MOLIGetValue;
return pDriv;
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 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);
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;
}
return 0;
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);
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;
}
return 0;
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);
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;
}
return -999999.99;
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);
}
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);
}
}