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

- JulCho now reads data in tow bunches, speeds and rest for optimization

Browse Source
This commit is contained in:
koennecke
2006-10-20 14:55:25 +00:00
parent ff7896ca53
commit 980efacf6f
5 changed files with 125 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

91
tabledrive.c
View File

@@ -55,7 +55,7 @@ static int TableDriveCheckLimits(void *pData, float fVal, char *error,
strncpy(error,"Path Table Not Defined!",25);
return 0;
}
if(fVal < 0. || fVal > self->tableLength){
if(fVal < 1. || fVal >= self->tableLength){
strncpy(error,"Out of Range",25);
return 0;
}
@@ -95,23 +95,11 @@ static int findOrientingMotor(pTableDrive self, ptdMotor moti){
return 0;
}
/*-------------------------------------------------------------------------*/
static float interpolateLocate(tdEntry lower, tdEntry upper,
float value, int count){
float diff, result = count -1;
result = count;
diff = upper.position - lower.position;
value -= lower.position;
if(ABS(diff) > .00001){
result += value/diff;
}
return result;
}
/*-------------------------------------------------------------------------*/
static float locatePosition(tdMotor moti, SConnection *pCon){
int status, count = 0;
int status;
float value;
tdEntry entry, back;
double diff;
tdEntry lower, upper;
status = MotorGetSoftPosition(moti.pMot,pCon,&value);
if(!status) {
@@ -119,17 +107,37 @@ static float locatePosition(tdMotor moti, SConnection *pCon){
}
status = LLDnodePtr2First(moti.table);
while(status != 0){
LLDnodeDataTo(moti.table,&entry);
if(entry.position > value){
if(count == 0){
return 1.;
}
LLDnodePtr2Prev(moti.table);
LLDnodeDataTo(moti.table,&back);
return interpolateLocate(back,entry,value, count);
}
count++;
status = LLDnodePtr2Next(moti.table);
LLDnodeDataTo(moti.table,&lower);
status = LLDnodePtr2Next(moti.table);
if(status != 0) {
LLDnodeDataTo(moti.table,&upper);
} else {
/*
* end of table
*/
return lower.tablePos;
}
/**
* we have to deal with ascending and descending tables. This is
* why we have to have some funny logic here
*/
if(upper.position > lower.position){
/*
* ascending table
*/
if(value >= lower.position && value < upper.position){
diff = upper.position - lower.position;
return lower.tablePos + (value - lower.position)/diff;
}
} else {
/*
* descending table
*/
if(value <= lower.position && value > upper.position){
diff = lower.position - upper.position;
return lower.tablePos + ABS(value - lower.position)/diff;
}
}
}
return -999.99;
}
@@ -143,7 +151,7 @@ static void findBracket(tdMotor moti, float value, ptdEntry upper,
status = LLDnodePtr2First(moti.table);
while(status != 0){
LLDnodeDataTo(moti.table,lower);
if(lower->tablePos >= targetCount){
if(lower->tablePos == targetCount){
LLDnodeDataTo(moti.table,upper);
if(LLDnodePtr2Next(moti.table)){
LLDnodeDataTo(moti.table,upper);
@@ -161,8 +169,12 @@ static float findTarget(tdMotor moti, float value){
targetCount = (int)floor(value);
findBracket(moti,value,&upper,&lower);
diff = upper.position - lower.position;
return (float)lower.position + (value - targetCount)*diff;
if(ABS(targetCount - value) < .00001){
return lower.position;
} else {
diff = upper.position - lower.position;
return (float)lower.position + (value - targetCount)*diff;
}
}
/*-------------------------------------------------------------------------*/
static void checkSync(pTableDrive self, SConnection *pCon, float value){
@@ -212,6 +224,14 @@ static float TableDriveGetValue(void *pData, SConnection *pCon){
self->currentPosition = value;
return value;
}
/*------------------------------------------------------------------------*/
static void tableSetPar(pMotor pMot, char *name, float value){
ObPar *ob = NULL;
ob = ObParFind(pMot->ParArray,name);
assert(ob != NULL);
ob->fVal = value;
}
/*------------------------------------------------------------------------
* Before I can drive the motors, I have to release the software limits..
-------------------------------------------------------------------------*/
@@ -225,8 +245,9 @@ static void liberateMotors(pTableDrive self, SConnection *pCon){
LLDnodeDataTo(self->motorTable,&moti);
MotorGetPar(moti.pMot,"hardupperlim",&upper);
MotorGetPar(moti.pMot,"hardlowerlim",&lower);
MotorSetPar(moti.pMot,pCon,"softupperlim",upper);
MotorSetPar(moti.pMot,pCon,"softlowerlim",lower);
tableSetPar(moti.pMot,"softupperlim",upper);
tableSetPar(moti.pMot,"softlowerlim",lower);
status = LLDnodePtr2Next(self->motorTable);
}
}
@@ -249,8 +270,8 @@ static void closeMotors(pTableDrive self, SConnection *pCon){
upper = tdLower.upper + (self->currentPosition - targetCount)*diff;
diff = tdUpper.lower - tdLower.lower;
lower = tdLower.lower + (self->currentPosition - targetCount)*diff;
MotorSetPar(moti.pMot,pCon,"softupperlim",upper);
MotorSetPar(moti.pMot,pCon,"softlowerlim",lower);
tableSetPar(moti.pMot,"softupperlim",upper);
tableSetPar(moti.pMot,"softlowerlim",lower);
status = LLDnodePtr2Next(self->motorTable);
}
}
@@ -381,6 +402,8 @@ static int TableDriveCheckStatus(void *pData, SConnection *pCon){
TableDriveGetValue(self,pCon);
closeMotors(self,pCon);
return status;
} else {
return HWBusy;
}
break;
case OUTOFSYNC: