Dit wordt inderdaad lastig.
We hadden:
Code: Selecteer alles
[1] xs = A*s5 + B*s4 + C*s3 + D*s2 + E*s + F
[2] vs = 5*A*s4 + 4*B*s3 + 3*C*s2 + 2*D*s + E
[3] as = 20*A*s3 + 12*B*s2 + 6*C*s + 2*D
[4] xe = A*e5 + B*e4 + C*e3 + D*e2 + E*e + F
[5] ve = 5*A*e4 + 4*B*e3 + 3*C*e2 + 2*D*e + E
[6] ae = 20*A*e3 + 12*B*e2 + 6*C*e + 2*D
Nu wordt
e = s + dt (waarbij dt de tijdsduur van de verplaatsing is)
xe = eindpositie master + offset = s + dt + offset
waarbij dt ook een onbekende is (in het stelsel ontstaan dan producten van onbekenden).
Bovendien moeten we de maximale acceleratie van de slave begrenzen.
Je zou dit numeriek kunnen oplossen.
We hadden al de functie
solvems(s,e,xs,vs,as,xe,ve,ae)
waarmee we de polynoomconstanten A t/m F kunnen bepalen voor gegeven e en xe.
Daarmee kunnen we de maximale acceleratie voor die gegeven e en xe berekenen, zie code hieronder:
aslave(xm) bepaalt de acceleratie als functie van de master positie
getamax() berekent de maximale acceleratie, met als input gegeven:
- dt = tijdsduur verplaatsing
- offset = offset eindpositie slave t.o.v. master
- s, xs, vs, as, ve, ae als in solvems()
De formules voor pm1 en pm2 had je al bepaald.
De versnellingen neem ik hieronder absoluut: de vertraging = negatieve versnelling verwacht ik eveneens begrensd (zo niet, pas dit dan aan).
Code: Selecteer alles
aslave(xm)={20*A*xm^3 + 12*B*xm^2 + 6*C*xm + 2*D}
getamax(s,dt,xs,vs,as,offset,ve,ae)={
\\ calculate A..F:
solvems(s,s+dt,xs,vs,as,s+dt+offset,ve,ae);
\\ find extreme acceleration master positions:
pm1=(-24*B-sqrt(576*B^2-1440*A*C))/(120*A);
pm2=(-24*B+sqrt(576*B^2-1440*A*C))/(120*A);
\\ calculate absolute value of accelerations:
a1 = abs(aslave(pm1));
a2 = abs(aslave(pm2));
\\ get maximum value of those two:
amax=max(a1,a2);
\\return value:
amax
}
In het eerdere voorbeeld 2 is dt=300 en offset=0.
We krijgen dan:
pm1 = 763.397459621...
pm2 = 936.602540378...
amax = a1 = a2 = 0.006415002990...
We hebben nu dus de functie
getamax() waar we dt in kunnen stoppen en amax uit krijgen.
Via het bisectie algoritme (
https://en.wikipedia.org/wiki/Bisection_method) kunnen we nu dt vinden voor een gegeven amax.
Dit gebeurt in de nieuwe functie
finddt() hieronder:
Code: Selecteer alles
finddt(atarget,s,xs,vs,as,offset,ve,ae)={
\\ determine start interval:
dthigh=100;
ahigh=getamax(s,dthigh,xs,vs,as,offset,ve,ae);
while(ahigh<atarget,
dthigh=dthigh/2;
ahigh=getamax(s,dthigh,xs,vs,as,offset,ve,ae);
);
while(ahigh>atarget,
dtlow=dthigh;
dthigh=dthigh*2;
ahigh=getamax(s,dthigh,xs,vs,as,offset,ve,ae);
);
\\ now: dtlow <= dttarget <= dthigh
\\ iterations bisection method:
for(i=1,50,
dtmid=(dtlow+dthigh)/2.0;
amid=getamax(s,dtmid,xs,vs,as,offset,ve,ae);
if(amid<atarget,
dthigh = dtmid
,\\ELSE:
dtlow = dtmid
);
);
\\return value:
(dtlow+dthigh)/2.0
}
Eerst bepalen we het interval [dtlow, dthigh] waarbinnen de tijd met de gewenste amax moet liggen. Daarna halveren we dit interval 50 keer: for(i=1,50,...), zodat het oorspronkelijke interval ongeveer een factor 10^15 kleiner wordt (elke 10 halveringen leveren een factor 2^10 ~= 1000 kleiner).
Stel in voorbeeld 2 wil je de maximale versnelling brengen naar
amax = 0.01,
dan vind je via
dt =
finddt(0.01,700,600,1,0,0,1,0)
een waarde van
dt = 240.2811414134753853...
en hierbij hoort een
amax = 0.01000000000000000355864879...
Ter controle:
solvems(700, 700+240.2811414134753853, 600, 1, 0, 700+240.2811414134753853+0, 1, 0)
ofwel
solvems(700, 940.2811414134753853, 600, 1, 0, 940.2811414134753853, 1, 0)
levert
A = 0.0000000007491224610518077656
B = -0.000003071928613681327500339
C = 0.0050027844015779016163304406
D = -4.043867160498191179397450613
E = 1623.6854978560383829807966872
F = -258774.9414804240429206434824
waarbij
pm1=(-24*B-sqrt(576*B^2-1440*A*C))/(120*A) = 750.77737986860741811...
en
aslave(pm1) = 0.01000000000000000356
aslave(pm1-0.1) = 0.0099999688081030...
aslave(pm1+0.1) = 0.0099999688380679...
dus de slave heeft een maximum acceleratie bij pm1=750.777 van ongeveer 0.0100000...