16 volatile char abd_str[128];
21 #ifdef test_t_interval
22 volatile int rec_t[8000 / 4];
25 volatile int abd_sampleCount = 0;
30 int abd_abs(
int value)
32 if(value < 0) value = -value;
36 void drive_com(
int arrayLcnt,
int arrayRcnt,
37 int centerL,
int centerR,
38 short *pwAddrL,
short *pwAddrR,
39 short *spdAddrL,
short *spdAddrR);
41 void abd_encoders(
void *par);
43 void interpolation_table_setup();
47 void set_drive_speed(
int left,
int right);
48 void interpolate2(
int *ltmp,
int *rtmp);
50 void abd_sample(
void);
54 volatile unsigned int _servoPulseReps;
55 volatile int abd_dsr = 800;
56 volatile int abd_zeroDelay =
ON;
58 volatile int abd_intTabSetup = 0;
60 volatile int abd_cog = 0;
61 unsigned int abd_stack[44 + 128];
72 static volatile int kp[6];
74 static volatile int ridx = 0;
76 static volatile short *pwL;
77 static volatile short *pwR;
78 static volatile short *spdL;
79 static volatile short *spdR;
81 static volatile int pcount;
82 static volatile unsigned int _sprOld;
84 static volatile int phs[2];
86 static volatile int phsr[2] = {0, 0};
88 volatile int encoderFeedback = 1;
90 volatile int abd_blockGoto = 1;
92 volatile int abd_speedOld[2];
93 volatile int abd_stopCtr[2];
97 volatile int abd_ticks[2]= {0, 0};
99 volatile int abd_speedT[2] = {0, 0};
101 volatile int abd_speed[2];
104 volatile int abd_ticksi[2];
107 volatile int abd_ticksf[2];
110 volatile int abd_gotoFlag[2] = {0, 0};
118 volatile int abd_ticksGuard[2] = {0, 0};
121 volatile int abd_dc[2];
124 volatile int abd_dca[2] = {0, 0};
127 volatile int abd_ed[2];
130 volatile int abd_p[2];
133 volatile int abd_i[2];
136 volatile int abd_ea[2] = {0, 0};
140 volatile int abd_sPin[2] = {12, 13};
143 volatile int abd_ePin[2] = {14, 15};
146 volatile int abd_elCnt[2];
147 volatile int abd_cntrIdx[2];
151 static volatile int cntrVal[2];
152 static volatile int ti[2];
153 static volatile int state[2];
154 static volatile int stateNow[2];
155 static volatile int statePrev[2];
157 static volatile int ssi[2];
158 static volatile int drive[2];
160 static volatile int speedPrev[2] = {0, 0};
161 volatile int abd_nudgeCtr[2];
162 volatile int abd_nudgeInc[2];
163 volatile int abd_distError[2];
165 volatile int sign[2];
166 volatile int abd_dist[2];
168 volatile int abd_ditherA[2];
169 volatile int abd_ditherAa[2];
170 volatile int abd_ditherAd[2];
171 volatile int abd_ditherAp[2];
173 volatile int abd_ditherV[2];
174 volatile int abd_ditherVa[2];
175 volatile int abd_ditherVd[2];
176 volatile int abd_ditherVp[2];
178 volatile int abd_speedi[2];
179 volatile int abd_speedd[2];
180 volatile int abd_dvFlag[2] = {0, 0};
182 volatile int abd_edMax = 10;
186 volatile int abd_zdir[2] = {0, 0};
187 volatile unsigned int abd_tdst;
188 volatile unsigned int abd_td;
191 void drive_com(
int arrayLcnt,
int arrayRcnt,
192 int centerL,
int centerR,
193 short* pwAddrL,
short* pwAddrR,
194 short* spdAddrL,
short* spdAddrR)
196 abd_elCnt[ABD_L] = arrayLcnt;
197 abd_elCnt[ABD_R] = arrayRcnt;
198 abd_cntrIdx[ABD_L] = centerL;
199 abd_cntrIdx[ABD_R] = centerR;
204 cntrVal[ABD_L] = pwAddrL[abd_cntrIdx[ABD_L]];
205 cntrVal[ABD_R] = pwAddrR[abd_cntrIdx[ABD_R]];
209 void interpolation_table_setup()
211 if(!abd_us) abd_us =
CLKFREQ/1000000;
213 unsigned char str[12];
223 for(r = 0; r < cntL; r++)
226 abd_spdrL[r] = (short)
ee_getInt(abd_eeAddr);
229 abd_spdmL[r] = (short)
ee_getInt(abd_eeAddr);
232 abd_spdmL[cntL - 1] = 1000;
241 for(r = 0; r < cntR; r++)
243 abd_spdrR[r] = (short)
ee_getInt(abd_eeAddr);
245 abd_spdmR[r] = (short)
ee_getInt(abd_eeAddr);
248 abd_spdmR[cntR - 1] = 1000;
251 drive_com(cntL, cntR, zstartL, zstartR, abd_spdrL, abd_spdrR, abd_spdmL, abd_spdmR);
254 unsigned char pinInfo[16];
256 for(
int i = 0; i < 16; i++)
259 if(pinInfo[0] ==
's' && pinInfo[1] ==
'p' && pinInfo[2] ==
'L' && pinInfo[5] ==
'R')
261 abd_sPin[ABD_L] = (int) pinInfo[3];
262 abd_sPin[ABD_R] = (int) pinInfo[6];
265 if(pinInfo[8] ==
'e' && pinInfo[9] ==
'p' && pinInfo[10] ==
'L' && pinInfo[13] ==
'R')
267 abd_ePin[ABD_L] = (int) pinInfo[11];
268 abd_ePin[ABD_R] = (int) pinInfo[14];
275 void interpolate2(
int *ltmp,
int *rtmp)
278 short left = (short) *ltmp;
279 short right = (short) *rtmp;
290 limit = abd_elCnt[ABD_L];
300 int rprev = abd_cntrIdx[ABD_L];
302 if((
CNT - abd_tdst) >= abd_td) abd_sample();
304 for(
int r = abd_cntrIdx[ABD_L]; r != limit; r+=listep)
306 if(spdL[r] == lookupval)
311 if((spdL[rprev] < lookupval) && (spdL[r] > lookupval))
313 short x = ((pwL[r]-pwL[rprev])*(lookupval-spdL[rprev]))/(spdL[r]-spdL[rprev]);
314 left = pwL[rprev] +
x;
323 limit = abd_elCnt[ABD_L];
333 rprev = abd_cntrIdx[ABD_R];
335 if((
CNT - abd_tdst) >= abd_td) abd_sample();
337 for(
int r = abd_cntrIdx[ABD_R]; r != limit; r+=listep)
339 if(spdR[r] == lookupval)
344 if((spdR[rprev] < lookupval) && (spdR[r] > lookupval))
346 short x = ((pwR[r]-pwR[rprev])*(lookupval-spdR[rprev]))/(spdR[r]-spdR[rprev]);
347 right = pwR[rprev] +
x;
353 if((
CNT - abd_tdst) >= abd_td) abd_sample();
360 void abd_encoders(
void *par)
365 OUTA &= ~(1 << abd_sPin[ABD_L]);
366 OUTA &= ~(1 << abd_sPin[ABD_R]);
367 DIRA |= 1 << abd_sPin[ABD_L];
368 DIRA |= 1 << abd_sPin[ABD_R];
373 int temp[2] = {0, 0};
374 interpolate2(&temp[ABD_L], &temp[ABD_R]);
380 CTRA = abd_sPin[ABD_L] | (4 << 26);
381 CTRB = abd_sPin[ABD_R] | (4 << 26);
383 phs[ABD_L] = (1500 + temp[ABD_L]);
384 phs[ABD_R] = (1500 - temp[ABD_R]);
386 phsr[ABD_L] = phs[ABD_L];
387 phsr[ABD_R] = phs[ABD_R];
393 phs[ABD_L] = phsr[ABD_L];
394 phs[ABD_R] = phsr[ABD_R];
395 PHSA = -phs[ABD_L]*abd_us;
396 PHSB = -phs[ABD_R]*abd_us;
402 PHSA = -phs[ABD_L]*abd_us;
403 PHSB = -phs[ABD_R]*abd_us;
407 stateNow[ABD_L] = (
INA >> abd_ePin[ABD_L]) & 1;
408 statePrev[ABD_L] = stateNow[ABD_L];
409 state[ABD_L] = stateNow[ABD_L];
410 stateNow[ABD_R] = (
INA >> abd_ePin[ABD_R]) & 1;
411 statePrev[ABD_R] = stateNow[ABD_R];
412 state[ABD_R] = stateNow[ABD_R];
414 while(!_servoPulseReps);
417 abd_td =
CNT + abd_tdst;
418 _sprOld = _servoPulseReps;
432 #ifdef test_t_interval
440 if((
CNT - abd_tdst) >= abd_td) abd_sample();
443 if((
CNT - t) >= (dt1 + dt2))
446 _sprOld = _servoPulseReps;
449 for(
int lr = 0; lr <= 1; lr++)
451 if((
CNT - abd_tdst) >= abd_td) abd_sample();
455 if((abd_gotoFlag[ABD_L] != 0) || (abd_gotoFlag[ABD_R] != 0)
456 || (abd_dvFlag[ABD_L] != 0) || (abd_dvFlag[ABD_R] != 0))
458 abd_ditherAa[lr] += abd_ditherA[lr];
459 abd_ditherAd[lr] = (abd_ditherAa[lr]/50) - (abd_ditherAp[lr]/50);
460 abd_ditherAp[lr] = abd_ditherAa[lr];
463 if((abd_gotoFlag[ABD_L] != 0) || (abd_gotoFlag[ABD_R] != 0))
465 abd_ditherVa[lr] += abd_ditherV[lr];
466 abd_ditherVd[lr] = (abd_ditherVa[lr]/50) - (abd_ditherVp[lr]/50);
467 abd_ditherVp[lr] = abd_ditherVa[lr];
471 if(abd_gotoFlag[lr] == 1)
474 if(abd_speed[lr] != 0)
476 abd_ticksGuard[lr] = (abd_speed[lr] * abd_abs(abd_speed[lr])) / (100 * abd_rampStep[lr]);
478 if(abd_abs(abd_ticksf[lr] - abd_ticks[lr]) > ((abd_abs(abd_ticksGuard[lr]) + abd_abs(abd_ditherVd[lr]))))
480 if(abd_ticksf[lr] > abd_ticks[lr])
482 abd_speedT[lr] = abd_speedLimit[lr] + abd_ditherVd[lr];
484 else if(abd_ticksf[lr] < abd_ticks[lr])
486 abd_speedT[lr] = -abd_speedLimit[lr] - abd_ditherVd[lr];
491 if((
CNT - abd_tdst) >= abd_td) abd_sample();
493 abd_gotoFlag[lr] = 2;
497 if((
CNT - abd_tdst) >= abd_td) abd_sample();
501 if((((abd_gotoFlag[lr] == 2) && (abd_stopCtr[lr] == 0) && (abd_speed[lr] == 0) && (abd_speedOld[lr] == abd_speed[lr])))
502 || (((abd_gotoFlag[lr] == 3) && (abd_stopCtr[lr] == 0))))
504 if((
CNT - abd_tdst) >= abd_td) abd_sample();
506 abd_distError[lr] = abd_ticksf[lr] - abd_ticks[lr];
507 if(abd_distError[lr] == 0)
509 abd_gotoFlag[lr] = 0;
510 abd_nudgeCtr[lr] = 0;
516 sign[lr] = abd_abs(abd_distError[lr]) / abd_distError[lr];
518 abd_gotoFlag[lr] = 3;
523 if(abd_speedT[lr] > abd_speedLimit[ABD_B]) abd_speedT[lr] = abd_speedLimit[ABD_B];
524 if(abd_speedT[lr] < -abd_speedLimit[ABD_B]) abd_speedT[lr] = -abd_speedLimit[ABD_B];
526 if((
CNT - abd_tdst) >= abd_td) abd_sample();
530 if((abd_stopCtr[lr] == 0) && (abd_gotoFlag[lr] != 3))
532 if(abd_speedT[lr] > (abd_speed[lr] + abd_rampStep[lr]))
534 abd_speed[lr] = abd_speed[lr] + abd_rampStep[lr] + abd_ditherAd[lr];
537 else if(abd_speedT[lr] < (abd_speed[lr] - abd_rampStep[lr]))
539 abd_speed[lr] = abd_speed[lr] - abd_rampStep[lr] - abd_ditherAd[lr];
541 else abd_speed[lr] = abd_speedT[lr];
554 if( (abd_speedOld[lr] != abd_speed[lr]) && (abd_speed[lr] == 0) )
556 abd_stopCtr[lr] = abd_stopPulseReps[lr];
559 abd_speedOld[lr] = abd_speed[lr];
560 temp[lr] = abd_speed[lr];
564 if((
CNT - abd_tdst) >= abd_td) abd_sample();
566 interpolate2(&temp[ABD_L], &temp[ABD_R]);
568 if((
CNT - abd_tdst) >= abd_td) abd_sample();
570 ssi[ABD_L] = temp[ABD_L];
571 ssi[ABD_R] = -temp[ABD_R];
575 for(
int lr = ABD_L; lr <= ABD_R; lr++)
578 if((
CNT - abd_tdst) >= abd_td) abd_sample();
589 abd_ed[lr] = abd_dc[lr] - abd_ticks[lr];
592 if(abd_ed[lr] > abd_edMax)
594 abd_ed[lr] = abd_edMax;
595 abd_dc[lr] = abd_ticks[lr] + abd_edMax;
596 abd_dca[lr] = abd_dc[lr] * abd_dsr;
598 else if(abd_ed[lr] < -abd_edMax)
600 abd_ed[lr] = -abd_edMax;
601 abd_dc[lr] = abd_ticks[lr] - abd_edMax;
602 abd_dca[lr] = abd_dc[lr] * abd_dsr;
607 abd_ea[lr] += abd_ed[lr];
611 if(abd_speed[lr] != 0)
614 if(abd_speed[lr] > 0)
616 abd_p[lr] = abd_ed[lr] * (3+(abd_speed[lr]/10));
617 if((abd_ed[lr]>0) && (abd_ed[lr] != abd_edMax))abd_i[lr]+=1;
else if((abd_ed[lr]<0) && (abd_ed[lr] != -abd_edMax)) abd_i[lr]-=1;
619 else if(abd_speed[lr] < 0)
621 abd_p[lr] = abd_ed[lr] * (-3+(abd_speed[lr]/10));
622 if((abd_ed[lr]>0) && (abd_ed[lr] != abd_edMax))abd_i[lr]-=1;
else if((abd_ed[lr]<0) && (abd_ed[lr] != -abd_edMax)) abd_i[lr]+=1;
625 if((
CNT - abd_tdst) >= abd_td) abd_sample();
627 maxIL = abd_speed[lr];
628 if(maxIL < 0) maxIL = -maxIL;
629 if(abd_i[lr] > maxIL) abd_i[lr] = maxIL;
630 if(abd_i[lr] < -maxIL) abd_i[lr] = -maxIL;
633 if(lr == ABD_L) {tsign = 1;}
else {tsign = -1;}
635 if(abd_speed[lr] > 0)
636 drive[lr] = (tsign * abd_i[lr]) + (tsign * abd_p[lr]) + ssi[lr] + 1500;
637 if(abd_speed[lr] < 0)
638 drive[lr] = (tsign * (-abd_i[lr])) - (tsign * abd_p[lr]) + ssi[lr] + 1500;
642 drive[lr] = ssi[lr] + 1500;
646 phsr[lr] = drive[lr];
650 phsr[lr] = ssi[lr] + 1500;
655 if(abd_speed[lr] != 0)
659 PHSA = -phs[lr]*abd_us;
663 PHSB = -phs[lr]*abd_us;
676 if(abd_stopCtr[lr] > 0)
678 if(abd_stopCtr[lr] == 0)
683 abd_dc[lr] = abd_ticks[lr];
685 abd_dca[lr] = abd_dc[lr] * abd_dsr;
697 void abd_sample(
void)
702 for(
int rl = ABD_L; rl <= ABD_R; rl++)
704 stateNow[rl] = ((
INA >> abd_ePin[rl]) & 1);
705 if( (stateNow[rl] == statePrev[rl]) && (state[rl] != stateNow[rl]) )
707 state[rl] = stateNow[rl];
708 statePrev[rl] = stateNow[rl];
710 if( ((rl == ABD_L) && (
PHSA == 0)) || ((rl == ABD_R) && (
PHSB == 0)) )
712 abd_ticks[rl] += abd_zdir[rl];
715 else if( ( (rl == ABD_L) && (phs[rl] > (cntrVal[rl] + 1500)) ) || ((rl == ABD_R) && (phs[rl] < (1500 - cntrVal[rl]))) )
721 else if( ( (rl == ABD_L) && (phs[rl] < (cntrVal[rl] + 1500)) ) || ((rl == ABD_R) && (phs[rl] > (1500 - cntrVal[rl]))) )
728 abd_ticks[rl] += abd_zdir[rl];
734 statePrev[rl] = stateNow[rl];
740 #ifdef test_t_interval
741 if( (n*4) < (
sizeof(rec_t)-8) )
755 abd_dca[ABD_L] += abd_speed[ABD_L];
756 abd_dca[ABD_R] += abd_speed[ABD_R];
759 abd_dc[ABD_L] = abd_dca[ABD_L]/abd_dsr;
760 abd_dc[ABD_R] = abd_dca[ABD_R]/abd_dsr;