AXY:Element Coordination/One Dimensional Tracking Algorithm
Contents
One Dimensional Tracking Algorithm
This one us used always in simple axis based MF tracking:
The algorithm is based on a state machine:
State: Full Synchronization follow the master
- track ← master
- Check if master velocity or acceleration exceeds max master frame's values if yes return an error
- flopstime = flops = 0;
State: Tracking process
Initial Setup and testing
- Check if master velocity or acceleration exceeds max master frame's values if yes return an error
- Compute differences:
pure delta-position
Pure delta-velocity
Time-to-Reach initial estimation
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
where K is 4 if () else it is 2.
Additionaly we mutliply by a prediction factor - to decrease acc/jerk
(rounding to integer number of samples):
time_to_reach *= PredictionFactor time_to_reach = T*int(time_to_reach/T+1)
Prediction of Randevous point
The predicted position of the randevous (minus T, if it will happen in this sample!)
pred_pos: position prediction assuming constant acceleration
pred_diff:Predicted position difference
pred_vel: velocity prediction
t = (time_to_reach-T); pred_pos = master_pos + t*(master_vel + t*master_acc/2); pred_diff = pred_pos - track_pos; pred_vel = master_vel + t*master_acc;
acceleration will be zero
pred_acc = 0;
Polynomial coefficients of 5-degree polynom connecting the current p,v,a to the randevous p,v,a
- not used, directly written into code
- not used, directly written into code
- not used, directly written into code
Next Sample Position
The position at the next sample will be: dp = ((((a5*T + a4)*T + a3)*T + 0.5*track_acc)*T + track_vel)*T - Initial increment
The accleration and jerk increments could be computed in this way also: .... volatile double ddp = (((5*4*a5*T + 4*3*a4)*T + 3*2*a3)*T + track_acc)*T*T; // a*T*T volatile double dddp = ((5*4*3*a5*T + 4*3*2*a4)*T + 3*2*a3)*T*T*T*T; // j*T*T*T 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, (BMDS...) as they all compute accleration & jerk by numeric differentation 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 accleeration 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 avergae values we observe as jerk/acc exceed over theri max values. So the avergae value approach is not perefect but gives results within the given limits.
T1 = T
limit = false if the filter did work - this flag will be on
state.satVel = state.satAcc = state.satJrk = false;
FILTER
if (dp > sync_velT)
dp = sync_velT; limit = state.satVel = true;
else if (dp < -sync_velT)
dp = -sync_velT; limit = state.satVel = true;
ddp = dp - dp0
if (ddp > sync_accT2)
dp = sync_accT2 + dp0; ddp = sync_accT2; limit = state.satAcc = true;
else if (ddp < -sync_accT2)
dp = -sync_accT2 + dp0; ddp = -sync_accT2; limit = state.satAcc = true;
dddp = ddp - ddp0; if (dddp > sync_jrkT3 )
ddp = sync_jrkT3 + ddp0; dp = ddp + dp0; limit = state.satJrk = true;
else if (dddp < -sync_jrkT3 )
ddp = -sync_jrkT3 + ddp0; dp = ddp + dp0; limit = state.satJrk = true;
track_pos += dp; // new position
if(limit) if the velocity,accleration or jerk is limited then the velocity computed using polynomials does not fit any more, this is observable as PCMD/VCDM missmatch (Issue 2596) </pre> track_vel = dp/T; track_acc = ddp/T/T;
else '' Velocity & Accleration will be computed exactly not using dp and ddp, it has been shown that this'' '' approach is much better - means more stable'' <pre> track_vel = (((5*a5*T1 + 4*a4)*T1 + 3*a3)*T1 + track_acc)*T1 + track_vel; track_acc = ((20*a5*T1 + 12*a4)*T1 + 6*a3)*T1 + track_acc; <pre> ''keep the old values for next sample'' ddp0 = ddp; dp0 = dp; ''System is synchronized if NEW delta's are under certesian thresholds:'' dv0 = dv; // keep the previous dv dv = master_vel - track_vel; if((fabs(dv) <= syncGain * sync_accT) && (fabs(master_pos - track_pos) <= syncGain * sync_accT2)) '' Synchronization'' '' 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
else if( dv0*dv < 0)
if (flops == -1) flops = 0; // ignore the first sample else flops++; flopstime = 0;
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; // position track_vel = master_direction*desync.path.vel; // velocity track_acc = master_direction*desync.path.acc; // acceleration if(desync.path.Status.stop) // if stopped,change the state to OFF SetState(MOT_MASTER_OFF);