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

- Added hmslave.c

Browse Source 
- Tasub only drives required motors
- Tasub handles fixed motors more gracefully
This commit is contained in:
koennecke
2007-05-30 11:57:50 +00:00
parent 55893f9133
commit 9737722b8c
9 changed files with 355 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
tasub.c
View File

@ -227,6 +227,13 @@ static int testMotor(ptasUB pNew, SConnection *pCon, char *name, int idx){
return 1;
}
}
/*-------------------------------------------------------------------*/
static void updateTargets(ptasUB pNew, SConnection *pCon){
tasAngles ang;
readTASAngles(pNew,pCon,&ang);
calcTasQEPosition(&pNew->machine, ang, &pNew->target);
}
/*--------------------------------------------------------------------*/
int TasUBFactory(SConnection *pCon,SicsInterp *pSics, void *pData,
int argc, char *argv[]){
@ -303,7 +310,7 @@ int TasUBFactory(SConnection *pCon,SicsInterp *pSics, void *pData,
SCWrite(pCon,"ERROR: a required motor is mssing, tasub NOT installed",eError);
return 0;
}
status = AddCommand(pSics,argv[1],
TasUBWrapper,
KillTasUB,
@ -1438,6 +1445,21 @@ static int setUB(SConnection *pCon, SicsInterp *pSics, ptasUB self,
return 1;
}
/*------------------------------------------------------------------*/
static int getUB(SConnection *pCon, SicsInterp *pSics, ptasUB self,
int argc, char *argv[]){
double value;
char pBueffel[512];
int status;
snprintf(pBueffel,511,"tasub.ub = %f %f %f %f %f %f %f %f %f",
self->machine.UB[0][0], self->machine.UB[0][1], self->machine.UB[0][2],
self->machine.UB[1][0], self->machine.UB[1][1], self->machine.UB[1][2],
self->machine.UB[2][0], self->machine.UB[2][1], self->machine.UB[2][2]);
SCWrite(pCon,pBueffel,eValue);
return 1;
}
/*------------------------------------------------------------------*/
static int setNormal(SConnection *pCon, SicsInterp *pSics, ptasUB self,
int argc, char *argv[]){
double value;
@ -1652,6 +1674,8 @@ int TasUBWrapper(SConnection *pCon,SicsInterp *pSics, void *pData,
return calcQFromAngles(self,pCon,pSics,argc,argv);
} else if(strcmp(argv[1],"setub") == 0){
return setUB(pCon,pSics,self,argc,argv);
} else if(strcmp(argv[1],"getub") == 0){
return getUB(pCon,pSics,self,argc,argv);
} else if(strcmp(argv[1],"setnormal") == 0){
return setNormal(pCon,pSics,self,argc,argv);
} else if(strcmp(argv[1],"settarget") == 0){
@ -1664,6 +1688,10 @@ int TasUBWrapper(SConnection *pCon,SicsInterp *pSics, void *pData,
return readReflection(pCon,pSics,&self->r1,argc,argv);
} else if(strcmp(argv[1],"r2") == 0){
return readReflection(pCon,pSics,&self->r2,argc,argv);
} else if(strcmp(argv[1],"updatetargets") == 0){
updateTargets(self,pCon);
SCSendOK(pCon);
return 1;
} else if(strcmp(argv[1],"const") == 0){
if(argc > 2){
strtolower(argv[2]);