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;
}
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 RelationManager::cleanMap(map<const Class *, set<const Class *> > &map)
{
unsigned deletedRecords = 0;
std::set<const Class*>::iterator setIter;
// associative-container erase idiom
for (auto it = map.begin(); it != map.end();) {
if (it->first == nullptr) {
map.erase(it++);
++deletedRecords;
} else if ((setIter = std::find(it->second.cbegin(), it->second.cend(), nullptr)) != it->second.cend()) {
deletedRecords += cleanSet(it->second);
++it;
++deletedRecords;
}
else {
++it;
}
}
return deletedRecords;
}
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 */
static unsigned int makeSet_auto(AS_phasorSet *Setp, const AS_shaper *shp, unsigned int *astsp, double SxDiff, double ext_match, double uc,
double amaxH, double amaxS, double umaxH, double umaxS, double umin, double omega, int verbose, int overlap)
{
unsigned int sts=0;
double uCmd, Sxad, sfact, ext_a, phi_d, ext_travel, ext_min, uCmd_minext;
AS_phasorSet aSet;
AS_phasorSet dSet;
aSet.ph=NULL;
AS_emptySet(&aSet);
dSet.ph=NULL;
AS_emptySet(&dSet);
AS_collapseSet(Setp, shp, amaxS); /* Collapse current set, regardless of whether shaper works optimally with collapse or not. */
/* Loop over domain of command velocities */
for (sfact=-1.0, ext_min=DBL_MAX, ext_travel=DBL_MAX, uCmd_minext=uc; sfact<=1.0; sfact+=AS_UAUTO_INCREMENT) {
assignSet(&aSet,Setp); /* create work copy */
uCmd=sfact*umaxS;
sts |= mergeSet(&aSet, shp, (uCmd-uc - aSet.extSum/omega), amaxS, omega, 0, 0);
sts |= mergeSet(&dSet, shp, uCmd, amaxS, omega, 0, 0); /* will be reflected, shifted and inverted, which is why -uCmd is not used */
if (!sts && uCmd!=0.0) { /* If no error from mergeSet, and non-zero uCmd - continue on. */
/* Calculate the distance that will be travelled during acceleration and deceleration. */
Sxad = delta_x(&aSet, uc, 1.0/omega); /* distance travelled during acc. */
Sxad += delta_x(&dSet, 0.0, 1.0/omega); /* add distance travelled during dec. Note that 0.0 is used as uc. */
if (aSet.N == 0) /* No phasors allocated for the acceleration (u already equals uCmd) */
ext_a=0.0;
else
ext_a=-(aSet.ph[aSet.N - 1].phiEnd); /* phase duration of acc=(end of last phasor) */
/* Determine phase location for deceleration = -omega*time_for_dec */
phi_d=-(omega*(SxDiff-Sxad)/uCmd + ext_a);
/* Determine total travel phase extension */
if ((phi_d <= 0.0) && (overlap || -phi_d >= ext_a)) { /* Will only tolerate negative (or zero) phase (i.e. positive time), and deceleration start after acc if not overlap */
if (dSet.N > 0)
ext_travel=-(phi_d + dSet.ph[dSet.N - 1].phiEnd); /* total travel phase extension = offset_dec + ext_dec */
else /* Empty set needs special treatment. Situation should not occur, however.. */
ext_travel=-phi_d;
if (fabs(ext_travel-ext_match) < ext_min) { /* Found new minimum extension */
ext_min=fabs(ext_travel-ext_match);
uCmd_minext=uCmd;
}
}
}
else /* Reset error from mergeSet */
sts=0;
AS_emptySet(&dSet);
AS_emptySet(&aSet);
}
/* Check that a minimum positive extension was actually found before using uCmd_minext. */
if (ext_min != DBL_MAX) {
/* Use uCmd that yields min time */
uCmd=uCmd_minext;
assignSet(&aSet,Setp);
sts=0;
sts |= mergeSet(&aSet, shp, (uCmd-uc - aSet.extSum/omega), amaxS, omega, 0, verbose);
sts |= mergeSet(&dSet, shp, uCmd, amaxS, omega, 0, verbose); /* will be reflected, shifted and inverted, which is why -uCmd is not used */
/* Calculate the distance that will be travelled during acceleration and deceleration. */
Sxad = delta_x(&aSet, uc, 1.0/omega); /* distance travelled during acc. */
Sxad += delta_x(&dSet, 0.0, 1.0/omega); /* add distance travelled during dec. Note that 0.0 is used as uc. */
if (aSet.N == 0) /* No phasors allocated for the acceleration (u already equals uCmd) */
ext_a=0.0;
else
ext_a=-(aSet.ph[aSet.N - 1].phiEnd); /* phase duration of acc=(end of last phasor) */
/* Determine phase location for deceleration = -omega*time_for_dec */
phi_d=-(omega*(SxDiff-Sxad)/uCmd+ext_a);
/* Allocate phasors to final set. Acc part and Dec part. Collect status */
reflectShiftAndInvertSet(&dSet, phi_d); /* Create the deceleration part by reflecting, shifting and inverting the phasors */
assignSet(Setp,&aSet); /* Replace Set with the acceleration part */
AS_emptySet(&aSet); /* get rid of aSet */
appendSet(Setp, &dSet); /* Append the deceleration part */
AS_emptySet(&dSet); /* get rid of dSet */
qsort(Setp->ph, Setp->N, sizeof(AS_phasor), AS_cmpPhasors);
cleanSet(Setp);
/* Check for unallowed acc */
if (supAmax(Setp, shp->AMax*amaxH)) {
if (AS_SHP_NEG) { /* Negative shaper - redo with non-negative (dpulse or ddpulse) */
*astsp |= AS_AUTO_REMADE_SHP;
if (AS_SHP_ROBUST)
sts |= makeSet_auto(Setp, AS_SetupIST(AS_DDPULSE), astsp, SxDiff, ext_match, uc, amaxH, amaxS, umaxH, umaxS, umin, omega, verbose, overlap);
else
sts |= makeSet_auto(Setp, AS_SetupIST(AS_DPULSE), astsp, SxDiff, ext_match, uc, amaxH, amaxS, umaxH, umaxS, umin, omega, verbose, overlap);
}
else if (overlap) { /* Overlap - redo without overlap */
*astsp |= AS_AUTO_REMADE_OVERLAP;
sts |= makeSet_auto(Setp, shp, astsp, SxDiff, ext_match, uc, amaxH, amaxS, umaxH, umaxS, umin, omega, verbose, 0);
}
else {/* No more tricks in the bag.. Give up. */
*astsp |= AS_AUTO_AMAX_FAILURE;
sts |= mergeSet(Setp, shp, (0.0-uc - Setp->extSum/omega), amaxS, omega, 0, verbose);
if (verbose >= AS_VERB_ERR)
printf("\nAntiSway/makeSet_auto: Could not make set without exceeding amax. Will set zero command velocity. amaxH=%f\n",shp->AMax*amaxH);
}
}
/* Check for too low velocities */
else if (subUmin(Setp, uc, 1.0/omega, umin)) { /* umin never exceeded */
if (ext_match != 0.0) {/* if matching a given ext, redo without matching */
*astsp |= AS_AUTO_REMADE_MATCH;
sts |= makeSet_auto(Setp, shp, astsp, SxDiff, 0.0, uc, amaxH, amaxS, umaxH, umaxS, umin, omega, verbose, overlap);
}
else {
*astsp |= AS_AUTO_UMIN_FAILURE;
sts |= mergeSet(Setp, shp, (0.0-uc - Setp->extSum/omega), amaxS, omega, 0, verbose);
if (verbose >= AS_VERB_ERR)
printf("\nAntiSway/makeSet_auto: Could not make set with uR > umin. Will set zero command velocity. umin=%f\n",umin);
}
}
/* Check for too high velocities */
else if (supUmax(Setp, uc, 1.0/omega, umaxH)) {
if (overlap) {/* Overlap - redo without overlap */
*astsp |= AS_AUTO_REMADE_OVERLAP;
sts |= makeSet_auto(Setp, shp, astsp, SxDiff, ext_match, uc, amaxH, amaxS, umaxH, umaxS, umin, omega, verbose, 0);
}
else /* Resign.. use the set with umax violation (will be cut down in exec) */
*astsp |= AS_AUTO_UMAX_FAILURE;
}
}
/* No minimum positive extension found.. */
else {
*astsp |= AS_AUTO_POSEXT_FAILURE;
sts |= mergeSet(Setp, shp, (0.0-uc - Setp->extSum/omega), amaxS, omega, 0, verbose);
if (verbose >= AS_VERB_ERR)
printf("\nAntiSway/makeSet_auto: Could not find a minimum positive extension. Will set zero command velocity.\n");
}
return sts;
}
