- Adapted indenation to new agreed upon system
- Added support for second generation scriptcontext based counter
This commit is contained in:
koennecke
258
oscillate.c
258
oscillate.c
@ -14,25 +14,28 @@
|
||||
#include "commandlog.h"
|
||||
#include "oscillate.h"
|
||||
|
||||
#define ABS(x) (x < 0 ? -(x) : (x))
|
||||
#define ABS(x) (x < 0 ? -(x) : (x))
|
||||
|
||||
/*================== real work =========================================*/
|
||||
static void StopOscillation(pOscillator self){
|
||||
static void StopOscillation(pOscillator self)
|
||||
{
|
||||
assert(self != NULL);
|
||||
if(self->taskID > 0){
|
||||
if (self->taskID > 0) {
|
||||
self->pMot->pDriver->Halt(self->pMot->pDriver);
|
||||
self->stopFlag = 1;
|
||||
self->taskID = -1;
|
||||
}
|
||||
MotorSetPar(self->pMot,self->pCon,"accesscode",usUser);
|
||||
if(self->debug > 0){
|
||||
WriteToCommandLog("oscillator>> ","Stopping");
|
||||
MotorSetPar(self->pMot, self->pCon, "accesscode", usUser);
|
||||
if (self->debug > 0) {
|
||||
WriteToCommandLog("oscillator>> ", "Stopping");
|
||||
}
|
||||
}
|
||||
|
||||
/*-------------------------------------------------------------------*/
|
||||
static float getNextPos(pOscillator self){
|
||||
static float getNextPos(pOscillator self)
|
||||
{
|
||||
float pos;
|
||||
if(self->nextTargetFlag == 1){
|
||||
if (self->nextTargetFlag == 1) {
|
||||
pos = self->upperLimit;
|
||||
self->nextTargetFlag = 0;
|
||||
} else {
|
||||
@ -41,242 +44,251 @@ static float getNextPos(pOscillator self){
|
||||
}
|
||||
return pos;
|
||||
}
|
||||
|
||||
/*-------------------------------------------------------------------*/
|
||||
static float getCurrentTarget(pOscillator self){
|
||||
static float getCurrentTarget(pOscillator self)
|
||||
{
|
||||
float pos;
|
||||
if(self->nextTargetFlag == 1){
|
||||
if (self->nextTargetFlag == 1) {
|
||||
pos = self->lowerLimit;
|
||||
} else {
|
||||
pos = self->upperLimit;
|
||||
}
|
||||
return pos;
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------*/
|
||||
static int OscillationTask(void *data){
|
||||
pOscillator self = (pOscillator)data;
|
||||
static int OscillationTask(void *data)
|
||||
{
|
||||
pOscillator self = (pOscillator) data;
|
||||
int status, code, errStatus;
|
||||
char error[256], message[132];
|
||||
float pos, curPos;
|
||||
|
||||
|
||||
assert(self);
|
||||
if(self->stopFlag == 1){
|
||||
if (self->stopFlag == 1) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
status = self->pMot->pDriver->GetStatus(self->pMot->pDriver);
|
||||
switch(status){
|
||||
switch (status) {
|
||||
case HWFault:
|
||||
case HWPosFault:
|
||||
self->pMot->pDriver->GetError(self->pMot->pDriver,&code,error,255);
|
||||
WriteToCommandLog("oscillator>> ",error);
|
||||
self->pMot->pDriver->GetError(self->pMot->pDriver, &code, error, 255);
|
||||
WriteToCommandLog("oscillator>> ", error);
|
||||
pos = getCurrentTarget(self);
|
||||
errStatus = self->pMot->pDriver->TryAndFixIt(self->pMot->pDriver,code,pos);
|
||||
errStatus =
|
||||
self->pMot->pDriver->TryAndFixIt(self->pMot->pDriver, code, pos);
|
||||
self->errorCount++;
|
||||
if(errStatus == MOTFAIL){
|
||||
if (errStatus == MOTFAIL) {
|
||||
/*
|
||||
try driving the other way on a serious error
|
||||
*/
|
||||
try driving the other way on a serious error
|
||||
*/
|
||||
pos = getNextPos(self);
|
||||
status = MotorRun(self->pMot,self->pCon,pos);
|
||||
if(self->debug > 0){
|
||||
snprintf(message,131,"Started oscillation to %f, ret code = %d",
|
||||
pos,status);
|
||||
WriteToCommandLog("oscillator>>",message);
|
||||
status = MotorRun(self->pMot, self->pCon, pos);
|
||||
if (self->debug > 0) {
|
||||
snprintf(message, 131, "Started oscillation to %f, ret code = %d",
|
||||
pos, status);
|
||||
WriteToCommandLog("oscillator>>", message);
|
||||
}
|
||||
}
|
||||
break;
|
||||
case HWWarn:
|
||||
MotorGetSoftPosition(self->pMot,self->pCon,&curPos);
|
||||
MotorGetSoftPosition(self->pMot, self->pCon, &curPos);
|
||||
pos = getCurrentTarget(self);
|
||||
if(ABS(curPos - pos) < .5){
|
||||
status = MotorRun(self->pMot,self->pCon,getNextPos(self));
|
||||
}
|
||||
if (ABS(curPos - pos) < .5) {
|
||||
status = MotorRun(self->pMot, self->pCon, getNextPos(self));
|
||||
}
|
||||
break;
|
||||
case HWBusy:
|
||||
break;
|
||||
case HWIdle:
|
||||
pos = getNextPos(self);
|
||||
status = MotorRun(self->pMot,self->pCon,pos);
|
||||
if(status == OKOK){
|
||||
status = MotorRun(self->pMot, self->pCon, pos);
|
||||
if (status == OKOK) {
|
||||
self->pMot->pDrivInt->iErrorCount = 0;
|
||||
}
|
||||
if(self->debug > 0){
|
||||
snprintf(message,131,"Started oscillation to %f, ret code = %d",
|
||||
pos,status);
|
||||
WriteToCommandLog("oscillator>>",message);
|
||||
if (self->debug > 0) {
|
||||
snprintf(message, 131, "Started oscillation to %f, ret code = %d",
|
||||
pos, status);
|
||||
WriteToCommandLog("oscillator>>", message);
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*--------------------------------------------------------------------*/
|
||||
static int StartOscillation(pOscillator self, SConnection *pCon){
|
||||
static int StartOscillation(pOscillator self, SConnection * pCon)
|
||||
{
|
||||
float fval;
|
||||
int status;
|
||||
char error[80], pBueffel[255];
|
||||
char error[80], pBueffel[255];
|
||||
|
||||
assert(self);
|
||||
|
||||
if(self->taskID > 0){
|
||||
SCWrite(pCon,"WARNING: oscillation already running",eWarning);
|
||||
SCWrite(pCon,"WARNING: restarting .. ",eWarning);
|
||||
if (self->taskID > 0) {
|
||||
SCWrite(pCon, "WARNING: oscillation already running", eWarning);
|
||||
SCWrite(pCon, "WARNING: restarting .. ", eWarning);
|
||||
StopOscillation(self);
|
||||
SicsWait(2);
|
||||
}
|
||||
}
|
||||
|
||||
MotorGetPar(self->pMot,"softlowerlim",&self->lowerLimit);
|
||||
MotorGetPar(self->pMot, "softlowerlim", &self->lowerLimit);
|
||||
self->lowerLimit += .5;
|
||||
MotorGetPar(self->pMot,"softupperlim",&self->upperLimit);
|
||||
MotorGetPar(self->pMot, "softupperlim", &self->upperLimit);
|
||||
self->upperLimit -= .5;
|
||||
MotorSetPar(self->pMot,self->pCon,"accesscode",(float)usInternal);
|
||||
MotorSetPar(self->pMot, self->pCon, "accesscode", (float) usInternal);
|
||||
self->nextTargetFlag = 0;
|
||||
self->errorCount = 0;
|
||||
self->stopFlag = 0;
|
||||
|
||||
/*
|
||||
check reachability of limits
|
||||
*/
|
||||
status = MotorCheckBoundary(self->pMot,self->lowerLimit,&fval,error,79);
|
||||
if(!status){
|
||||
snprintf(pBueffel,255,"ERROR: cannot reach %f: %s reported",
|
||||
self->lowerLimit,error);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
check reachability of limits
|
||||
*/
|
||||
status =
|
||||
MotorCheckBoundary(self->pMot, self->lowerLimit, &fval, error, 79);
|
||||
if (!status) {
|
||||
snprintf(pBueffel, 255, "ERROR: cannot reach %f: %s reported",
|
||||
self->lowerLimit, error);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
status = MotorCheckBoundary(self->pMot,self->upperLimit,&fval,error,79);
|
||||
if(!status){
|
||||
snprintf(pBueffel,255,"ERROR: cannot reach %f: %s reported",
|
||||
self->upperLimit,error);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
status =
|
||||
MotorCheckBoundary(self->pMot, self->upperLimit, &fval, error, 79);
|
||||
if (!status) {
|
||||
snprintf(pBueffel, 255, "ERROR: cannot reach %f: %s reported",
|
||||
self->upperLimit, error);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
start task
|
||||
*/
|
||||
start task
|
||||
*/
|
||||
self->taskID = TaskRegister(pServ->pTasker,
|
||||
OscillationTask,
|
||||
NULL,
|
||||
NULL,
|
||||
self,
|
||||
10);
|
||||
if(self->taskID < 0){
|
||||
SCWrite(pCon,"ERROR: failed to start oscillation task",eError);
|
||||
OscillationTask, NULL, NULL, self, 10);
|
||||
if (self->taskID < 0) {
|
||||
SCWrite(pCon, "ERROR: failed to start oscillation task", eError);
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*===================== life and death =================================*/
|
||||
static void KillOscillator(void *data){
|
||||
pOscillator self = (pOscillator)data;
|
||||
if(self != NULL){
|
||||
if(self->pDes != NULL){
|
||||
static void KillOscillator(void *data)
|
||||
{
|
||||
pOscillator self = (pOscillator) data;
|
||||
if (self != NULL) {
|
||||
if (self->pDes != NULL) {
|
||||
DeleteDescriptor(self->pDes);
|
||||
}
|
||||
if(self->pCon != NULL){
|
||||
if (self->pCon != NULL) {
|
||||
SCDeleteConnection(self->pCon);
|
||||
}
|
||||
free(self);
|
||||
}
|
||||
}
|
||||
|
||||
/*========================================================================*/
|
||||
int MakeOscillator(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[]){
|
||||
int MakeOscillator(SConnection * pCon, SicsInterp * pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
pOscillator pNew = NULL;
|
||||
pMotor pMot = NULL;
|
||||
char pBueffel[132];
|
||||
int status;
|
||||
|
||||
if(argc < 3){
|
||||
SCWrite(pCon,"ERROR: insufficient number of arguments to MakeOscilator",
|
||||
eError);
|
||||
if (argc < 3) {
|
||||
SCWrite(pCon,
|
||||
"ERROR: insufficient number of arguments to MakeOscilator",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
pMot = FindMotor(pSics,argv[2]);
|
||||
if(pMot == NULL){
|
||||
snprintf(pBueffel,131,"ERROR: %s is no motor",argv[2]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
pMot = FindMotor(pSics, argv[2]);
|
||||
if (pMot == NULL) {
|
||||
snprintf(pBueffel, 131, "ERROR: %s is no motor", argv[2]);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
pNew = (pOscillator)malloc(sizeof(Oscillator));
|
||||
if(pNew == NULL){
|
||||
SCWrite(pCon,"ERROR: out of memory creating oscillator",eError);
|
||||
pNew = (pOscillator) malloc(sizeof(Oscillator));
|
||||
if (pNew == NULL) {
|
||||
SCWrite(pCon, "ERROR: out of memory creating oscillator", eError);
|
||||
return 0;
|
||||
}
|
||||
memset(pNew,0,sizeof(Oscillator));
|
||||
memset(pNew, 0, sizeof(Oscillator));
|
||||
pNew->pDes = CreateDescriptor("Oscillator");
|
||||
pNew->pMot = pMot;
|
||||
pNew->pCon = SCCreateDummyConnection(pSics);
|
||||
if(!pNew->pDes || !pNew->pCon){
|
||||
SCWrite(pCon,"ERROR: out of memory creating oscillator",eError);
|
||||
if (!pNew->pDes || !pNew->pCon) {
|
||||
SCWrite(pCon, "ERROR: out of memory creating oscillator", eError);
|
||||
return 0;
|
||||
}
|
||||
SCSetWriteFunc(pNew->pCon,SCFileWrite);
|
||||
SCSetRights(pNew->pCon,usInternal);
|
||||
|
||||
status = AddCommand(pSics,argv[1],
|
||||
OscillatorWrapper,
|
||||
KillOscillator,
|
||||
pNew);
|
||||
if(!status){
|
||||
snprintf(pBueffel,131,"ERROR: duplicate command %s not created",argv[1]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
SCSetWriteFunc(pNew->pCon, SCFileWrite);
|
||||
SCSetRights(pNew->pCon, usInternal);
|
||||
|
||||
status = AddCommand(pSics, argv[1],
|
||||
OscillatorWrapper, KillOscillator, pNew);
|
||||
if (!status) {
|
||||
snprintf(pBueffel, 131, "ERROR: duplicate command %s not created",
|
||||
argv[1]);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*========================================================================*/
|
||||
int OscillatorWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[]){
|
||||
pOscillator self = (pOscillator)pData;
|
||||
int OscillatorWrapper(SConnection * pCon, SicsInterp * pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
pOscillator self = (pOscillator) pData;
|
||||
char pBueffel[256];
|
||||
|
||||
assert(self);
|
||||
if(argc < 2){
|
||||
SCWrite(pCon,"ERROR: need start/stop argument for oscillator",eError);
|
||||
if (argc < 2) {
|
||||
SCWrite(pCon, "ERROR: need start/stop argument for oscillator",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if(!SCMatchRights(pCon,usUser)){
|
||||
if (!SCMatchRights(pCon, usUser)) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
strtolower(argv[1]);
|
||||
if(strcmp(argv[1],"start") == 0){
|
||||
return StartOscillation(self,pCon);
|
||||
} else if(strcmp(argv[1],"stop") == 0) {
|
||||
if (strcmp(argv[1], "start") == 0) {
|
||||
return StartOscillation(self, pCon);
|
||||
} else if (strcmp(argv[1], "stop") == 0) {
|
||||
StopOscillation(self);
|
||||
snprintf(pBueffel,255,"Oscillation stopped with %d errors, %s",
|
||||
self->errorCount,
|
||||
"see commandlog for details");
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
snprintf(pBueffel, 255, "Oscillation stopped with %d errors, %s",
|
||||
self->errorCount, "see commandlog for details");
|
||||
SCWrite(pCon, pBueffel, eValue);
|
||||
return 1;
|
||||
} else if(strcmp(argv[1],"debug") == 0) {
|
||||
if(argc >= 3){
|
||||
} else if (strcmp(argv[1], "debug") == 0) {
|
||||
if (argc >= 3) {
|
||||
self->debug = atoi(argv[2]);
|
||||
SCSendOK(pCon);
|
||||
return 1;
|
||||
}
|
||||
snprintf(pBueffel,255,"%s.debug = %d", argv[0],self->debug);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
snprintf(pBueffel, 255, "%s.debug = %d", argv[0], self->debug);
|
||||
SCWrite(pCon, pBueffel, eValue);
|
||||
return 1;
|
||||
} else if(strcmp(argv[1],"status") == 0) {
|
||||
if(self->taskID > 0){
|
||||
snprintf(pBueffel,255,"Oscillation running, %d errors so far, %s",
|
||||
self->errorCount,
|
||||
" error details in commandlog");
|
||||
} else if (strcmp(argv[1], "status") == 0) {
|
||||
if (self->taskID > 0) {
|
||||
snprintf(pBueffel, 255, "Oscillation running, %d errors so far, %s",
|
||||
self->errorCount, " error details in commandlog");
|
||||
} else {
|
||||
snprintf(pBueffel,255,"Oscillation stopped");
|
||||
snprintf(pBueffel, 255, "Oscillation stopped");
|
||||
}
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
SCWrite(pCon, pBueffel, eValue);
|
||||
return 1;
|
||||
} else {
|
||||
SCWrite(pCon,"ERROR: invalid sub command for oscillator requested",
|
||||
eError);
|
||||
SCWrite(pCon, "ERROR: invalid sub command for oscillator requested",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user