AXY:Element Coordination/One Dimensional Tracking Algorithm

One Dimensional Tracking Algorithm

This algorithm is used always in simple axis based MF tracking:

The algorithm consist of a state machine with the following states:

1. Tracking process
2. Full Synchronization follow the master
3. Stopping (de-synchronization process)

State: Full Synchronization follow the master

• Check if master velocity or acceleration exceeds max master frame's values if yes return an error
• track ← master copying all derivatives
• reset: flopstime = flops = 0

State: Tracking process

Initial Setup and testing

Scaled value of AccMaxTran (or AccMaxRot) is used of checking of acceleration limit.

Scaled value of VelMaxTran (or VelMaxRot) is used of checking of velocity limit.

• Check if master velocity or acceleration exceeds max master frame's values if yes return an error
  

• Compute differences:
  

$pos_{diff}=master_{pos}-track_{pos}$    pure delta-position
$vel_{diff}=master_{vel}-track_{vel}$    pure delta-velocity

Time-to-Reach initial estimation

${sync_{jrk}}$ defined as a scaled value SyncJerkTran or SyncJerkRot

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle {sync_{acc}}} defined as a scaled value SyncAcclerationTran or SyncAcclerationRot

This is an estimation of time needed to reach the target. It is not an exact formula
if the velocity difference is positive, it means the robot velocity is lower then conveyors
therefore we need more time once to get to a higher velocity then the conveyers and once
to get from that (higher) velocity to conveyers - therefore the multiplication by 2

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle time_{toreach} = \sqrt{\frac {K*|{vel_{diff}}|} {sync_{jrk}} } + \sqrt{\frac{4*|{pos_{diff}}|}{sync_{acc}}}}

where Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle K = \begin{cases} 4, & \mbox{if }vel_{diff} > 0 \\2, & \mbox{if }vel_{diff} \le 0 \end{cases}}

DampingFactor - user defined by: MF.DAMPINGFACTOR

Additionally we multiply by a prediction factor - to decrease acc/jerk
(rounding to integer number of samples):

time_to_reach *= DampingFactor
time_to_reach = T*int(time_to_reach/T+1) 

Prediction of the Rendezvous-Point

The predicted position of the rendezvous (minus T, if it will happen in this sample!)
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle pred_{pos}} :  position prediction assuming constant acceleration
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle pred_{diff}} : Predicted position difference
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle pred_{vel}} :  velocity prediction

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle t = (time_{toreach}-T)}
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle pred_{pos} = master_{pos} + t*master_{vel} + t^2*master_{acc}/2}
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle pred_{diff} = pred_{pos} - track_{pos}}
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle pred_{vel} = master_{vel} + t*master_{acc}}

Assuming the acceleration will be zero:
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle pred_{acc} = 0}

Polynomial coefficients of 5-degree polynom connecting the current p,v,a to the randevous p,v,a
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle a0 = track_{pos} } - not used, directly written into code
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle a1 = track_{vel}} - not used, directly written into code
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle a2 = 0.5*track_{acc}} - not used, directly written into code
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle a3 = (0.5*(pred_{acc}-3*track_{acc})~+~(10*pred_{diff}/time_{toreach}} Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle -~6*track_{vel}~-~4*pred_{vel})/time_{toreach})/time_{toreach}}
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle a4 = (((5*pred_{diff}/time_{toreach}~-~(pred_{vel}+4*track_{vel}))/time_{toreach}} Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle - 1.5*track_{acc})/time_{toreach}~-~2*a3)/time_{toreach}}
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle a5 = ((((pred_{diff}/time_{toreach}~-~track_{vel})/time_{toreach}} Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle -0.5*track_{acc})/time_{toreach}~-~a3)/time_{toreach}~-~a4)/time_{toreach}}

Next SamplePosition

The position at the next sample will be:
Initial increment (full displacement): Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle dp = a5*T^5 + a4*T^4 + a3*T^3 + 0.5*track_{acc}*T^2 + track_{vel}*T}

The accleration and jerk increments could be computed in this way also.
But they are computed as average values for the time of sample duration, this approach doeas fit more to
the tools we are using to test this feature, as they all compute acceleration & jerk by numeric differentiation
inside one sample.
The side effect of all this is that we can have a change in jerk/acc sign inside one sample and the average value we are using
ddp,dddp could have an opposite sign, so we are seeing effects like jerk or acceleration is limited from a "wrong side" i.e. instead
using jmax -jmax is used. There is not much we can do against it, using exact jerk/acc values (from polynom) returns than wrong average values
we observe as jerk/acc exceed over theri max values. So the average value approach is not perfect but gives results within the given limits.

Initial setting of predicted time and the filter limit flag (if the filter did work - this flag will be on)

limit        = false 
state.satVel = false 
state.satAcc = false  
state.satJrk = false 

Velocity, Acceleration and Jerk Filter

sync_vel defined as a scaled value VelSyncTran or VelSyncRot

sync_acc defined as a scaled value AccSyncTran or AccSyncRot

sync_jrk defined as a scaled value JrkSyncTran or JrkSyncRot

T is sampling time

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle dp = \begin{cases} sync_{velT} , & \mbox{if } dp > sync_{velT} \\ limit = true \\ -sync_{velT}, & \mbox{if }dp < -sync_{velT} \\ limit = true\end{cases}}

Setting Initial velocity delta

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle ddp = dp - dp_0}

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle ddp = \begin{cases} sync_{accT2} , & \mbox{if } ddp > sync_{accT2} \\ dp = sync_{accT2} + dp_0 \\limit = true \\ -sync_{accT2}, & \mbox{if }ddp < -sync_{accT2} \\dp = -sync_{accT2} + dp_0 \\ limit = true\end{cases}}

Setting Initial acceleration delta

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle dddp = ddp - ddp_0}

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle dddp = \begin{cases} sync_{jrkT3} , & \mbox{if } dddp > sync_{jrkT3} \\ ddp = sync_{jrkT3} + ddp_0 \\dp = ddp + dp_{0}\\limit = true \\ -sync_{jrkT3}, & \mbox{if }dddp < -sync_{jrkT3} \\ddp = -sync_{jrkT3} + ddp_0 \\dp = ddp + dp_{0} \\ limit = true\end{cases}}

New position is:

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle track_{pos} = track_{pos} + dp}

If the filter was activated then:

if the velocity,accleration or jerk is limited (limit = true) then the velocity computed using polynomials does
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle track_{vel} = dp/T }
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle track_{acc} = ddp/T^2}

Velocity & Accleration will be computed exactly not using dp and ddp, it has been shown that this approach is much better - means more stable

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle track_{vel} = 5*a5*T^4 + 4*a4)*T^3 + 3*a3*T^2 + track_{acc}*T + track_{vel}}
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle track_{acc} = 20*a5*T^3 + 12*a4*T^2 + 6*a3*T + track_{acc}}

keep the old values for next sample

ddp0 = ddp; 
dp0  = dp;

System is synchronized if NEW delta's are under certesian thresholds:

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle dv_0 = dv} ; - keep the previous dv
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle dv = master_{vel} - track_{vel}}

syncGain defined by the user with: MF.FilterFactor

Successful tracking completion

if((fabs(dv) <= syncGain * sync_accT) && (fabs(master_pos - track_pos) <= syncGain * sync_accT2))
Synchronization
Take the middle for an additional smoothing:

track_pos	   =	0.5*(track_pos + master_pos);
track_vel	   =	0.5*(track_vel + master_vel);
ResetFilter();		// Reset the filter
state.tracking     =    MOT_MASTER_SYNC; // System synchronized<br>

else if( dv0*dv < 0)
Check against over-oscillation

if (flops == -1) flops = 0; // ignore the first sample
else	         flops++;
flopstime = 0;

Over-Oscillation Check

maxFlops defined by the user with: MF.MaxFlops

if (flops > maxFlops && (flopstime > maxFlopsTime && fabs(dv) > syncGain * sync_accT2))

state.tracking 	= MOT_MASTER_OSC; // OScilationss

else

flopstime++;

State: Stopping (de-synchronization process)

flopstime = flops = 0;

• De-Syncing profile, follow the given profile, ignore the real source

desync.Profile();
track_pos = track_pos_0+master_direction*desync.path.curr_pos;   	
track_vel = master_direction*desync.path.vel;			
track_acc = master_direction*desync.path.acc;			
if(desync.path.Status.stop)			// if stopped,change the state to OFF
	 SetState(MOT_MASTER_OFF);