From 68fea9edeb6edadeed65b07f55c4dccf6949ba48 Mon Sep 17 00:00:00 2001 From: Andy Lindsay Date: Sun, 15 Oct 2017 21:20:34 -0700 Subject: [PATCH] servo360 - try adding delay in calculated distance during acceleration #149 (step 1) Added as commented code (that does work). Needs either tuning or extra conditions so that it doesn't introduce errors when slowing down to a stop. --- .../Motor/libservo360/cmm/libservo360.a | Bin 46860 -> 46860 bytes .../Motor/libservo360/servo360.c | 39 +++++++++++++++++- .../Motor/libservo360/servo360.h | 14 ++++++- .../Motor/libservo360/z_dev_console.c | 4 +- .../libabdrive360/cmm/libabdrive360.a | Bin 19074 -> 19074 bytes version.txt | 2 +- 6 files changed, 54 insertions(+), 5 deletions(-) diff --git a/Learn/Simple Libraries/Motor/libservo360/cmm/libservo360.a b/Learn/Simple Libraries/Motor/libservo360/cmm/libservo360.a index 4ce17f3972b365a115bec460673244f34e9033cd..cfbd3a44b84f902083be89a142b08d0f4de539ef 100644 GIT binary patch delta 2424 zcmaJ?drVVj6#s6^ymD#jrR{CYqfoIx6>zjb3kF7cI3t3f@rAKKQO2CsB8WOhDNA*8 z4iNpEaZ@*R6ZdewE+lTtA}nsp{@FD1@yE$NHJ;^}gQw3&=&2q+Yo}mL=o&}#YYw#5FYmA9vPSaXf2J1B8hcN||30q{A zC_rQIpR(C8=^A}gB|=Orh!4xsL3}}G0s4o`3UrP<6X+^=8qgm3GeFPFM+qNm19~pD z2d zOP80Si(-jcT#f=D!(8-ADwoT%55a4P&u3Qd23RGo4 zUs5FjJ)l}f8Sy<;EAS2Kxj^4i&j$LXn$&7az0erAZG=2gi?ssTdJyNft|nwV;#^iE z3*0DP&&>e!J}wvN7*`1NFOHOUu14=QQLKw*{W*gOo|cotAx*;i-_E&?`iH1Zx+ide zed(IsA*4}|g46VsT0hflH40LP(l`P!{mD0$(dZXqmt-1%8#lZ<8X_An;xMYbno83r*~!5-t8E zt)4RRRq5|Yf+LM5OnvYm-LvdQM0Lif9Y$pb3k}YfY_P&a@q4V_icdFCq{Cf> zEdbsuJRSzS0FW2o2?Inc3)}6vAlPg_6)CWnuYtO~S57ALhjJ54Ce3-_u>+%NN5?46 z4a-vFx=+hwOv{KBw#^iuQDKk@?U0Wjm2`mH3-eD&LJ-;M1C?uJ(4*5x;n-HE3IAQ0 z8Qe0*B*DQqAb;#Pwk{Osvieap3nR}jSf~j*P9DSdd_*c5x1d zi)QU$c9U1s+!k}1T1=v8TQU-kX3jEza8>#sU$LAH!zayJyl&;E@HL>Xwc0fv&61Ak z8FsBl+tfKdo6Rok_$gb%$RY6Zx;&~1Z>!6o?EGMzFG^^V+LbzNuU`wd9jgz$br76~ zo81~rH_1WBHQf?pBf?f6so}V9D|NP|hKg{t;d!zy$9*61JJ+QM7JHjX(X;Tx`eHDe zv*AhzkB14&Ih$D_~c7x8i?<)~J@ybpi z9sjGUFqnXqyEcO6%e%h#H+ZS z_ubIV2)*@MeCdNt(Adq;=hs?m$##PsZNkOEg#?WDG ztp$o+>)5I-+KPSH`dFA@#&(2`w$nc?o%;Aue+26|jAI>a>8Pz^dp6l6v38mtC+B|O zIp>~p?zuPVyDjUxEgMXH!p7wnaHq}ZsE~$d(hc|l^;;}UP^U<}JB@a+_6*i@aVsT?6H4(P**RG`l*azWlza3B{eGeE9aT0r(Fp9XnOIWFjNHjra+ zg&_Zkdj+INWj3?K^9Yf2qh2L*!3K>j2#t_>M6JsoLexALi%+UJzZWr1Z6#AoEw|1h zrV2o5wgcMNBqi;sDcVe2`9zi6(5` zP@?DPM0yt4q6x;G>R>#y4+_FJ>1UzPrSYq^EHRhJ$H+Dk@;MTp1oU`(J;(%gGRP(B zMadDS*~nG^#rvN$%XS_|-Fk{}vA9>As)BSTw;(i1CLwBvZ5EHI(}_AfrCtj$3N$4k zU)4Mb@_?q6Fyq@AFX#>0B_Q9^E&_Q$E1cB>?J737+o;f@7Oo80dJyB`HV6VVH^>&SQgri-eGx=G9c9V1s4W;sT5cH5G9YVB&lq{K^8O!1P!XFW!3<` ztYaXPTe>vJWI*Q}&=n8p8pz9y0d6Z)V3Cy~RE3bb61*WGBpN^(r8;*tg0TTCKOKc( z7$Pn044yjW0P^?Qm@;4?$9xFpw8(a-X*b|=;hAE_18b<`a$`auRK_C?9YF{FguYKs z4D$9wO;iy}AhZf0Bu_vn9rS8ZUm-)NR@5s%UoYxya)cU0-OIn3{LH+F`)c-NVs9;+ z3ty4?fozWGhnDPE=wKh9?pUppRd>=bvVDlDWEyQ|f~oSGpqkuH6QE}P+QK_wPXnIG z9E6gDafQB#n%^!d!YPQO+`^3@+ZamnB;D{^C7zZPQdDSflFF}-^isf0l}2Jg&CnJdPC&*$`)g|_u)Nbyr7#V9HNwYAc(mgqY!mVYNVPwo(Q z^$9!0hIZX8>ND_-MO^_0^OW6cj{M~0X9u55JXmPN7g{rL1y=@*-@|<`wQsP{fVbOQ z0K9HL9tO7qpe(u>1_Y~2>~Q1(vDI->BC6|*xVW?l&hDd9;eP&Hnhy7qWc<+Z;01M{ zV+7-YCsXIXLn;(h%P0qL&OJV@%q$n%Rb3Yu{QUBW;08Qc{+t4)b)Im5wAaPrzsoa1 z@69n;=m{{RK-_n>?I_;OT}3f8Tz-D#D%~T&SGUiXMj*Cj9cL!sp#&$bn{iT14DA4? z0I#Z=4p(KsbT^b+lLO_p*Nlq9-ex0SR=W}EtVkW=%hr-}xB+jjI}5)JU*O?9oXA!z>bZZV5?&rrIv@V zWw_a+!(@{ZIHu`_l;7kwBQ`dB0G#UWBF?l_5n?L^!o3*?Jk|Rdzw3qMu!jMgxlBCM zS_EE;+b)F(BmUKr!B=>G3PT25*xn35j<&BQX7T;@1w;=vbyR@oiybaG85}8V0bX2d zz&E$G0lcp3kZcZqxTXXKg~4mO%|NwxpN^oG>T$8J5#Xafsj2*ip1YET9X|vMskCpT zj8441ODrkyh9(RqVD;`T5bL$wUq251*#8K4W>9EX%AN_yvS+_x@4l6QmG8?DF}`R2 zf(ROqA1DT1_U=N7&pwzI$!|T}0Q|+n-$sICLjq_xa#jL|h7A17hjWgw#Ma?1sOQ9R z3RwOyd_BSvPkz!4_u-{+V vP4YybCq6&(I4#`UnNz&j==@y#MXMf9@Ld-1k@~+U$)v)?Z5{mNNTKW>8b636 diff --git a/Learn/Simple Libraries/Motor/libservo360/servo360.c b/Learn/Simple Libraries/Motor/libservo360/servo360.c index c148ba58..50d8017e 100644 --- a/Learn/Simple Libraries/Motor/libservo360/servo360.c +++ b/Learn/Simple Libraries/Motor/libservo360/servo360.c @@ -487,11 +487,48 @@ int servo360_pidV(int p) int opMax = _fs[p].speedLimit; _fs[p].speedMeasured = (_fs[p].angle - _fs[p].angleP) * 50; + if(abs(_fs[p].angleError) < S360_UNITS_ENCODER/2) { + + + + // _fs[p].angleDeltaCalc = _fs[p].speedTarget / S360_CS_HZ; _fs[p].angleCalc += _fs[p].angleDeltaCalc; - } + // + + + + /* + // This could remedy the overshoot problem, but it seems to reduce + // drive_goto accuracy. + if(_fs[p].speedTarget == _fs[p].speedTargetP) + { + _fs[p].offset--; + if(_fs[p].offset < 0) _fs[p].offset = 0; + } + else + { + _fs[p].offset++; + if(_fs[p].offset > FB360_OFFSET_MAX) _fs[p].offset = FB360_OFFSET_MAX; + } + + int idx = _fb360c.pulseCount % FB360_V_ARRAY; + _fs[p].vT[idx] = _fs[p].speedTarget; + + idx -= _fs[p].offset; + if(idx < 0) idx += FB360_V_ARRAY; + + _fs[p].speedTargetTemp = _fs[p].vT[_fs[p].offset]; + + _fs[p].angleDeltaCalc = _fs[p].speedTargetTemp / S360_CS_HZ; + _fs[p].angleCalc += _fs[p].angleDeltaCalc; + */ + + + + } _fs[p].angleError = _fs[p].angleCalc - _fs[p].angle; diff --git a/Learn/Simple Libraries/Motor/libservo360/servo360.h b/Learn/Simple Libraries/Motor/libservo360/servo360.h index a6bea00a..7280f5d0 100644 --- a/Learn/Simple Libraries/Motor/libservo360/servo360.h +++ b/Learn/Simple Libraries/Motor/libservo360/servo360.h @@ -289,7 +289,7 @@ typedef volatile struct servo360_s volatile int speedOut; volatile int pw; - // speed control system + // velocity control system volatile int opPidV; volatile int approachFlag; volatile int ticksDiff; @@ -314,6 +314,18 @@ typedef volatile struct servo360_s volatile int stepDir; volatile int lag; volatile int accelerating; + volatile int speedTargetTemp; + + /* + // This could remedy the overshoot problem, but it seems to reduce + // drive_goto accuracy. + + #define FB360_OFFSET_MAX 5 + #define FB360_V_ARRAY 8 + + volatile int vT[FB360_V_ARRAY]; + volatile int offset; + */ // position control system volatile int Kp; diff --git a/Learn/Simple Libraries/Motor/libservo360/z_dev_console.c b/Learn/Simple Libraries/Motor/libservo360/z_dev_console.c index 5ee08c5e..028f0eb2 100644 --- a/Learn/Simple Libraries/Motor/libservo360/z_dev_console.c +++ b/Learn/Simple Libraries/Motor/libservo360/z_dev_console.c @@ -255,8 +255,8 @@ void console() fbt[p].pV, fbt[p].iV, fbt[p].dV, fbt[p].drive, fbt[p].opV, fbt[p].opPidV); */ - dprint(term, "spR: %d, spT: %d, spM: %d\r", - fbt[p].speedReq, fbt[p].speedTarget, fbt[p].speedMeasured); + dprint(term, "spR: %d, spT: %d, spTT: %d, spM: %d\r", + fbt[p].speedReq, fbt[p].speedTarget, _fs[p].speedTargetTemp, fbt[p].speedMeasured); dprint(term, "aC: %d, aM: %d, aE: %d\r", fbt[p].angleCalc, fbt[p].angle, fbt[p].angleError); dprint(term, "pV: %d, iV: %d, dV: %d\r", diff --git a/Learn/Simple Libraries/Robotics/ActivityBot360/libabdrive360/cmm/libabdrive360.a b/Learn/Simple Libraries/Robotics/ActivityBot360/libabdrive360/cmm/libabdrive360.a index eeda4c6ad3cbf5e1c25e7cfa5eb0be7fd4b94464..dc1bfd5be1aaaf085c44b1bb840a6506cd8b810a 100644 GIT binary patch delta 324 zcmZpg%GflOae_3vg`ts=k%{?6C075+3d&yliwNZY!