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

106
motreg.c
View File

@ -10,39 +10,41 @@
Mark Koennecke, August 2002
-----------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include <assert.h>
#include "fortify.h"
#include "motreg.h"
/*--------------------------------------------------------------------*/
pMotReg RegisterMotor(char *name, SicsInterp *pSics,
long (*NewSetValue)(void *pData, SConnection *pCon,
float farget),
int (*NewCheckStatus)(void *pData,SConnection *pCon) ){
pMotReg RegisterMotor(char *name, SicsInterp * pSics,
long (*NewSetValue) (void *pData, SConnection * pCon,
float farget),
int (*NewCheckStatus) (void *pData,
SConnection * pCon))
{
CommandList *pCom = NULL;
pIDrivable pDriv = NULL;
pMotReg pNew = NULL;
/*
find motor data structures
*/
pCom = FindCommand(pSics,name);
if(pCom == NULL){
find motor data structures
*/
pCom = FindCommand(pSics, name);
if (pCom == NULL) {
return NULL;
}
pDriv = GetDrivableInterface(pCom->pData);
if(pDriv == NULL){
if (pDriv == NULL) {
return NULL;
}
/*
everything seems OK. Allocate data structure and initialize
*/
pNew = (pMotReg)malloc(sizeof(MotReg));
if(pNew == NULL){
everything seems OK. Allocate data structure and initialize
*/
pNew = (pMotReg) malloc(sizeof(MotReg));
if (pNew == NULL) {
return NULL;
}
memset(pNew,0,sizeof(MotReg));
memset(pNew, 0, sizeof(MotReg));
pNew->motorData = pCom->pData;
pNew->motorName = strdup(name);
@ -55,45 +57,56 @@ pMotReg RegisterMotor(char *name, SicsInterp *pSics,
return pNew;
}
/*---------------------------------------------------------------------*/
void KillRegMot(void *pData){
pMotReg self = (pMotReg)pData;
if(self == NULL){
/*---------------------------------------------------------------------*/
void KillRegMot(void *pData)
{
pMotReg self = (pMotReg) pData;
if (self == NULL) {
return;
}
if(self->motorName != NULL){
if (self->motorName != NULL) {
free(self->motorName);
}
free(self);
}
/*------------------------------------------------------------------------*/
void SetRegMotTarget(pMotReg self, float fValue){
void SetRegMotTarget(pMotReg self, float fValue)
{
assert(self);
self->targetPosition = fValue;
self->iActive = 1;
}
/*------------------------------------------------------------------------*/
void CreateTargetString(pMotReg self, char pBueffel[80]) {
void CreateTargetString(pMotReg self, char pBueffel[80])
{
assert(self);
if(strlen(self->motorName) + 20 < 80) {
sprintf(pBueffel," %s %12.4f ", self->motorName, self->targetPosition);
if (strlen(self->motorName) + 20 < 80) {
sprintf(pBueffel, " %s %12.4f ", self->motorName,
self->targetPosition);
}
}
/*-----------------------------------------------------------------------*/
int RegMotMatch(pMotReg self, char *name){
int RegMotMatch(pMotReg self, char *name)
{
assert(self);
if(strcmp(self->motorName, name) == 0) {
if (strcmp(self->motorName, name) == 0) {
return 1;
}
return 0;
}
/*----------------------------------------------------------------------*/
int StartRegMot(pMotReg self, SConnection *pCon, float fValue){
int StartRegMot(pMotReg self, SConnection * pCon, float fValue)
{
int ret;
long (*oldSet)(void *pmotorData, SConnection *pCon, float fValue);
long (*oldSet) (void *pmotorData, SConnection * pCon, float fValue);
pIDrivable pDriv = NULL;
char pBueffel[132];
@ -104,35 +117,35 @@ int StartRegMot(pMotReg self, SConnection *pCon, float fValue){
oldSet = pDriv->SetValue;
pDriv->SetValue = self->originalSetValue;
ret = StartDevice(pServ->pExecutor, self->motorName,
FindDescriptor(self->motorData),
self->motorData,
pCon,
fValue);
FindDescriptor(self->motorData),
self->motorData, pCon, fValue);
/*
sprintf(pBueffel,"anticollision started %s to %f",self->motorName,
fValue);
SCWrite(pCon,pBueffel,eValue);
*/
sprintf(pBueffel,"anticollision started %s to %f",self->motorName,
fValue);
SCWrite(pCon,pBueffel,eValue);
*/
pDriv->SetValue = oldSet;
if(ret == 1){
self->iActive = 1;
if (ret == 1) {
self->iActive = 1;
} else {
snprintf(pBueffel,131,"ERROR: failed to start motor %s",
self->motorName);
SCWrite(pCon,pBueffel,eError);
self->iActive = 0;
snprintf(pBueffel, 131, "ERROR: failed to start motor %s",
self->motorName);
SCWrite(pCon, pBueffel, eError);
self->iActive = 0;
}
return ret;
}
/*----------------------------------------------------------------------*/
int CheckRegMot(pMotReg self, SConnection *pCon){
int CheckRegMot(pMotReg self, SConnection * pCon)
{
int stat;
assert(self);
if(self->iActive){
stat = self->originalCheckStatus(self->motorData,pCon);
if(stat != HWBusy){
if (self->iActive) {
stat = self->originalCheckStatus(self->motorData, pCon);
if (stat != HWBusy) {
self->iActive = 0;
}
return stat;
@ -140,4 +153,3 @@ int CheckRegMot(pMotReg self, SConnection *pCon){
return HWIdle;
}
}