View source for 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:

# Tracking process
# Full Synchronization follow the master
# 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 ===
<table style="float:right" border = "1">
<tr><td>
Scaled value of [[MC-Basic:movingFrame.ACCELERATIONMAXTRAN|AccMaxTran]] (or [[MC-Basic:movingFrame.ACCELERATIONMAXROT|AccMaxRot]]) is used of checking of acceleration limit.
</td></tr>
<tr><td>
Scaled value of [[Axystems:MC-Basic:movingFrame.VELOCITYMAXTRANS|VelMaxTran]] (or [[MC-Basic:movingFrame.VELOCITYMAXROT|VelMaxRot]]) is used of checking of velocity limit.
</td></tr>
</table>

* Check if master velocity or acceleration exceeds max master frame's values if yes return an error<br>

* Compute differences:<br>
<math>pos_{diff} = master_{pos} - track_{pos}</math>  &nbsp;&nbsp;  ''pure delta-position''<br>
<math>vel_{diff} = master_{vel} - track_{vel}</math>  &nbsp;&nbsp;  ''pure delta-velocity''<br>

=== Time-to-Reach initial estimation ===

<table style="float:right" border = "1">
<tr><td>
<math>{sync_{jrk}}</math> defined as a scaled value [[Axystems:MC-Basic:robot.JERKSYNCTRAN|SyncJerkTran]] or [[Axystems:MC-Basic:robot.JERKSYNCROT|SyncJerkRot]]
</td></tr>
<tr><td>
<math>{sync_{acc}}</math> defined as a scaled value [[Axystems:MC-Basic:robot.ACCELERATIONSYNCTRAN|SyncAcclerationTran]] or [[Axystems:MC-Basic:robot.ACCELERATIONSYNCROT|SyncAcclerationRot]]
</td></tr>
</table>


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

<br>
<math>time_{toreach} = \sqrt{\frac {K*|{vel_{diff}}|} {sync_{jrk}} }  + \sqrt{\frac{4*|{pos_{diff}}|}{sync_{acc}}}</math><br>
<br>

''where'' <math> K = \begin{cases} 4,  & \mbox{if }vel_{diff} > 0 \\2, & \mbox{if }vel_{diff} \le 0 \end{cases}</math> <br>


<table style="float:right" border = "1">
<tr><td>
DampingFactor -  user defined by: [[MC-Basic:movingFrame.DAMPINGFACTOR|MF.DAMPINGFACTOR]]
</td></tr>
</table>

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

<pre>
time_to_reach *= DampingFactor
time_to_reach = T*int(time_to_reach/T+1) 
</pre>

=== Prediction of the Rendezvous-Point ===
''The predicted position of the rendezvous (minus T, if it will happen in this sample!)''<br>
<math>pred_{pos}</math>:&nbsp; ''position prediction assuming constant acceleration''<br>
<math>pred_{diff}</math>:&nbsp;''Predicted position difference''<br>
<math>pred_{vel}</math>:&nbsp;'' velocity prediction''<br>


<math>t = (time_{toreach}-T)</math><br>
<math>pred_{pos} = master_{pos} + t*master_{vel} + t^2*master_{acc}/2</math><br>                   
<math>pred_{diff} = pred_{pos} - track_{pos}</math><br>		
<math>pred_{vel} = master_{vel} + t*master_{acc}</math><br>	



'' Assuming the acceleration will be zero:''<br>
<math>pred_{acc} = 0</math><br>		
																			  

'' Polynomial coefficients of 5-degree polynom connecting the current p,v,a to the randevous p,v,a''<br>
<math>a0 = track_{pos}	</math>		 - ''not used, directly written into code''<br>
<math>a1 = track_{vel}</math>			 - ''not used, directly written into code''<br>
<math>a2 = 0.5*track_{acc}</math>		 - ''not used, directly written into code''<br>
<math>a3 = (0.5*(pred_{acc}-3*track_{acc})~+~(10*pred_{diff}/time_{toreach}</math><math>-~6*track_{vel}~-~4*pred_{vel})/time_{toreach})/time_{toreach}</math><br>
<math>a4 = (((5*pred_{diff}/time_{toreach}~-~(pred_{vel}+4*track_{vel}))/time_{toreach}</math><math>- 1.5*track_{acc})/time_{toreach}~-~2*a3)/time_{toreach}</math><br>
<math>a5 = ((((pred_{diff}/time_{toreach}~-~track_{vel})/time_{toreach}</math><math>-0.5*track_{acc})/time_{toreach}~-~a3)/time_{toreach}~-~a4)/time_{toreach}</math><br>

=== Next SamplePosition ===
''The position  at the next sample will be:''<br>
''Initial increment  (full displacement):&nbsp;''<math>dp     = a5*T^5 + a4*T^4 + a3*T^3 + 0.5*track_{acc}*T^2 + track_{vel}*T</math><br>

'' The accleration and jerk increments could be computed in this way also.''<br>
'' But they are computed as average values for the time of sample duration, this approach doeas fit more to ''<br>
'' the tools we are using to test this feature, as they all compute acceleration & jerk by numeric differentiation''<br>
'' inside one sample.''<br>
'' 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 ''<br>
'' 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''<br>
'' 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 ''<br>
'' 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.''<br>


'' Initial setting of predicted time and the filter limit flag (if the filter did work - this flag will be on)''
<pre>
limit        = false 
state.satVel = false 
state.satAcc = false  
state.satJrk = false 
</pre>

=== Velocity, Acceleration and Jerk Filter ===

<table style="float:right" border = "1">
<tr><td>
sync_vel defined as a scaled value [[Axystems:MC-Basic:robot.VELOCITYSYNCTRAN|VelSyncTran]] or [[Axystems:MC-Basic:robot.VELOCITYSYNCROT|VelSyncRot]]
</td></tr>
<tr><td>
sync_acc defined as a scaled value [[Axystems:MC-Basic:robot.ACCELERATIONSYNCTRAN|AccSyncTran]] or [[Axystems:MC-Basic:robot.ACCELERATIONSYNCROT|AccSyncRot]]
</td></tr>
<tr><td>
sync_jrk defined as a scaled value [[Axystems:MC-Basic:robot.JERKSYNCTRAN|JrkSyncTran]] or [[Axystems:MC-Basic:robot.JERKSYNCROT|JrkSyncRot]]
</td></tr>
<tr><td>
T is sampling time
</td></tr>

</table>


<math> dp = \begin{cases} sync_{velT} ,  & \mbox{if } dp > sync_{velT} \\ limit = true \\ -sync_{velT}, & \mbox{if }dp < -sync_{velT} \\ limit = true\end{cases}</math> 

==== Setting Initial velocity delta====	
<math>ddp  = dp - dp_0</math> <br>


<math> 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}</math>

==== Setting Initial acceleration delta====	
<math>dddp = ddp - ddp_0</math><br>



<math> 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}</math>

==== New position is: ====	
<math>track_{pos}  = track_{pos} + dp</math>

==== If the filter was activated then: ====
'' if the velocity,accleration or jerk is limited ('''limit = true''') then the velocity computed using polynomials does''<br>
<math> track_{vel}   = dp/T  </math><br>
<math> track_{acc}   = ddp/T^2</math><br>

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

<math> track_{vel}   = 5*a5*T^4  + 4*a4)*T^3  + 3*a3*T^2  + track_{acc}*T + track_{vel}</math><br>
<math> track_{acc}   = 20*a5*T^3 + 12*a4*T^2  + 6*a3*T   + track_{acc}</math><br>

====keep the old values for next sample====
<pre>
ddp0 = ddp; 
dp0  = dp;
</pre>

====System is synchronized if NEW delta's are under certesian thresholds:====
<math>dv_0 = dv</math>; - ''keep the previous dv''<br>
<math>dv = master_{vel} - track_{vel}</math><br>


<table style="float:right" border = "1">
<tr><td>
syncGain defined by the user with: [[MC-Basic:movingFrame.FILTERFACTOR|MF.FilterFactor]]
</td></tr>
</table>



===== Successful tracking completion =====

'''<u>if((fabs(dv) <= syncGain * sync_accT) && (fabs(master_pos - track_pos) <= syncGain * sync_accT2))</u> ''' <br>
'' Synchronization'' <br>
'' Take the middle for an additional smoothing:''
<pre>
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>
</pre>
'''else if( dv0*dv < 0)'''<br>
'' Check against over-oscillation ''<br>
<pre>
if (flops == -1) flops = 0; // ignore the first sample
else	         flops++;
flopstime = 0;
</pre>

===== Over-Oscillation Check =====

<table style="float:right" border = "1">
<tr><td>
maxFlops defined by the user with: [[MC-Basic:movingFrame.MAXFLOPS|MF.MaxFlops]]
</td></tr>
</table>


'''<u>if (flops > maxFlops && (flopstime > maxFlopsTime && fabs(dv) > syncGain * sync_accT2))</u>'''<br>
<pre>
state.tracking 	= MOT_MASTER_OSC; // OScilationss
</pre>
'''else'''
<pre>
flopstime++;
</pre>

== State: Stopping (de-synchronization process) ==

flopstime = flops = 0;
* De-Syncing profile, follow the given profile, ignore the real source
<pre>
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);
</pre>


[[Category:Axystems:Motion:MovingFrame]]