292 lines
7.9 KiB
C
292 lines
7.9 KiB
C
/*-----------------------------------------------------------------------
|
|
Oscillator runs a motor back and forth between its software limits.
|
|
|
|
copyright: see file COPYRIGHT
|
|
|
|
Mark Koennecke, November 2004
|
|
|
|
Fixed to work with second generation motors too
|
|
|
|
Mark Koennecke, September 2013
|
|
------------------------------------------------------------------------*/
|
|
#include <stdio.h>
|
|
#include <assert.h>
|
|
#include <tcl.h>
|
|
#include "fortify.h"
|
|
#include "sics.h"
|
|
#include "task.h"
|
|
#include "oscillate.h"
|
|
|
|
#define ABS(x) (x < 0 ? -(x) : (x))
|
|
|
|
/*================== real work =========================================*/
|
|
static void StopOscillation(pOscillator self)
|
|
{
|
|
assert(self != NULL);
|
|
if (self->taskID > 0) {
|
|
self->pMot->pDrivInt->Halt(self->pMot);
|
|
self->stopFlag = 1;
|
|
self->taskID = -1;
|
|
}
|
|
MotorSetPar(self->pMot, self->pCon, "accesscode", usUser);
|
|
if (self->debug > 0) {
|
|
Log(INFO,"com","%s","oscillator stopping");
|
|
}
|
|
}
|
|
|
|
/*-------------------------------------------------------------------*/
|
|
static float getNextPos(pOscillator self)
|
|
{
|
|
float pos;
|
|
if (self->nextTargetFlag == 1) {
|
|
pos = self->upperLimit;
|
|
self->nextTargetFlag = 0;
|
|
} else {
|
|
pos = self->lowerLimit;
|
|
self->nextTargetFlag = 1;
|
|
}
|
|
return pos;
|
|
}
|
|
|
|
/*-------------------------------------------------------------------*/
|
|
static float getCurrentTarget(pOscillator self)
|
|
{
|
|
float pos;
|
|
if (self->nextTargetFlag == 1) {
|
|
pos = self->lowerLimit;
|
|
} else {
|
|
pos = self->upperLimit;
|
|
}
|
|
return pos;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------*/
|
|
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) {
|
|
return 0;
|
|
}
|
|
|
|
status = self->pMot->pDrivInt->CheckStatus(self->pMot,pServ->dummyCon);
|
|
switch (status) {
|
|
case HWFault:
|
|
case HWPosFault:
|
|
Log(ERROR,"com","%s:%s","oscillator:",
|
|
"ERROR occurred in oscillation, try running motor manually to find out more");
|
|
Log(INFO,"com","%s:%s","oscillator",
|
|
"Trying to run other direction");
|
|
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);
|
|
Log(INFO,"com","%s:%s","oscillator", message);
|
|
}
|
|
break;
|
|
case HWWarn:
|
|
MotorGetSoftPosition(self->pMot, self->pCon, &curPos);
|
|
pos = getCurrentTarget(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) {
|
|
self->pMot->pDrivInt->iErrorCount = 0;
|
|
}
|
|
if (self->debug > 0) {
|
|
snprintf(message, 131, "Started oscillation to %f, ret code = %d",
|
|
pos, status);
|
|
Log(INFO,"com","%s:%s","oscillator", message);
|
|
}
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
static int StartOscillation(pOscillator self, SConnection * pCon)
|
|
{
|
|
float fval;
|
|
int status;
|
|
char error[80], pBueffel[255];
|
|
|
|
assert(self);
|
|
|
|
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);
|
|
self->lowerLimit += .5;
|
|
MotorGetPar(self->pMot, "softupperlim", &self->upperLimit);
|
|
self->upperLimit -= .5;
|
|
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);
|
|
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);
|
|
return 0;
|
|
}
|
|
|
|
/*
|
|
start task
|
|
*/
|
|
snprintf(pBueffel,sizeof(pBueffel),"Oscillate-%s", self->pMot->name);
|
|
self->taskID = TaskRegisterN(pServ->pTasker,pBueffel,
|
|
OscillationTask, NULL, NULL, self, TASK_PRIO_HIGH);
|
|
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) {
|
|
DeleteDescriptor(self->pDes);
|
|
}
|
|
if (self->pCon != NULL) {
|
|
SCDeleteConnection(self->pCon);
|
|
}
|
|
free(self);
|
|
}
|
|
}
|
|
|
|
/*========================================================================*/
|
|
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);
|
|
return 0;
|
|
}
|
|
|
|
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);
|
|
return 0;
|
|
}
|
|
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);
|
|
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);
|
|
return 0;
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
/*========================================================================*/
|
|
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);
|
|
return 0;
|
|
}
|
|
|
|
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) {
|
|
StopOscillation(self);
|
|
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) {
|
|
self->debug = atoi(argv[2]);
|
|
SCSendOK(pCon);
|
|
return 1;
|
|
}
|
|
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 {
|
|
snprintf(pBueffel, 255, "Oscillation stopped");
|
|
}
|
|
SCWrite(pCon, pBueffel, eValue);
|
|
return 1;
|
|
} else {
|
|
SCWrite(pCon, "ERROR: invalid sub command for oscillator requested",
|
|
eError);
|
|
return 0;
|
|
}
|
|
return 1;
|
|
}
|