From dff21b0d52e2ab0fb0245759d37388d602931d29 Mon Sep 17 00:00:00 2001 From: Nischay Joshi Date: Sat, 13 Apr 2024 13:17:49 -0700 Subject: [PATCH 01/14] Init --- .vscode/settings.json | 9 +++++ .../FS90R Servo Characterization.xlsx | Bin 0 -> 16372 bytes python/measureGpsRate.py | 33 ++++++++++++++++++ 3 files changed, 42 insertions(+) create mode 100644 .vscode/settings.json create mode 100644 Servo_Control_Theory/FS90R Servo Characterization.xlsx create mode 100644 python/measureGpsRate.py diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..a658815 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,9 @@ +{ + "files.associations": { + "*.tcc": "cpp", + "unordered_map": "cpp", + "numeric": "cpp", + "ostream": "cpp", + "cstdint": "cpp" + } +} \ No newline at end of file diff --git a/Servo_Control_Theory/FS90R Servo Characterization.xlsx b/Servo_Control_Theory/FS90R Servo Characterization.xlsx new file mode 100644 index 0000000000000000000000000000000000000000..71c8b607ead1a81a6e32a4287b3eed5854f8d018 GIT binary patch literal 16372 zcmeHu1zTLnwl>x{!QEYhYjAh>5Q4ij?(R;|;1)DU2=4Cg?oNUPg6r3lnR7CSGxzy^ zz`YxuE_T;mZ&Ryl)slKkNfr_c0}L7r77PrG6wFchcDEKB3``6L3=AC%7D7i9XzOHR z>!hdVZfD}C%jjlfO_B!%L6ZXp0eb%buK&dvC`%eu_|A+He66`fNCs5i?t;iIE!6!{ z4Mnj59>s4{_NHzZH^(mluJnq6LeBaT_t;0BlLh7E!s>G>7%Z%Kcmq}nAMVT(h0WQ zBD2E(Y@F2(@BwNy^`r>lmVp=Bo5&@HAL52_vtT$-v-x*#=jdusN}3vJEr;!A+#v_K z?CqB|HAt?-7$adQ(oF_ha=SJc9>=v=4lhP54-Bop`CU?s;=-l<3}ovmlkO zhjnr${LYBv=cN;Zb|Fkyjf3MCC&FQRq zd)W>u(u%G1eb{h=T*g|Kh$TZ1CPP@4chRiAqdJeXcj%kQl7=36RX1X`ae)8^_WTS9 zrt}}yUaQVbegT@oav)TpRs~0)MrgngB%&FA!4Fx7FuiXnB<{`r82M)fRhMBpL=U zd7W!{$j3W-XBavvhhz!+vdtb;m)XnNt5iuDcN&-0Sh~`N;#}Fmb#jU6GqJBImRzNaEr(kmLJrBMC&RGpvII17m>$149OB#?6|^73g4P2n1UF;SS5x zm4RE_C_aLRPojy}GcHmPVg>Xf0Tn9EpZ)4w6$dB^wdA)ljxm&KpS}Cxa*rL}d|fRs zCS2YgUmx)@((_rwY`pBs-^wFc+<{oeqF0SOJGU@iXDS@)s7ov~k#JP5ifc6+dUCkj zC;q5Inyo3qrvs++Ij(iG2_9ADFa%RgH!LElvI?sz2QTCtu0)14tpv%aX&^4r-Vc6A zwpko@mj}nNb&obu{4I9b%?J18@GZtHDvrS6*=dP6&Y$6m_lW4W0!5PjB}X3#AoeDEr+je6oaGBuuRm>Ui>`=qXiL!Oo2wbSga}&c3?y~&#KD(E>EoW4 zOIE86r{OsGu`&O}vZ6D~8j5Bm?W?a9BQ6Hk0fJX>av`a13GZhvjn_Xp1K?#U-WSk1 zM?3~EDGkzf4r{nz21z8cOHzh@&i8aJamoZ=r99Fq#BesrpYUeaUW?r)@vaZu<52Ho zp>+t}V?K5whneGNH#-++Kz5XcP59t>QGs`0arh|4KQRM^E1bA2RkJQAK>$J4t?<+J z?b{#TgzG|U`xgK;u(!jnKK5|ryH|Q0Q*QfgU4BJxV6^93kIo!GxKr4H78VK_Bsrv9 z??Au}D#@u`BSDEWuY)z zjZ19HNRNv#Zw~-?MI5?pdKPEq0l7qAh%C3%*pB171ZK2#Jt|Pl(QR$qzUAiEYW|Ja z2ifcTuCoWX*(j`^xy$|oR6-CUzQh4x!CoD-r93c6K6rE#Nnt~qxiy^8Yj`4q^b6aR z@t5V~DZbzcR$U`*iUmSEgt}=i87}^?R7n1^D zUS$lo7-B_FpX{E5p6s?bj*~Bf?%PO8)#=K*-8TA+0j30LE*>fT3Kh%YqFDkMy4;cS zE6B~y<94ss_K)PY54%%C628<&F0FS^ThN z4>O|pb?{T*WSc#!w^E3`>7+`EO}dYtT_U%fA&idq&Z(8ko579`wpA=rUDrh6&C9;7 zcxIT!((nOgE+$4+fYaXLQSz`3b6M+5N&qBaVSiq;S<^CZyGTDxAgtIh5Tbhp zSb(^s6iRB(zsOyZar(jN&}#9MfT6rYLLu_hI0uQ~G)Ab3zhGiLNkne!*t1phXG?Z| z-p9$fT?8!F9}|55G5#?qL9&gCjE_XjbV*}NpbuSm~a?Wz8U-Qn|d{xZE{38d?1AH?y=;ODGa zFD{3Nh{$CvRxlc{kVYry)ej7=o>nhtb~MxzEfHX&hm;B~8J%^->4v^K^u%!56=B31 zA*nXn&nl?A!$JMB+VKM)LSE@C-q*J&qemEQ2k$5 zPFy)ryhC}l#K1;1%P;@XH6mA*$=Sj-zhd>HxTdHO&=N2&BWj-WsboX`90g# zpL#MIQr<~j-xCL&k!wytS|PZJ1Ax^#{n#3I!NI^FN^GtBCgUbE@erTjPSz3Z;g0va z2?d1Fd$RYFk&vEou?f2rQhT5JAGp%w5uh<`T}`Clnwp;!Df)!1EY7e`a7n~|Gqtv8 z@yX?&OcOyP))oxjV1lZz#qZ~uj>^{7pdO|z_qH{wq?&1rF#rYfEjOInNG^ExRE z55=yQ+e%RCK-7!%Yye)jlg8;`Kz73-@+b-G!Lxp?Wc*EW)}-)}a~#`wt2FmrJqn#k z5Xu+0etQDS?Pr=#3UFUFp~pxlxmkHgU51#6j5!a3P&^G-rMSO=+RdDKctO!^48t#x z8E!klPwU?|(;S|D#UsCFTmHvo+htG~rbo%_G3k=Rpo~TfkoI$y&X|Xtf?Ad5q7bid zeWb}Y9iqcRXs>HVr?cWipa*=gI;`p)UVa1V zb1j>?ZE^eH>DiV!_T@nZ-llr=8-`(9P>!={6#(D%;mZ4y#_hdnQnqDmQQxN;xNz^E z^SNL(L_A;0bh9$Qw%EYFHEXKoTYSE6&J8{BJPIW$JXJ>Iw_e|7z6c}m(B8^m5LGJH z6FQoPq9<3YUG!Fw2EUs0SzPHR7g9jJeSAf%aZ3rC2WBRndR5|@ts0j{U)3+UYwV*( zH%sqW3U{zLX4`Piyq-p@y)Qv{Zl<^}Yx)*z6jq zn)gG|KO9=Jva5S9y|m3d-=1K)ZXDwSGs_}V@Onrv;_ZXRP~5_)7Dam#eT0YeCik%x zra!bnkS9`oNt)^7&dG;)#d5iMhg`$QewQKmojMpCJgZCsSf;u`@zKQ?;iuZ|kD$en zoH~ZDYr8j#>U~X(!P1_*X`b%t1p}h4__+muqQUsNvA0r_ic-a8iC}?UtYF=OL#H)g zjGzP6g(wYz#G_Tv=0h?>BR?TN5`>Tz8)%&rp9HO`&%x&rT)5Q4u!wDDD*GL?CAXKU zu#hUVu^XhjVmpV)5(P?hNtF-i{|r(#ml?~|kf>cydxK`_*gcvrnrlQ%UL<7K+v>RD zITWZIF2Pcu{E6HK%Hue!OdTy>fy7V~`E*Y(UUO z(C5+DQZj{B{Q~eR2=Yt0YOmO(xfc3V@sOlyx^!5f-9uPpWI{%B6Z)W}KEZa?T?E( z5rC_EcE`Li@xc*?9iBnX8bNY8e}F^ScboY3Ceen7RKB^09p0&eI@s2*a^;%QDo;{R zQxp@Ky=Y3Fk-WkzB+{{``lqB}Qo z;Ay}!QN(q1;<1^eyCWr?81jD1PDrJh9s?YHe@50H!eUa^f6ap?0@~aUx^X9#MW0;$ zIgHOiB5gAwqV-8A-q4kaZRV8!X|J`vpDWh!I-xWr7lDjoS9%`dc~Tp zYFlPmRAtjR23JvPUI+cq+u?LO_GAkZGcJYXgW?YOiAS8up>Qpo8FSPwVOBhpkB%Lx z5un~sI}^bR&cjFp$-7-Unnj*3aRX;Y;4Q;JR8F|Y;p#FCF-wDd;tV7k)mV+Cp)}6X z1K$K}V=^Sw1i(}mc`xn(0imy!F=?q|K&Oj*wegp(%utm?@tqzcI$fg$-9pGQZuaiYN;{|=aC2Ih}Rn| z`(@+7(_qM#?Kf-hur>0nU5nrE?Q(6ms%DN8-~ zjlS~$>!j49xxf|omdm6{ohZSS3Z5X1?qssU7PhN3N#8Ci9#8#V61vkR z(N)zkBJ3lQV$yf-R$zCe!qdq@kN*H$Iv{>-{o2%IMT+jnMLctigd|UJT{%f8` z(vY4#r+XHieohq{kCQ3)ARZ?2&<8J+O>Sq0LTR-=AG&c;F_t&GLjb|OEBMLl4Va80 z=WA6N2G@20R6c(w%|bP(uqP8rg0>dLhE}3^YyWWi;R`WqlliQiv4evz$r|Rws9irO zv(N*wyuRsA?3bAQ47dn{tr{Y=2h2fQ_kV6v3=3kFp66($Jr9$iHy$5|Kiza}X-jsS z;|~A2P;?8`VjFy-)?w(RFj5z>|e_Udx%TLZj51QZq<ox45+LOsU907>U8usC9jEJMVXhFN!{U__mXQl>GeE)JLTQO!ZV z-a?>!%Sd2+DYLMB$BZgb3v=5C5>IiFyXLc6h;7}u@F-ApwNf`t<9mo-aw}Z;lrd*` zTA-K}+YKICljD_-mJ2cSOPwzro?HR-mlgBHW>GohFdTR9?r9w_`F1;CsjxaH7X(@XS*$U==$qxhS_(b?=JC8+%CxE^wOb1o&+3mOyJ$BO|T)x+Y|fM(3Gp#`hU&#zmO76__sgXOIz#dUOT6lR>uZB6H14QDOo z8yL|xEY*_L3<5%?cz8!f9hi1f>60{I6%W?%l~!~II)G8cKn>%K9d9w*b$QKzF|)A>~LSMc$&hJHFHdHV#a8$2U{ zfq{Pi$om|f+^tO<|48;mG?#6+IZ?csCVWYtE|e#G<26|sn6@Y&uoj>ZC;)ddvWfBx zOc3H`yI7CcuV@EV;+^N6xoq^T9k|)}*vz=|lTUTVCeW3UlE@qb=+}kCFskwm(|OwY z_H4IuB#e{|sQ`l+kQN!&rNq8koOc}q-7zT99l_?TW<}1eKUxYGP-D{Rwb8-plhg>T zQa+c@&qQogu}EaN=bn!9$CTK;K}L9=u^nV|>8{rtyu&f)B#Ju=^L<#jMaydezT`&; z_zY*5o&S=4g~-fj19W=tTI8bO_~^0=%$rMN^~r#ikaUld(8x)2x3&>g6i=TUhAR!- z1KM*8_q-4Rv)Z!!2_(9m_Xi`XkV**wy=J8CVJi&a-K}$$iw_}L%#hL>ZlxxqscKS? z&L4Q_QBx8Wwwvavgs-(|Z+j0kQdC_I=0wOV6R(FdAiyOKaG447pT|wxgs(W`M)eBL z^j44V9qU*|UWuRAO)kE*$kC!P>c5MYFA31E4GOFXN8^!G67=DlQ7VS;P|>u~IK^Mm z2}Tyi$p%hHNyr8odb05oWro7hs2=TuaUUXESSpnHUNXv0Q>0K!$4gX`L>!$hT`a3A z$0sM+Rkd>k^z7|ep)Pj#rADh!uAqqm;YjH-&1;SGrp?tFYp_!CPw%qF-eYyX`u7~N(YhqnWc2Dv!!ljR#db2-zvJ}N2+BFkp}HpWFLP~-uNzJdDjfd@9IZmJ5r zA=Eo*A(z*K;x?D^$)sieyJkEgE3R`NAY{uLcSj#rN1@Y>SMA^zd$=&RwS*1zU&Nh|L>MHLZ2Q;Em(3SfjL54!depY;g@GDtKTGoI?K$42hRGF0qt^d*F8OCNveh~DG2Pq<}p`N<6n z+@HK-9NUJwy6NI-Jp~9~JL`A!Bgpd@s3E>XLXS70xb~EpsXI_uZ6adDN zpyM+#N}+w>L}9hO#KmzrLjWQX-;>_c!)ENV_YTyl8YR}r=xiOE5MLsS`_bt4;pyE> zaOEAp66VXjgr1{^+f;OA^Ja$DRJDCiE0YwOPu9b7D^$P&(;2Go<|Y+E!c=6VgYR8e zqepQTJCv~I)>iPexQVbmZYI~HFBBfL`o+dRyI$dEY=DctGnNl+p~7S9l*6=Co|!}8 z6zT3=3gKgfqaTpN&{t(y@LeD(xx6Dk7D`;Y-ndOnE`NAqzP$W^{f~>MxO%_{9Y|Xf zQ2#EP89Nxb{-F-0zyA2MXr`tMOlQXM&aQdZzs>aP%*6nF_Ucfxu)ME)1RGBAgK{x$ zmj3+t*}IO#X3lwP?&FRF)Bd~r3{UOVjDB!}RJGO(fLJ6fL|=)yM;p_|II;5w;T-l& zBwQ#nfR`csTB2Ti*;w?5q9wII=>{EFEa0d{V8@`?yk8;@YpbK7q)sG8EM2k@8DCl< z#Dk!VJ7P+Skb*q=8_sld2?Rg!E!47<$17P^R&O*NPDCpOUeY2p`d)5vHAIst_+upN z12C!v;4+#xtxWP4j&uhTS_R5;cl>ebdyzEptWh8*0$-N!bZBJM!}+YGmf z`#$qr!ddyl{T-e8T%+ z1vp5|cbromB{_}qo)6{C8>Be&1)|ns;f7nBkbQam*aIyum$i|N<#BFCeXu>y0ID)n zbuN}QJ>`&mXe6X@UnT+e^hRfrelg?@XAGUgXS)blhG*wJw{0aH^NAYL`^Whu9V*Nq z0m{Nd@cwOcMj)F56=Z)kf2QHutH5PW6kp=XXJPd7N`uOB;Gje%S=_dZ))JKSa%k%$ znvGSiR8l@#v-tBhq4eNaLSEuWu)17tJIZsnru$WxQ^xTuCRsyWBSv_M(7I)3YlP{j zT|g%Cu`94Yv!^adj9oHFnXg7uYhgUr_vuiZa{w!BSUQ;18XTPtdU4{tls1wFf81-k zaIuf-DP3z(XK2!HyFcHLO(HQfNsl{b6c?o~AVFRL5M|URuK0XM+&R-Raf!Z`F|s5{ zz{kMF`}t{MB$+pw*!HyY5)6}V%T=ZxnU)>lnwG0{j39YhmB)bR6G$IjWp*@47@PCD zD85D$+cyQD9%~NQ-YF~O8B^_}O)&~=8-b=4GgRIz#uDXq4Ag-QD1zANY0cl~Z~>UC>XYn5=zsd~-A!hB z?XLN45DS@cL=}DTDD+v{g^M2H3P3b-$~K?Km=sO}88IU*XBO)uuW6@$e8R8FUKn=3 zA3DhP^~Dkw^A(E{m$}*e|AN$>aBgGEo)<%Ctd7jMs zj`66<^xSAT@O8S5=9xD+LpPGkO`FAdOIxDR+;y+T*tDP#q}JN<(bjZ>aiyqs#Dq)q z=xV1961ErdKS+2%O?7~~7ry;0@;oP+E^_dhd9N2Uh}i`F(O5@wL(L8fLb)_&ut-W@ zrC7q?n;F62>Ei?cgRda6mgE_9Hyi_$x= zrA*{v=KSP$>Tb~PCg#uYr(VT{5K8dk0O~4YcF3f|h?RoWTs4V;YpoU> zi6YNfj`6*)lcB9j2gg=vB5EDxQwKbx%3{vIN1e41w-|g;_Pl|DaFQqJ@tNr(x2g`U zrgnl>KKLX{56lM=aeXOq9*e{aqACD-{E!$N4I1AJvu$ z4lPWYqAjD*=@xTv_T;yD&|V_1!=R)>Nx9z@@?}py2s4RlL(?kucT_YVWR#Fy_*bga zPWaJi>?U?b7gc?Ig@C?Z4>f!Z`;H%fqNyct>wpH2AUfuqZ#1P+`B(P(cS8!p=b-O_d{!Z{9Hxb_e(~e$8 zu_nx@EW1Ta5v~(&916v^kEQ){%0ta6dzRnDqvYt~+;f@sn?&5lps)Bf?Pi46Bq!y^ zEfw>zqJrOG47NPN6Ed7zMC&ZeahSt_dvV zPn>iGkNTA{Bz@2*d;~ov02@*peR1=y+ij=KZRo@*MVGMLs2;*l-z3AcDSUPg{g(E} z%R~9xc8Fpv_v>iJmux%AQQ+N%BqIr1xDtp@F4W*lk*NQO!twLOMSi|B6*bhrRvm5KmTB1v%dE=*MF1no`=-!)z zTTq&nO$|;Q0|+Q8^EVPxRXJZKs=yrrndiA4k~z)Y2h%*fELmN`pG+SBOb+TaxEb}f z4L%3KqX4eJ2xy{Vx=_~GvD`9p$Kpn{;A_g+q~`V}86r(x{*xvs@>O+|sk4T}xnn}E znsnJ$GLjmB$Qjrd@PWzExEzu7;w}a78OQfnH`s}1qUmU_{GTg?i#FDQslq-$z?X=K zQ<%nj;`19Fk+$})2SP*h&rkA563+>i4CpT-`eHLviLL$#Fgvv~hHxFIK>_H%k3HItY#ykh~q{S?I+&u2N8 zm3hmcNsDI&XGuaqxZGL0!2D^~6_%N&Pd)ZI^O34%)w3H<3F*-d`#0&2(ATq4c+o;i zDKYQWJiZ-ED3N#9;9u!Ka=bx(8y>AuGH-p#55M8(qEim5_I-jaN)10xB1g#*m^dTU zo;6<=(g+;k&O9Z2O0k&oZJt|u{XoRF&+)UOXpF7X-(7eYN;ai(%z3)11C}bxC`H7u z!Ymmx&4^}xAAso*v=W^gt?XNO&{qd=G=FSQbNi0u_^$PqlN~xD(p~#!KZRZPl^g$6{?;K6bXcdZS~720kZ@pA zTR-K^84lUze3W66Jf-Jrv>)73X9orE`4~dodB(@)W6HOkE~E$Jm^YGb+Dq#Z(+K8& zb+(eSv-H!6xrk-bj0GWahLzRKA4E13XUKV9JP;qA%7uq0x#oeDAuozoJrD$cBSxVv z7bTEjnLLi_m||kL*&75GrFF19Sp?qJ>c1OjR@lzgU4n0a7A^6kt2!2D!7(q)ehp=c zCePoZjlTCS4jU<^SI|3NjzfOU1L{oy&T&!dy~rEokdh-qKAZ-ocqu6%Zey&?{@P*rH&w!O8pw8OV4L+Wkzp*vE@k`LcOOAp7Wig$9FR z%L$J}R2}mXV44%f7&XoXa!!tOiVhSq%ZdX!EwBn3EUT6aN>nv}E? zk94!O(>Gh#mcw(`jqy~_qD zV~{^JgSqg6SS@WG0}Y)&9Gu+4HAc+2q$pYQWZfGK z&G~`D*$vXRKXa5~?3fR%)D=N%SraXOb5@`V4f*}p>edTnd?l@xxBCU_N;4$=^uwy=|k2c{xr{yDfRILzm%qu@KY z3_{$9=Z9X5XpC@A$JGa=s)X-LDCaURb5gahHX7A)7<|?TFKWh(dC*}$ra-RU3lMS!P%AVm{kY;&jl{{~Ue|r6CiS?HMhk?nkVP|2HvZOy{cd!iAvHf*sS;MKQ zzOqdTkvn{Ym1I`A-D@K_GpCXm3(q6`1^zeI09kIUfo;-E_@6pYOi<;E{T^p*G@Eu( zZp4G@lKErd$@&bX9N@1h>ZCjuM20y_G{;RwuE^$RW5g@qy8u~^oN>>k2L{U4w42yl zgn5#fvN9r{1t6+p?_oz;LZ~v8QJ=_Kb7Isozc|Y^uv5=VS9&Edt!UE`X9#AfN#>VH zYMf4~v3yo+rN4GjEG;y9^N>^HLsK47c$?I;oNXQJ^sVVt0a5+25cguDxh30vnK#gN z;jQ;4zop~pwr{zuYTHLPeq5?uYw&|`H5No+Sw-^*7czoU378qB(IB&W8zVK{dXAr4c}Dpo8PD^(;R@*tUxqMf?leEtowT zNjX_p;r9ptHIBTs;oud?a)F&^hLX7`DOxA*{rC1;Ukav4O{ugMDuP<6R}#xN8&-26 zm8IO*3-FKv9Poth@ucFc8To27Zrd9=i?5lgS~eB}5cq>1^*4xvUAp>E<1_p>R2`NP zx(}oVWo(7ClW|a&(l$|R^}{VKlg+GagFY*g3)IM7wZoP3DX(B@16cMr;?>??>()o7}YL86qBup z)1I>7-n{N}1^+GlYJQ;mDmKk&XZ^JH%`b|=%bF+;OKsdTNKX@>Uh+Tn^#_ycFD}fV z)T-Z%m|w)Ivba%85YN=_G_Wmjvejj(8|RY(lhMTYC4N6M`fzPBd|93O`fOJkhJwmr z+dJWGf!yO0tE?Gnte9|ynkMkz9n~;)EjNCFFT_G$x#4YPg0RL0D!*&EE@Nk8%#acp znX1$FU`fP1m?Pe!n)Sai$zM8DP0gICL8Rh+K7nRWFKp!6<$`Pis2&FfS0Upo^*dIi zKbsb{#S&%5Kc~d)^ueP?@Xij?O(lW%2F!iL(+kkhG-0l{S$eJ+q>R_+$&pIh3Z7LT zf45ui6L~>yf#rM9_K&)GC3dmi0@4ZPZ{7U6v8mP%sJSD4q*Np!S$WX3^GILf zhc6h4tCaM1hqypP{O=l=AO4E}6sRW_GBe1hDEC)|*k28(cHO zDs_smlT&nffjgrZ>w_7AluVjOE4x1=4c-xO%v|1eABGhr3iK|Oq+${@<0YB4gfHWD>Au(17OSH*Zz*AkKx zrQ_^cQ5^`~S42`tH2e z(;MP(PM9!XU^|_ID~ngxpb z<8wr@s?;KM8t~^&Gyorg6U&<6roW5$SzzTK z*9!Kq{jbIQbDsMDqdN!n*<|xUH$GZXYW+u7K$Ku66`@SCBXdu)Hry&B=M8)&_C0pk=#9l#;C;dEJJ3n=G0onRxN>xJA z7@xo>RNoEA`)6rJ{(uL}DGyo{P+$}TBKLN0wa3^EhB`he<3*wCLn_86GwbG*JErMd zFJf6*lAnHf|BhZ&bwAcLki+EE36eW{TlQVxr;Ildy!yqGWJ}~c_;GjLYW#j|h?p}Z znZ4498^AA+s>^~f9=ikD2=kWf7R1*nql+XlCz+U^zFMRer(UplJwdfIo@U>D&i$}j zfs@0!AH@tgL#rwP+YYw`?K)&D9uu2r8;MzKdx+hJ)$wI{eNQ!z`dS>C??;CuxoBMA zIcsC1?rWvgJ`1cq?G!nU!Ki#}iu6|J)8~}ayjR$l0=@0wi;2e5#ywSV(FxjgVzMF>ufWo)JzDjai zSk?8x1jNJDp0Nl~#`tYX=y|E66QCPA_yHuL%EV+7HB|C!a@iX6c`B!RR&cjCPfe`x z%r-ffKE8q&EUjuuwj+V%nE7~!K~vbHu1Jhj{)zDBhrO#DV%cncVoP<%P4Zn)HIJcn z@r_UAMp8y(#9^JjUe%{hj7b^wmR8ZB95CPe+@L7ep}!H0RMte>;d)XiO#&L}6eX8OewI^Y-F^HI@df6Rp<|WnaW(E6x z-d#X#dCs`_7NGE50BMFF$a+s|yqZ=SdN=#Cotf`x`=a~u+(X%i@mAN2jdJ~|>gnGH z+`u7TgZ9_{`9SNxzOH|@|8TrjN%lVh{_~97zXbniPe3;DXV~&9Sa~V<^4!pGQFPFb z#9#FAf1M(FDg2)&5`K$68`gp|Lc6hOPrUi^xsHtKxBpgiKG4!x|KV&yA0bk|_zXA1F|31in%otvR zy^OGbgW0nE0(%i(GrhR~6>q-;eHowr1`THa^FRE1wE9x|KLfhovS48G9AIGo72drR l|4)zncX1T%zls0lpOs_*f5g}z3NktvGU#;5_?tiO{vUX9!PEc% literal 0 HcmV?d00001 diff --git a/python/measureGpsRate.py b/python/measureGpsRate.py new file mode 100644 index 0000000..306c245 --- /dev/null +++ b/python/measureGpsRate.py @@ -0,0 +1,33 @@ +import serial +import time + +# Replace '/dev/ttyUSB0' with your GPS device's serial port +serial_port = 'COM5' +# Replace 9600 with your GPS device's baud rate +baud_rate = 921600 +# Duration over which to measure the data rate (in seconds) +measure_duration = 60 + +def measure_data_rate(): + try: + with serial.Serial(serial_port, baud_rate, timeout=1) as ser: + start_time = time.time() + data_received = 0 # in bytes + + while time.time() - start_time < measure_duration: + data = ser.readline() + data_received += len(data) + + # Optional: Print the received data + # print(data.decode('utf-8'), end='') + + # Calculate and print the data rate + data_rate = data_received / measure_duration + print(f"Data rate: {data_rate} bytes/second") + print(f"Total Data received: {data_received}") + + except serial.SerialException as e: + print(f"Error opening serial port {serial_port}: {e}") + +if __name__ == "__main__": + measure_data_rate() \ No newline at end of file From f13f94ab0dcd900efac1d9e6619c37101282fd4e Mon Sep 17 00:00:00 2001 From: Nischay Joshi Date: Sat, 13 Apr 2024 18:13:44 -0700 Subject: [PATCH 02/14] Added time based control --- sensorDemo/SteeringTest/SteeringTest.ino | 38 ++++++ sensorDemo/SteeringTest/advancedSteering.cpp | 131 +++++++++++++++++++ sensorDemo/SteeringTest/advancedSteering.h | 41 ++++++ 3 files changed, 210 insertions(+) create mode 100644 sensorDemo/SteeringTest/SteeringTest.ino create mode 100644 sensorDemo/SteeringTest/advancedSteering.cpp create mode 100644 sensorDemo/SteeringTest/advancedSteering.h diff --git a/sensorDemo/SteeringTest/SteeringTest.ino b/sensorDemo/SteeringTest/SteeringTest.ino new file mode 100644 index 0000000..d29dd68 --- /dev/null +++ b/sensorDemo/SteeringTest/SteeringTest.ino @@ -0,0 +1,38 @@ +#include "advancedSteering.h" + +void setup() { + Serial.begin(921600); // Start serial communication at 921600 baud rate + steeringQueue = xQueueCreate(1, sizeof(SteeringData)); // Create a queue that can hold 1 item of SteeringData type + + if (steeringQueue == NULL) { + Serial.println("Failed to create steering queue"); + return; // Early exit if queue creation fails + } + + motorSetup(); // Setup motors and start the steering task +} + +void loop() { + // Check if we have incoming data + if (Serial.available()) { + String inputString = Serial.readStringUntil('\n'); // Read the incoming data until newline + inputString.trim(); // Trim whitespace and newline characters + + // Declare a variable to hold the parsed data + SteeringData incomingData; + + // Parse the string to extract lengths + if (sscanf(inputString.c_str(), "%lf,%lf", &incomingData.length1, &incomingData.length2) == 2) { + // Successfully parsed two doubles + // Overwrite the current value in the queue with the new data + incomingData.length1 *= 0.01745; + incomingData.length2 *= 0.01745; + Serial.printf("Length1: %lf\tLength2: %lf\n", incomingData.length1, incomingData.length2); + if (xQueueOverwrite(steeringQueue, &incomingData) != pdPASS) { + Serial.println("Failed to overwrite data in steering queue"); + } + } else { + Serial.println("Invalid format or incomplete data"); + } + } +} \ No newline at end of file diff --git a/sensorDemo/SteeringTest/advancedSteering.cpp b/sensorDemo/SteeringTest/advancedSteering.cpp new file mode 100644 index 0000000..c04c9b9 --- /dev/null +++ b/sensorDemo/SteeringTest/advancedSteering.cpp @@ -0,0 +1,131 @@ +/* + * Autonomous Parachute Steering Functon Implementation + * Keeping track of the angle of a continous rotation servo + * Author: Nischay Joshi + * Created: April 13th, 2024 + */ + +#include "advancedSteering.h" + +//Declare servo motor objects +Servo leftMotor; +Servo rightMotor; + +// Define the global variables +TaskHandle_t _steeringTask = NULL; +QueueHandle_t steeringQueue = NULL; + +void motorSetup(){ + ESP32PWM::allocateTimer(0); + ESP32PWM::allocateTimer(1); + ESP32PWM::allocateTimer(2); + ESP32PWM::allocateTimer(3); + leftMotor.attach(LEFTPIN,MINUS,MAXUS); + rightMotor.attach(RIGHTPIN,MINUS,MAXUS); + leftMotor.setPeriodHertz(SERVOFREQ); // Standard 50hz servo + rightMotor.setPeriodHertz(SERVOFREQ); // Standard 50hz servo + //set at 0 speed value + leftMotor.write(90); + rightMotor.write(90); + + //start the servo control task + BaseType_t ret = xTaskCreatePinnedToCore( + steering, /* Function to implement the task */ + "steeringTask", /* Name of the task */ + 4096, /* Stack size in words */ + NULL, /* Task input parameter */ + configMAX_PRIORITIES - 1, /* Priority of the task */ + &_steeringTask, /* Task handle. */ + 0); /* Core where the task should run */ + if (ret != pdPASS) { + Serial.println("Failed to create steering task"); + } + else { + Serial.println("Steering task created"); + } +} + +//steering function to control the steering of the parachute +void steering(void *pvParameters){ + SteeringData desiredLengths; + desiredLengths.length1 = INITIAL_LENGTH_M; + desiredLengths.length2 = INITIAL_LENGTH_M; + + double currentLength1 = desiredLengths.length1; + double currentLength2 = desiredLengths.length2; + + double desiredLength1 = currentLength1; + double desiredLength2 = currentLength2; + + double errorLength1 = 0; + double errorLength2 = 0; + + while(1){ + //if we have data in the queue, if not, keep the previous value + if (xQueueReceive(steeringQueue, &desiredLengths, 0) == pdTRUE){ + Serial.printf("Servo1: %lf\tServo2: %lf\n", desiredLengths.length1, desiredLengths.length2); + desiredLength1 = desiredLengths.length1; + desiredLength2 = desiredLengths.length2; + } + //update the distance to travel + errorLength1 = desiredLength1 - currentLength1; + errorLength2 = desiredLength2 - currentLength2; + + if(abs(errorLength1) > ERROR_DEADZONE || abs(errorLength2) > ERROR_DEADZONE){ + //calculate the amount of time to move the servo + double time1 = abs(errorLength1) / SERVO_SPEED_M_SEC * 1000; + double time2 = abs(errorLength2) / SERVO_SPEED_M_SEC * 1000; + + //move the servo + int leftMotorvalue = 90 + (errorLength1 > 0 ? SERVO_SPEED_VALUE_ANGLE : -SERVO_SPEED_VALUE_ANGLE); + int rightMotorvalue = 90 + (errorLength2 > 0 ? SERVO_SPEED_VALUE_ANGLE : -SERVO_SPEED_VALUE_ANGLE); + + if(abs(errorLength1) < ERROR_DEADZONE){ + leftMotorvalue = 90; + } + if(abs(errorLength2) < ERROR_DEADZONE){ + rightMotorvalue = 90; + } + + leftMotor.write(leftMotorvalue); + rightMotor.write(rightMotorvalue); + Serial.printf("CurrentLeng: %lf\tDesiredLength %lf\tError: %lf\tTime1: %lf\tServoValue: %d\n", currentLength1, desiredLength1, errorLength1, time1, leftMotorvalue); + unsigned long startTime = millis(); + + //wait for the servo to move + bool leftMotordone = false; + bool rightMotordone = false; + + int i = 0; + while(1){ + + if(!leftMotordone && ((millis() - startTime) > time1)){ + leftMotordone = true; + leftMotor.write(90); // Reset servo to neutral position after moving + Serial.println("Servo Left done"); + } + if(!rightMotordone && ((millis() - startTime) > time2)){ + rightMotordone = true; + rightMotor.write(90); // Reset servo to neutral position after moving + Serial.println("Servo right done"); + } + if(leftMotordone && rightMotordone){ + break; // Exit loop if both servos have completed their movements + } + // Wait for 1 ms to prevent hogging the CPU + // vTaskDelay(1 / portTICK_PERIOD_MS); + delay(1); + Serial.println(i++); + } + + leftMotor.write(90); + rightMotor.write(90); + // Update the current lengths + currentLength1 = desiredLengths.length1; + currentLength2 = desiredLengths.length2; + } + + vTaskDelay(1 / portTICK_PERIOD_MS); + + } +} \ No newline at end of file diff --git a/sensorDemo/SteeringTest/advancedSteering.h b/sensorDemo/SteeringTest/advancedSteering.h new file mode 100644 index 0000000..db699d4 --- /dev/null +++ b/sensorDemo/SteeringTest/advancedSteering.h @@ -0,0 +1,41 @@ +/* + * Autonomous Parachute Steering Functon Header + * Keeping track of the angle of a continous rotation servo + * Author: Nischay Joshi + * Created: April 13th, 2024 + */ + +#ifndef ADVANCEDSTEERING_H +#define ADVANCEDSTEERING_H + +#include +#include + +#define MINUS 700 +#define MAXUS 2300 +#define LEFTPIN 26 +#define RIGHTPIN 27 +#define SERVOFREQ 50 + +#define WHEELRADIUS_M 1 +#define INITIAL_LENGTH_M 0.00 +#define ERROR_DEADZONE 0.001 + +#define SERVO_SPEED_RAD_SEC 14.38378555 +#define SERVO_SPEED_M_SEC SERVO_SPEED_RAD_SEC * WHEELRADIUS_M +#define SERVO_SPEED_VALUE_ANGLE 80 + + +typedef struct { + double length1; + double length2; +} SteeringData; + +//a queue to send the data to the steering function +extern TaskHandle_t _steeringTask; +extern QueueHandle_t steeringQueue; + +void motorSetup(); +void steering(void *pvParameters); + +#endif \ No newline at end of file From 92610e927cf3f342c50115872a921caf5743a618 Mon Sep 17 00:00:00 2001 From: Nischay Joshi Date: Sat, 13 Apr 2024 18:14:58 -0700 Subject: [PATCH 03/14] Added time based control --- src/main/advancedSteering.cpp | 102 ++++++++++++++++++++++++++++++++++ src/main/advancedSteering.h | 40 +++++++++++++ 2 files changed, 142 insertions(+) create mode 100644 src/main/advancedSteering.cpp create mode 100644 src/main/advancedSteering.h diff --git a/src/main/advancedSteering.cpp b/src/main/advancedSteering.cpp new file mode 100644 index 0000000..c1a2557 --- /dev/null +++ b/src/main/advancedSteering.cpp @@ -0,0 +1,102 @@ +/* + * Autonomous Parachute Steering Functon Implementation + * Keeping track of the angle of a continous rotation servo + * Author: Nischay Joshi + * Created: April 13th, 2024 + */ + +#include "advancedSteering.h" + +//Declare servo motor objects +Servo leftMotor; +Servo rightMotor; + +void motorSetup(){ + ESP32PWM::allocateTimer(0); + ESP32PWM::allocateTimer(1); + ESP32PWM::allocateTimer(2); + ESP32PWM::allocateTimer(3); + leftMotor.attach(LEFTPIN,MINUS,MAXUS); + rightMotor.attach(RIGHTPIN,MINUS,MAXUS); + leftMotor.setPeriodHertz(SERVOFREQ); // Standard 50hz servo + rightMotor.setPeriodHertz(SERVOFREQ); // Standard 50hz servo + //set at 0 speed value + leftMotor.write(90); + rightMotor.write(90); + + //start the servo control task + BaseType_t ret = xTaskCreatePinnedToCore( + steering, /* Function to implement the task */ + "steeringTask", /* Name of the task */ + 4096, /* Stack size in words */ + NULL, /* Task input parameter */ + configMAX_PRIORITIES - 1, /* Priority of the task */ + &_steeringTask, /* Task handle. */ + 0); /* Core where the task should run */ + if (ret != pdPASS) { + Serial.println("Failed to create steering task"); + } + else { + Serial.println("Steering task created"); + } +} + +//steering function to control the steering of the parachute +void steering(void *pvParameters){ + SteeringData desiredLengths; + desiredLengths.length1 = INITIAL_LENGTH_M; + desiredLengths.length2 = INITIAL_LENGTH_M; + + double currentLength1 = data.length1; + double currentLength2 = data.length2; + + double errorLength1 = 0; + double errorLength2 = 0; + + while(1){ + //if we have data in the queue, if not, keep the previous value + if (xQueueReceive(steeringQueue, &desiredLengths, 0) == pdTRUE){ + } + //update the distance to travel + errorLength1 = desiredLengths.length1 - currentLength1; + errorLength2 = desiredLengths.length2 - currentLength2; + + //calculate the amount of time to move the servo + double time1 = abs(errorLength1) / SERVO_SPEED_M_SEC; + double time2 = abs(errorLength2) / SERVO_SPEED_M_SEC; + + //move the servo + int servo1value = 90 + (errorLength1 > 0 ? SERVO_SPEED_VALUE_ANGLE : -SERVO_SPEED_VALUE_ANGLE); + int servo2value = 90 + (errorLength2 > 0 ? SERVO_SPEED_VALUE_ANGLE : -SERVO_SPEED_VALUE_ANGLE); + + servo1.write(servo1value); + servo2.write(servo2value); + unsigned long startTime = millis(); + + //wait for the servo to move + bool servo1done = false; + bool servo2done = false; + while(1){ + if(!servo1done && (millis() - startTime > time1)){ + servo1done = true; + servo1.write(90); // Reset servo to neutral position after moving + } + if(!servo2done && (millis() - startTime > time2)){ + servo2done = true; + servo2.write(90); // Reset servo to neutral position after moving + } + if(servo1done && servo2done){ + break; // Exit loop if both servos have completed their movements + } + // Wait for 1 ms to prevent hogging the CPU + vTaskDelay(1 / portTICK_PERIOD_MS); + } + + // Update the current lengths + currentLength1 = desiredLengths.length1; + currentLength2 = desiredLengths.length2; + + vTaskDelay(1 / portTICK_PERIOD_MS); + + } +} \ No newline at end of file diff --git a/src/main/advancedSteering.h b/src/main/advancedSteering.h new file mode 100644 index 0000000..640be80 --- /dev/null +++ b/src/main/advancedSteering.h @@ -0,0 +1,40 @@ +/* + * Autonomous Parachute Steering Functon Header + * Keeping track of the angle of a continous rotation servo + * Author: Nischay Joshi + * Created: April 13th, 2024 + */ + +#ifndef ADVANCEDSTEERING_H +#define ADVANCEDSTEERING_H + +#include +#include + +#define MINUS 700 +#define MAXUS 2300 +#define LEFTPIN 26 +#define RIGHTPIN 27 +#define SERVOFREQ 50 + +#define WHEELRADIUS_M 1 +#define INITIAL_LENGTH_M 0.00 + +#define SERVO_SPEED_RAD_SEC 6.98412 +#define SERVO_SPEED_M_SEC SERVO_SPEED_RAD_SEC * WHEELRADIUS_M +#define SERVO_SPEED_VALUE_ANGLE 20 + + +typedef struct { + double length1; + double length2; +} SteeringData; + +//a queue to send the data to the steering function +extern TaskHandle_t _steeringTask = NULL; +extern QueueHandle_t steeringQueue; + +void motorSetup(); +void steering(void *pvParameters); + +#endif \ No newline at end of file From c2c11f5498a1091ce1ca435d4cd35499e4adc5ad Mon Sep 17 00:00:00 2001 From: Nischay Joshi Date: Fri, 3 May 2024 20:03:52 -0700 Subject: [PATCH 04/14] Add all files and current work --- config.json | 40 +++++++++++++ src/main/ConfigParse.h | 2 +- src/main/Steering.cpp | 44 -------------- src/main/Steering.h | 20 ------- src/main/advancedSteering.cpp | 107 +++++++++++++++++++++------------- src/main/advancedSteering.h | 9 +-- src/main/main.ino | 2 +- 7 files changed, 115 insertions(+), 109 deletions(-) create mode 100644 config.json delete mode 100644 src/main/Steering.cpp delete mode 100644 src/main/Steering.h diff --git a/config.json b/config.json new file mode 100644 index 0000000..0245b47 --- /dev/null +++ b/config.json @@ -0,0 +1,40 @@ +{ + "BottleID": 1, + "SSID": "UBC_UAS_Parachute_1", + "Password": "UBCUAS2023", + "AcquireRate": 57, + "GRAVITY": 9.809000015, + "ACC_X_STD": 0.3, + "ACC_Y_STD": 0.3, + "ACC_Z_STD": 0.05, + "BARO_ALT_STD": 1.466, + "GPS_POS_STD": 2.5, + "PID": { + "KP": 1, + "KI": 0, + "KD": 0 + }, + "Print_Enable": true, + "BufferSize": 512, + "OutputData": { + "Barometer": { + "Altitude": true, + "Pressure": true + }, + "IMU": { + "LinearAccel": true, + "Orientation_Quaternion": true, + "EulerAngles": true + }, + "KalmanFilter": { + "Velocity": true, + "Position": true + }, + "GPS": { + "Coordinates": true, + "Altitude": true, + "Satellites": true, + "Lock": true + } + } + } \ No newline at end of file diff --git a/src/main/ConfigParse.h b/src/main/ConfigParse.h index da909a4..9dabc5c 100644 --- a/src/main/ConfigParse.h +++ b/src/main/ConfigParse.h @@ -68,7 +68,7 @@ typedef struct{ float AcquireRate; float SampleTime; double GRAVITY; - double ACC_X_STD; + double ACC_X_STD; double ACC_Y_STD; double ACC_Z_STD; double BARO_ALT_STD; diff --git a/src/main/Steering.cpp b/src/main/Steering.cpp deleted file mode 100644 index d2d5242..0000000 --- a/src/main/Steering.cpp +++ /dev/null @@ -1,44 +0,0 @@ -#include "Steering.h" -#include "Arduino.h" - - - -//Define servo motor classes -Servo leftMotor; -Servo rightMotor; - -void motorSetup(){ - ESP32PWM::allocateTimer(0); - ESP32PWM::allocateTimer(1); - ESP32PWM::allocateTimer(2); - ESP32PWM::allocateTimer(3); - leftMotor.attach(LEFTPIN,MINUS,MAXUS); - rightMotor.attach(RIGHTPIN,MINUS,MAXUS); - leftMotor.setPeriodHertz(SERVOFREQ); // Standard 50hz servo - rightMotor.setPeriodHertz(SERVOFREQ); // Standard 50hz servo - leftMotor.write(90); - rightMotor.write(90); -} - -void steering(double yaw){ - //This part needs to be fixed. Euler->PID->steering. Change PID to be an angle or multiply value here to get angle? - //-x to +x ->Map from 0 to 180 ->change double to int - //-x to +x ->Map from 0 to 180 ->change double to int - //pv is the value that we observe, setpoint is what we want course to be. - //Set setpoint, then update pv. - int left_motor_value = map(yaw,-1000,1000,0,180); - int right_motor_value = map(yaw,-1000,1000,0,180); - - - Serial.print(left_motor_value);Serial.print("\t"); - Serial.print(right_motor_value);Serial.print("\n"); - - leftMotor.write(left_motor_value); - rightMotor.write(right_motor_value); - - -//Uncomment to get the values - // Serial.print(right_motor_value);Serial.print("\t"); - // Serial.print(left_motor_value);Serial.print("\n"); - -} \ No newline at end of file diff --git a/src/main/Steering.h b/src/main/Steering.h deleted file mode 100644 index b39a272..0000000 --- a/src/main/Steering.h +++ /dev/null @@ -1,20 +0,0 @@ -/*Autonomous Parachute Steering Functon Header - *Author: Bobsy Narayan and Nischay Joshi - *Edited: Feb 19, 2022 - */ -//Steering function declarations -#ifndef _STEERING_H -#define _STEERING_H -#include - -#define MINUS 450 -#define MAXUS 2450 -#define LEFTPIN 26 -#define RIGHTPIN 27 -#define ATTACHFREQ 1000 -#define SERVOFREQ 50 - -void motorSetup(); -void steering(double yaw); - -#endif \ No newline at end of file diff --git a/src/main/advancedSteering.cpp b/src/main/advancedSteering.cpp index c1a2557..3d3e366 100644 --- a/src/main/advancedSteering.cpp +++ b/src/main/advancedSteering.cpp @@ -11,7 +11,11 @@ Servo leftMotor; Servo rightMotor; -void motorSetup(){ +// Define the global variables +TaskHandle_t _steeringTask = NULL; +QueueHandle_t steeringQueue = NULL; + +void mmotorSetup(){ ESP32PWM::allocateTimer(0); ESP32PWM::allocateTimer(1); ESP32PWM::allocateTimer(2); @@ -47,8 +51,11 @@ void steering(void *pvParameters){ desiredLengths.length1 = INITIAL_LENGTH_M; desiredLengths.length2 = INITIAL_LENGTH_M; - double currentLength1 = data.length1; - double currentLength2 = data.length2; + double currentLength1 = desiredLengths.length1; + double currentLength2 = desiredLengths.length2; + + double desiredLength1 = currentLength1; + double desiredLength2 = currentLength2; double errorLength1 = 0; double errorLength2 = 0; @@ -56,45 +63,67 @@ void steering(void *pvParameters){ while(1){ //if we have data in the queue, if not, keep the previous value if (xQueueReceive(steeringQueue, &desiredLengths, 0) == pdTRUE){ + Serial.printf("Servo1: %lf\tServo2: %lf\n", desiredLengths.length1, desiredLengths.length2); + desiredLength1 = desiredLengths.length1; + desiredLength2 = desiredLengths.length2; } //update the distance to travel - errorLength1 = desiredLengths.length1 - currentLength1; - errorLength2 = desiredLengths.length2 - currentLength2; - - //calculate the amount of time to move the servo - double time1 = abs(errorLength1) / SERVO_SPEED_M_SEC; - double time2 = abs(errorLength2) / SERVO_SPEED_M_SEC; - - //move the servo - int servo1value = 90 + (errorLength1 > 0 ? SERVO_SPEED_VALUE_ANGLE : -SERVO_SPEED_VALUE_ANGLE); - int servo2value = 90 + (errorLength2 > 0 ? SERVO_SPEED_VALUE_ANGLE : -SERVO_SPEED_VALUE_ANGLE); - - servo1.write(servo1value); - servo2.write(servo2value); - unsigned long startTime = millis(); - - //wait for the servo to move - bool servo1done = false; - bool servo2done = false; - while(1){ - if(!servo1done && (millis() - startTime > time1)){ - servo1done = true; - servo1.write(90); // Reset servo to neutral position after moving - } - if(!servo2done && (millis() - startTime > time2)){ - servo2done = true; - servo2.write(90); // Reset servo to neutral position after moving - } - if(servo1done && servo2done){ - break; // Exit loop if both servos have completed their movements - } - // Wait for 1 ms to prevent hogging the CPU - vTaskDelay(1 / portTICK_PERIOD_MS); - } + errorLength1 = desiredLength1 - currentLength1; + errorLength2 = desiredLength2 - currentLength2; + + if(abs(errorLength1) > ERROR_DEADZONE || abs(errorLength2) > ERROR_DEADZONE){ + //calculate the amount of time to move the servo + double time1 = abs(errorLength1) / SERVO_SPEED_M_SEC * 1000; + double time2 = abs(errorLength2) / SERVO_SPEED_M_SEC * 1000; + + //move the servo + int leftMotorvalue = 90 + (errorLength1 > 0 ? SERVO_SPEED_VALUE_ANGLE : -SERVO_SPEED_VALUE_ANGLE); + int rightMotorvalue = 90 + (errorLength2 > 0 ? SERVO_SPEED_VALUE_ANGLE : -SERVO_SPEED_VALUE_ANGLE); - // Update the current lengths - currentLength1 = desiredLengths.length1; - currentLength2 = desiredLengths.length2; + if(abs(errorLength1) < ERROR_DEADZONE){ + leftMotorvalue = 90; + } + if(abs(errorLength2) < ERROR_DEADZONE){ + rightMotorvalue = 90; + } + + leftMotor.write(leftMotorvalue); + rightMotor.write(rightMotorvalue); + Serial.printf("CurrentLeng: %lf\tDesiredLength %lf\tError: %lf\tTime1: %lf\tServoValue: %d\n", currentLength1, desiredLength1, errorLength1, time1, leftMotorvalue); + unsigned long startTime = millis(); + + //wait for the servo to move + bool leftMotordone = false; + bool rightMotordone = false; + + int i = 0; + while(1){ + + if(!leftMotordone && ((millis() - startTime) > time1)){ + leftMotordone = true; + leftMotor.write(90); // Reset servo to neutral position after moving + Serial.println("Servo Left done"); + } + if(!rightMotordone && ((millis() - startTime) > time2)){ + rightMotordone = true; + rightMotor.write(90); // Reset servo to neutral position after moving + Serial.println("Servo right done"); + } + if(leftMotordone && rightMotordone){ + break; // Exit loop if both servos have completed their movements + } + // Wait for 1 ms to prevent hogging the CPU + // vTaskDelay(1 / portTICK_PERIOD_MS); + delay(1); + Serial.println(i++); + } + + leftMotor.write(90); + rightMotor.write(90); + // Update the current lengths + currentLength1 = desiredLengths.length1; + currentLength2 = desiredLengths.length2; + } vTaskDelay(1 / portTICK_PERIOD_MS); diff --git a/src/main/advancedSteering.h b/src/main/advancedSteering.h index 640be80..bb99dca 100644 --- a/src/main/advancedSteering.h +++ b/src/main/advancedSteering.h @@ -19,10 +19,11 @@ #define WHEELRADIUS_M 1 #define INITIAL_LENGTH_M 0.00 +#define ERROR_DEADZONE 0.001 -#define SERVO_SPEED_RAD_SEC 6.98412 +#define SERVO_SPEED_RAD_SEC 14.38378555 #define SERVO_SPEED_M_SEC SERVO_SPEED_RAD_SEC * WHEELRADIUS_M -#define SERVO_SPEED_VALUE_ANGLE 20 +#define SERVO_SPEED_VALUE_ANGLE 80 typedef struct { @@ -31,10 +32,10 @@ typedef struct { } SteeringData; //a queue to send the data to the steering function -extern TaskHandle_t _steeringTask = NULL; +extern TaskHandle_t _steeringTask; extern QueueHandle_t steeringQueue; -void motorSetup(); +void mmotorSetup(); void steering(void *pvParameters); #endif \ No newline at end of file diff --git a/src/main/main.ino b/src/main/main.ino index 7f98cf0..2a2abf6 100644 --- a/src/main/main.ino +++ b/src/main/main.ino @@ -12,7 +12,7 @@ #include "WebStreamServer.h" #include "SDCard.h" #include "PID.h" -#include "Steering.h" +#include "advancedSteering.h" #include "Reciever.h" #define BufferLen 500 From df5df4059ddcf179eaaa65b53877ff069d81ca34 Mon Sep 17 00:00:00 2001 From: p-eanat Date: Sun, 5 May 2024 12:22:42 -0700 Subject: [PATCH 05/14] Added continuous servo calibration script and encoder test script --- .../cont_servo_calibration.ino | 17 +++++++ sensorDemo/encoder_test/encoder.cpp | 23 ++++++++++ sensorDemo/encoder_test/encoder.h | 22 +++++++++ sensorDemo/encoder_test/encoder_test.ino | 45 +++++++++++++++++++ 4 files changed, 107 insertions(+) create mode 100644 sensorDemo/cont_servo_calibration/cont_servo_calibration.ino create mode 100644 sensorDemo/encoder_test/encoder.cpp create mode 100644 sensorDemo/encoder_test/encoder.h create mode 100644 sensorDemo/encoder_test/encoder_test.ino diff --git a/sensorDemo/cont_servo_calibration/cont_servo_calibration.ino b/sensorDemo/cont_servo_calibration/cont_servo_calibration.ino new file mode 100644 index 0000000..2925801 --- /dev/null +++ b/sensorDemo/cont_servo_calibration/cont_servo_calibration.ino @@ -0,0 +1,17 @@ +#include + +int servoPin = 27; + +Servo servo; + +void setup() +{ + servo.attach(servoPin); +} + + +void loop() +{ + servo.write(90); + delay(15); +} \ No newline at end of file diff --git a/sensorDemo/encoder_test/encoder.cpp b/sensorDemo/encoder_test/encoder.cpp new file mode 100644 index 0000000..0735c69 --- /dev/null +++ b/sensorDemo/encoder_test/encoder.cpp @@ -0,0 +1,23 @@ +// #include "Arduino.h" +// #include "encoder.h" + +// /* +// * Constructor +// */ +// Encoder::Encoder() { +// forward = true; +// time = millis(); +// count = 0; +// } + +// /* +// * Divides count by 2 since each tick triggers encoder twice using this scuffed debouncing method +// */ +// int Encoder::get_count() { +// int count_adj = (int)count / 2; +// return count_adj; +// } + + + + diff --git a/sensorDemo/encoder_test/encoder.h b/sensorDemo/encoder_test/encoder.h new file mode 100644 index 0000000..1db33b0 --- /dev/null +++ b/sensorDemo/encoder_test/encoder.h @@ -0,0 +1,22 @@ +// // Encoder class + +// #ifndef ENCODER_H +// #define ENCODER_H + +// class Encoder { +// public: + +// Encoder(); +// int get_count(); +// bool forward; +// volatile unsigned long time; +// volatile int count; + +// friend void IRAM_ATTR enc_1_isr(); // CAREFUL!! If you modify these in another file, you have to update here! +// friend void IRAM_ATTR enc_2_isr(); + +// private: + +// } + +// #endif \ No newline at end of file diff --git a/sensorDemo/encoder_test/encoder_test.ino b/sensorDemo/encoder_test/encoder_test.ino new file mode 100644 index 0000000..4b6b505 --- /dev/null +++ b/sensorDemo/encoder_test/encoder_test.ino @@ -0,0 +1,45 @@ +#define ENC_1 25 +#define ENC_2 33 + +// Global variables for encoders +bool forward_1 = true; +bool forward_2 = true; +volatile unsigned long time_1 = millis(); +volatile unsigned long time_2 = millis(); +volatile int count_1 = 0; +volatile int count_2 = 0; + +void IRAM_ATTR enc_1_isr() { + unsigned long current = millis(); + if (current - time_1 > 20) { // "Debounce" time interval of 20ms + if (forward_1) { count_1++; } + else { count_1--; } + time_1 = current; + } +} + +void IRAM_ATTR enc_2_isr() { + unsigned long current = millis(); + if (current - time_2 > 20) { // "Debounce" time interval of 20ms + if (forward_2) { count_2++; } + else { count_2--; } + time_2 = current; + } +} + + +void setup() { + Serial.begin(921600); + delay(1000); + + pinMode(ENC_1, INPUT); + attachInterrupt(ENC_1, enc_1_isr, RISING); + pinMode(ENC_2, INPUT); + attachInterrupt(ENC_2, enc_2_isr, RISING); +} + +void loop() { + Serial.println(count_1); + delay(500); + +} From 9188a759108fd0799e72363fb41c49b572aaaf90 Mon Sep 17 00:00:00 2001 From: p-eanat Date: Sun, 5 May 2024 14:01:06 -0700 Subject: [PATCH 06/14] Move init to .h file --- sensorDemo/encoder_test/encoder.h | 47 +++++++++++++++--------- sensorDemo/encoder_test/encoder_test.ino | 32 +--------------- 2 files changed, 32 insertions(+), 47 deletions(-) diff --git a/sensorDemo/encoder_test/encoder.h b/sensorDemo/encoder_test/encoder.h index 1db33b0..365a354 100644 --- a/sensorDemo/encoder_test/encoder.h +++ b/sensorDemo/encoder_test/encoder.h @@ -1,22 +1,35 @@ -// // Encoder class +#ifndef ENCODER_H +#define ENCODER_H -// #ifndef ENCODER_H -// #define ENCODER_H +#define ENC_1 25 +#define ENC_2 33 -// class Encoder { -// public: +#define DEBOUNCE_INTERVAL 15 // ms -// Encoder(); -// int get_count(); -// bool forward; -// volatile unsigned long time; -// volatile int count; +// Global variables for encoders +bool forward_1 = true; +bool forward_2 = true; +volatile unsigned long time_1 = millis(); +volatile unsigned long time_2 = millis(); +volatile int count_1 = 0; +volatile int count_2 = 0; -// friend void IRAM_ATTR enc_1_isr(); // CAREFUL!! If you modify these in another file, you have to update here! -// friend void IRAM_ATTR enc_2_isr(); +// Interrupt service routines +void IRAM_ATTR enc_1_isr() { + unsigned long current = millis(); + if (current - time_1 > DEBOUNCE_INTERVAL) { + if (forward_1) { count_1++; } + else { count_1--; } + time_1 = current; + } +} +void IRAM_ATTR enc_2_isr() { + unsigned long current = millis(); + if (current - time_2 > DEBOUNCE_INTERVAL) { + if (forward_2) { count_2++; } + else { count_2--; } + time_2 = current; + } +} -// private: - -// } - -// #endif \ No newline at end of file +#endif \ No newline at end of file diff --git a/sensorDemo/encoder_test/encoder_test.ino b/sensorDemo/encoder_test/encoder_test.ino index 4b6b505..aeb432d 100644 --- a/sensorDemo/encoder_test/encoder_test.ino +++ b/sensorDemo/encoder_test/encoder_test.ino @@ -1,32 +1,4 @@ -#define ENC_1 25 -#define ENC_2 33 - -// Global variables for encoders -bool forward_1 = true; -bool forward_2 = true; -volatile unsigned long time_1 = millis(); -volatile unsigned long time_2 = millis(); -volatile int count_1 = 0; -volatile int count_2 = 0; - -void IRAM_ATTR enc_1_isr() { - unsigned long current = millis(); - if (current - time_1 > 20) { // "Debounce" time interval of 20ms - if (forward_1) { count_1++; } - else { count_1--; } - time_1 = current; - } -} - -void IRAM_ATTR enc_2_isr() { - unsigned long current = millis(); - if (current - time_2 > 20) { // "Debounce" time interval of 20ms - if (forward_2) { count_2++; } - else { count_2--; } - time_2 = current; - } -} - +#include "encoder.h" void setup() { Serial.begin(921600); @@ -39,7 +11,7 @@ void setup() { } void loop() { - Serial.println(count_1); + Serial.println((int)count_1 / 2); delay(500); } From 94a2b2430cca40d3e23bf9f57247f58dcda8a13c Mon Sep 17 00:00:00 2001 From: p-eanat Date: Sun, 5 May 2024 16:59:08 -0700 Subject: [PATCH 07/14] Compiles but untested --- sensorDemo/SteeringTest/PID_peanat.cpp | 200 ++++++++++++++ sensorDemo/SteeringTest/PID_peanat.h | 63 +++++ sensorDemo/SteeringTest/SteeringTest.ino | 60 +++-- sensorDemo/SteeringTest/advancedSteering.cpp | 260 +++++++++---------- sensorDemo/SteeringTest/advancedSteering.h | 62 ++--- sensorDemo/SteeringTest/encoder.cpp | 27 ++ sensorDemo/SteeringTest/encoder.h | 23 ++ sensorDemo/SteeringTest/encoder_steering.cpp | 95 +++++++ sensorDemo/SteeringTest/encoder_steering.h | 30 +++ sensorDemo/encoder_test/encoder.cpp | 23 -- 10 files changed, 631 insertions(+), 212 deletions(-) create mode 100644 sensorDemo/SteeringTest/PID_peanat.cpp create mode 100644 sensorDemo/SteeringTest/PID_peanat.h create mode 100644 sensorDemo/SteeringTest/encoder.cpp create mode 100644 sensorDemo/SteeringTest/encoder.h create mode 100644 sensorDemo/SteeringTest/encoder_steering.cpp create mode 100644 sensorDemo/SteeringTest/encoder_steering.h delete mode 100644 sensorDemo/encoder_test/encoder.cpp diff --git a/sensorDemo/SteeringTest/PID_peanat.cpp b/sensorDemo/SteeringTest/PID_peanat.cpp new file mode 100644 index 0000000..b430fa8 --- /dev/null +++ b/sensorDemo/SteeringTest/PID_peanat.cpp @@ -0,0 +1,200 @@ +/* + * PID library made for robots! + * Extra: custom initial output, deadband limits + * Date created: Aug. 16, 2022 + */ + +#include "Arduino.h" +#include "PID_peanat.h" + +const double SECS_IN_MS = 0.001; + + +/* + * Initialize PID object. + * + * @param user_kp: proportional coefficient, should be >= 0 + * @param user_ki: integral coefficient, should be >= 0 + * @param user_kd: derivative coefficient, should be >= 0 + * + * @returns PID object + */ +PID::PID(double user_kp, double user_ki, double user_kd) { + kp = user_kp; + ki = user_ki; + kd = user_kd; + init_output = 0.0; + reverse = false; + sample_time = 100; + min_output = 0.0; + max_output = 255.0; + deadband = 0.0; + + // Reverse coefficients if in reverse mode + if (reverse == true) { + kp *= -1.0; + ki *= -1.0; + kd *= -1.0; + } + + // Initiate other fields + first_run = true; + cumulative_error = 0.0; +} + +/* + * Updates PID coefficients. + * + * @param user_kp: proportional coefficient, should be >= 0 + * @param user_ki: integral coefficient, should be >= 0 + * @param user_kd: derivative coefficient, should be >= 0 + * + * @returns none + */ +void PID::updateCoeffs(double user_kp, double user_ki, double user_kd) { + kp = user_kp; + ki = user_ki; + kd = user_kd; +} + +/* + * Set initial output; must be within or on output bounds. + * + * @param user_init_output: desired initial output, for when we want the output to start at a specific value + * + * @returns none + */ +void PID::setInitOutput(double user_init_output) { + init_output = user_init_output; +} + +/* + * Toggle reverse mode on or off. + * + * @param user_reverse: true to turn reverse mode on, false to turn off + * + * @returns none + */ +void PID::setReverse(bool user_reverse) { + if (reverse != user_reverse) { + reverse = user_reverse; + kp *= -1.0; + ki *= -1.0; + kd *= -1.0; + } +} + +/* + * Set sample time (recommended < 100). + * + * @param user_sample_time: sample time in ms + * + * @returns none + */ +void PID::setSampleTime(unsigned long user_sample_time) { + sample_time = user_sample_time; +} + +/* + * Set output bounds; useful if something like a servo has input limits. + * Make sure that the initial output is on or within these bounds! + * + * @param user_min_output: minimum output value (inclusive) + * @param user_max_output: maximum output value (inclusive) + * + * @returns none + */ +void PID::setOutputBounds(double user_min_output, double user_max_output) { + min_output = user_min_output; + max_output = user_max_output; +} + +/* + * Set deadband range (difference between previous and current calculated output, in which the + * previous output is returned) if we need to prevent mechanical wear. Deadband must be less + * than max output - min output. + * + * @param user_deadband: deadband range, in units of output + * + * @returns none + */ +void PID::setDeadband(double user_deadband) { + deadband = user_deadband; +} + +/* + * Computes PID output based on standard PID algorithm; implements deadband functionality and + * measures to prevent integral windup. The first run will output the initial output value. + * This loop can run for a maximum of 49 days before unsigned long overflows. + * + * @param user_setpoint: desired value of something (e.g. servo position) + * @param user_input: the acutal value + * + * @returns output to be fed back into the controller + */ +double PID::compute(double user_setpoint, double user_input) { + // If first run, initialize variables + if (first_run == true) { + setpoint = user_setpoint; + input = user_input; + + prev_time = millis(); + prev_error = setpoint - input; + prev_output = init_output; + + output = init_output; + first_run = false; + } + + // Subsequent runs + else { + current_time = millis(); + unsigned long time_interval = current_time - prev_time; + + if (time_interval >= sample_time) { + setpoint = user_setpoint; + input = user_input; + double error = setpoint - input; + + // Proportional + output = init_output + kp * error; + // Integral + cumulative_error += ki * error * ((double)time_interval * SECS_IN_MS); + cumulative_error = min(max(cumulative_error, min_output), max_output); // Limit cumulative error to prevent windup + output += cumulative_error; + // Derivative + output += kd * (prev_error - error) / ((double)time_interval * SECS_IN_MS); + + // Limit output + output = min(max(output, min_output), max_output); + + // Don't change output if within deadband + if (fabs(output - prev_output) < deadband) { + output = prev_output; + } + + // For the next run + prev_time = current_time; + prev_error = error; + prev_output = output; + } + } + + return output; +} + +double PID::getOutput() { + return output; +} + +double PID::getkp() { + return kp; +} + +double PID::getki() { + return ki; +} + +double PID::getkd() { + return kd; +} diff --git a/sensorDemo/SteeringTest/PID_peanat.h b/sensorDemo/SteeringTest/PID_peanat.h new file mode 100644 index 0000000..cf413bf --- /dev/null +++ b/sensorDemo/SteeringTest/PID_peanat.h @@ -0,0 +1,63 @@ +#ifndef PID_PEANAT_H +#define PID_PEANAT_H + +/* + * This is a PID class to be used in PID control loops. + */ + +class PID { + + public: + + // Constructor + PID(double user_kp, double user_ki, double user_kd); + + // Change settings + void updateCoeffs(double user_kp, double user_ki, double user_kd); + void setInitOutput(double user_init_output); + void setReverse(bool user_reverse); + void setSampleTime(unsigned long user_sample_time); + void setOutputBounds(double user_min_output, double user_max_output); + void setDeadband(double user_deadband); + + // Calculate PID output + double compute(double user_setpoint, double user_input); + + // Get private variables + double getOutput(); + double getkp(); + double getki(); + double getkd(); + + + private: + + double setpoint; // Target value + + double input; // What the value really is + + double output; // Calculated output + double init_output; // Output value to start from + double prev_output; // Previous output + + double kp; // Proportional coefficient + double ki; // Integral coefficient + double kd; // Derivative coefficient + + double prev_error; // Previous error + double cumulative_error; // Cumulative error for calculating integral output + + unsigned long current_time; // Current time in milliseconds + unsigned long prev_time; // Time at which previous input was taken + unsigned long sample_time; // Input sample time + + double min_output; // Max output + double max_output; // Min output + + double deadband; // The range in which we don't change the output (to prevent mechanical wear) + + bool reverse; // Reverse mode (if true, output * -1) + bool first_run; // True if it is the very first call to the `compute` function +}; + +#endif \ No newline at end of file diff --git a/sensorDemo/SteeringTest/SteeringTest.ino b/sensorDemo/SteeringTest/SteeringTest.ino index d29dd68..d41f3fa 100644 --- a/sensorDemo/SteeringTest/SteeringTest.ino +++ b/sensorDemo/SteeringTest/SteeringTest.ino @@ -1,38 +1,42 @@ -#include "advancedSteering.h" +// #include "advancedSteering.h" +#include +#include +#include "encoder.h" +#include "encoder_steering.h" + +double desired_heading; +double current_heading; + +QueueHandle_t steeringQueue = NULL; void setup() { - Serial.begin(921600); // Start serial communication at 921600 baud rate - steeringQueue = xQueueCreate(1, sizeof(SteeringData)); // Create a queue that can hold 1 item of SteeringData type + Serial.begin(921600); // Start serial communication at 921600 baud rate - if (steeringQueue == NULL) { - Serial.println("Failed to create steering queue"); - return; // Early exit if queue creation fails - } + // Create a queue that can hold 1 item of double type + steeringQueue = xQueueCreate(1, sizeof(double)); + if (steeringQueue == NULL) { + Serial.println("Failed to create steering queue"); + return; // Early exit if queue creation fails + } - motorSetup(); // Setup motors and start the steering task + steering_setup(); } void loop() { - // Check if we have incoming data - if (Serial.available()) { - String inputString = Serial.readStringUntil('\n'); // Read the incoming data until newline - inputString.trim(); // Trim whitespace and newline characters - - // Declare a variable to hold the parsed data - SteeringData incomingData; + // Check if we have incoming data + if (Serial.available()) { + String inputString = Serial.readStringUntil('\n'); // Read the incoming data until newline + inputString.trim(); // Trim whitespace and newline characters - // Parse the string to extract lengths - if (sscanf(inputString.c_str(), "%lf,%lf", &incomingData.length1, &incomingData.length2) == 2) { - // Successfully parsed two doubles - // Overwrite the current value in the queue with the new data - incomingData.length1 *= 0.01745; - incomingData.length2 *= 0.01745; - Serial.printf("Length1: %lf\tLength2: %lf\n", incomingData.length1, incomingData.length2); - if (xQueueOverwrite(steeringQueue, &incomingData) != pdPASS) { - Serial.println("Failed to overwrite data in steering queue"); - } - } else { - Serial.println("Invalid format or incomplete data"); - } + // Parse the string to extract desired and current headings + double incomingDesiredHeading, incomingCurrentHeading; + if (sscanf(inputString.c_str(), "%lf,%lf", &incomingDesiredHeading, &incomingCurrentHeading) == 2) { + // Successfully parsed two doubles + // Update the global variables with the new data + desired_heading = incomingDesiredHeading * 0.01745; // Convert to radians + current_heading = incomingCurrentHeading * 0.01745; // Convert to radians + } else { + Serial.println("Invalid format or incomplete data"); } + } } \ No newline at end of file diff --git a/sensorDemo/SteeringTest/advancedSteering.cpp b/sensorDemo/SteeringTest/advancedSteering.cpp index c04c9b9..5c7df94 100644 --- a/sensorDemo/SteeringTest/advancedSteering.cpp +++ b/sensorDemo/SteeringTest/advancedSteering.cpp @@ -1,131 +1,131 @@ -/* - * Autonomous Parachute Steering Functon Implementation - * Keeping track of the angle of a continous rotation servo - * Author: Nischay Joshi - * Created: April 13th, 2024 - */ - -#include "advancedSteering.h" - -//Declare servo motor objects -Servo leftMotor; -Servo rightMotor; - -// Define the global variables -TaskHandle_t _steeringTask = NULL; -QueueHandle_t steeringQueue = NULL; - -void motorSetup(){ - ESP32PWM::allocateTimer(0); - ESP32PWM::allocateTimer(1); - ESP32PWM::allocateTimer(2); - ESP32PWM::allocateTimer(3); - leftMotor.attach(LEFTPIN,MINUS,MAXUS); - rightMotor.attach(RIGHTPIN,MINUS,MAXUS); - leftMotor.setPeriodHertz(SERVOFREQ); // Standard 50hz servo - rightMotor.setPeriodHertz(SERVOFREQ); // Standard 50hz servo - //set at 0 speed value - leftMotor.write(90); - rightMotor.write(90); - - //start the servo control task - BaseType_t ret = xTaskCreatePinnedToCore( - steering, /* Function to implement the task */ - "steeringTask", /* Name of the task */ - 4096, /* Stack size in words */ - NULL, /* Task input parameter */ - configMAX_PRIORITIES - 1, /* Priority of the task */ - &_steeringTask, /* Task handle. */ - 0); /* Core where the task should run */ - if (ret != pdPASS) { - Serial.println("Failed to create steering task"); - } - else { - Serial.println("Steering task created"); - } -} - -//steering function to control the steering of the parachute -void steering(void *pvParameters){ - SteeringData desiredLengths; - desiredLengths.length1 = INITIAL_LENGTH_M; - desiredLengths.length2 = INITIAL_LENGTH_M; - - double currentLength1 = desiredLengths.length1; - double currentLength2 = desiredLengths.length2; - - double desiredLength1 = currentLength1; - double desiredLength2 = currentLength2; - - double errorLength1 = 0; - double errorLength2 = 0; - - while(1){ - //if we have data in the queue, if not, keep the previous value - if (xQueueReceive(steeringQueue, &desiredLengths, 0) == pdTRUE){ - Serial.printf("Servo1: %lf\tServo2: %lf\n", desiredLengths.length1, desiredLengths.length2); - desiredLength1 = desiredLengths.length1; - desiredLength2 = desiredLengths.length2; - } - //update the distance to travel - errorLength1 = desiredLength1 - currentLength1; - errorLength2 = desiredLength2 - currentLength2; - - if(abs(errorLength1) > ERROR_DEADZONE || abs(errorLength2) > ERROR_DEADZONE){ - //calculate the amount of time to move the servo - double time1 = abs(errorLength1) / SERVO_SPEED_M_SEC * 1000; - double time2 = abs(errorLength2) / SERVO_SPEED_M_SEC * 1000; - - //move the servo - int leftMotorvalue = 90 + (errorLength1 > 0 ? SERVO_SPEED_VALUE_ANGLE : -SERVO_SPEED_VALUE_ANGLE); - int rightMotorvalue = 90 + (errorLength2 > 0 ? SERVO_SPEED_VALUE_ANGLE : -SERVO_SPEED_VALUE_ANGLE); - - if(abs(errorLength1) < ERROR_DEADZONE){ - leftMotorvalue = 90; - } - if(abs(errorLength2) < ERROR_DEADZONE){ - rightMotorvalue = 90; - } - - leftMotor.write(leftMotorvalue); - rightMotor.write(rightMotorvalue); - Serial.printf("CurrentLeng: %lf\tDesiredLength %lf\tError: %lf\tTime1: %lf\tServoValue: %d\n", currentLength1, desiredLength1, errorLength1, time1, leftMotorvalue); - unsigned long startTime = millis(); - - //wait for the servo to move - bool leftMotordone = false; - bool rightMotordone = false; - - int i = 0; - while(1){ +// /* +// * Autonomous Parachute Steering Functon Implementation +// * Keeping track of the angle of a continous rotation servo +// * Author: Nischay Joshi +// * Created: April 13th, 2024 +// */ + +// #include "advancedSteering.h" + +// //Declare servo motor objects +// Servo leftMotor; +// Servo rightMotor; + +// // Define the global variables +// TaskHandle_t _steeringTask = NULL; +// QueueHandle_t steeringQueue = NULL; + +// void motorSetup(){ +// ESP32PWM::allocateTimer(0); +// ESP32PWM::allocateTimer(1); +// ESP32PWM::allocateTimer(2); +// ESP32PWM::allocateTimer(3); +// leftMotor.attach(LEFTPIN,MINUS,MAXUS); +// rightMotor.attach(RIGHTPIN,MINUS,MAXUS); +// leftMotor.setPeriodHertz(SERVOFREQ); // Standard 50hz servo +// rightMotor.setPeriodHertz(SERVOFREQ); // Standard 50hz servo +// //set at 0 speed value +// leftMotor.write(90); +// rightMotor.write(90); + +// //start the servo control task +// BaseType_t ret = xTaskCreatePinnedToCore( +// steering, /* Function to implement the task */ +// "steeringTask", /* Name of the task */ +// 4096, /* Stack size in words */ +// NULL, /* Task input parameter */ +// configMAX_PRIORITIES - 1, /* Priority of the task */ +// &_steeringTask, /* Task handle. */ +// 0); /* Core where the task should run */ +// if (ret != pdPASS) { +// Serial.println("Failed to create steering task"); +// } +// else { +// Serial.println("Steering task created"); +// } +// } + +// //steering function to control the steering of the parachute +// void steering(void *pvParameters){ +// SteeringData desiredLengths; +// desiredLengths.length1 = INITIAL_LENGTH_M; +// desiredLengths.length2 = INITIAL_LENGTH_M; + +// double currentLength1 = desiredLengths.length1; +// double currentLength2 = desiredLengths.length2; + +// double desiredLength1 = currentLength1; +// double desiredLength2 = currentLength2; + +// double errorLength1 = 0; +// double errorLength2 = 0; + +// while(1){ +// //if we have data in the queue, if not, keep the previous value +// if (xQueueReceive(steeringQueue, &desiredLengths, 0) == pdTRUE){ +// Serial.printf("Servo1: %lf\tServo2: %lf\n", desiredLengths.length1, desiredLengths.length2); +// desiredLength1 = desiredLengths.length1; +// desiredLength2 = desiredLengths.length2; +// } +// //update the distance to travel +// errorLength1 = desiredLength1 - currentLength1; +// errorLength2 = desiredLength2 - currentLength2; + +// if(abs(errorLength1) > ERROR_DEADZONE || abs(errorLength2) > ERROR_DEADZONE){ +// //calculate the amount of time to move the servo +// double time1 = abs(errorLength1) / SERVO_SPEED_M_SEC * 1000; +// double time2 = abs(errorLength2) / SERVO_SPEED_M_SEC * 1000; + +// //move the servo +// int leftMotorvalue = 90 + (errorLength1 > 0 ? SERVO_SPEED_VALUE_ANGLE : -SERVO_SPEED_VALUE_ANGLE); +// int rightMotorvalue = 90 + (errorLength2 > 0 ? SERVO_SPEED_VALUE_ANGLE : -SERVO_SPEED_VALUE_ANGLE); + +// if(abs(errorLength1) < ERROR_DEADZONE){ +// leftMotorvalue = 90; +// } +// if(abs(errorLength2) < ERROR_DEADZONE){ +// rightMotorvalue = 90; +// } + +// leftMotor.write(leftMotorvalue); +// rightMotor.write(rightMotorvalue); +// Serial.printf("CurrentLeng: %lf\tDesiredLength %lf\tError: %lf\tTime1: %lf\tServoValue: %d\n", currentLength1, desiredLength1, errorLength1, time1, leftMotorvalue); +// unsigned long startTime = millis(); + +// //wait for the servo to move +// bool leftMotordone = false; +// bool rightMotordone = false; + +// int i = 0; +// while(1){ - if(!leftMotordone && ((millis() - startTime) > time1)){ - leftMotordone = true; - leftMotor.write(90); // Reset servo to neutral position after moving - Serial.println("Servo Left done"); - } - if(!rightMotordone && ((millis() - startTime) > time2)){ - rightMotordone = true; - rightMotor.write(90); // Reset servo to neutral position after moving - Serial.println("Servo right done"); - } - if(leftMotordone && rightMotordone){ - break; // Exit loop if both servos have completed their movements - } - // Wait for 1 ms to prevent hogging the CPU - // vTaskDelay(1 / portTICK_PERIOD_MS); - delay(1); - Serial.println(i++); - } - - leftMotor.write(90); - rightMotor.write(90); - // Update the current lengths - currentLength1 = desiredLengths.length1; - currentLength2 = desiredLengths.length2; - } - - vTaskDelay(1 / portTICK_PERIOD_MS); - - } -} \ No newline at end of file +// if(!leftMotordone && ((millis() - startTime) > time1)){ +// leftMotordone = true; +// leftMotor.write(90); // Reset servo to neutral position after moving +// Serial.println("Servo Left done"); +// } +// if(!rightMotordone && ((millis() - startTime) > time2)){ +// rightMotordone = true; +// rightMotor.write(90); // Reset servo to neutral position after moving +// Serial.println("Servo right done"); +// } +// if(leftMotordone && rightMotordone){ +// break; // Exit loop if both servos have completed their movements +// } +// // Wait for 1 ms to prevent hogging the CPU +// // vTaskDelay(1 / portTICK_PERIOD_MS); +// delay(1); +// Serial.println(i++); +// } + +// leftMotor.write(90); +// rightMotor.write(90); +// // Update the current lengths +// currentLength1 = desiredLengths.length1; +// currentLength2 = desiredLengths.length2; +// } + +// vTaskDelay(1 / portTICK_PERIOD_MS); + +// } +// } \ No newline at end of file diff --git a/sensorDemo/SteeringTest/advancedSteering.h b/sensorDemo/SteeringTest/advancedSteering.h index db699d4..75f2dc1 100644 --- a/sensorDemo/SteeringTest/advancedSteering.h +++ b/sensorDemo/SteeringTest/advancedSteering.h @@ -1,41 +1,41 @@ -/* - * Autonomous Parachute Steering Functon Header - * Keeping track of the angle of a continous rotation servo - * Author: Nischay Joshi - * Created: April 13th, 2024 - */ +// /* +// * Autonomous Parachute Steering Functon Header +// * Keeping track of the angle of a continous rotation servo +// * Author: Nischay Joshi +// * Created: April 13th, 2024 +// */ -#ifndef ADVANCEDSTEERING_H -#define ADVANCEDSTEERING_H +// #ifndef ADVANCEDSTEERING_H +// #define ADVANCEDSTEERING_H -#include -#include +// #include +// #include -#define MINUS 700 -#define MAXUS 2300 -#define LEFTPIN 26 -#define RIGHTPIN 27 -#define SERVOFREQ 50 +// #define MINUS 700 +// #define MAXUS 2300 +// #define LEFTPIN 26 +// #define RIGHTPIN 27 +// #define SERVOFREQ 50 -#define WHEELRADIUS_M 1 -#define INITIAL_LENGTH_M 0.00 -#define ERROR_DEADZONE 0.001 +// #define WHEELRADIUS_M 1 +// #define INITIAL_LENGTH_M 0.00 +// #define ERROR_DEADZONE 0.001 -#define SERVO_SPEED_RAD_SEC 14.38378555 -#define SERVO_SPEED_M_SEC SERVO_SPEED_RAD_SEC * WHEELRADIUS_M -#define SERVO_SPEED_VALUE_ANGLE 80 +// #define SERVO_SPEED_RAD_SEC 14.38378555 +// #define SERVO_SPEED_M_SEC SERVO_SPEED_RAD_SEC * WHEELRADIUS_M +// #define SERVO_SPEED_VALUE_ANGLE 80 -typedef struct { - double length1; - double length2; -} SteeringData; +// typedef struct { +// double length1; +// double length2; +// } SteeringData; -//a queue to send the data to the steering function -extern TaskHandle_t _steeringTask; -extern QueueHandle_t steeringQueue; +// //a queue to send the data to the steering function +// extern TaskHandle_t _steeringTask; +// extern QueueHandle_t steeringQueue; -void motorSetup(); -void steering(void *pvParameters); +// void motorSetup(); +// void steering(void *pvParameters); -#endif \ No newline at end of file +// #endif \ No newline at end of file diff --git a/sensorDemo/SteeringTest/encoder.cpp b/sensorDemo/SteeringTest/encoder.cpp new file mode 100644 index 0000000..170de86 --- /dev/null +++ b/sensorDemo/SteeringTest/encoder.cpp @@ -0,0 +1,27 @@ +#include "encoder.h" + +// Definitions of global variables +volatile bool forward_1 = true; +volatile bool forward_2 = true; +volatile unsigned long time_1 = millis(); +volatile unsigned long time_2 = millis(); +volatile int count_1 = 0; +volatile int count_2 = 0; + +// Interrupt service routines +void IRAM_ATTR enc_1_isr() { + unsigned long current = millis(); + if (current - time_1 > DEBOUNCE_INTERVAL) { + if (forward_1) { count_1++; } + else { count_1--; } + time_1 = current; + } +} +void IRAM_ATTR enc_2_isr() { + unsigned long current = millis(); + if (current - time_2 > DEBOUNCE_INTERVAL) { + if (forward_2) { count_2++; } + else { count_2--; } + time_2 = current; + } +} diff --git a/sensorDemo/SteeringTest/encoder.h b/sensorDemo/SteeringTest/encoder.h new file mode 100644 index 0000000..ef40d56 --- /dev/null +++ b/sensorDemo/SteeringTest/encoder.h @@ -0,0 +1,23 @@ +#ifndef ENCODER_H +#define ENCODER_H + +#include + +#define ENC_1 25 +#define ENC_2 33 + +#define DEBOUNCE_INTERVAL 15 // ms + +// Function prototypes +void enc_1_isr(); +void enc_2_isr(); + +// Declaration of global variables (extern keyword indicates that they are defined elsewhere) +extern volatile bool forward_1; +extern volatile bool forward_2; +extern volatile unsigned long time_1; +extern volatile unsigned long time_2; +extern volatile int count_1; +extern volatile int count_2; + +#endif diff --git a/sensorDemo/SteeringTest/encoder_steering.cpp b/sensorDemo/SteeringTest/encoder_steering.cpp new file mode 100644 index 0000000..3a76833 --- /dev/null +++ b/sensorDemo/SteeringTest/encoder_steering.cpp @@ -0,0 +1,95 @@ +// Steering with feedback from encoders + +#include +#include +#include "PID_peanat.h" +#include "encoder.h" +#include "encoder_steering.h" + +// Declare servo motor objects +Servo servo_1; +Servo servo_2; + +// Initialize PID +PID pid(10.0, 0, 0); + +void steering_setup() { + pid.setOutputBounds(0, 150); // Limit to 150 mm extension/retraction + pid.setSampleTime(10); // ms + pid.setDeadband(DIST_PER_TICK); + + servo_1.attach(SERVO_1, MIN_US, MAX_US); + servo_1.setPeriodHertz(SERVO_FREQ); + servo_2.attach(SERVO_2, MIN_US, MAX_US); + servo_2.setPeriodHertz(SERVO_FREQ); + // No spin + servo_1.write(90); + servo_2.write(90); + + // Start task + xTaskCreatePinnedToCore( + servoControlTask, // Task function + "ServoControlTask", // Task name + 4096, // Stack size (words) + NULL, // Task parameters + -1, // Priority (1 is default) + NULL, // Task handle + 0 // Core number (0 or 1) + ); +} + + +// Get difference between two angles in radians [-pi, pi) +double angle_diff(double angle1, double angle2) { + double diff = angle1 - angle2; + while (diff >= PI) { + diff -= 2 * PI; + } + while (diff < -PI) { + diff += 2 * PI; + } + return diff; +} + + +/* + * Function to control servos + * @param des_heading + * @param current_heading + */ +void servo_control(double des_heading, double current_heading) { + SteeringData des_lengths; + SteeringData current_lengths = {DIST_PER_TICK*count_1, DIST_PER_TICK*count_2}; + + double input = angle_diff(des_heading, current_heading); + if (input < FORWARD_THRESH) { // Go forward if we don't need to change our heading that much + des_lengths.l1 = -100.0; // Servo 1 + des_lengths.l2 = -100.0; // Servo 2 + } + else { + double output = pid.compute(0.0, input); + des_lengths.l1 = output; + des_lengths.l2 = -output; + } + + // Write to servos + int val_1 = 90 + (des_lengths.l1 > current_lengths.l1 ? SERVO_SPEED_VALUE_ANGLE : (des_lengths.l1 < current_lengths.l1 ? -SERVO_SPEED_VALUE_ANGLE : 0)); + servo_1.write(val_1); + int val_2 = 90 + (des_lengths.l2 > current_lengths.l2 ? SERVO_SPEED_VALUE_ANGLE : (des_lengths.l2 < current_lengths.l2 ? -SERVO_SPEED_VALUE_ANGLE : 0)); + servo_2.write(val_2); +} + + +void servoControlTask(void *pvParameters) { + (void) pvParameters; + + while (true) { + // Call servo_control function here + servo_control(desired_heading, current_heading); // Modify parameters as needed + + // Delay for the desired period (in milliseconds) + vTaskDelay(pdMS_TO_TICKS(10)); // Adjust delay as needed + } +} + + diff --git a/sensorDemo/SteeringTest/encoder_steering.h b/sensorDemo/SteeringTest/encoder_steering.h new file mode 100644 index 0000000..652328a --- /dev/null +++ b/sensorDemo/SteeringTest/encoder_steering.h @@ -0,0 +1,30 @@ +#ifndef ENCODER_STEERING_H +#define ENCODER_STEERING_H + +#define SERVO_1 26 +#define SERVO_2 27 + +#define MIN_US 700 +#define MAX_US 2300 +#define SERVO_FREQ 50 // Standard 50hz servo +#define SERVO_SPEED_VALUE_ANGLE 80 +#define PI 3.14159 + +static const double DRUM_DIAMETER = 2.2; // mm +static const double DIST_PER_TICK = PI*DRUM_DIAMETER / 4; // mm (There are 4 markers around the wheel) +static const double FORWARD_THRESH = 10.0 * PI/180; // rad (Go forward if we are within 10 degrees) + +typedef struct { + double l1; + double l2; +} SteeringData; + +extern double desired_heading; +extern double current_heading; + +void steering_setup(); +double angle_diff(double angle1, double angle2); +void servo_control(double des_heading, double current_heading); +void servoControlTask(void *pvParameters); + +#endif \ No newline at end of file diff --git a/sensorDemo/encoder_test/encoder.cpp b/sensorDemo/encoder_test/encoder.cpp deleted file mode 100644 index 0735c69..0000000 --- a/sensorDemo/encoder_test/encoder.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// #include "Arduino.h" -// #include "encoder.h" - -// /* -// * Constructor -// */ -// Encoder::Encoder() { -// forward = true; -// time = millis(); -// count = 0; -// } - -// /* -// * Divides count by 2 since each tick triggers encoder twice using this scuffed debouncing method -// */ -// int Encoder::get_count() { -// int count_adj = (int)count / 2; -// return count_adj; -// } - - - - From 03aeda25de8e4d12e034bfdcb113ff8f224991a9 Mon Sep 17 00:00:00 2001 From: p-eanat Date: Tue, 7 May 2024 22:47:56 -0700 Subject: [PATCH 08/14] Manual PID control with encoders working --- sensorDemo/SteeringTest/PID_peanat.cpp | 1 + sensorDemo/SteeringTest/SteeringTest.ino | 13 +++++- sensorDemo/SteeringTest/encoder.h | 2 +- sensorDemo/SteeringTest/encoder_steering.cpp | 42 +++++++++++++++----- sensorDemo/SteeringTest/encoder_steering.h | 8 ++-- src/SUAS_Link_2024/src/main.cpp | 15 ++++--- src/SUAS_Link_2024/src/params.h | 4 +- 7 files changed, 59 insertions(+), 26 deletions(-) diff --git a/sensorDemo/SteeringTest/PID_peanat.cpp b/sensorDemo/SteeringTest/PID_peanat.cpp index b430fa8..2ad128a 100644 --- a/sensorDemo/SteeringTest/PID_peanat.cpp +++ b/sensorDemo/SteeringTest/PID_peanat.cpp @@ -5,6 +5,7 @@ */ #include "Arduino.h" +#include #include "PID_peanat.h" const double SECS_IN_MS = 0.001; diff --git a/sensorDemo/SteeringTest/SteeringTest.ino b/sensorDemo/SteeringTest/SteeringTest.ino index d41f3fa..a5d33e4 100644 --- a/sensorDemo/SteeringTest/SteeringTest.ino +++ b/sensorDemo/SteeringTest/SteeringTest.ino @@ -4,14 +4,19 @@ #include "encoder.h" #include "encoder_steering.h" -double desired_heading; -double current_heading; +double desired_heading = 0; +double current_heading = 0; QueueHandle_t steeringQueue = NULL; void setup() { Serial.begin(921600); // Start serial communication at 921600 baud rate + pinMode(ENC_1, INPUT); + attachInterrupt(ENC_1, enc_1_isr, RISING); + pinMode(ENC_2, INPUT); + attachInterrupt(ENC_2, enc_2_isr, RISING); + // Create a queue that can hold 1 item of double type steeringQueue = xQueueCreate(1, sizeof(double)); if (steeringQueue == NULL) { @@ -35,8 +40,12 @@ void loop() { // Update the global variables with the new data desired_heading = incomingDesiredHeading * 0.01745; // Convert to radians current_heading = incomingCurrentHeading * 0.01745; // Convert to radians + Serial.printf("Desired heading: %lf, Current heading: %lf\n", desired_heading, current_heading); } else { Serial.println("Invalid format or incomplete data"); } } + + Serial.printf("Count: %d %d\n", count_1, count_2); + delay(100); } \ No newline at end of file diff --git a/sensorDemo/SteeringTest/encoder.h b/sensorDemo/SteeringTest/encoder.h index ef40d56..4b0f4a8 100644 --- a/sensorDemo/SteeringTest/encoder.h +++ b/sensorDemo/SteeringTest/encoder.h @@ -6,7 +6,7 @@ #define ENC_1 25 #define ENC_2 33 -#define DEBOUNCE_INTERVAL 15 // ms +#define DEBOUNCE_INTERVAL 10 // ms // Function prototypes void enc_1_isr(); diff --git a/sensorDemo/SteeringTest/encoder_steering.cpp b/sensorDemo/SteeringTest/encoder_steering.cpp index 3a76833..c4d5eb1 100644 --- a/sensorDemo/SteeringTest/encoder_steering.cpp +++ b/sensorDemo/SteeringTest/encoder_steering.cpp @@ -2,6 +2,7 @@ #include #include +#include #include "PID_peanat.h" #include "encoder.h" #include "encoder_steering.h" @@ -11,12 +12,12 @@ Servo servo_1; Servo servo_2; // Initialize PID -PID pid(10.0, 0, 0); +PID pid(50.0, 0, 0); void steering_setup() { - pid.setOutputBounds(0, 150); // Limit to 150 mm extension/retraction + pid.setOutputBounds(-150, 150); // Limit to 150 mm extension/retraction pid.setSampleTime(10); // ms - pid.setDeadband(DIST_PER_TICK); + // pid.setDeadband(DIST_PER_TICK); servo_1.attach(SERVO_1, MIN_US, MAX_US); servo_1.setPeriodHertz(SERVO_FREQ); @@ -59,24 +60,45 @@ double angle_diff(double angle1, double angle2) { */ void servo_control(double des_heading, double current_heading) { SteeringData des_lengths; - SteeringData current_lengths = {DIST_PER_TICK*count_1, DIST_PER_TICK*count_2}; + SteeringData current_lengths = {(double)DIST_PER_TICK*((int)count_1/2), (double)DIST_PER_TICK*((int)count_2/2)}; double input = angle_diff(des_heading, current_heading); - if (input < FORWARD_THRESH) { // Go forward if we don't need to change our heading that much + Serial.printf("Input: %lf\n", input); + if (fabs(input) < FORWARD_THRESH) { // Go forward if we don't need to change our heading that much des_lengths.l1 = -100.0; // Servo 1 des_lengths.l2 = -100.0; // Servo 2 } else { double output = pid.compute(0.0, input); + // Serial.printf("Output: %lf\n", output); des_lengths.l1 = output; des_lengths.l2 = -output; } - // Write to servos - int val_1 = 90 + (des_lengths.l1 > current_lengths.l1 ? SERVO_SPEED_VALUE_ANGLE : (des_lengths.l1 < current_lengths.l1 ? -SERVO_SPEED_VALUE_ANGLE : 0)); - servo_1.write(val_1); - int val_2 = 90 + (des_lengths.l2 > current_lengths.l2 ? SERVO_SPEED_VALUE_ANGLE : (des_lengths.l2 < current_lengths.l2 ? -SERVO_SPEED_VALUE_ANGLE : 0)); - servo_2.write(val_2); + Serial.printf("Lengths: %lf %lf\n", des_lengths.l1, des_lengths.l2); + + // Servo 1 + if (des_lengths.l1 - current_lengths.l1 > (DIST_PER_TICK)) { + servo_1.write(90 + SERVO_SPEED_VALUE_ANGLE); + forward_1 = true; + } else if (des_lengths.l1 - current_lengths.l1 < -(DIST_PER_TICK)) { + servo_1.write(90 - SERVO_SPEED_VALUE_ANGLE); + forward_1 = false; + } else { + servo_1.write(90); + } + + // Servo 2 + if (des_lengths.l2 - current_lengths.l2 > (DIST_PER_TICK)) { + servo_2.write(90 - SERVO_SPEED_VALUE_ANGLE); + forward_2 = true; + } else if (des_lengths.l2 - current_lengths.l2 < -(DIST_PER_TICK)) { + servo_2.write(90 + SERVO_SPEED_VALUE_ANGLE); + forward_2 = false; + } else { + servo_2.write(90); + } + } diff --git a/sensorDemo/SteeringTest/encoder_steering.h b/sensorDemo/SteeringTest/encoder_steering.h index 652328a..7013b94 100644 --- a/sensorDemo/SteeringTest/encoder_steering.h +++ b/sensorDemo/SteeringTest/encoder_steering.h @@ -10,9 +10,9 @@ #define SERVO_SPEED_VALUE_ANGLE 80 #define PI 3.14159 -static const double DRUM_DIAMETER = 2.2; // mm -static const double DIST_PER_TICK = PI*DRUM_DIAMETER / 4; // mm (There are 4 markers around the wheel) -static const double FORWARD_THRESH = 10.0 * PI/180; // rad (Go forward if we are within 10 degrees) +static const double DRUM_DIAMETER = 22; // mm +static const double DIST_PER_TICK = (double)PI*DRUM_DIAMETER / 4.0; // mm (There are 4 markers around the wheel) +static const double FORWARD_THRESH = 10.0 * (double)PI/180.0; // rad (Go forward if we are within 10 degrees) typedef struct { double l1; @@ -21,6 +21,8 @@ typedef struct { extern double desired_heading; extern double current_heading; +extern volatile bool forward_1; +extern volatile bool forward_2; void steering_setup(); double angle_diff(double angle1, double angle2); diff --git a/src/SUAS_Link_2024/src/main.cpp b/src/SUAS_Link_2024/src/main.cpp index e6c813e..e30cf3d 100644 --- a/src/SUAS_Link_2024/src/main.cpp +++ b/src/SUAS_Link_2024/src/main.cpp @@ -78,8 +78,8 @@ void loop() { // Read drop point and bottle number from Pi (blocking) // drop_data = recieveData(); - drop_data.lon = -123.2478006; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!! - drop_data.lat = 49.2624333; + drop_data.lon = -123.247933; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!! , + drop_data.lat = 49.262253; drop_data.heading = 0.0; drop_data.bottleID = 1; @@ -102,7 +102,8 @@ void loop() { PiSerial.print(buffer); Serial.print("Sent desired drop point to Pi: "); Serial.print(buffer); - notDropped = false; // REMOVE THIS LATER!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + msg = pixhawk.read_messages(); + Serial.println(msg.heartbeat.system_status); // Wait to get close enough to desired drop point digitalWrite(LED_RED, HIGH); @@ -111,7 +112,7 @@ void loop() { msg = pixhawk.read_messages(); double lat = (double) msg.global_position_int.lat / 10000000.0; double lon = (double) msg.global_position_int.lon / 10000000.0; - // Serial.println("Coords: " + String(lat) + ", " + String(lon)); + // Serial.println("Coords: " + String(lat, 8) + ", " + String(lon, 8)); double dist_from_drop_point = distance(lat, lon, des_drop_data.lat, des_drop_data.lon); Serial.println("Dist: " + String(dist_from_drop_point)); @@ -122,10 +123,8 @@ void loop() { } digitalWrite(LED_RED, LOW); - // Send message to parachutes - broadcastMessage(des_drop_data); - - delay(3000); // REMOVE LATER!!!!!!!!!!!!!!!!!!!!!! + // // Send message to parachutes + // broadcastMessage(des_drop_data); if (des_drop_data.bottleID == 1 || des_drop_data.bottleID == 3 || des_drop_data.bottleID == 5) { servo_front_R.write(160); diff --git a/src/SUAS_Link_2024/src/params.h b/src/SUAS_Link_2024/src/params.h index 0e7083f..c4983e0 100644 --- a/src/SUAS_Link_2024/src/params.h +++ b/src/SUAS_Link_2024/src/params.h @@ -21,8 +21,8 @@ extern HardwareSerial CubeSerial; #define AIRCRAFT_SPEED 7.0 // m/s #define DRIFT_FACTOR 0.1 // TODO: find this from testing, it's arbitrary rn!! -#define RELEASE_DELAY 1.0 // s -#define RELEASE_MARGIN 5.0 // Tolerance margin from desired drop point that we will trigger release +#define RELEASE_DELAY 0.0 // s // TODO!!! +#define RELEASE_MARGIN 3.0 // Tolerance margin from desired drop point that we will trigger release #define WINDOW_SIZE 10 From ed30654d0b6846b57c1e7a91bc4af471e63a94c3 Mon Sep 17 00:00:00 2001 From: Nischay Joshi Date: Sat, 11 May 2024 13:26:59 -0700 Subject: [PATCH 09/14] Modified Steering to work with queue --- sensorDemo/SteeringTest/SteeringTest.ino | 23 +--- sensorDemo/SteeringTest/advancedSteering.cpp | 131 ------------------- sensorDemo/SteeringTest/advancedSteering.h | 41 ------ sensorDemo/SteeringTest/encoder_steering.cpp | 92 ++++++++++--- sensorDemo/SteeringTest/encoder_steering.h | 20 ++- 5 files changed, 95 insertions(+), 212 deletions(-) delete mode 100644 sensorDemo/SteeringTest/advancedSteering.cpp delete mode 100644 sensorDemo/SteeringTest/advancedSteering.h diff --git a/sensorDemo/SteeringTest/SteeringTest.ino b/sensorDemo/SteeringTest/SteeringTest.ino index a5d33e4..8db3dae 100644 --- a/sensorDemo/SteeringTest/SteeringTest.ino +++ b/sensorDemo/SteeringTest/SteeringTest.ino @@ -1,30 +1,15 @@ // #include "advancedSteering.h" #include -#include -#include "encoder.h" #include "encoder_steering.h" double desired_heading = 0; double current_heading = 0; -QueueHandle_t steeringQueue = NULL; + void setup() { Serial.begin(921600); // Start serial communication at 921600 baud rate - - pinMode(ENC_1, INPUT); - attachInterrupt(ENC_1, enc_1_isr, RISING); - pinMode(ENC_2, INPUT); - attachInterrupt(ENC_2, enc_2_isr, RISING); - - // Create a queue that can hold 1 item of double type - steeringQueue = xQueueCreate(1, sizeof(double)); - if (steeringQueue == NULL) { - Serial.println("Failed to create steering queue"); - return; // Early exit if queue creation fails - } - - steering_setup(); + steering_setup(100.0, 50.0, 0, 0); // Initialize steering } void loop() { @@ -44,6 +29,10 @@ void loop() { } else { Serial.println("Invalid format or incomplete data"); } + // create the data packet + AngleData data = {desired_heading, current_heading, 0}; + // send the data packet to the queue + sendSteeringData(data); } Serial.printf("Count: %d %d\n", count_1, count_2); diff --git a/sensorDemo/SteeringTest/advancedSteering.cpp b/sensorDemo/SteeringTest/advancedSteering.cpp deleted file mode 100644 index 5c7df94..0000000 --- a/sensorDemo/SteeringTest/advancedSteering.cpp +++ /dev/null @@ -1,131 +0,0 @@ -// /* -// * Autonomous Parachute Steering Functon Implementation -// * Keeping track of the angle of a continous rotation servo -// * Author: Nischay Joshi -// * Created: April 13th, 2024 -// */ - -// #include "advancedSteering.h" - -// //Declare servo motor objects -// Servo leftMotor; -// Servo rightMotor; - -// // Define the global variables -// TaskHandle_t _steeringTask = NULL; -// QueueHandle_t steeringQueue = NULL; - -// void motorSetup(){ -// ESP32PWM::allocateTimer(0); -// ESP32PWM::allocateTimer(1); -// ESP32PWM::allocateTimer(2); -// ESP32PWM::allocateTimer(3); -// leftMotor.attach(LEFTPIN,MINUS,MAXUS); -// rightMotor.attach(RIGHTPIN,MINUS,MAXUS); -// leftMotor.setPeriodHertz(SERVOFREQ); // Standard 50hz servo -// rightMotor.setPeriodHertz(SERVOFREQ); // Standard 50hz servo -// //set at 0 speed value -// leftMotor.write(90); -// rightMotor.write(90); - -// //start the servo control task -// BaseType_t ret = xTaskCreatePinnedToCore( -// steering, /* Function to implement the task */ -// "steeringTask", /* Name of the task */ -// 4096, /* Stack size in words */ -// NULL, /* Task input parameter */ -// configMAX_PRIORITIES - 1, /* Priority of the task */ -// &_steeringTask, /* Task handle. */ -// 0); /* Core where the task should run */ -// if (ret != pdPASS) { -// Serial.println("Failed to create steering task"); -// } -// else { -// Serial.println("Steering task created"); -// } -// } - -// //steering function to control the steering of the parachute -// void steering(void *pvParameters){ -// SteeringData desiredLengths; -// desiredLengths.length1 = INITIAL_LENGTH_M; -// desiredLengths.length2 = INITIAL_LENGTH_M; - -// double currentLength1 = desiredLengths.length1; -// double currentLength2 = desiredLengths.length2; - -// double desiredLength1 = currentLength1; -// double desiredLength2 = currentLength2; - -// double errorLength1 = 0; -// double errorLength2 = 0; - -// while(1){ -// //if we have data in the queue, if not, keep the previous value -// if (xQueueReceive(steeringQueue, &desiredLengths, 0) == pdTRUE){ -// Serial.printf("Servo1: %lf\tServo2: %lf\n", desiredLengths.length1, desiredLengths.length2); -// desiredLength1 = desiredLengths.length1; -// desiredLength2 = desiredLengths.length2; -// } -// //update the distance to travel -// errorLength1 = desiredLength1 - currentLength1; -// errorLength2 = desiredLength2 - currentLength2; - -// if(abs(errorLength1) > ERROR_DEADZONE || abs(errorLength2) > ERROR_DEADZONE){ -// //calculate the amount of time to move the servo -// double time1 = abs(errorLength1) / SERVO_SPEED_M_SEC * 1000; -// double time2 = abs(errorLength2) / SERVO_SPEED_M_SEC * 1000; - -// //move the servo -// int leftMotorvalue = 90 + (errorLength1 > 0 ? SERVO_SPEED_VALUE_ANGLE : -SERVO_SPEED_VALUE_ANGLE); -// int rightMotorvalue = 90 + (errorLength2 > 0 ? SERVO_SPEED_VALUE_ANGLE : -SERVO_SPEED_VALUE_ANGLE); - -// if(abs(errorLength1) < ERROR_DEADZONE){ -// leftMotorvalue = 90; -// } -// if(abs(errorLength2) < ERROR_DEADZONE){ -// rightMotorvalue = 90; -// } - -// leftMotor.write(leftMotorvalue); -// rightMotor.write(rightMotorvalue); -// Serial.printf("CurrentLeng: %lf\tDesiredLength %lf\tError: %lf\tTime1: %lf\tServoValue: %d\n", currentLength1, desiredLength1, errorLength1, time1, leftMotorvalue); -// unsigned long startTime = millis(); - -// //wait for the servo to move -// bool leftMotordone = false; -// bool rightMotordone = false; - -// int i = 0; -// while(1){ - -// if(!leftMotordone && ((millis() - startTime) > time1)){ -// leftMotordone = true; -// leftMotor.write(90); // Reset servo to neutral position after moving -// Serial.println("Servo Left done"); -// } -// if(!rightMotordone && ((millis() - startTime) > time2)){ -// rightMotordone = true; -// rightMotor.write(90); // Reset servo to neutral position after moving -// Serial.println("Servo right done"); -// } -// if(leftMotordone && rightMotordone){ -// break; // Exit loop if both servos have completed their movements -// } -// // Wait for 1 ms to prevent hogging the CPU -// // vTaskDelay(1 / portTICK_PERIOD_MS); -// delay(1); -// Serial.println(i++); -// } - -// leftMotor.write(90); -// rightMotor.write(90); -// // Update the current lengths -// currentLength1 = desiredLengths.length1; -// currentLength2 = desiredLengths.length2; -// } - -// vTaskDelay(1 / portTICK_PERIOD_MS); - -// } -// } \ No newline at end of file diff --git a/sensorDemo/SteeringTest/advancedSteering.h b/sensorDemo/SteeringTest/advancedSteering.h deleted file mode 100644 index 75f2dc1..0000000 --- a/sensorDemo/SteeringTest/advancedSteering.h +++ /dev/null @@ -1,41 +0,0 @@ -// /* -// * Autonomous Parachute Steering Functon Header -// * Keeping track of the angle of a continous rotation servo -// * Author: Nischay Joshi -// * Created: April 13th, 2024 -// */ - -// #ifndef ADVANCEDSTEERING_H -// #define ADVANCEDSTEERING_H - -// #include -// #include - -// #define MINUS 700 -// #define MAXUS 2300 -// #define LEFTPIN 26 -// #define RIGHTPIN 27 -// #define SERVOFREQ 50 - -// #define WHEELRADIUS_M 1 -// #define INITIAL_LENGTH_M 0.00 -// #define ERROR_DEADZONE 0.001 - -// #define SERVO_SPEED_RAD_SEC 14.38378555 -// #define SERVO_SPEED_M_SEC SERVO_SPEED_RAD_SEC * WHEELRADIUS_M -// #define SERVO_SPEED_VALUE_ANGLE 80 - - -// typedef struct { -// double length1; -// double length2; -// } SteeringData; - -// //a queue to send the data to the steering function -// extern TaskHandle_t _steeringTask; -// extern QueueHandle_t steeringQueue; - -// void motorSetup(); -// void steering(void *pvParameters); - -// #endif \ No newline at end of file diff --git a/sensorDemo/SteeringTest/encoder_steering.cpp b/sensorDemo/SteeringTest/encoder_steering.cpp index c4d5eb1..dd0753a 100644 --- a/sensorDemo/SteeringTest/encoder_steering.cpp +++ b/sensorDemo/SteeringTest/encoder_steering.cpp @@ -1,10 +1,4 @@ // Steering with feedback from encoders - -#include -#include -#include -#include "PID_peanat.h" -#include "encoder.h" #include "encoder_steering.h" // Declare servo motor objects @@ -13,12 +7,32 @@ Servo servo_2; // Initialize PID PID pid(50.0, 0, 0); +TaskHandle_t _servoControlTask = NULL; +QueueHandle_t steeringQueue = NULL; + +#define SERVO_CONTROL_PERIOD 10 // ms +volatile int PID_Control_Period = SERVO_CONTROL_PERIOD; + +void steering_setup(double acquireRate, double kp, double ki, double kd) { -void steering_setup() { + // Initialize encoders + Serial.println("Initializing encoders"); + pinMode(ENC_1, INPUT); + attachInterrupt(ENC_1, enc_1_isr, RISING); + pinMode(ENC_2, INPUT); + attachInterrupt(ENC_2, enc_2_isr, RISING); + + // Initialize PID + Serial.println("Initializing PID"); + Serial.printf("kp: %lf, ki: %lf, kd: %lf\n", kp, ki, kd); + pid.updateCoeffs(kp, ki, kd); pid.setOutputBounds(-150, 150); // Limit to 150 mm extension/retraction - pid.setSampleTime(10); // ms + pid.setSampleTime((1.0/acquireRate)*1000); // ms + PID_Control_Period = (1.0/acquireRate)*1000; // pid.setDeadband(DIST_PER_TICK); + // Initialize servos + Serial.println("Initializing servos"); servo_1.attach(SERVO_1, MIN_US, MAX_US); servo_1.setPeriodHertz(SERVO_FREQ); servo_2.attach(SERVO_2, MIN_US, MAX_US); @@ -27,16 +41,33 @@ void steering_setup() { servo_1.write(90); servo_2.write(90); + // Create queue + steeringQueue = xQueueCreate(1, sizeof(AngleData)); // this queue will take the yaw value + if (steeringQueue == NULL) { + Serial.println("Failed to create steering queue"); + while(1){ + Serial.println("Failed to create steering queue"); + delay(500); + } // if queue creation fails + } + Serial.println("Steering queue created"); + // Start task - xTaskCreatePinnedToCore( + BaseType_t ret = xTaskCreatePinnedToCore( servoControlTask, // Task function "ServoControlTask", // Task name 4096, // Stack size (words) NULL, // Task parameters - -1, // Priority (1 is default) - NULL, // Task handle + configMAX_PRIORITIES -1,// Priority (1 is default) + &_servoControlTask, // Task handle 0 // Core number (0 or 1) ); + if (ret != pdPASS) { + Serial.println("Failed to create servo control task"); + } else { + Serial.println("Servo control task created"); + } + } @@ -54,28 +85,27 @@ double angle_diff(double angle1, double angle2) { /* - * Function to control servos - * @param des_heading - * @param current_heading + * Function to control servos + * @param data: AngleData struct containing desired and current yaw values */ -void servo_control(double des_heading, double current_heading) { +void servo_control(AngleData data) { SteeringData des_lengths; SteeringData current_lengths = {(double)DIST_PER_TICK*((int)count_1/2), (double)DIST_PER_TICK*((int)count_2/2)}; - double input = angle_diff(des_heading, current_heading); - Serial.printf("Input: %lf\n", input); + double input = angle_diff(data.desiredYaw, data.currentYaw); + Serial.printf("PID Input: %lf\n", input); // Comment When testing is done if (fabs(input) < FORWARD_THRESH) { // Go forward if we don't need to change our heading that much des_lengths.l1 = -100.0; // Servo 1 des_lengths.l2 = -100.0; // Servo 2 } else { double output = pid.compute(0.0, input); - // Serial.printf("Output: %lf\n", output); + Serial.printf("PID Output: %lf\n", output); des_lengths.l1 = output; des_lengths.l2 = -output; } - Serial.printf("Lengths: %lf %lf\n", des_lengths.l1, des_lengths.l2); + Serial.printf("Lengths: %lf %lf\n", des_lengths.l1, des_lengths.l2); //- -- Comment me when testing is done // Servo 1 if (des_lengths.l1 - current_lengths.l1 > (DIST_PER_TICK)) { @@ -105,13 +135,33 @@ void servo_control(double des_heading, double current_heading) { void servoControlTask(void *pvParameters) { (void) pvParameters; + AngleData incomingData; + // initialize the incoming data + incomingData.desiredYaw = 0; + incomingData.currentYaw = 0; + incomingData.desiredForward = 0; + while (true) { + // Check if we have incoming data + if (xQueueReceive(steeringQueue, &incomingData, 0) == pdTRUE) { + // Successfully received data + Serial.printf("Desired yaw: %lf, Current yaw: %lf, Desired forward: %lf\n", incomingData.desiredYaw, incomingData.currentYaw, incomingData.desiredForward); + } // Call servo_control function here - servo_control(desired_heading, current_heading); // Modify parameters as needed + servo_control(incomingData); // Modify parameters as needed // Delay for the desired period (in milliseconds) - vTaskDelay(pdMS_TO_TICKS(10)); // Adjust delay as needed + vTaskDelay(pdMS_TO_TICKS(PID_Control_Period)); // Adjust delay as needed } } +/* +* Function to send the steering data to the steering task +* @param data: AngleData struct containing desired and current yaw values +*/ +void sendSteeringData(AngleData data) { + if (xQueueOverwrite(steeringQueue, &data) != pdPASS) { + Serial.println("Failed to overwrite data in steering queue"); + } +} diff --git a/sensorDemo/SteeringTest/encoder_steering.h b/sensorDemo/SteeringTest/encoder_steering.h index 7013b94..7fea64f 100644 --- a/sensorDemo/SteeringTest/encoder_steering.h +++ b/sensorDemo/SteeringTest/encoder_steering.h @@ -1,6 +1,12 @@ #ifndef ENCODER_STEERING_H #define ENCODER_STEERING_H +#include +#include +#include +#include "PID_peanat.h" +#include "encoder.h" + #define SERVO_1 26 #define SERVO_2 27 @@ -19,14 +25,24 @@ typedef struct { double l2; } SteeringData; +typedef struct { + double desiredYaw; + double currentYaw; + double desiredForward; +} AngleData; + +extern TaskHandle_t _servoControlTask; +extern QueueHandle_t steeringQueue; + extern double desired_heading; extern double current_heading; extern volatile bool forward_1; extern volatile bool forward_2; -void steering_setup(); +void steering_setup(double acquireRate, double kp, double ki, double kd); double angle_diff(double angle1, double angle2); -void servo_control(double des_heading, double current_heading); +void servo_control(AngleData data); void servoControlTask(void *pvParameters); +void sendSteeringData(AngleData data); #endif \ No newline at end of file From 20f65c4db1f885d7cdc72b7c64e086c609136c71 Mon Sep 17 00:00:00 2001 From: Nischay Joshi Date: Sat, 11 May 2024 13:56:16 -0700 Subject: [PATCH 10/14] Added new motor control strategy in main --- src/main/ConfigParse.cpp | 8 +- src/main/ConfigParse.h | 1 + src/main/PID.cpp | 68 ------------ src/main/PID.h | 53 --------- src/main/PID_peanat.cpp | 201 ++++++++++++++++++++++++++++++++++ src/main/PID_peanat.h | 63 +++++++++++ src/main/advancedSteering.cpp | 131 ---------------------- src/main/advancedSteering.h | 41 ------- src/main/encoder.cpp | 27 +++++ src/main/encoder.h | 23 ++++ src/main/encoder_steering.cpp | 167 ++++++++++++++++++++++++++++ src/main/encoder_steering.h | 48 ++++++++ src/main/main.ino | 36 ++---- 13 files changed, 547 insertions(+), 320 deletions(-) delete mode 100644 src/main/PID.cpp delete mode 100644 src/main/PID.h create mode 100644 src/main/PID_peanat.cpp create mode 100644 src/main/PID_peanat.h delete mode 100644 src/main/advancedSteering.cpp delete mode 100644 src/main/advancedSteering.h create mode 100644 src/main/encoder.cpp create mode 100644 src/main/encoder.h create mode 100644 src/main/encoder_steering.cpp create mode 100644 src/main/encoder_steering.h diff --git a/src/main/ConfigParse.cpp b/src/main/ConfigParse.cpp index 5dcc13f..2dd2be6 100644 --- a/src/main/ConfigParse.cpp +++ b/src/main/ConfigParse.cpp @@ -25,9 +25,10 @@ void ConfigParser::createDefaultConfigFile(fs::SDFS fs){ // PID values JsonObject PID = doc.createNestedObject("PID"); - PID["KP"] = 1.0; + PID["KP"] = 50.0; PID["KI"] = 0.0; PID["KD"] = 0.0; + PID["PID_ControlRate"] = 100.0; // Print enable doc["Print_Enable"] = true; @@ -130,6 +131,7 @@ ConfigParseStatus ConfigParser::parseConfigFile(fs::SDFS fs, ConfigData_t *confi configData->PID.KP = doc["PID"]["KP"]; configData->PID.KI = doc["PID"]["KI"]; configData->PID.KD = doc["PID"]["KD"]; + configData->PID.PID_ControlRate = doc["PID"]["PID_ControlRate"]; //Printing Parameters configData->Print_Enable = doc["Print_Enable"]; @@ -173,9 +175,10 @@ void ConfigParser::getConfigNoSD(ConfigData_t *ConfigData){ ConfigData->BARO_ALT_STD = 1.466; ConfigData->GPS_POS_STD = 2.5; - ConfigData->PID.KP = 1.0; + ConfigData->PID.KP = 50.0; ConfigData->PID.KI = 0.0; ConfigData->PID.KD = 0.0; + ConfigData->PID.PID_ControlRate = 100.0; ConfigData->Print_Enable = true; ConfigData->BufferSize = 512; @@ -216,6 +219,7 @@ void ConfigParser::printConfigData(ConfigData_t *configData){ Serial.printf("PID KP: %f\n", configData->PID.KP); Serial.printf("PID KI: %f\n", configData->PID.KI); Serial.printf("PID KD: %f\n", configData->PID.KD); + Serial.printf("PID Control Rate: %f\n", configData->PID.PID_ControlRate); Serial.printf("Print Enable: %d\n", configData->Print_Enable); Serial.printf("Buffer Size: %d\n", configData->BufferSize); diff --git a/src/main/ConfigParse.h b/src/main/ConfigParse.h index 9dabc5c..eeea2f3 100644 --- a/src/main/ConfigParse.h +++ b/src/main/ConfigParse.h @@ -33,6 +33,7 @@ typedef struct{ double KP; double KI; double KD; + double PID_ControlRate }PIDConfig_t ; // Output data settings for each sensor diff --git a/src/main/PID.cpp b/src/main/PID.cpp deleted file mode 100644 index 466ba64..0000000 --- a/src/main/PID.cpp +++ /dev/null @@ -1,68 +0,0 @@ -/* - * Author: Bobsy Narayan - * Date: Feb 24, 2024 - * Notes: - * Payload Auto Parachute 2023-2024 - * Parachute Steering PID Controller Header File - * Bunch of functions to Initialize, calculate the controller and to change the setpoint. - * Teensy 4.0 - * - * Extra comments added in for personal development - * - * Taken from PIDSelfSteer.Cpp from AUVSI Rover 2021-2022 - * - * - */ - -#include "PID.h" -/* - * PID Initalize function - * Parameters: setpoint = input setpoint - * control frequency = acquire rate of PID - */ -void Pid::PIDInit(double controlFrequency, double KP, double KI, double KD){ - DT=1.0/controlFrequency; - this->KP = KP; - this->KI = KI; - this->KD = KD; -} - -/* - * PID Calculate function - * Parameters:processVariable - The reading from the sensor (feedback signal) - */ -double Pid::PIDcalculate(double processVariable){ - // Set Error - error = setpoint - processVariable; - - // Calculate the terms: K, I, and D - Do I need to adjust anything for this for yaw angle? 2)!! - double KTerm = error * KP; - double ITerm = (integral + error * DT) * KI; - double DTerm = ((error - prevError) / DT) * KD; - - // Compute Output - output = KTerm + ITerm + DTerm; - - // Check if the output is within limits - if(output < minPIDVal){ - output = minPIDVal; - } - if(output > maxPIDVal){ - output = maxPIDVal; - } - - // Set Integral and Previous Error - prevError = error; - integral += error * DT; - - return output; -} - -/* - * Function to Update the setpoint for the controller - * Parameters: PIDstructure - A pointer to an array storing the state of the controller - * setpoint - The new value of the setpoint - */ -void Pid::PIDupdateSetpoint(double setpoint){ - this->setpoint = setpoint; -} \ No newline at end of file diff --git a/src/main/PID.h b/src/main/PID.h deleted file mode 100644 index 969f629..0000000 --- a/src/main/PID.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * Author: Nischay Joshi/Bobsy Narayan - * Date: Feb 24, 2024 - * Notes: - * Payload Auto Parachute 2023-2024 - * Parachute Steering PID Controller Header File - * Bunch of functions to Initialize, calculate the controller and to change the setpoint. - * Teensy 4.0 - * - * Extra comments added in for personal development - */ - -#ifndef _PID_H -#define _PID_H - -#define INITIALSETPOINT 0.0 -#define MAX_OUTPUT 255.0 -#define MIN_OUTPUT -255.0 -#define DEFAULT_KP 1.0 -#define DEFAULT_KI 1.8 -#define DEFAULT_KD 0.6 - - -class Pid { -public: - // Empty constructor - //Pid(); - - // Function to initialize the PID controller with specific parameters - void PIDInit(double controlFrequency, double KP, double KI, double KD); - - // Calculate the PID output based on the process variable - double PIDcalculate(double processVariable); - - // Update the setpoint of the PID controller - void PIDupdateSetpoint(double setpoint); - -private: - double DT; - double maxPIDVal = MAX_OUTPUT; - double minPIDVal = MIN_OUTPUT; - double KP = DEFAULT_KP; - double KI = DEFAULT_KI; - double KD = DEFAULT_KD; - double setpoint=INITIALSETPOINT; - double error=0; - double output=0; - double prevError=0; - double integral=0; -}; - - -#endif \ No newline at end of file diff --git a/src/main/PID_peanat.cpp b/src/main/PID_peanat.cpp new file mode 100644 index 0000000..2ad128a --- /dev/null +++ b/src/main/PID_peanat.cpp @@ -0,0 +1,201 @@ +/* + * PID library made for robots! + * Extra: custom initial output, deadband limits + * Date created: Aug. 16, 2022 + */ + +#include "Arduino.h" +#include +#include "PID_peanat.h" + +const double SECS_IN_MS = 0.001; + + +/* + * Initialize PID object. + * + * @param user_kp: proportional coefficient, should be >= 0 + * @param user_ki: integral coefficient, should be >= 0 + * @param user_kd: derivative coefficient, should be >= 0 + * + * @returns PID object + */ +PID::PID(double user_kp, double user_ki, double user_kd) { + kp = user_kp; + ki = user_ki; + kd = user_kd; + init_output = 0.0; + reverse = false; + sample_time = 100; + min_output = 0.0; + max_output = 255.0; + deadband = 0.0; + + // Reverse coefficients if in reverse mode + if (reverse == true) { + kp *= -1.0; + ki *= -1.0; + kd *= -1.0; + } + + // Initiate other fields + first_run = true; + cumulative_error = 0.0; +} + +/* + * Updates PID coefficients. + * + * @param user_kp: proportional coefficient, should be >= 0 + * @param user_ki: integral coefficient, should be >= 0 + * @param user_kd: derivative coefficient, should be >= 0 + * + * @returns none + */ +void PID::updateCoeffs(double user_kp, double user_ki, double user_kd) { + kp = user_kp; + ki = user_ki; + kd = user_kd; +} + +/* + * Set initial output; must be within or on output bounds. + * + * @param user_init_output: desired initial output, for when we want the output to start at a specific value + * + * @returns none + */ +void PID::setInitOutput(double user_init_output) { + init_output = user_init_output; +} + +/* + * Toggle reverse mode on or off. + * + * @param user_reverse: true to turn reverse mode on, false to turn off + * + * @returns none + */ +void PID::setReverse(bool user_reverse) { + if (reverse != user_reverse) { + reverse = user_reverse; + kp *= -1.0; + ki *= -1.0; + kd *= -1.0; + } +} + +/* + * Set sample time (recommended < 100). + * + * @param user_sample_time: sample time in ms + * + * @returns none + */ +void PID::setSampleTime(unsigned long user_sample_time) { + sample_time = user_sample_time; +} + +/* + * Set output bounds; useful if something like a servo has input limits. + * Make sure that the initial output is on or within these bounds! + * + * @param user_min_output: minimum output value (inclusive) + * @param user_max_output: maximum output value (inclusive) + * + * @returns none + */ +void PID::setOutputBounds(double user_min_output, double user_max_output) { + min_output = user_min_output; + max_output = user_max_output; +} + +/* + * Set deadband range (difference between previous and current calculated output, in which the + * previous output is returned) if we need to prevent mechanical wear. Deadband must be less + * than max output - min output. + * + * @param user_deadband: deadband range, in units of output + * + * @returns none + */ +void PID::setDeadband(double user_deadband) { + deadband = user_deadband; +} + +/* + * Computes PID output based on standard PID algorithm; implements deadband functionality and + * measures to prevent integral windup. The first run will output the initial output value. + * This loop can run for a maximum of 49 days before unsigned long overflows. + * + * @param user_setpoint: desired value of something (e.g. servo position) + * @param user_input: the acutal value + * + * @returns output to be fed back into the controller + */ +double PID::compute(double user_setpoint, double user_input) { + // If first run, initialize variables + if (first_run == true) { + setpoint = user_setpoint; + input = user_input; + + prev_time = millis(); + prev_error = setpoint - input; + prev_output = init_output; + + output = init_output; + first_run = false; + } + + // Subsequent runs + else { + current_time = millis(); + unsigned long time_interval = current_time - prev_time; + + if (time_interval >= sample_time) { + setpoint = user_setpoint; + input = user_input; + double error = setpoint - input; + + // Proportional + output = init_output + kp * error; + // Integral + cumulative_error += ki * error * ((double)time_interval * SECS_IN_MS); + cumulative_error = min(max(cumulative_error, min_output), max_output); // Limit cumulative error to prevent windup + output += cumulative_error; + // Derivative + output += kd * (prev_error - error) / ((double)time_interval * SECS_IN_MS); + + // Limit output + output = min(max(output, min_output), max_output); + + // Don't change output if within deadband + if (fabs(output - prev_output) < deadband) { + output = prev_output; + } + + // For the next run + prev_time = current_time; + prev_error = error; + prev_output = output; + } + } + + return output; +} + +double PID::getOutput() { + return output; +} + +double PID::getkp() { + return kp; +} + +double PID::getki() { + return ki; +} + +double PID::getkd() { + return kd; +} diff --git a/src/main/PID_peanat.h b/src/main/PID_peanat.h new file mode 100644 index 0000000..cf413bf --- /dev/null +++ b/src/main/PID_peanat.h @@ -0,0 +1,63 @@ +#ifndef PID_PEANAT_H +#define PID_PEANAT_H + +/* + * This is a PID class to be used in PID control loops. + */ + +class PID { + + public: + + // Constructor + PID(double user_kp, double user_ki, double user_kd); + + // Change settings + void updateCoeffs(double user_kp, double user_ki, double user_kd); + void setInitOutput(double user_init_output); + void setReverse(bool user_reverse); + void setSampleTime(unsigned long user_sample_time); + void setOutputBounds(double user_min_output, double user_max_output); + void setDeadband(double user_deadband); + + // Calculate PID output + double compute(double user_setpoint, double user_input); + + // Get private variables + double getOutput(); + double getkp(); + double getki(); + double getkd(); + + + private: + + double setpoint; // Target value + + double input; // What the value really is + + double output; // Calculated output + double init_output; // Output value to start from + double prev_output; // Previous output + + double kp; // Proportional coefficient + double ki; // Integral coefficient + double kd; // Derivative coefficient + + double prev_error; // Previous error + double cumulative_error; // Cumulative error for calculating integral output + + unsigned long current_time; // Current time in milliseconds + unsigned long prev_time; // Time at which previous input was taken + unsigned long sample_time; // Input sample time + + double min_output; // Max output + double max_output; // Min output + + double deadband; // The range in which we don't change the output (to prevent mechanical wear) + + bool reverse; // Reverse mode (if true, output * -1) + bool first_run; // True if it is the very first call to the `compute` function +}; + +#endif \ No newline at end of file diff --git a/src/main/advancedSteering.cpp b/src/main/advancedSteering.cpp deleted file mode 100644 index 3d3e366..0000000 --- a/src/main/advancedSteering.cpp +++ /dev/null @@ -1,131 +0,0 @@ -/* - * Autonomous Parachute Steering Functon Implementation - * Keeping track of the angle of a continous rotation servo - * Author: Nischay Joshi - * Created: April 13th, 2024 - */ - -#include "advancedSteering.h" - -//Declare servo motor objects -Servo leftMotor; -Servo rightMotor; - -// Define the global variables -TaskHandle_t _steeringTask = NULL; -QueueHandle_t steeringQueue = NULL; - -void mmotorSetup(){ - ESP32PWM::allocateTimer(0); - ESP32PWM::allocateTimer(1); - ESP32PWM::allocateTimer(2); - ESP32PWM::allocateTimer(3); - leftMotor.attach(LEFTPIN,MINUS,MAXUS); - rightMotor.attach(RIGHTPIN,MINUS,MAXUS); - leftMotor.setPeriodHertz(SERVOFREQ); // Standard 50hz servo - rightMotor.setPeriodHertz(SERVOFREQ); // Standard 50hz servo - //set at 0 speed value - leftMotor.write(90); - rightMotor.write(90); - - //start the servo control task - BaseType_t ret = xTaskCreatePinnedToCore( - steering, /* Function to implement the task */ - "steeringTask", /* Name of the task */ - 4096, /* Stack size in words */ - NULL, /* Task input parameter */ - configMAX_PRIORITIES - 1, /* Priority of the task */ - &_steeringTask, /* Task handle. */ - 0); /* Core where the task should run */ - if (ret != pdPASS) { - Serial.println("Failed to create steering task"); - } - else { - Serial.println("Steering task created"); - } -} - -//steering function to control the steering of the parachute -void steering(void *pvParameters){ - SteeringData desiredLengths; - desiredLengths.length1 = INITIAL_LENGTH_M; - desiredLengths.length2 = INITIAL_LENGTH_M; - - double currentLength1 = desiredLengths.length1; - double currentLength2 = desiredLengths.length2; - - double desiredLength1 = currentLength1; - double desiredLength2 = currentLength2; - - double errorLength1 = 0; - double errorLength2 = 0; - - while(1){ - //if we have data in the queue, if not, keep the previous value - if (xQueueReceive(steeringQueue, &desiredLengths, 0) == pdTRUE){ - Serial.printf("Servo1: %lf\tServo2: %lf\n", desiredLengths.length1, desiredLengths.length2); - desiredLength1 = desiredLengths.length1; - desiredLength2 = desiredLengths.length2; - } - //update the distance to travel - errorLength1 = desiredLength1 - currentLength1; - errorLength2 = desiredLength2 - currentLength2; - - if(abs(errorLength1) > ERROR_DEADZONE || abs(errorLength2) > ERROR_DEADZONE){ - //calculate the amount of time to move the servo - double time1 = abs(errorLength1) / SERVO_SPEED_M_SEC * 1000; - double time2 = abs(errorLength2) / SERVO_SPEED_M_SEC * 1000; - - //move the servo - int leftMotorvalue = 90 + (errorLength1 > 0 ? SERVO_SPEED_VALUE_ANGLE : -SERVO_SPEED_VALUE_ANGLE); - int rightMotorvalue = 90 + (errorLength2 > 0 ? SERVO_SPEED_VALUE_ANGLE : -SERVO_SPEED_VALUE_ANGLE); - - if(abs(errorLength1) < ERROR_DEADZONE){ - leftMotorvalue = 90; - } - if(abs(errorLength2) < ERROR_DEADZONE){ - rightMotorvalue = 90; - } - - leftMotor.write(leftMotorvalue); - rightMotor.write(rightMotorvalue); - Serial.printf("CurrentLeng: %lf\tDesiredLength %lf\tError: %lf\tTime1: %lf\tServoValue: %d\n", currentLength1, desiredLength1, errorLength1, time1, leftMotorvalue); - unsigned long startTime = millis(); - - //wait for the servo to move - bool leftMotordone = false; - bool rightMotordone = false; - - int i = 0; - while(1){ - - if(!leftMotordone && ((millis() - startTime) > time1)){ - leftMotordone = true; - leftMotor.write(90); // Reset servo to neutral position after moving - Serial.println("Servo Left done"); - } - if(!rightMotordone && ((millis() - startTime) > time2)){ - rightMotordone = true; - rightMotor.write(90); // Reset servo to neutral position after moving - Serial.println("Servo right done"); - } - if(leftMotordone && rightMotordone){ - break; // Exit loop if both servos have completed their movements - } - // Wait for 1 ms to prevent hogging the CPU - // vTaskDelay(1 / portTICK_PERIOD_MS); - delay(1); - Serial.println(i++); - } - - leftMotor.write(90); - rightMotor.write(90); - // Update the current lengths - currentLength1 = desiredLengths.length1; - currentLength2 = desiredLengths.length2; - } - - vTaskDelay(1 / portTICK_PERIOD_MS); - - } -} \ No newline at end of file diff --git a/src/main/advancedSteering.h b/src/main/advancedSteering.h deleted file mode 100644 index bb99dca..0000000 --- a/src/main/advancedSteering.h +++ /dev/null @@ -1,41 +0,0 @@ -/* - * Autonomous Parachute Steering Functon Header - * Keeping track of the angle of a continous rotation servo - * Author: Nischay Joshi - * Created: April 13th, 2024 - */ - -#ifndef ADVANCEDSTEERING_H -#define ADVANCEDSTEERING_H - -#include -#include - -#define MINUS 700 -#define MAXUS 2300 -#define LEFTPIN 26 -#define RIGHTPIN 27 -#define SERVOFREQ 50 - -#define WHEELRADIUS_M 1 -#define INITIAL_LENGTH_M 0.00 -#define ERROR_DEADZONE 0.001 - -#define SERVO_SPEED_RAD_SEC 14.38378555 -#define SERVO_SPEED_M_SEC SERVO_SPEED_RAD_SEC * WHEELRADIUS_M -#define SERVO_SPEED_VALUE_ANGLE 80 - - -typedef struct { - double length1; - double length2; -} SteeringData; - -//a queue to send the data to the steering function -extern TaskHandle_t _steeringTask; -extern QueueHandle_t steeringQueue; - -void mmotorSetup(); -void steering(void *pvParameters); - -#endif \ No newline at end of file diff --git a/src/main/encoder.cpp b/src/main/encoder.cpp new file mode 100644 index 0000000..170de86 --- /dev/null +++ b/src/main/encoder.cpp @@ -0,0 +1,27 @@ +#include "encoder.h" + +// Definitions of global variables +volatile bool forward_1 = true; +volatile bool forward_2 = true; +volatile unsigned long time_1 = millis(); +volatile unsigned long time_2 = millis(); +volatile int count_1 = 0; +volatile int count_2 = 0; + +// Interrupt service routines +void IRAM_ATTR enc_1_isr() { + unsigned long current = millis(); + if (current - time_1 > DEBOUNCE_INTERVAL) { + if (forward_1) { count_1++; } + else { count_1--; } + time_1 = current; + } +} +void IRAM_ATTR enc_2_isr() { + unsigned long current = millis(); + if (current - time_2 > DEBOUNCE_INTERVAL) { + if (forward_2) { count_2++; } + else { count_2--; } + time_2 = current; + } +} diff --git a/src/main/encoder.h b/src/main/encoder.h new file mode 100644 index 0000000..4b0f4a8 --- /dev/null +++ b/src/main/encoder.h @@ -0,0 +1,23 @@ +#ifndef ENCODER_H +#define ENCODER_H + +#include + +#define ENC_1 25 +#define ENC_2 33 + +#define DEBOUNCE_INTERVAL 10 // ms + +// Function prototypes +void enc_1_isr(); +void enc_2_isr(); + +// Declaration of global variables (extern keyword indicates that they are defined elsewhere) +extern volatile bool forward_1; +extern volatile bool forward_2; +extern volatile unsigned long time_1; +extern volatile unsigned long time_2; +extern volatile int count_1; +extern volatile int count_2; + +#endif diff --git a/src/main/encoder_steering.cpp b/src/main/encoder_steering.cpp new file mode 100644 index 0000000..dd0753a --- /dev/null +++ b/src/main/encoder_steering.cpp @@ -0,0 +1,167 @@ +// Steering with feedback from encoders +#include "encoder_steering.h" + +// Declare servo motor objects +Servo servo_1; +Servo servo_2; + +// Initialize PID +PID pid(50.0, 0, 0); +TaskHandle_t _servoControlTask = NULL; +QueueHandle_t steeringQueue = NULL; + +#define SERVO_CONTROL_PERIOD 10 // ms +volatile int PID_Control_Period = SERVO_CONTROL_PERIOD; + +void steering_setup(double acquireRate, double kp, double ki, double kd) { + + // Initialize encoders + Serial.println("Initializing encoders"); + pinMode(ENC_1, INPUT); + attachInterrupt(ENC_1, enc_1_isr, RISING); + pinMode(ENC_2, INPUT); + attachInterrupt(ENC_2, enc_2_isr, RISING); + + // Initialize PID + Serial.println("Initializing PID"); + Serial.printf("kp: %lf, ki: %lf, kd: %lf\n", kp, ki, kd); + pid.updateCoeffs(kp, ki, kd); + pid.setOutputBounds(-150, 150); // Limit to 150 mm extension/retraction + pid.setSampleTime((1.0/acquireRate)*1000); // ms + PID_Control_Period = (1.0/acquireRate)*1000; + // pid.setDeadband(DIST_PER_TICK); + + // Initialize servos + Serial.println("Initializing servos"); + servo_1.attach(SERVO_1, MIN_US, MAX_US); + servo_1.setPeriodHertz(SERVO_FREQ); + servo_2.attach(SERVO_2, MIN_US, MAX_US); + servo_2.setPeriodHertz(SERVO_FREQ); + // No spin + servo_1.write(90); + servo_2.write(90); + + // Create queue + steeringQueue = xQueueCreate(1, sizeof(AngleData)); // this queue will take the yaw value + if (steeringQueue == NULL) { + Serial.println("Failed to create steering queue"); + while(1){ + Serial.println("Failed to create steering queue"); + delay(500); + } // if queue creation fails + } + Serial.println("Steering queue created"); + + // Start task + BaseType_t ret = xTaskCreatePinnedToCore( + servoControlTask, // Task function + "ServoControlTask", // Task name + 4096, // Stack size (words) + NULL, // Task parameters + configMAX_PRIORITIES -1,// Priority (1 is default) + &_servoControlTask, // Task handle + 0 // Core number (0 or 1) + ); + if (ret != pdPASS) { + Serial.println("Failed to create servo control task"); + } else { + Serial.println("Servo control task created"); + } + +} + + +// Get difference between two angles in radians [-pi, pi) +double angle_diff(double angle1, double angle2) { + double diff = angle1 - angle2; + while (diff >= PI) { + diff -= 2 * PI; + } + while (diff < -PI) { + diff += 2 * PI; + } + return diff; +} + + +/* + * Function to control servos + * @param data: AngleData struct containing desired and current yaw values + */ +void servo_control(AngleData data) { + SteeringData des_lengths; + SteeringData current_lengths = {(double)DIST_PER_TICK*((int)count_1/2), (double)DIST_PER_TICK*((int)count_2/2)}; + + double input = angle_diff(data.desiredYaw, data.currentYaw); + Serial.printf("PID Input: %lf\n", input); // Comment When testing is done + if (fabs(input) < FORWARD_THRESH) { // Go forward if we don't need to change our heading that much + des_lengths.l1 = -100.0; // Servo 1 + des_lengths.l2 = -100.0; // Servo 2 + } + else { + double output = pid.compute(0.0, input); + Serial.printf("PID Output: %lf\n", output); + des_lengths.l1 = output; + des_lengths.l2 = -output; + } + + Serial.printf("Lengths: %lf %lf\n", des_lengths.l1, des_lengths.l2); //- -- Comment me when testing is done + + // Servo 1 + if (des_lengths.l1 - current_lengths.l1 > (DIST_PER_TICK)) { + servo_1.write(90 + SERVO_SPEED_VALUE_ANGLE); + forward_1 = true; + } else if (des_lengths.l1 - current_lengths.l1 < -(DIST_PER_TICK)) { + servo_1.write(90 - SERVO_SPEED_VALUE_ANGLE); + forward_1 = false; + } else { + servo_1.write(90); + } + + // Servo 2 + if (des_lengths.l2 - current_lengths.l2 > (DIST_PER_TICK)) { + servo_2.write(90 - SERVO_SPEED_VALUE_ANGLE); + forward_2 = true; + } else if (des_lengths.l2 - current_lengths.l2 < -(DIST_PER_TICK)) { + servo_2.write(90 + SERVO_SPEED_VALUE_ANGLE); + forward_2 = false; + } else { + servo_2.write(90); + } + +} + + +void servoControlTask(void *pvParameters) { + (void) pvParameters; + + AngleData incomingData; + // initialize the incoming data + incomingData.desiredYaw = 0; + incomingData.currentYaw = 0; + incomingData.desiredForward = 0; + + while (true) { + // Check if we have incoming data + if (xQueueReceive(steeringQueue, &incomingData, 0) == pdTRUE) { + // Successfully received data + Serial.printf("Desired yaw: %lf, Current yaw: %lf, Desired forward: %lf\n", incomingData.desiredYaw, incomingData.currentYaw, incomingData.desiredForward); + } + // Call servo_control function here + servo_control(incomingData); // Modify parameters as needed + + // Delay for the desired period (in milliseconds) + vTaskDelay(pdMS_TO_TICKS(PID_Control_Period)); // Adjust delay as needed + } +} + +/* +* Function to send the steering data to the steering task +* @param data: AngleData struct containing desired and current yaw values +*/ +void sendSteeringData(AngleData data) { + if (xQueueOverwrite(steeringQueue, &data) != pdPASS) { + Serial.println("Failed to overwrite data in steering queue"); + } +} + diff --git a/src/main/encoder_steering.h b/src/main/encoder_steering.h new file mode 100644 index 0000000..7fea64f --- /dev/null +++ b/src/main/encoder_steering.h @@ -0,0 +1,48 @@ +#ifndef ENCODER_STEERING_H +#define ENCODER_STEERING_H + +#include +#include +#include +#include "PID_peanat.h" +#include "encoder.h" + +#define SERVO_1 26 +#define SERVO_2 27 + +#define MIN_US 700 +#define MAX_US 2300 +#define SERVO_FREQ 50 // Standard 50hz servo +#define SERVO_SPEED_VALUE_ANGLE 80 +#define PI 3.14159 + +static const double DRUM_DIAMETER = 22; // mm +static const double DIST_PER_TICK = (double)PI*DRUM_DIAMETER / 4.0; // mm (There are 4 markers around the wheel) +static const double FORWARD_THRESH = 10.0 * (double)PI/180.0; // rad (Go forward if we are within 10 degrees) + +typedef struct { + double l1; + double l2; +} SteeringData; + +typedef struct { + double desiredYaw; + double currentYaw; + double desiredForward; +} AngleData; + +extern TaskHandle_t _servoControlTask; +extern QueueHandle_t steeringQueue; + +extern double desired_heading; +extern double current_heading; +extern volatile bool forward_1; +extern volatile bool forward_2; + +void steering_setup(double acquireRate, double kp, double ki, double kd); +double angle_diff(double angle1, double angle2); +void servo_control(AngleData data); +void servoControlTask(void *pvParameters); +void sendSteeringData(AngleData data); + +#endif \ No newline at end of file diff --git a/src/main/main.ino b/src/main/main.ino index 2a2abf6..538df18 100644 --- a/src/main/main.ino +++ b/src/main/main.ino @@ -11,8 +11,7 @@ #include "kalmanfilter.h" #include "WebStreamServer.h" #include "SDCard.h" -#include "PID.h" -#include "advancedSteering.h" +#include "encoder_steering.h" #include "Reciever.h" #define BufferLen 500 @@ -27,7 +26,6 @@ ConfigParser configParser_inst; ConfigData_t configData_inst; TinyGPSPlus GPS; -Pid PID; Sensors::sensors mySensor_inst; Sensors::sensorData_t sensorData_inst; KalmanFilter myKalmanFilter_inst_Z(NUM_STATES, NUM_MEASUREMENTS, NUM_CONTROL_INPUTS); @@ -95,9 +93,8 @@ void setup() myKalmanFilter_inst_Y.initialize(configData_inst.SampleTime, configData_inst.ACC_Y_STD, configData_inst.GPS_POS_STD); myKalmanFilter_inst_X.initialize(configData_inst.SampleTime, configData_inst.ACC_X_STD, configData_inst.GPS_POS_STD); - //Initializes PID object - PID.PIDInit(configData_inst.AcquireRate, configData_inst.PID.KP, configData_inst.PID.KI, configData_inst.PID.KD); - motorSetup(); //Initialize two servo motors + //Steering setup + steering_setup(configData_inst.AcquireRate, configData_inst.PID.KP, configData_inst.PID.KI, configData_inst.PID.KD); // Set up ESP-NOW communication if(InitESPNow(configData_inst.BottleID) == false){ @@ -128,7 +125,7 @@ void loop() DoKalman(); PrintSensorData(); - PIDTesting(); + ComputePID(); //keep reading battery data // if(mySensor_inst.UpdateBatteryData(&sensorData_inst) != Sensors::SENSORS_OK){ @@ -359,7 +356,7 @@ void DoCount(){ } } -void PIDTesting(){ +void ComputePID(){ char outputBuffer[BufferLen]; //This is for printing values out to terminal double target_lon=-122.12071003376428; //Example target longitude double target_lat=37.4181048968111; //Example target latitude @@ -371,7 +368,7 @@ void PIDTesting(){ // Take current GPS coordinates and add x,y,z, from kalman filter // Lat_Fast = GPS.Fast+X_Moved*ConversionFactor - //radius_calc= 180*radius of earth/pi + // radius_calc= 180*radius of earth/pi double radius_calc = 365285454.545; double rEarth_m = 6371000.0; @@ -391,27 +388,17 @@ void PIDTesting(){ setpoint = setpoint - 360; } - double pv = setpoint - sensorData_inst.imuData.EulerAngles.v2; - //Always get the shortest path - if(pv > 180.0){ - pv = pv - 360.0; - } - else if(pv < -180.0){ - pv = pv + 360.0; - } - - //compute PID angle using process variable - // double motor_value = PID.PIDcalculate(pv); + // make the Data packet + AngleData data = {setpoint, sensorData_inst.imuData.EulerAngles.v2, 0}; + // send the data packet to the queue + sendSteeringData(data); - //Send to servos - // steering(motor_value); double distance = GPS.distanceBetween(lon_now, lat_now, target_lon, target_lat); //Print to terminal - snprintf(outputBuffer, BufferLen, "Yaw: %.3lf\nSetpoint: %.3lf\nPV: %.3lf\nDistance: %.3lf\nlattitude: %.6lf\nlongitude: %.6lf\nlat_now: %.6lf\nlon_now: %.6lf\n,x: %.6lf\ny: %.6lf\nz: %.6lf\n", + snprintf(outputBuffer, BufferLen, "Yaw: %.3lf\nSetpoint: %.3lf\nDistance: %.3lf\nlattitude: %.6lf\nlongitude: %.6lf\nlat_now: %.6lf\nlon_now: %.6lf\n,x: %.6lf\ny: %.6lf\nz: %.6lf\n", sensorData_inst.imuData.EulerAngles.v2, setpoint, - pv, distance, sensorData_inst.gpsData.Latitude, sensorData_inst.gpsData.Longitude, @@ -421,7 +408,6 @@ void PIDTesting(){ X_Yaxis(0,0), X_Zaxis(0,0) ); - // sprintf(outputBuffer, "Pv: %.5lf \t Error: %.5lf \t Output: %.5lf\t Integral: %.5lf \t \n", pv, PID.error, yaw, PID.integral); SERIAL_PORT.print(outputBuffer); } From c0db3b01ab8bd0c1964d6472b80016f27b2cdbfe Mon Sep 17 00:00:00 2001 From: p-eanat Date: Fri, 7 Jun 2024 00:48:24 -0700 Subject: [PATCH 11/14] Remove continuous servo min/max us adjustments as they cause drift, flip signs on PID output, add height threshold to activate steering --- sensorDemo/SteeringTest/encoder_steering.cpp | 8 ++++---- sensorDemo/SteeringTest/encoder_steering.h | 2 +- src/main/ConfigParse.cpp | 8 ++++++++ src/main/ConfigParse.h | 1 + src/main/encoder_steering.cpp | 8 ++++---- src/main/encoder_steering.h | 2 +- src/main/main.ino | 9 ++++++++- 7 files changed, 27 insertions(+), 11 deletions(-) diff --git a/sensorDemo/SteeringTest/encoder_steering.cpp b/sensorDemo/SteeringTest/encoder_steering.cpp index dd0753a..acb135c 100644 --- a/sensorDemo/SteeringTest/encoder_steering.cpp +++ b/sensorDemo/SteeringTest/encoder_steering.cpp @@ -33,9 +33,9 @@ void steering_setup(double acquireRate, double kp, double ki, double kd) { // Initialize servos Serial.println("Initializing servos"); - servo_1.attach(SERVO_1, MIN_US, MAX_US); + servo_1.attach(SERVO_1); servo_1.setPeriodHertz(SERVO_FREQ); - servo_2.attach(SERVO_2, MIN_US, MAX_US); + servo_2.attach(SERVO_2); servo_2.setPeriodHertz(SERVO_FREQ); // No spin servo_1.write(90); @@ -101,8 +101,8 @@ void servo_control(AngleData data) { else { double output = pid.compute(0.0, input); Serial.printf("PID Output: %lf\n", output); - des_lengths.l1 = output; - des_lengths.l2 = -output; + des_lengths.l1 = -output; + des_lengths.l2 = output; } Serial.printf("Lengths: %lf %lf\n", des_lengths.l1, des_lengths.l2); //- -- Comment me when testing is done diff --git a/sensorDemo/SteeringTest/encoder_steering.h b/sensorDemo/SteeringTest/encoder_steering.h index 7fea64f..aa28176 100644 --- a/sensorDemo/SteeringTest/encoder_steering.h +++ b/sensorDemo/SteeringTest/encoder_steering.h @@ -17,7 +17,7 @@ #define PI 3.14159 static const double DRUM_DIAMETER = 22; // mm -static const double DIST_PER_TICK = (double)PI*DRUM_DIAMETER / 4.0; // mm (There are 4 markers around the wheel) +static const double DIST_PER_TICK = (double)PI*DRUM_DIAMETER / 6.0; // mm (There are 4 markers around the wheel) static const double FORWARD_THRESH = 10.0 * (double)PI/180.0; // rad (Go forward if we are within 10 degrees) typedef struct { diff --git a/src/main/ConfigParse.cpp b/src/main/ConfigParse.cpp index 2dd2be6..eac0097 100644 --- a/src/main/ConfigParse.cpp +++ b/src/main/ConfigParse.cpp @@ -22,6 +22,7 @@ void ConfigParser::createDefaultConfigFile(fs::SDFS fs){ doc["ACC_Z_STD"] = 0.05; doc["BARO_ALT_STD"] = 1.466; doc["GPS_POS_STD"] = 2.5; + doc["HEIGHT_THRESH"] = 17.0f; // PID values JsonObject PID = doc.createNestedObject("PID"); @@ -116,6 +117,9 @@ ConfigParseStatus ConfigParser::parseConfigFile(fs::SDFS fs, ConfigData_t *confi configData->AcquireRate = doc["AcquireRate"]; configData->SampleTime = 1.0 / configData->AcquireRate; + //Activation Parameters + configData->HEIGHT_THRESH = doc["HEIGHT_THRESH"]; + //Sensor Fusion Parameters configData->GRAVITY = doc["GRAVITY"]; configData->ACC_X_STD = doc["ACC_X_STD"]; @@ -167,6 +171,8 @@ void ConfigParser::getConfigNoSD(ConfigData_t *ConfigData){ ConfigData->AcquireRate = 57.0f; ConfigData->SampleTime = 1.0 / ConfigData->AcquireRate; + + ConfigData->HEIGHT_THRESH = 17.0f; ConfigData->GRAVITY = 9.809; ConfigData->ACC_X_STD = 0.3 * ConfigData->GRAVITY; @@ -208,6 +214,8 @@ void ConfigParser::printConfigData(ConfigData_t *configData){ Serial.printf("Acquire Rate: %f\n", configData->AcquireRate); Serial.printf("Sample Time: %f\n", configData->SampleTime); + + Serial.printf("HEIGHT_THRESH: %f\n", configData->HEIGHT_THRESH); Serial.printf("Gravity: %f\n", configData->GRAVITY); Serial.printf("ACC_X_STD: %f\n", configData->ACC_X_STD); diff --git a/src/main/ConfigParse.h b/src/main/ConfigParse.h index eeea2f3..204de14 100644 --- a/src/main/ConfigParse.h +++ b/src/main/ConfigParse.h @@ -78,6 +78,7 @@ typedef struct{ bool Print_Enable; uint16_t BufferSize; OutputDataConfig_t OutputData; + float HEIGHT_THRESH; } ConfigData_t; class ConfigParser{ diff --git a/src/main/encoder_steering.cpp b/src/main/encoder_steering.cpp index dd0753a..acb135c 100644 --- a/src/main/encoder_steering.cpp +++ b/src/main/encoder_steering.cpp @@ -33,9 +33,9 @@ void steering_setup(double acquireRate, double kp, double ki, double kd) { // Initialize servos Serial.println("Initializing servos"); - servo_1.attach(SERVO_1, MIN_US, MAX_US); + servo_1.attach(SERVO_1); servo_1.setPeriodHertz(SERVO_FREQ); - servo_2.attach(SERVO_2, MIN_US, MAX_US); + servo_2.attach(SERVO_2); servo_2.setPeriodHertz(SERVO_FREQ); // No spin servo_1.write(90); @@ -101,8 +101,8 @@ void servo_control(AngleData data) { else { double output = pid.compute(0.0, input); Serial.printf("PID Output: %lf\n", output); - des_lengths.l1 = output; - des_lengths.l2 = -output; + des_lengths.l1 = -output; + des_lengths.l2 = output; } Serial.printf("Lengths: %lf %lf\n", des_lengths.l1, des_lengths.l2); //- -- Comment me when testing is done diff --git a/src/main/encoder_steering.h b/src/main/encoder_steering.h index 7fea64f..aa28176 100644 --- a/src/main/encoder_steering.h +++ b/src/main/encoder_steering.h @@ -17,7 +17,7 @@ #define PI 3.14159 static const double DRUM_DIAMETER = 22; // mm -static const double DIST_PER_TICK = (double)PI*DRUM_DIAMETER / 4.0; // mm (There are 4 markers around the wheel) +static const double DIST_PER_TICK = (double)PI*DRUM_DIAMETER / 6.0; // mm (There are 4 markers around the wheel) static const double FORWARD_THRESH = 10.0 * (double)PI/180.0; // rad (Go forward if we are within 10 degrees) typedef struct { diff --git a/src/main/main.ino b/src/main/main.ino index 538df18..eb5045f 100644 --- a/src/main/main.ino +++ b/src/main/main.ino @@ -391,7 +391,14 @@ void ComputePID(){ // make the Data packet AngleData data = {setpoint, sensorData_inst.imuData.EulerAngles.v2, 0}; // send the data packet to the queue - sendSteeringData(data); + if (sensorData_inst.barometerData.Altitude <= configData_inst.HEIGHT_THRESH){ + sendSteeringData(data); + } + else { + count_1 = 0; // Keep resetting encoders to zero until steering activated + count_2 = 0; + } + double distance = GPS.distanceBetween(lon_now, lat_now, target_lon, target_lat); From ecbeac19d910db36599299c77b43940403d5d4c6 Mon Sep 17 00:00:00 2001 From: p-eanat Date: Sun, 9 Jun 2024 23:05:55 -0700 Subject: [PATCH 12/14] Steering activates only within altitude range --- src/main/ConfigParse.h | 4 ++-- src/main/encoder_steering.cpp | 14 ++++++++++++++ src/main/encoder_steering.h | 1 + src/main/main.ino | 22 +++++++++++++--------- 4 files changed, 30 insertions(+), 11 deletions(-) diff --git a/src/main/ConfigParse.h b/src/main/ConfigParse.h index 204de14..1c8c5fa 100644 --- a/src/main/ConfigParse.h +++ b/src/main/ConfigParse.h @@ -33,8 +33,8 @@ typedef struct{ double KP; double KI; double KD; - double PID_ControlRate -}PIDConfig_t ; + double PID_ControlRate; +} PIDConfig_t ; // Output data settings for each sensor typedef struct{ diff --git a/src/main/encoder_steering.cpp b/src/main/encoder_steering.cpp index acb135c..2130291 100644 --- a/src/main/encoder_steering.cpp +++ b/src/main/encoder_steering.cpp @@ -89,6 +89,12 @@ double angle_diff(double angle1, double angle2) { * @param data: AngleData struct containing desired and current yaw values */ void servo_control(AngleData data) { + + if (data.desiredForward == -1){ + do_nothing(); + return; + } + SteeringData des_lengths; SteeringData current_lengths = {(double)DIST_PER_TICK*((int)count_1/2), (double)DIST_PER_TICK*((int)count_2/2)}; @@ -165,3 +171,11 @@ void sendSteeringData(AngleData data) { } } +/* + * Don't move the servos + */ +void do_nothing() { + servo_1.write(90); + servo_2.write(90); +} + diff --git a/src/main/encoder_steering.h b/src/main/encoder_steering.h index aa28176..8df0577 100644 --- a/src/main/encoder_steering.h +++ b/src/main/encoder_steering.h @@ -44,5 +44,6 @@ double angle_diff(double angle1, double angle2); void servo_control(AngleData data); void servoControlTask(void *pvParameters); void sendSteeringData(AngleData data); +void do_nothing(); #endif \ No newline at end of file diff --git a/src/main/main.ino b/src/main/main.ino index eb5045f..5f0a4f8 100644 --- a/src/main/main.ino +++ b/src/main/main.ino @@ -379,26 +379,30 @@ void ComputePID(){ double lon_now = sensorData_inst.gpsData.refLongitude + delta_long; double lat_now = sensorData_inst.gpsData.refLatitude + delta_lat; - //Height = current GPS_Position & use kinematics to get new height? To be done + // Height = current GPS_Position & use kinematics to get new height? To be done - //Get process variable - pv is the error between (lot & lat_fast direction)-(target) / current heading-target heading + // Get process variable - pv is the error between (lot & lat_fast direction)-(target) / current heading-target heading double setpoint = GPS.courseTo(lat_now, lon_now, target_lat, target_lon); //Get the heading to the target - //convert heading from 0-360 to -180-180 + // Convert heading from 0-360 to -180-180 if(setpoint > 180){ setpoint = setpoint - 360; } - - // make the Data packet - AngleData data = {setpoint, sensorData_inst.imuData.EulerAngles.v2, 0}; - // send the data packet to the queue - if (sensorData_inst.barometerData.Altitude <= configData_inst.HEIGHT_THRESH){ + + // Send the data packet to the queue (i.e. activate steering), if detect that parachute has fallen below HEIGHT_THRESH + // double height = X_Zaxis(0, 0); + double height = sensorData_inst.barometerData.Altitude - sensorData_inst.barometerData.AltitudeOffset; +// Serial.println("\nHeight: " + String(height) + "\n"); + if (height <= configData_inst.HEIGHT_THRESH && height >= 0.0){ + // Make the Data packet + AngleData data = {setpoint, sensorData_inst.imuData.EulerAngles.v2, 0}; sendSteeringData(data); } else { count_1 = 0; // Keep resetting encoders to zero until steering activated count_2 = 0; + AngleData data = {0, 0, -1}; // I'm using the desiredForward as a flag to turn off the servos (-1 = off) + sendSteeringData(data); } - double distance = GPS.distanceBetween(lon_now, lat_now, target_lon, target_lat); From 967ffa3c3c20fb4fff4bb96ad1b663cd4440441e Mon Sep 17 00:00:00 2001 From: p-eanat Date: Tue, 11 Jun 2024 01:31:53 -0700 Subject: [PATCH 13/14] Reads gps coordinates from main esp --- src/main/Reciever.cpp | 10 ++++--- src/main/encoder_steering.cpp | 8 +++--- src/main/main.ino | 49 +++++++++++++++++++---------------- src/main/sensors.cpp | 2 +- 4 files changed, 37 insertions(+), 32 deletions(-) diff --git a/src/main/Reciever.cpp b/src/main/Reciever.cpp index a590e25..7d263e5 100644 --- a/src/main/Reciever.cpp +++ b/src/main/Reciever.cpp @@ -7,19 +7,21 @@ */ #include "Reciever.h" -datapacket myData; +extern datapacket myData; responsepacket response; uint8_t Bottle_ID; bool InitESPNow(uint8_t Bottle_id) { Bottle_ID = Bottle_id; - // WiFi.mode(WIFI_STA); + WiFi.mode(WIFI_AP_STA); // https://www.electrosoftcloud.com/en/esp32-wifi-and-esp-now-simultaneously/ if (esp_now_init() != ESP_OK) { Serial.println("Error initializing ESP-NOW"); return false; } esp_now_register_recv_cb(OnDataRecv); + Serial.print("MAC address: "); + Serial.println(WiFi.macAddress()); return true; } @@ -29,9 +31,9 @@ void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) { Serial.print("Bytes received: "); Serial.println(len); Serial.print("lat: "); - Serial.println(myData.lat); + Serial.println(myData.lat, 7); Serial.print("lon: "); - Serial.println(myData.lon); + Serial.println(myData.lon, 7); Serial.print("heading: "); Serial.println(myData.heading); Serial.print("bottleID: "); diff --git a/src/main/encoder_steering.cpp b/src/main/encoder_steering.cpp index 2130291..67654a0 100644 --- a/src/main/encoder_steering.cpp +++ b/src/main/encoder_steering.cpp @@ -99,19 +99,19 @@ void servo_control(AngleData data) { SteeringData current_lengths = {(double)DIST_PER_TICK*((int)count_1/2), (double)DIST_PER_TICK*((int)count_2/2)}; double input = angle_diff(data.desiredYaw, data.currentYaw); - Serial.printf("PID Input: %lf\n", input); // Comment When testing is done + // Serial.printf("PID Input: %lf\n", input); // Comment When testing is done if (fabs(input) < FORWARD_THRESH) { // Go forward if we don't need to change our heading that much des_lengths.l1 = -100.0; // Servo 1 des_lengths.l2 = -100.0; // Servo 2 } else { double output = pid.compute(0.0, input); - Serial.printf("PID Output: %lf\n", output); + // Serial.printf("PID Output: %lf\n", output); des_lengths.l1 = -output; des_lengths.l2 = output; } - Serial.printf("Lengths: %lf %lf\n", des_lengths.l1, des_lengths.l2); //- -- Comment me when testing is done + // Serial.printf("Lengths: %lf %lf\n", des_lengths.l1, des_lengths.l2); //- -- Comment me when testing is done // Servo 1 if (des_lengths.l1 - current_lengths.l1 > (DIST_PER_TICK)) { @@ -151,7 +151,7 @@ void servoControlTask(void *pvParameters) { // Check if we have incoming data if (xQueueReceive(steeringQueue, &incomingData, 0) == pdTRUE) { // Successfully received data - Serial.printf("Desired yaw: %lf, Current yaw: %lf, Desired forward: %lf\n", incomingData.desiredYaw, incomingData.currentYaw, incomingData.desiredForward); + // Serial.printf("Desired yaw: %lf, Current yaw: %lf, Desired forward: %lf\n", incomingData.desiredYaw, incomingData.currentYaw, incomingData.desiredForward); } // Call servo_control function here servo_control(incomingData); // Modify parameters as needed diff --git a/src/main/main.ino b/src/main/main.ino index 5f0a4f8..c5bb7a9 100644 --- a/src/main/main.ino +++ b/src/main/main.ino @@ -40,6 +40,8 @@ uint8_t GPS_Loop_Counter = 0; // esp_now_peer_info_t peerInfo; +datapacket myData; + void PrintSensorData(); void DoKalman(); void DoCount(); @@ -68,6 +70,11 @@ void setup() // Print the configuration data configParser_inst.printConfigData(&configData_inst); + // Set up ESP-NOW communication + if(InitESPNow(configData_inst.BottleID) == false){ + SERIAL_PORT.println("ESP-NOW init failed"); + } + // Initialize the WebStreamServer webStreamServer_inst.init((const char *)configData_inst.SSID, (const char *)configData_inst.Password); webStreamServer_inst.setCustomFunction([&]() { mySensor_inst.resetGPSReference(); }); @@ -95,11 +102,6 @@ void setup() //Steering setup steering_setup(configData_inst.AcquireRate, configData_inst.PID.KP, configData_inst.PID.KI, configData_inst.PID.KD); - - // Set up ESP-NOW communication - if(InitESPNow(configData_inst.BottleID) == false){ - SERIAL_PORT.println("ESP-NOW init failed"); - } delay(1000); timeStart = millis(); @@ -358,9 +360,10 @@ void DoCount(){ void ComputePID(){ char outputBuffer[BufferLen]; //This is for printing values out to terminal - double target_lon=-122.12071003376428; //Example target longitude - double target_lat=37.4181048968111; //Example target latitude - + // double target_lon=-122.12071003376428; //Example target longitude + // double target_lat=37.4181048968111; //Example target latitude + double target_lon=myData.lon; + double target_lat=myData.lat; //To access kalman filter values for current x,y,&z direction MatrixXd X_Zaxis = myKalmanFilter_inst_Z.getState(); MatrixXd X_Yaxis = myKalmanFilter_inst_Y.getState(); @@ -392,7 +395,7 @@ void ComputePID(){ // double height = X_Zaxis(0, 0); double height = sensorData_inst.barometerData.Altitude - sensorData_inst.barometerData.AltitudeOffset; // Serial.println("\nHeight: " + String(height) + "\n"); - if (height <= configData_inst.HEIGHT_THRESH && height >= 0.0){ + if (height <= configData_inst.HEIGHT_THRESH && height >= 1.0){ // Make the Data packet AngleData data = {setpoint, sensorData_inst.imuData.EulerAngles.v2, 0}; sendSteeringData(data); @@ -406,19 +409,19 @@ void ComputePID(){ double distance = GPS.distanceBetween(lon_now, lat_now, target_lon, target_lat); - //Print to terminal - snprintf(outputBuffer, BufferLen, "Yaw: %.3lf\nSetpoint: %.3lf\nDistance: %.3lf\nlattitude: %.6lf\nlongitude: %.6lf\nlat_now: %.6lf\nlon_now: %.6lf\n,x: %.6lf\ny: %.6lf\nz: %.6lf\n", - sensorData_inst.imuData.EulerAngles.v2, - setpoint, - distance, - sensorData_inst.gpsData.Latitude, - sensorData_inst.gpsData.Longitude, - lat_now, - lon_now, - X_Xaxis(0,0), - X_Yaxis(0,0), - X_Zaxis(0,0) - ); - SERIAL_PORT.print(outputBuffer); + // //Print to terminal + // snprintf(outputBuffer, BufferLen, "Yaw: %.3lf\nSetpoint: %.3lf\nDistance: %.3lf\nlattitude: %.6lf\nlongitude: %.6lf\nlat_now: %.6lf\nlon_now: %.6lf\n,x: %.6lf\ny: %.6lf\nz: %.6lf\n", + // sensorData_inst.imuData.EulerAngles.v2, + // setpoint, + // distance, + // sensorData_inst.gpsData.Latitude, + // sensorData_inst.gpsData.Longitude, + // lat_now, + // lon_now, + // X_Xaxis(0,0), + // X_Yaxis(0,0), + // X_Zaxis(0,0) + // ); + // SERIAL_PORT.print(outputBuffer); } diff --git a/src/main/sensors.cpp b/src/main/sensors.cpp index f68f241..e5a7e39 100644 --- a/src/main/sensors.cpp +++ b/src/main/sensors.cpp @@ -446,7 +446,7 @@ SENSORS_Status_t sensors::readGPSData(){ SENSORS_Status_t status = SENSORS_OK; // Read the GPS if(gps.update() != gpsLowLevel::GPS_OK){ - SERIAL_PORT.println(F("GPS update failed, maybe no data in buffer? or wrong Pin connection (RX-TX)")); + // SERIAL_PORT.println(F("GPS update failed, maybe no data in buffer? or wrong Pin connection (RX-TX)")); status = SENSORS_FAIL; return status; } From 2189a9b86ebbc6a69328bee73611e7d8e4515e0c Mon Sep 17 00:00:00 2001 From: p-eanat Date: Wed, 12 Jun 2024 00:13:31 -0700 Subject: [PATCH 14/14] Tested communication and PID on the ground, minor corrections, should work theoretically --- src/main/encoder_steering.cpp | 6 +++--- src/main/main.ino | 14 +++++++++++--- src/main/sensors.cpp | 6 ++++++ src/main/sensors.h | 3 +++ 4 files changed, 23 insertions(+), 6 deletions(-) diff --git a/src/main/encoder_steering.cpp b/src/main/encoder_steering.cpp index 67654a0..cac3408 100644 --- a/src/main/encoder_steering.cpp +++ b/src/main/encoder_steering.cpp @@ -98,7 +98,7 @@ void servo_control(AngleData data) { SteeringData des_lengths; SteeringData current_lengths = {(double)DIST_PER_TICK*((int)count_1/2), (double)DIST_PER_TICK*((int)count_2/2)}; - double input = angle_diff(data.desiredYaw, data.currentYaw); + double input = angle_diff(data.desiredYaw*0.01745329, data.currentYaw*0.01745329); // Convert to radians // Serial.printf("PID Input: %lf\n", input); // Comment When testing is done if (fabs(input) < FORWARD_THRESH) { // Go forward if we don't need to change our heading that much des_lengths.l1 = -100.0; // Servo 1 @@ -111,7 +111,7 @@ void servo_control(AngleData data) { des_lengths.l2 = output; } - // Serial.printf("Lengths: %lf %lf\n", des_lengths.l1, des_lengths.l2); //- -- Comment me when testing is done + Serial.printf("Lengths: %lf %lf\n", des_lengths.l1, des_lengths.l2); //- -- Comment me when testing is done // Servo 1 if (des_lengths.l1 - current_lengths.l1 > (DIST_PER_TICK)) { @@ -151,7 +151,7 @@ void servoControlTask(void *pvParameters) { // Check if we have incoming data if (xQueueReceive(steeringQueue, &incomingData, 0) == pdTRUE) { // Successfully received data - // Serial.printf("Desired yaw: %lf, Current yaw: %lf, Desired forward: %lf\n", incomingData.desiredYaw, incomingData.currentYaw, incomingData.desiredForward); + Serial.printf("Desired yaw: %lf, Current yaw: %lf, Desired forward: %lf\n", incomingData.desiredYaw, incomingData.currentYaw, incomingData.desiredForward); } // Call servo_control function here servo_control(incomingData); // Modify parameters as needed diff --git a/src/main/main.ino b/src/main/main.ino index c5bb7a9..dd93507 100644 --- a/src/main/main.ino +++ b/src/main/main.ino @@ -362,8 +362,16 @@ void ComputePID(){ char outputBuffer[BufferLen]; //This is for printing values out to terminal // double target_lon=-122.12071003376428; //Example target longitude // double target_lat=37.4181048968111; //Example target latitude - double target_lon=myData.lon; - double target_lat=myData.lat; + double target_lon; + double target_lat; + if (myData.bottleID == configData_inst.BottleID){ + target_lon = myData.lon; + target_lat = myData.lat; + } + else { + target_lon = 0; + target_lat = 0; + } //To access kalman filter values for current x,y,&z direction MatrixXd X_Zaxis = myKalmanFilter_inst_Z.getState(); MatrixXd X_Yaxis = myKalmanFilter_inst_Y.getState(); @@ -394,7 +402,7 @@ void ComputePID(){ // Send the data packet to the queue (i.e. activate steering), if detect that parachute has fallen below HEIGHT_THRESH // double height = X_Zaxis(0, 0); double height = sensorData_inst.barometerData.Altitude - sensorData_inst.barometerData.AltitudeOffset; -// Serial.println("\nHeight: " + String(height) + "\n"); + // Serial.println("\nHeight: " + String(height) + "\n"); if (height <= configData_inst.HEIGHT_THRESH && height >= 1.0){ // Make the Data packet AngleData data = {setpoint, sensorData_inst.imuData.EulerAngles.v2, 0}; diff --git a/src/main/sensors.cpp b/src/main/sensors.cpp index e5a7e39..947f486 100644 --- a/src/main/sensors.cpp +++ b/src/main/sensors.cpp @@ -363,6 +363,12 @@ SENSORS_Status_t sensors::readIMUData(){ double t4 = +1.0 - 2.0 * (q2sqr + q3 * q3); double yaw = -1 * atan2(t3, t4) * 180.0 / PI; + // Add 180 degrees offset to yaw + yaw += YAW_OFFSET; + if (yaw > 180.0) { + yaw -= 360.0; + } + //update the Values sensorData.imuData.EulerAngles.set(roll, pitch, yaw); } diff --git a/src/main/sensors.h b/src/main/sensors.h index e68f251..d4023b5 100644 --- a/src/main/sensors.h +++ b/src/main/sensors.h @@ -42,6 +42,9 @@ // BATTERY MEASUREMENT #define BATTERY_PIN 14 +// YAW OFFSET +#define YAW_OFFSET 180.0 + namespace Sensors{ // Define a storage struct for the biases. Include a non-zero header and a simple checksum