static unsigned int mergeSet(AS_phasorSet *Setp, const AS_shaper *shp, double SuDiff, double amax, double omega, int reflected, int verbose)
{
double ext, aRatio, uDiff, offset, S, tmp, offsetlim;
int n, count, addCount;
/* Initialize variables */
S=AS_SIGN(SuDiff); /* Extracting the sign */
uDiff=fabs(SuDiff); /* Absolute value */
offset=0.0; /* Phase offset. Positive*/
offsetlim=(reflected ? (shp->phi[shp->N - 1] - Setp->ph[Setp->N - 1].phiEnd) : DBL_MAX);
count = 0;
addCount = 0;
while (fabs(uDiff) > AS_EPSILON && (offsetlim-offset > AS_EPSILON && count < AS_MERGE_ITER_LIM)) { /* Allocate phasors until uCommand is attained */
ext=shp->DMin; /* Maximum phase extension of added phasors, initialized to DMin of shaper. Positive */
aRatio=(reflected ? 1.0 : (shp->AMin + shp->AMax)/shp->AMin); /* Maximum possible relative magnitude of acceleration. Positive */
for (n=0; n < shp->N; n++) /* Repeat for all the pulses in the shaper */
phaseInterval(Setp, shp->phi[n]-offset, S*shp->A[n]*amax, AS_SIGN(S*shp->A[n])*AS_MAX(1.0,fabs(shp->A[n]))*amax, &aRatio, &ext, reflected);
if (aRatio>0.0) { /* Possible to allocate phasors at current offset value ? */
tmp=omega/(aRatio*shp->ASum*amax);
ext=AS_MIN(ext,uDiff*tmp);
uDiff-=ext/tmp; /* count down uDiff, the velocity change remaining to place out */
for (n=0; n < shp->N; n++,addCount++) { /* allocate phasors at shaper locations */
tmp=shp->phi[n]-offset;
addPhasor(Setp, tmp, tmp-ext, S*aRatio*shp->A[n]*amax); /* add phasor at phi[n]-offset of extension ext and */
/* acceleration sign*ratio*shaperMag*amax to the set */
}
cleanSet(Setp);
}
offset+=ext; /* count up offset */
count++;
} /* End of while loop */
if (verbose >= AS_VERB_ALL)
printf("\nAntiSway/mergeSet exited while loop, count=%d. Added %d phasors totally.\n",count,addCount);
if (offsetlim-offset <= AS_EPSILON) /* Could not allocate phasors further (should be possible only in reflected mode). */
return AS_ERR_MERGE_OFFSETLIM;
if (count>=AS_MERGE_ITER_LIM) {
if (verbose >= AS_VERB_ERR)
AS_diagnose(Setp,omega,reflected,SuDiff,uDiff);
return AS_ERR_MERGE_ITER;
}
return 0;
}
unsigned int AS_Engine_man( int newCall, int hoisting, int hoisted, int verbose, const AS_shaper *shp,
double uCommand, double uR, double DLc, double omega, double amaxH, double amaxS, double dt,
double *aRp, AS_phasorSet *Setp )
{
unsigned int sts=0;
double SaComp,Sa0;
if (shp->ID == AS_NO_AS) { /* If NO_AS shaper, remove unnecessary constraints */
hoisting=0;
hoisted=0;
amaxS=amaxH;
}
/* Allocate or reallocate phasors if needed */
/* Correct for hoisting */
if (hoisting) {
/* Calculate compensation acceleration SaComp=-1.5*DLc*Dth/cos(thc) */
SaComp=-1.5*DLc*(-getX(Setp))*omega/cos(-getY(Setp));
/* If there is a chance SaComp can't be allocated - reduce and allocate residual separately */
if (fabs(SaComp)>amaxH-amaxS) {
Sa0=AS_SIGN(SaComp)*(amaxH-amaxS);
addPhasor(Setp, 0.0, -omega*dt, SaComp-Sa0); /* Add compensation residual */
AS_collapseSet(Setp, shp, amaxS);
SaComp=Sa0;
sts |= AS_ERR_ACC_MARGIN;
if (verbose >= AS_VERB_ERR)
printf("AntiSway/Engine_man: Partial compensation through residual phasor addition: %f SaComp needed, %f available |amax margin|\n",SaComp,amaxH-amaxS);
}
/* Adjust set so that uCommand is still met, given the compensation phasor */
sts |= makeSet_man(Setp, shp, uCommand-SaComp*dt, uR, amaxS, omega, verbose);
/* Add the compensation phasor */
addPhasor(Setp, 0.0, -omega*dt, SaComp);
cleanSet(Setp);
}
/* If no hoisting - adjust set if new command or new omega */
else if (newCall || hoisted) {
sts |= makeSet_man(Setp, shp, uCommand, uR, amaxS, omega, verbose);
}
/* Step up time and return error status */
*aRp=timeStepSet(Setp, omega*dt);
return sts;
}
void AS_collapseSet(AS_phasorSet *Setp, const AS_shaper *shp, double amax)
{
double XSum,YSum,PhiNew,RNew,extp5,sArg,base;
int n;
if (Setp->N == 0) /* If no phasors in set - do nothing */
return;
if (shp->ID == AS_NO_AS) { /* If NO_AS shaper - remove all */
AS_emptySet(Setp);
return;
}
/* sum up phasors */
XSum=getX(Setp);
YSum=getY(Setp);
/* Remove phasors */
AS_emptySet(Setp);
/* do loop over half planes */
base=0.0;
n=0;
do {
/* Calculate residual phasor */
PhiNew=arctan360N(YSum, XSum);
RNew=sqrt(XSum*XSum + YSum*YSum);
sArg=RNew*AS_GRAV_ACCEL/(2.0*amax);
if (sArg>1.0) { /* Too large arcsin argument - reduce! (residual allocated in next half plane) */
RNew=2.0*amax/AS_GRAV_ACCEL;
extp5=0.5*AS_PI;
}
else
extp5=asin(sArg); /* Half extension of residual phasor, given a=amax */
/* Make sure PhiNew is in the half plane [base, base-AS_PI) */
while (PhiNew <= base-AS_PI) {
PhiNew+=AS_PI;
RNew*=-1.0;
}
while (PhiNew > base) {
PhiNew-=AS_PI;
RNew*=-1.0;
}
/* Add the calculated residual phasor. */
if (PhiNew+extp5 > base) { /* Overlap into previous phase? */
addPhasor(Setp, base, PhiNew-extp5, AS_SIGN(-RNew)*amax);
addPhasor(Setp, -AS_PI+PhiNew+extp5, base-AS_PI, AS_SIGN(RNew)*amax);
}
else if (PhiNew-extp5 < base-AS_PI) { /* Overlap into next half plane ? */
addPhasor(Setp, base, PhiNew-extp5+AS_PI, AS_SIGN(RNew)*amax);
addPhasor(Setp, PhiNew+extp5, base-AS_PI, AS_SIGN(-RNew)*amax);
}
else /* No overlap */
addPhasor(Setp, PhiNew+extp5, PhiNew-extp5, AS_SIGN(-RNew)*amax);
/* Count down sums. Loop only over the newly added phasors. (Using that phasors are added at end positions)*/
for ( ; n < Setp->N; n++) {
XSum -= Setp->ph[n].R*cos(Setp->ph[n].Phi);
YSum -= Setp->ph[n].R*sin(Setp->ph[n].Phi);
}
base-=AS_PI;
} while (sArg>1.0);
cleanSet(Setp);
} /* end of function */
/*_*
@aref ssab_servoreg Ssab_ServoReg
*/
void Ssab_ServoReg_exec(plc_sThread *tp, pwr_sClass_Ssab_ServoReg *object)
{
double aD, uD, xD, dt, control, uRamp, xDiff;
int delaysteps;
dt = tp->PlcThread->ScanTime; /* Constant scan time (the ideal value, not the measurement of last cycle time) */
object->enable = *object->enableP;
if (!object->enable) { //bypass the reference and reset active flags and elapsed time
object->uReg = *object->uRP;
object->RampActive = FALSE;
object->DZActive = FALSE;
object->TDZElapsedTime = 0.0;
object->TDZActive = FALSE;
return;
}
/* 1. Retrieve input */
object->aR = *object->aRP;
object->uR = *object->uRP;
control = object->uR;
object->xR = *object->xRP;
object->ac = *object->acP;
object->uc = *object->ucP;
object->xc = *object->xcP;
object->xCommand = *object->xCommandP;
xDiff = object->xCommand - object->xc;
object->umaxP = *object->umaxPP;
object->umaxN = *object->umaxNP;
object->positioning = *object->positioningP;
if (object->positioning && object->enableRamp) //uR not used - use the regulator for positioning
control = (xDiff > 0.0 ? object->umaxP : object->umaxN);
/* 2. PID regulator */
// Should change some stuff here: Relative gain kP, Integration time Ti, Derivation time Td.
if (object->enablePID && !object->positioning) {
SR_addNewRef(SR_OBJ_LISTPP, object->aR, object->uR, object->xR);
delaysteps = AS_ROUND(object->DelayPID/dt);
if (delaysteps < 0)
delaysteps = 0;
if (delaysteps > object->maxdelaysteps)
delaysteps = object->maxdelaysteps;
SR_extractRef(SR_OBJ_LISTPP, delaysteps, &aD, &uD, &xD);
control = uD + object->kPID[0]*(uD - object->uc) + object->kPID[1]*(xD - object->xc) + object->kPID[2]*(aD - object->ac);
}
/* 3. Square root ramp */
//This will give erroneous behavior if the command position is changed to a position close to the current in the midst of a travel.
//Possibly solve this problem by taking into account further conditions.
if (object->enableRamp) {
uRamp = AS_SIGN(xDiff) * ( -object->DelayRamp*object->amaxS +
sqrt(object->DelayRamp*object->DelayRamp*object->amaxS*object->amaxS + 2.0*fabs(xDiff)*object->amaxS) );
if ( (xDiff >= 0.0 && control > uRamp) || (xDiff < 0.0 && control < uRamp) ) {
control = uRamp;
object->RampActive = TRUE;
}
else
object->RampActive = FALSE;
}
else
object->RampActive = FALSE;
/* 4. Dead zone */
if (object->enableDZ && (object->uR == 0.0) && (fabs(xDiff) < object->DeadZone)) {
control = 0.0;
object->DZActive = TRUE;
}
else
object->DZActive = FALSE;
/* 5. Timer dead zone */
if (object->enableTDZ && (object->uR == 0.0) && (fabs(xDiff) < object->TimerDeadZone)) {
object->TDZElapsedTime += *(object->ScanTime);
if (object->TDZElapsedTime > object->TDZTime) {
control = 0.0;
object->TDZActive = TRUE;
object->TDZElapsedTime = object->TDZTime;
}
else
object->TDZActive = FALSE;
}
else {
object->TDZElapsedTime = 0.0;
object->TDZActive = FALSE;
}
/* 6. Check bounds */
if (control > object->umaxP)
control = object->umaxP;
if (control < object->umaxN)
control = object->umaxN;
/* 8. Set output */
object->uReg = control;
}