From 1d03af6dd38d29364593486e0285f898d031e07e Mon Sep 17 00:00:00 2001 From: Nicolas Trimborn Date: Mon, 3 May 2021 21:14:57 +0200 Subject: [PATCH] tidied up and added motor instances. commit before merging DMA ADC and additional Commutation --- BLDC_E54/.vs/BLDC_E54/v14/.atsuo | Bin 203776 -> 167936 bytes BLDC_E54/BLDC_E54/bldc.c | 243 +++++++++++++--------- BLDC_E54/BLDC_E54/bldc.h | 119 +++++++---- BLDC_E54/BLDC_E54/control.h | 8 +- BLDC_E54/BLDC_E54/ethercat/ethercat_e54.c | 73 +++---- BLDC_E54/BLDC_E54/interrupt_handlers.h | 2 +- BLDC_E54/BLDC_E54/main.c | 61 ++---- BLDC_E54/BLDC_E54/motor_params.h | 107 +++++----- BLDC_E54/BLDC_E54/utilities.h | 31 +++ 9 files changed, 364 insertions(+), 280 deletions(-) diff --git a/BLDC_E54/.vs/BLDC_E54/v14/.atsuo b/BLDC_E54/.vs/BLDC_E54/v14/.atsuo index 260493293680280005a4919b17e3dbe6c459c442..fad8109c9daa9ad15be3136e54468dfdc03a9e08 100644 GIT binary patch delta 13087 zcmd5?33yaRw(e86yXmZSI-P|OLMH?aAtWJyBC>QKDuQegkx?WG5Fn92lLQx362Juo z5xSU^yAapsH=@!=Gp2Fc{$ej7a+$3>Ai4WDkC2YLhJfdaq^Tn>}~p+F(P zlI3SQbM4PIHqO`ci;(sNX6x?@^fU?S1i52I6h`JYF1$YsbOr7JxGWs$ZF-&unTY%d zpa)Q{mor@@^70J1g4Xqw&6K4_o5~W^Tv@mmeu5+!BP)0;R*zEPm#PQ%#_GW<*9m}n zbW}Skeztn>V6CZ#83VI=j0IRbcqQA`4yaLU>cPt%P!Aq7pdOrWt{(h;fqI}Vmk&eQ zrgm6B#Og7P)dS3MqhG5Y+y@DRK>!+H1bCUvNIL-*pa&2PgaDyH7;v@%M4geb0ug|X z3sGiA8U;iHF?v}Sq;Wu3AYLy^&{GG}L?B6j=RO#{@O}x9qL-&4?E|Fg@BPw|$pA9- zf-I!{fdTsarARLW1_76I*_wh&Wawpe?(lN@ms}V=PHq+b=o@(=yu$o8OvZMDT-14y z`EdyNid1|$#qK{GE;=HY=Y$->&94aMJaD!-Ok$0CR80V$kWHsYJD%` zoq@W&A4PGic-e5gx2*1yTM>>oD@Nm|J-M?OE|5FQeuda88rzaP`fgY5KU?hFY16Z2 zUYsZNGCO%3w!PG`l~~j(#gjDXEJ;Vri^gJzEg*DHohy{w(OW%Ry}YWK!C zrK2v{f=yyr5-7~q?B?DO|5+^bnQM6U%{~3EzGaI^vfKsId(V+a>8rZv#AsP7GDG`; zx(?O+xVMy>`(Pb+rFs@^8AnF;1p&r_hT~;0FS6&S?wUnS|3Kt>@>i{+>Qsw6$srx< zk}a%PY|j1ce(uu})~(UZmx%{9`*bF$Pm)jDTGc3%PoreR`a8{k#gJz(xk;lfY)9Bi z2Us%wlj~$d^>|}hvtrsKPsI0?)xB~ak&8$^`>LKgD3zOHF;0bX~0T+hxcyi|KCV|01oK&KO#K@90uxuqrfrXIB)_8v_FM> zJ;yX2Anc4f9uw8y!$me#nEs&mYjFNH|B#WJM#>3M#?ANl3L13&`M z2jKF+GgV%`sYl;O_0q|D`UKJ^fv13Ky)4k;MFfs9a^0qs#7%n5>qs{PUf>PAjHzpU zlr^tg#EQhSUg{05i}DumC|oFc1QS0%1Tn&>3Ls8v)pWNWcz6 z0ntDV&;_^}W5yxv3d955fCQjB;2?yziO3`YJ%D7OC(sMH1V{mT1E~OuEe&a3ARQPe z-@B=6=%vVA1`Gn^-e=Roa*(+axC(%u`i_oB%J|Z=XI2uyU?|66O17}S`7QQiXw!Z~ zhtPA^-8U27$q#Xov8s4ckSyr}p+C zMD>ALeEf2j`sec^SO(8amV1hmcxI~>hFWc}mTh>^JNwJ!H%HKHbJ=29nH?Q5-7wcM z6UUZ3!z@D)Eapy0j^Vn&{=+(o^Vg<>V#7VR{e9z=Ci&2JV^SYZeXSuN7+JH(Cm9T* zYSxQnHDRfUmZQcd%O@r$>rzv>r)ZdGSmmF{W4z}K$qA2!%kZ^Xa{Kc`OpT5644?UV zpCq3hnyOuE*zJ3*9Vxel_j{(imQ2x@@3-=f4=Ors@y*?!vUiAOo~6qd6NXJILD*Oh zlwuk#oJLB45h{By65HR$fKQc9JHmLmP!dy%P?@ zw({e0s$Ok$FE@M4i%yfuDHcz8o~oK|l1@h^s}4BrwGez3LF7fgv{2_U&zjYc^?<3< zt{k%A{{u-q7r6%HbBlmZ$%6 zU&yGCZYSKY#;Y|kl%d`|NG1e|`Ys7~r$nq>5nook@b8-r?%Q|inl8mfzOmGvCq=As z4ieVRg;)b#jTw;TOhaIU(RA53zlThYjF-dg_XbfC%>vxW^0N~xd_HEU6xgM*?(B9;Dhl`r<)2KW zqk4m2wfW^s@1Ms7OMkvbu#&gG^i<|)yt9^q4Aa0OAGQ5#-tBNFOtP?%jzO7cYQnQr zy;-<_*cxPDugsSKe!}m+mRsLm;TiYFM&5BOy8j#fdAKA)Zdhxhj|18`F3V=^mbWeP zbl16&U9;-ziy!>kT6cDINSR?Ot`>No9n`hk*?g*UYC?{k=gr`?X(-7TV{-Ty&?J8_>uI>AlWxNd*nw=&bt&@dI6K*@65Vh8=uOxM#RG8*5!{`P6v z&%eIDE@geimY4k&VtYj<;@xsnBslZMx2k$D^)m9&p^2@gX!lMtN1OUduiBPmVUJwd zj5LwYvc6~+g^+3EJ$lM{K3lBtMObMAa^&}>Q0qFBa0JI;6;sHS15(K2BiS^-F82;F zI^NW)-a`5|@DA`Uz%t=S8V63X>_)*q06BBukbzwD16~dQ2Z0{}4($&EM}VWiae!N& z&{N{TWR1((SEr6F!NJoMU+2RoU&xw<9;rT$Wh4-}%rcUZJQa`z=?!>4bZ1GN&$2LIS4MH?knK8{XZGSaEs|K)ET@LUG?FwKB5Kyq;$UA|HD4N~K zL^9>(-R^C5G|b2QQ09GOa7x>!n&gh0YNRvFp8p#iV|>%O*ePb??i^tZ!;%N=dB{Mh zD@Vdy*7*0H^4VPu3#%+2NCN6F>vm1xyQfCJG7A)X%0FExsy`Fe$6zmOE$5p{&(;Gb zk{=$q^O=!9!t7enGOAf?S$=4I5OJRz+XVWQb%(~wt$!UWS0C`fN*Gi1I_nGEotlp@Z{kh zcpxs8Sy-LDAB0dgRg6P^Y*X~r4-r@MSgZ`zw*Xk^i*sfm<*1<0Fb`#;P%{_*Cip1I zQCf;}|9v#m6d-OGi(CHyox$L^V65D5olqxIYXn9032#Bim*_1^`s*bSyo#_G=X+g~6i>!22$yE~>0FLOBK5y~0WpgqmfeRJGkknX1M{V{O(E zxR%c`j0`j~SX5plC3ep<`S184L^51J)X58?$28idyR56dH9hD(>O2pD)Le*fCgwZU zz`oDrby~?Ll+8xz5j;NzKfF2ywL>6mV@A~9Y}CJdS14|Xzw#oZT6olbV&kjX6xQN2 zARC8g(OyZ83U}30-lrTi11Hu*+S0_|WU57Nrk_ihQAbwXd-VnEO;MGZ zG)Ybxk;bvxDi!`OI;J8raWe1+@IKxmblxsx2FEOB=6bo)8SiH%Je{ILu0U)U3Gtf^ zJ5}Ovy4yZ|?#vl^vx-VfZYwA)%qyQ$SXNY~jt-?rm0%X>;;tR-k!AjoOe)Duy{TQ& z{8W)npS9PatCm7k*a(rL9=nnjx6WWl$^7#9rFpJ`(t;AT;9BZKU$@cq#4pIM#@~!R zmNi85w%*0cAPkRuH$AK}eSx!s!Z4|fSu|Mn9!?8e=SwwBAS`5IdC}aW@}k1B^f^Up z;sCm=oo3V_D)nP6#rpNCNi{ZqPflW$ajFH8RQbY!M@fWyfb#;cw$aeBwP}~CX|aFq z+Ndu_x3-o$im0cmA4MtZ@MJOvO@HIRww>yFi?idrkdc?AAAQ-z@}C=|xQHUj)y85l zX}bj8`Ni~#=3VeO?WJf3L>zcn+&JeMayq0^L3f!;3X0~X&zRW;LxJ6BrVdf5zMaG@ zRk4e%QJLdKRQua9u&Al(;Z$J~?eoG7I!0!EeYM!BMZX-7j=@s1L(k{zP&)=vtlD=o zC3ziR(jHQ-p;Xw$27&n)*uDtBT>FCZg6gamy`qUPgW+-HLxG%*Taoa)!V6(BOJ@|6 z=M@edp!PjNgD=kiIqS3k5xTq`8t;fodO*CeuZ)Cu-{a(>-h3D5e==(Lyuo}jG=HHX z^T5}yoO-9==j<7wUh6F?FU|=o+y&M(EWDY$EvxDyTA3<04*i9s-t0=+uBm+sX|OMn z(5Gj34Vf9QlrNOxP_8mk36Dz1C+cSFZ;i^`Alm6&6MDc=qf@) zZ~nd?csS0F1)8gnddd3cfzBoEs}MVJ-1E0Tv)_EXI)(%$fOHUGJAV!ni^P-CTA8j^ zpCZ=_G@S%Z^3p+S)Ktwfk)!etQ<~Rxj8=zizDsN&@0JWPN~j6_#Z_|M42O5trDC4o z@Cp}@#n@Zpy#<5B(^}PXVewjWL_KYOP$ZaCLXp_79KRO{-h{bgKdG~Bk?c*b6km`` z$WK@uu-^Yh60cfAcqM*EwQOh;#| zu`QZ~@C_b*^8&o1mV79lmiZ4Pc$a@6LP_N}i1{k!sJKK$d@5Ebt6l5FpLTq|-f7n_ z`Lz7~dSO!=PKsO9aH|%izKYPgsJJj~j{2lQbXH@mcy&c-vz2R?FfvJ~+S7nr+_3#( zv3ev{TcQrwQR{e(!qok-T0}EqM`wK6-Ed08tCzyGrK%=Qo1hMUDuQt_E@D(#IL3%L zE)waMw!N%F?Y5%dUG+k$)zO+!y>VPDrbGg!1$r+bE{_KiW2p2D+WBrHaH1V zCnB}3JhRp6(_nDi{aXrGj~o*b?K`ix9dcCRhSm1Pe#+0D_^B=DC==Z zDDEh}a0J#OQ#}%_4Q1AsD4(1$@_p@($Ien+$CHMXXRC9sXg=uTwl?B~aQGOD`dXBz zr;kG}L+eEJ3pA62DmWn`)gPlZhr0i$h*nS3iA427h*qr{qqH@YuOjSP26K9e8XJuD zrCHR~!QbipkEO<;=UZ5�I)XhA+zUZk{JblUfq3Rk2hSsI)k3t-9N;75ai-xw^q0 z`?{T#%26Yn^qIsLBzLw`y~ZDbefv7w>2~6O+aGGSR3AU|&D2r<-f?C1V|lZ8$NkI? zH5Xof@uv;`j?;}V8uD;*z7^LHcj%v79vT{anm;UTZ?-$}0Wix33pB$^#iEf^=4r89 z{o#2LFVh#<6yy-AjK#vG+bSpS-O{n2XQG#uMgylnl=DUV9^TIry^i^!)TrEbw65zgx60n{wPIaVEA(1>S;t#9^Xc z&E?eP;vb-JL*gBKAATmJOUU;*DU_zFezjtUTs1V#yYoRYO;cGVO@p8NmKsZwe01Kk ztHc1IlD&Q!{@8#kZ6{6kCRPfe9ImDzOfj%P%MS>^vl)Ou_GBo>MDsNkP4 z_3K!2N4;s=#9M;CZN5?xAM3EbQ@~scpA&FZaJ5*OH{-qAPm6j@)va$@nWmq2M58@N zRxR2$^I0wY8<-jA(`Y%aFjIZFQ*3L=uu6DYgqwed?r=#pu}+LpPaYLr*v8+>M!glq z_njks1``@SoPXcobIGdT3OJW@ZtXlP{i|JAz#9#?xJ^EHy!bH+o9`DnJb02;=DZ|M z8{Xs(+u8S$SbeURgm2pWxrCNBr_jLnmDme^%(^ zQ+LEqmFGvo2$+7)@nvjdN9}p9&QmOobt1T@pASwbn0eyfxPNV<0p~BV3N(1u$=K5A%204)Am3m`7(D6isXeMk($29s5QLWue$a;k{d_j2ag8$1|4{xXyj#j znukV$E(Ll4cY1i^LAwF%fet_t&>4{LO#}@G?ggws93b15lei7($tqQ?Y624Rz+gbW zlngom=mN-b4h6CSS!M(c0aAfMfNV?++7B=RVZiOc2tZDH3@{4V01O18Ahul4On?K! z(Z4DXiFrVOKsG3aF&EGSveVI^Ujb8qL?8_q2NVIr09hvn^g&Nr=2MW?0zp6qPzXqV zj)zJKEJS(+`d8UKOj#iv=>jKr(P6B?<;#Dv|8mSBd2A_(zmTzUNgUnQm1gnekO3nfEGD0UB?s zM6&$5lt^m0EcYr=1oFO0WcHMQONpdr_5^+kB?@R&BF8tGy(#KX*lKcsZNc-h6Kmz{ zj-tW`z%k&sr}7EVlfZ|b{QrV}4EzQ7#KZdx^mE`8aN5KBE9e>EOW>@BcMkMCaKV$m z2zm*)OsbBmzj>J7ND8z7D(1MFnMAeUHS)GIk&}?T!+7^y;0W*@AbCGvazFo}v-^(8 zVQUB5X+NJIJdO{IPN6sW`QTd3Fbw!L;?*J5!CN4LsrC55^ZivdTTo?cJ+BT=bR>iy zqsDD71t`{ufv-3j*S2D{)I_hx;r3tW^L-;yg_^OIuUNDNAqfP!_BiqAWJ32u-A2k>G z*1m#bBk{|QDOx@FQWG3gpV>xD2JMc0uaMezQo-(FqZ*#3BrR_?XhiNN8X?X+N*b}M zKMQsgPT4|@L)0CXqsj_EPWvukHL#3t^^f3v4w<_wK~9QHK3^ie6=(t;0{EIkiL?s= zLT~Omq&DmZ>-WHOz#iav;02%=*b8v=;mLezRX1LD*iic^7@q;31E+w~z!$(@0Xa4K zOhQ`HC=a~=deM`<1bP|x8*l~q2GB!7vc7y|8w@Jr2dDvmK=RswY8hN}01|;7W-w?7 z5DJ8Oc){zdj#yw!;vvk_UDRauCkGF409{LCtwYKCe)Z>is_dw$gRmSwuakEh2k*z@K1ad}o1^`kUT1v9tKp9DF<(%~^WB>L z2K1{cot=5*K9y8U%H;78alF~`rZ{6}y6&?O{*|ics0z?`Ke#6&VwR5Y?HrL-_SLJp z`mj%ujy$_Wd65?`i{vI281hHxi2;z$Szh1$R}RCDZKR!ztPF3w`!ncwsPnO`q*95u zR@0M??Gq+YW4ifUo8SL^)HJ%cZl?q-R=TAUFL@jCccIwDFQt#?Bcl8Ge}tUDU3pr9 zRyy_?pUOd>qF8D&9ud!JuA_WXqu&P0WK%~lqk&scBD?5}bQd5Vka^!H8N3aa^e(AB zWobzF^W+C9<=xa16`G%QJLn|fXTW6O=fD);4qz%U4VVrT0EK`BmObTL$b&H+k+ zGQhii<#_HyeuE{d+d|MqKs8VUEC+nYE7Of{9@WZsCHO}(`>2&f0=f1nI?r zT@>p`pZGd86*1b)$@(i^m>AyK4h?`lVcq|-;H$6~+NVACM6dj}d+PFUbFaJWX9nIp z=}l3%h7930*x)Ky)+*(Ndj$!M%20b#R=I3ZkY;CZ3L< z>B99iY2$obu&Op5o;T&A=v%stK9TZZ*J-Q$l`h4WCelY(RrAq}9RYtCw6hfST~3BK z_|lr-_A8QfJ!4;b`d0^UyY&~4ng)VRRlFytqaT1M8&NL(E{IOgA) zMk22((>S&-oIp(**^V7AQ2^Oc=lj%Fh+_^ibS_nuD1$LVSWlF1-&-8Ixk~@~eP37y zRB@WsLEPG#qG_d|csd{8mXB3s5x3UUAV+T9%hXA(koUO@tG5EU8(7I>vN}4{Lk^Hb z-Ef7t zEwT#cSih!%1bc-F9fgTosA;{ru|dt%%LhzpYmerlmm&ydmTD?|pG{SXbOrj9DGOGs zpw+Hev2xzJa#St1>F4ZO_E65qZktAY6!G4Ao?oEnre>XD#!mP7_Oj1H(^h3dk3D;+ z_E6d0k3Q6lw(*UhKH+_JlYurnHgB5YkI`oOFQnsGCGy=gOR>|x-B&~8fT zL-0#|U=l}b<-xVutM&3aL4%?sggviS`v)P304Z#=R-WDD>9*Cv>QK~;UrMWKV~S-^ z^G}=Nz-y+MUwz}fYsSgtHBP$yXPO>njR}T_4CCNRA9HzCZ!uJFJKGscgGo$SPJ_th z_A^tvQULS~hjX9NbNjS4*>{5>rRWXCWLiGUz+Vac0$2sa0`3y`Z*QJGs%uP&$DTgm zDg7m=j2<@wk0L_r$+wJ3?BGD|8Q@u9C*TBNl14A1Gx#lN9Id6V6&HII*)H5vt|oF^ zeU`DtSi7bKU8KZ0s$<8Ix;I{Q+od>zXc)(4IA-kLOpWiIqilZuy;{oR^WR_HxO<$~ za*#rEfAZa>s9Myc*UV|5*Rb1Jq zQKp;`ue+pk=#Hh4JmiQvMrywB_(~DJh9)USRtyMaQI551D@mlTq+CbtffuRqp0lwP zFesHUX>Ol0ri#5a@TuFTg}r`JJ8{OIW8AP;-?;$AHrR2gu%WMlCdhn%sv}%1Wv|i6I{kAvXcFG_(5w=E&}#T4bhtg3!nW)=PAy-#~=KmK`@=vLF4H$SQ8XDW2G;@WI_N@-N0 zH|a5EqHp({Dqnu0$IJHD=9Zkg-1VNVV;3p1;YSn0*%(!$sZMdNKglVq@Mcb7L6CPj zY{1Q&!WwsBb6rT`_H+44R2<<|OV_Rs(2Gwnrb{Vi&m5t5MUBbEkii?`gvc}6JWCX5B z!oJDHp}xz=0II_~(#CpQ)g+@Z2M*`5aw^xPBO8voF{88%s3z~vyj6pKv_)YlKbIKdLPWqtG>zW#vT-wqRYir3$Nr2AAD5-T>hoVhgC((IXPO%Gds zSyJOyKlOIMmV`T0;)M_zDzilSv>-94od4P+HvYFjuK0 zkPfMIG7>gJB?@ruFHf=l&Vu%Izh8*Fp_KBI_ynhi-K5AzG>6cTAFG?KY^R>;NzB+p z31WT*g6q%F(-^TKoYp(n8)zO0ilmDn7ut@++1!El`o4D=?~N7IgYrZpq8?{m584ae zQIfr6Ra`&Cit(9rd$1DwyJtL9*n zxDHWjU{T4e`IQ!X$=q`BXj)uj8&i z3RtwT!|rqPtswpxgsK@X9aLe1ACFLtb@K2qq-LGm8gElou>OMGU1^Wwdc zoR-SZ~3`9$w$oEYK~z<34h#Tfu^aE0+@cFFr)(925T4 z3{D+BEHiyrO5Y)w>1lmaQp~2lgNLOI?rSm)HVw@-4Ie&yXxfs+1ICtAS%;UD+NyHJ zTcaq;J-5PnOrsrZnPV$LiHI+wM7H>hyFcjAU|}pne^PJg{cEGpRFo7HrKP&H^%D<& z3hOT(U&R94YIoYrJu{7s6zTo(`ZdM~sn0QfsY{MrYf=1LN&u@L5@Ir$hG%8=%gi>X zE;->dV&^dmRU{K8tr1h*{bKpr3nNSO+UlaZoyx56BKTX*WHHvpbpH~8iZ4gdY@aO< z%9lDDM7fOu_-f~TB_U4UNwGiPgnTxfF4Sf(wpCg!_5xeKbfa~tNI619BG1OcH3}n1 zwD)IA#fNuNj4;`0nKNfH-RBn}uj#zc0;G)yT3QBDPRfs~(?N?{b=>YAnbi9LuipFc z`gwVMEVk54OU4XSUt3ywzrN|ISy_FvGiDa`?Ps$TrDdnvY-t(UMw2N8|1B2l>PWBe z-8z{b&0{=ib6X=PrjDfuQCG_rDL>8#JDnzHTm_vV5jTl40y91GFt@cxK>^me<@U?2Fy3M z6rojii``acu@;w<+l<8`{}|0ze!e|Kc?BSK$b3Wl8WvJ)DJ`%qv=@|A&NEtTq(00N zx;tR3r46RGjJ6w3jGRQlF==Q&00;&U1S-$PnY-`$u=m`&@A07O5S7{2c1y>v)s$IE z%8k}iw`PK+K^&wQ0ht!}-wtyyZaC$*H@<$Zr#pO?YQD#lHh)i_p z8G+%RX0K}v@tZpa3JaN8S+c-ZSx{15qO_*SMix_*TTU|lZ9SNPR^^!6w(5=wuW!|Q zt)*b*n^6qEkKNU$dd4b=35Wr%>j6?w!mq^ZrGQ4=Qt%|ii90DyX;{FZr-}XZDMcEy z{p2j0Og2A7&jQ<_b*X39X-%ug*J@^z&a@hfJ?bL)Ue%Fl=M#(RqMu$~7kZx$JPGp$ zKK{5hUJ4GhRF)N#S?tBe;tDz3S;Dv)d%`E(R?JD%fyc85X1-yl6}mWRzP+Th#9m^n zG8WHsua*qN$R{m68AMUe^){+t&WZ_CLHb&V?h+;kSrd^2rFJ?Oh)vOujnKrtB1*z4QEvHHlP zlizi@wmL3VFH$GBjx=r3ly>;~m`2pkqgv@`yNmscXuuCO-lT*Mw}|FcRwoYD(OMDO zkIwR~JL80m59Peyrk!TW!--Q+o|xC3-c&xj-PW>I>woUPqtC6qcVd_|JXVE^?hZHk z)w|PoeDzk1bJB8Z@q46hDK1S`;6N<8=<? zxNTxiN_I+Cc4}5eW{NzpTC6FyjDA*Y-rCs+erZLv zB|pp^xxLubxZ{?oCs;Y2ZSqI+_{D zX_+GLKflANeT2WLlkq~p;>9&tL(;NRv&2(^d|Z+2RBRxFvyXsjK zhs3c+CL(`BSH+~R>@4YVY5Qk!=C|nF+=CrcJ0ttBE2!`cU2$GDF-9Wr8446ljr580 zKn8nVEr)bStbYM_+7CXDi|&enEYV*i^khlS=-<+XfYz!Bm>43B1Rnn4d*hS8aHz=Gb zjkflYC1&iVsbY5)(=|5aX-prj7kOj(@P_J5=`=Cud;U~S9NbL@+lWCx3`t_I3$e-S zpEG>#kr8n28m1L{X0py=^deR!#uuaDfd&-3m4+M6b&um5>gvt)iHxT)j(K z9pL#77@U=PY^Hx3DMfql0BI$w5%M-gd_J2civ4A*SbQ^?^%VN$Y@x`1pMEB~SeRa( za#sk`5&DH3z;bc5hAB6Ysp91V79*xrBYxVyh^>`xlu7B$6XVxncsLZrily(+9ML?B zCCKKhMCUtL7qP{k6^ivosGVq6$hzZCs@&}*%bP@hQTh&ri=+jtUSv&Y;qtXPqM{O) zlVOLbv-stwP@}zRG{`-1@8x}zQ$3blY`gM^RK=JY7B9zhmw56hHOTP{SF}L>FB|`e z7IgF(-``!6ISm%U~yvINqluUx)=)$Nt(nPpHfe;=On(FVF6~j7i!m? ztdnehmU2HQR{3LjW`9CWV*haju+1OPdNJ@_OcSeBEC#J%?ZlsoSahqzweEWc?FzI4 z5p(%Qv8XsrVPawpc-P749x>)PEt4}Umc4D3{jL-FrO?3#PLoZ{nTtj7u5_??%IaP% z&(Mw+V`ybH>|XJ=#cY|65S2TH{>Q1k56`p^I*q2P#Dj;i`i~x>da3)XTIUfjmM&!* UrE=wqHIphase_pu.Bus = phase_current; } -void BLDC_init(BLDCMotor_t *motor) +void BldcInitStruct(BLDCMotor_t *motor) { // ---------------------------------------------------------------------- // Initialize all voltage and current objects, variables and helpers: - motor->actualDirection = 0; - motor->desiredDirection = 0; - motor->duty_cycle = 0; - motor->calc_rpm = 0; - motor->Num_Steps = 0; - motor->cur_comm_step = 0; - motor->currentHallPattern = 1; - motor->nextHallPattern = 3; - motor->directionOffset = 0; + motor->motor_status.actualDirection = 0; + motor->motor_setpoints.desiredDirection = 0; + motor->motor_status.duty_cycle = 0; + motor->motor_status.calc_rpm = 0; + motor->motor_status.Num_Steps = 0; + motor->motor_status.cur_comm_step = 0; + motor->motor_status.currentHallPattern = 1; + motor->motor_status.nextHallPattern = 3; + motor->motor_setpoints.directionOffset = 0; //motor->hall_state = 1; - motor->dir_hall_code = 1; + //motor->dir_hall_code = 1; - motor->Iphase_pu.A = 0; - motor->Iphase_pu.B = 0; - motor->Iphase_pu.C = 0; - motor->Iphase_pu.Bus = 0; - - motor->Voffset_lsb.A = 0; - motor->Voffset_lsb.B = 0; - motor->timerflags.pwm_cycle_tic = false; - motor->timerflags.control_loop_tic = false; - motor->timerflags.motor_telemetry_flag = false; - motor->timerflags.control_loop_tic = false; - motor->regulation_loop_count = 0; + motor->Iphase_pu.A = 0; + motor->Iphase_pu.B = 0; + motor->Iphase_pu.C = 0; + motor->Iphase_pu.Bus = 0; + motor->Voffset_lsb.A = 0; + motor->Voffset_lsb.B = 0; + motor->timerflags.pwm_cycle_tic = false; + motor->timerflags.control_loop_tic = false; + motor->timerflags.motor_telemetry_flag = false; + motor->timerflags.control_loop_tic = false; + motor->regulation_loop_count = 0; motor->VdcBus_pu = DEVICE_DC_VOLTAGE_V; motor->VoneByDcBus_pu = 1.0f/DEVICE_DC_VOLTAGE_V; motor->motor_commutation_Pattern = COMMUTATION_PATTERN_M1; - motor->desired_torque = 0.0; - motor->desired_speed = 0; - motor->desired_position = 0; - motor->max_current = 0.0; - motor->max_torque = 0.0; - motor->max_velocity = 0; + motor->motor_setpoints.desired_torque = 0.0; + motor->motor_setpoints.desired_speed = 0; + motor->motor_setpoints.desired_position = 0; + motor->motor_setpoints.max_current = 0.0; + motor->motor_setpoints.max_torque = 0.0; + motor->motor_setpoints.max_velocity = 0; + + // ------------------------------------------------------------------------------ + // Function + // ------------------------------------------------------------------------------ + //motor->ReadHall = HallFunc; //// ------------------------------------------------------------------------------ //// pi current control init @@ -131,6 +135,16 @@ void BLDC_init(BLDCMotor_t *motor) motor->controllers.Pi_Pos.OutMin_pu = -MOTOR_MAX_SPD_RPM; } +void BldcInitFunctions() +{ + Motor1.ReadHall = readM1Hall; + Motor2.ReadHall = readM2Hall; + Motor3.ReadHall = readM3Hall; + Motor1.DisableMotor = DisableM1GateDrivers; + Motor2.DisableMotor = DisableM2GateDrivers; + Motor3.DisableMotor = DisableM3GateDrivers; +} + // ---------------------------------------------------------------------- // Before Power up, Measure and average offset voltage for Current Sensor // This voltage is subtracted from all reading for Bi-Directional Current @@ -166,55 +180,102 @@ void read_zero_current_offset_value(void) phase_B_zero_current_offset_temp += zero_current_offset_temp[1]; samples--; } + //Motor1.Iphase_pu Motor1.Voffset_lsb.A = phase_A_zero_current_offset_temp/samples; Motor1.Voffset_lsb.B = phase_B_zero_current_offset_temp/samples; adc_async_disable_channel(&ADC_0, 0); adc_async_disable_channel(&ADC_1, 0); } -uint8_t get_dir_hall_code(void) +//uint8_t get_dir_hall_code(void) +//{ + //return ((Motor1.desiredDirection << 3) | + //(_gpio_get_level(HALL_A_PIN) << 2) | + //(_gpio_get_level(HALL_B_PIN) << 1) | + //(_gpio_get_level(HALL_C_PIN) << 0)); +//} +// +//uint8_t HALLPatternGet(void) +//{ + // + // + //volatile uint8_t motor_read = 0; + //motor_read = (motor_read & HALL_A_MASK) | (uint8_t)((PORT->Group[HALL_A_GROUP].IN.reg & HALL_A_PORT)>>(HALL_A_LSR)); + //motor_read = (motor_read & HALL_B_MASK) | (uint8_t)((PORT->Group[HALL_B_GROUP].IN.reg & HALL_B_PORT)>>(HALL_B_LSR)); + //motor_read = (motor_read & HALL_C_MASK) | (uint8_t)((PORT->Group[HALL_C_GROUP].IN.reg & HALL_C_PORT)>>(HALL_C_LSR)); + // + //return motor_read; + // + ////return ((gpio_get_pin_level(HALL_A_PIN) << 2)| + ////(gpio_get_pin_level(HALL_B_PIN) << 1)| + ////(gpio_get_pin_level(HALL_C_PIN) << 0)); +//} + + +// ---------------------------------------------------------------------- +// Hall Reading Functions +// ---------------------------------------------------------------------- + +uint8_t readM1Hall(void) { - return ((Motor1.desiredDirection << 3) | - (_gpio_get_level(HALL_A_PIN) << 2) | - (_gpio_get_level(HALL_B_PIN) << 1) | - (_gpio_get_level(HALL_C_PIN) << 0)); + volatile uint8_t a = gpio_get_pin_level(HALL_A_PIN); + volatile uint8_t b = gpio_get_pin_level(HALL_B_PIN); + volatile uint8_t c = gpio_get_pin_level(HALL_C_PIN); + + return ((a << 2) | + (b << 1) | + (c << 0)); } -uint8_t HALLPatternGet(void) +uint8_t readM2Hall(void) { + volatile uint8_t a = gpio_get_pin_level(HALL_A_PIN); + volatile uint8_t b = gpio_get_pin_level(HALL_B_PIN); + volatile uint8_t c = gpio_get_pin_level(HALL_C_PIN); - - volatile uint8_t motor_read = 0; - motor_read = (motor_read & HALL_A_MASK) | (uint8_t)((PORT->Group[HALL_A_GROUP].IN.reg & HALL_A_PORT)>>(HALL_A_LSR)); - motor_read = (motor_read & HALL_B_MASK) | (uint8_t)((PORT->Group[HALL_B_GROUP].IN.reg & HALL_B_PORT)>>(HALL_B_LSR)); - motor_read = (motor_read & HALL_C_MASK) | (uint8_t)((PORT->Group[HALL_C_GROUP].IN.reg & HALL_C_PORT)>>(HALL_C_LSR)); - - return motor_read; - - //return ((gpio_get_pin_level(HALL_A_PIN) << 2)| - //(gpio_get_pin_level(HALL_B_PIN) << 1)| - //(gpio_get_pin_level(HALL_C_PIN) << 0)); + return ((a << 2) | + (b << 1) | + (c << 0)); } -uint8_t PDEC_HALLPatternGet(void) +uint8_t readM3Hall(void) +{ + volatile uint8_t a = gpio_get_pin_level(HALL_A_PIN); + volatile uint8_t b = gpio_get_pin_level(HALL_B_PIN); + volatile uint8_t c = gpio_get_pin_level(HALL_C_PIN); + + return ((a << 2) | + (b << 1) | + (c << 0)); + +} + +// ---------------------------------------------------------------------- +// Disable Functions +// ---------------------------------------------------------------------- + +void DisableM1GateDrivers(BLDCMotor_t *motor) +{ + hri_tcc_write_PATTBUF_reg(motor->hw, motor->motor_commutation_Pattern[0]); +} + +void DisableM2GateDrivers(BLDCMotor_t *motor) +{ + hri_tcc_write_PATTBUF_reg(motor->hw, motor->motor_commutation_Pattern[0]); +} + +void DisableM3GateDrivers(BLDCMotor_t *motor) { - PDEC->CTRLBSET.reg = PDEC_CTRLBSET_CMD_READSYNC; - while(PDEC->SYNCBUSY.reg) - { - /* Wait for read Synchronization */ - } - while((PDEC->CTRLBSET.reg & PDEC_CTRLBSET_CMD_Msk) != PDEC_CTRLBSET_CMD_NONE) - { - /* Wait for CMD to become zero */ - } - return (uint8_t)(PDEC->COUNT.reg & 0x07); } void exec_commutation(void) { //CRITICAL_SECTION_ENTER(); - Motor1.currentHallPattern = HALLPatternGet(); + //Motor1.currentHallPattern = HALLPatternGet(); + Motor1.motor_status.currentHallPattern = Motor1.ReadHall(); + Motor2.motor_status.currentHallPattern = Motor2.ReadHall(); + Motor3.motor_status.currentHallPattern = Motor3.ReadHall(); //tic(DEBUG_3); ////if ((Motor1.nextHallPattern == Motor1.currentHallPattern) && @@ -225,8 +286,10 @@ void exec_commutation(void) //} - TCC1->PATTBUF.reg = COMMUTATION_PATTERN_M1[(Motor1.currentHallPattern + Motor1.directionOffset)]; - TCC1->CCBUF->reg = (uint16_t)Motor1.duty_cycle; + TCC1->PATTBUF.reg = COMMUTATION_PATTERN_M1[(Motor1.motor_status.currentHallPattern + + Motor1.motor_setpoints.directionOffset)]; + + TCC1->CCBUF->reg = (uint16_t)Motor1.motor_status.duty_cycle; //hri_tcc_write_PATTBUF_reg(TCC1, (COMMUTATION_PATTERN[(Motor1.currentHallPattern + Motor1.directionOffset)])); //hri_tcc_write_CCBUF_CCBUF_bf(TCC1, 0, 150); @@ -249,28 +312,28 @@ void exec_commutation(void) //volatile uint8_t curHallState = hallCode & 0x07; //ABC format //volatile uint8_t curHallState = get_hall_state(); //ABC format - Motor1.cur_comm_step = MOTOR_COMMUTATION_STEPS[Motor1.currentHallPattern]; - volatile int8_t step_change = Motor1.cur_comm_step - Motor1.prev_comm_step; + Motor1.motor_status.cur_comm_step = MOTOR_COMMUTATION_STEPS[Motor1.motor_status.currentHallPattern]; + volatile int8_t step_change = Motor1.motor_status.cur_comm_step - Motor1.motor_status.prev_comm_step; switch(step_change) { case 1: case -5: - Motor1.Num_Steps = Motor1.Num_Steps+1; - Motor1.actualDirection = CW; + Motor1.motor_status.Num_Steps = Motor1.motor_status.Num_Steps+1; + Motor1.motor_status.actualDirection = CW; //Motor1.directionOffset = DIRECTION_CW_OFFSET; break; case -1: case 5: - Motor1.Num_Steps = Motor1.Num_Steps-1; - Motor1.actualDirection = CCW; + Motor1.motor_status.Num_Steps = Motor1.motor_status.Num_Steps-1; + Motor1.motor_status.actualDirection = CCW; //Motor1.directionOffset = DIRECTION_CCW_OFFSET; break; default: // do nothing break; } - Motor1.prev_comm_step = Motor1.cur_comm_step; + Motor1.motor_status.prev_comm_step = Motor1.motor_status.cur_comm_step; //calculate_motor_speed(); //toc(DEBUG_3); //toc(DEBUG_4); @@ -283,7 +346,7 @@ void calculate_motor_speed(void) hri_tccount32_read_CC_reg(TC0, 0); /* Read CC0 but throw away)*/ volatile uint32_t period_after_capture = hri_tccount32_read_CC_reg(TC0, 1); if((period_after_capture > UINT32_MAX)|| (period_after_capture > 10712046)) { - Motor1.calc_rpm = 0; + Motor1.motor_status.calc_rpm = 0; } else { //uint32_t test = (SPEEDFACTOR, period_after_capture); //temp_rpm = SPEEDFACTOR / period_after_capture; @@ -297,26 +360,26 @@ void calculate_motor_speed(void) if(count >= n_SAMPLE) { count = 1; - Motor1.calc_rpm = (speed_average >> 3); // divide by 32 + Motor1.motor_status.calc_rpm = (speed_average >> 3); // divide by 32 speed_average = 0; //*Spare_byte1 = motorState.actualDirection; - if(Motor1.actualDirection == CCW) /* Changed from CCW */ + if(Motor1.motor_status.actualDirection == CCW) /* Changed from CCW */ { //*motor_speed = -1* Motor1.calc_rpm; - Motor1.calc_rpm = -1* Motor1.calc_rpm; + Motor1.motor_status.calc_rpm = -1* Motor1.motor_status.calc_rpm; } else { //*motor_speed = Motor1.calc_rpm; - Motor1.calc_rpm = Motor1.calc_rpm; + Motor1.motor_status.calc_rpm = Motor1.motor_status.calc_rpm; } return; } else count++; #else - Motor1.calc_rpm = (int16_t)temp_rpm; + Motor1.motor_status.calc_rpm = (int16_t)temp_rpm; #endif } -void DisableGateDrivers(BLDCMotor_t *motor) +void DisableMotor(BLDCMotor_t *motor) { hri_tcc_write_PATTBUF_reg(motor->hw, motor->motor_commutation_Pattern[0]); @@ -338,16 +401,16 @@ void BLDC_runCurrentCntl(BLDCMotor_t *motor, volatile float curfbk, volatile flo PI_run_series(&motor->controllers.Pi_Idc); if(motor->controllers.Pi_Idc.Out_pu > 0) { - motor->desiredDirection = 0; - motor->directionOffset = 0; + motor->motor_setpoints.desiredDirection = 0; + motor->motor_setpoints.directionOffset = 0; } else { - motor->desiredDirection = 1; - motor->directionOffset = 8; + motor->motor_setpoints.desiredDirection = 1; + motor->motor_setpoints.directionOffset = 8; } volatile float duty_pu = f_abs((motor->controllers.Pi_Idc.Out_pu * motor->VoneByDcBus_pu)); - volatile duty_cycle = f_clamp(duty_pu * (float32_t)MAX_PWM, 0.0f, (float32_t)MAX_PWM); - motor->duty_cycle = (uint16_t)duty_cycle; + volatile uint32_t duty_cycle = f_clamp(duty_pu * (float32_t)MAX_PWM, 0.0f, (float32_t)MAX_PWM); + motor->motor_status.duty_cycle = (uint16_t)duty_cycle; // Remove Low duty cycle values //if(duty_cycle < 80.0) motor->duty_cycle = (uint16_t)0; //else motor->duty_cycle = (uint16_t)duty_cycle; @@ -371,16 +434,16 @@ void BLDC_runSpeedCntl(BLDCMotor_t *motor, volatile float speedfbk, volatile flo PID_run_parallel(&motor->controllers.Pid_Speed); if(motor->controllers.Pid_Speed.Out_pu > 0) { - motor->desiredDirection = 0; - motor->directionOffset = 0; + motor->motor_setpoints.desiredDirection = 0; + motor->motor_setpoints.directionOffset = 0; } else { - motor->desiredDirection = 1; - motor->directionOffset = 8; + motor->motor_setpoints.desiredDirection = 1; + motor->motor_setpoints.directionOffset = 8; } /* Process unit is Volts */ volatile float32_t duty_pu = f_abs((motor->controllers.Pid_Speed.Out_pu * motor->VoneByDcBus_pu)); volatile duty_cycle = f_clamp(duty_pu * (float32_t)MAX_PWM, 0.0f, (float32_t)MAX_PWM); - motor->duty_cycle = (uint16_t)duty_cycle; + motor->motor_status.duty_cycle = (uint16_t)duty_cycle; } else { /* Pu in Current (Amps) */ motor->controllers.Pid_Speed.OutMax_pu = (MOTOR_MAX_CURRENT_IDC_A); @@ -403,13 +466,3 @@ void BLDC_runPosCntl(BLDCMotor_t *motor, int16_t posfbk, int16_t posRef) PI_run_series(&motor->controllers.Pi_Pos); motor->controllers.Pid_Speed.Ref_pu = motor->controllers.Pi_Pos.Out_pu; } - -inline void tic(const uint8_t pin) -{ - gpio_set_pin_level(pin, true); -} - -inline void toc(const uint8_t pin) -{ - gpio_set_pin_level(pin, false); -} \ No newline at end of file diff --git a/BLDC_E54/BLDC_E54/bldc.h b/BLDC_E54/BLDC_E54/bldc.h index 3d91c2d..6f694fe 100644 --- a/BLDC_E54/BLDC_E54/bldc.h +++ b/BLDC_E54/BLDC_E54/bldc.h @@ -35,9 +35,9 @@ #define DEVICE_MCU_FREQUENCY_Hz (100000000U) #define DEVICE_SPEEDTC_DIV (4U) #define DEVICE_SPEEDTC_FREQUENCY_Hz (100000000U/DEVICE_SPEEDTC_DIV) -#define DEVICE_PWM_FREQUENCY_kHz (25.0f) -#define DEVICE_ISR_FREQUENCY_kHz DEVICE_PWM_FREQUENCY_kHz -#define DEVICE_ISR_PERIOD_Sec (0.001f/DEVICE_ISR_FREQUENCY_kHz) +#define DEVICE_PWM_FREQUENCY_kHz (25.0f) +#define DEVICE_ISR_FREQUENCY_kHz DEVICE_PWM_FREQUENCY_kHz +#define DEVICE_ISR_PERIOD_Sec (0.001f/DEVICE_ISR_FREQUENCY_kHz) // ---------------------------------------------------------------------- @@ -47,8 +47,9 @@ #define DEVICE_SHUNT_CURRENT_A 2.5f // phase current(PEAK) [A] #define CURRENT_SENSOR_SENSITIVITY 0.4f //V/A #define ONEON_CURRENT_SENSOR_SENSITIVITY 2.5f //V/A + // ---------------------------------------------------------------------- -// global variables +// Type Definitions // ---------------------------------------------------------------------- volatile typedef struct { @@ -74,31 +75,52 @@ volatile typedef struct timerflags volatile typedef struct { - volatile PI_t Pi_Idc; - volatile PID_t Pid_Speed; - volatile PI_t Pi_Pos; + volatile PI_t Pi_Idc; + volatile PID_t Pid_Speed; + volatile PI_t Pi_Pos; } MOTOR_Control_Structs; +volatile typedef struct +{ + volatile uint8_t desiredDirection; //! The desired direction of rotation. + volatile uint8_t directionOffset; + volatile float32_t desired_torque; + volatile int16_t desired_speed; + volatile int16_t desired_position; + volatile float32_t max_torque; + volatile float32_t max_current; + volatile int16_t max_velocity; +} MOTOR_Setpoints; + +volatile typedef struct +{ + volatile uint8_t actualDirection; //! The actual direction of rotation. + volatile uint16_t duty_cycle; + volatile float32_t calc_rpm; + volatile int32_t Num_Steps; + /* Hall States */ + volatile uint8_t prevHallPattern; + volatile uint8_t currentHallPattern; + volatile uint8_t nextHallPattern; + /* Commutation State */ + volatile uint8_t cur_comm_step; + volatile uint8_t prev_comm_step; +} MOTOR_Status; + +// ---------------------------------------------------------------------- +// Function Pointers +// ---------------------------------------------------------------------- +typedef uint8_t (*ReadHallFunc)(void); +typedef void (*DisableMotorFunc)(void); + + volatile typedef struct BLDCmotor { /* Hardware */ const Tcc *hw; - uint16_t *motor_commutation_Pattern; + const uint16_t *motor_commutation_Pattern; /* Status */ - volatile uint8_t actualDirection; //! The actual direction of rotation. - volatile uint8_t desiredDirection; //! The desired direction of rotation. - volatile uint16_t duty_cycle; - volatile float32_t calc_rpm; - volatile int32_t Num_Steps; // Total Step Count - /* Commutation State */ - volatile uint8_t cur_comm_step; - volatile uint8_t prev_comm_step; - /* Hall States */ - volatile uint8_t prevHallPattern; - volatile uint8_t currentHallPattern; - volatile uint8_t nextHallPattern; - volatile uint8_t dir_hall_code; //DIR_ABC - volatile uint8_t directionOffset; + MOTOR_Status motor_status; /* Measured Values */ volatile MOTOR_3PHASES_t Iphase_pu; volatile MOTOR_phase_offset_t Voffset_lsb; @@ -108,15 +130,12 @@ volatile typedef struct BLDCmotor volatile TIMERflags_t timerflags; volatile uint8_t regulation_loop_count; /* Controllers */ - volatile MOTOR_Control_Structs controllers; - /* Setpoints */ - volatile float32_t desired_torque; - volatile int16_t desired_speed; - volatile int16_t desired_position; - volatile float32_t max_current; - volatile float32_t max_torque; - volatile int16_t max_velocity; - + volatile MOTOR_Control_Structs controllers; + /* Setpoints */ + volatile MOTOR_Setpoints motor_setpoints; + /* Functions */ + ReadHallFunc ReadHall; + DisableMotorFunc DisableMotor; } BLDCMotor_t; @@ -124,21 +143,22 @@ volatile typedef struct BLDCmotor // global variables // ---------------------------------------------------------------------- -static const uint8_t HALL_PATTERN_ARRAY[16] = {0, 5, 3, 1, 6, 4, 2, 0, 0, 3, 6, 2, 5, 1, 4, 0 }; -static const uint8_t MOTOR_COMMUTATION_STEPS[8] = {20,1,3,2,5,6,4,20}; +static const uint8_t HALL_PATTERN_ARRAY[16] = {0, 5, 3, 1, 6, 4, 2, 0, 0, 3, 6, 2, 5, 1, 4, 0 }; +static const uint8_t MOTOR_COMMUTATION_STEPS[8] = {9, 1, 3, 2, 5, 6, 4, 9}; volatile BLDCMotor_t Motor1; - -// ---------------------------------------------------------------------- -// all controller objects, variables and helpers: -// ---------------------------------------------------------------------- - +volatile BLDCMotor_t Motor2; +volatile BLDCMotor_t Motor3; // ---------------------------------------------------------------------- // functions // ---------------------------------------------------------------------- +void BldcInitStruct(BLDCMotor_t *motor); +void BldcInitFunctions(void); + + + void select_active_phase(BLDCMotor_t *Motor, uint8_t hall_state); -void BLDC_init(BLDCMotor_t *motor); void read_zero_current_offset_value(void); int32_t adc_sync_read_channel(struct adc_async_descriptor *const descr, const uint8_t channel, uint8_t *const buffer, const uint16_t length); static uint16_t adc_read(struct adc_async_descriptor *const descr, const uint8_t channel); @@ -149,9 +169,28 @@ uint8_t get_hall_state(void); uint8_t HALLPatternGet(void); uint8_t PDEC_HALLPatternGet(void); void calculate_motor_speed(void); -void DisableGateDrivers(BLDCMotor_t *motor); +void DisableMotor(BLDCMotor_t *motor); void BLDC_runSpeedCntl(BLDCMotor_t *motor, volatile float speedfbk, volatile float speedRef); void BLDC_runCurrentCntl(BLDCMotor_t *motor, volatile float curfbk, volatile float curRef); void BLDC_runPosCntl(BLDCMotor_t *motor, int16_t posfbk, int16_t posRef); +// ---------------------------------------------------------------------- +// Functions used with function pointers +// ---------------------------------------------------------------------- +uint8_t readM1Hall(void); +uint8_t readM2Hall(void); +uint8_t readM3Hall(void); +void DisableM1GateDrivers(BLDCMotor_t *motor); +void DisableM2GateDrivers(BLDCMotor_t *motor); +void DisableM3GateDrivers(BLDCMotor_t *motor); + +// ---------------------------------------------------------------------- +// all controller objects, variables and helpers: +// ---------------------------------------------------------------------- + +//Motor2.ReadHall = readM2Hall; +//Motor3.ReadHall = readM3Hall; + + + #endif /* BLDC_H_ */ \ No newline at end of file diff --git a/BLDC_E54/BLDC_E54/control.h b/BLDC_E54/BLDC_E54/control.h index da91def..7701f8c 100644 --- a/BLDC_E54/BLDC_E54/control.h +++ b/BLDC_E54/BLDC_E54/control.h @@ -126,7 +126,7 @@ inline void PID_objectInit(volatile PID_t *pPiD_obj) static inline void PI_run_series(volatile PI_t *pPi_obj) { PI_t *obj = (PI_t *)pPi_obj; - volatile float32_t Up, preOut; + volatile float32_t Up; // Compute the controller error obj->error = obj->Ref_pu - obj->Fbk_pu; @@ -144,7 +144,7 @@ static inline void PI_run_series(volatile PI_t *pPi_obj) static inline void PI_run_parallel(volatile PI_t *pPi_obj) { PI_t *obj = (PI_t *)pPi_obj; - volatile float32_t Up, preOut; + volatile float32_t Up; // Compute the controller error obj->error = obj->Ref_pu - obj->Fbk_pu; @@ -165,7 +165,7 @@ static inline void PI_run_parallel(volatile PI_t *pPi_obj) static inline void PID_run_series(volatile PID_t *pPid_obj) { PID_t *obj = (PID_t *)pPid_obj; - volatile float32_t Up, Ud_tmp, preOut; + volatile float32_t Up, Ud_tmp; // Compute the controller error obj->error = obj->Ref_pu - obj->Fbk_pu; @@ -189,7 +189,7 @@ static inline void PID_run_series(volatile PID_t *pPid_obj) static inline void PID_run_parallel(volatile PID_t *pPid_obj) { PID_t *obj = (PID_t *)pPid_obj; - volatile float32_t Up, Ud_tmp, preOut; + volatile float32_t Up, Ud_tmp; // Compute the controller error diff --git a/BLDC_E54/BLDC_E54/ethercat/ethercat_e54.c b/BLDC_E54/BLDC_E54/ethercat/ethercat_e54.c index f6fc7c2..6821d9b 100644 --- a/BLDC_E54/BLDC_E54/ethercat/ethercat_e54.c +++ b/BLDC_E54/BLDC_E54/ethercat/ethercat_e54.c @@ -22,27 +22,6 @@ #define DEBUG_3_PORT PORT_PC02 #define DEBUG_4_PORT PORT_PC30 -inline void tic(const uint8_t pin) -{ - gpio_set_pin_level(pin, true); -} - -inline void toc(const uint8_t pin) -{ - gpio_set_pin_level(pin, false); -} - -inline void tic_port(const uint32_t port) -{ - REG_PORT_OUTSET2 = port; -} - -inline void toc_port(const uint32_t port) -{ - REG_PORT_OUTCLR2 = port; -} - - void update_telemetry(void) @@ -54,17 +33,17 @@ void update_telemetry(void) *state = applicationStatus.currentstate; *status = applicationStatus.fault; - *motor_rel_position = Motor1.Num_Steps; + *motor_rel_position = Motor1.motor_status.Num_Steps; *motor_abs_position = 0; - *motor_dutyCycle = Motor1.duty_cycle; - *motor_speed = (int16_t)Motor1.calc_rpm; + *motor_dutyCycle = Motor1.motor_status.duty_cycle; + *motor_speed = (int16_t)Motor1.motor_status.calc_rpm; *motor_torque = 0; *motor_currentPHA = convert_to_mA(Motor1.Iphase_pu.A); *motor_currentPHB = convert_to_mA(Motor1.Iphase_pu.B); *motor_currentPHC = convert_to_mA(Motor1.Iphase_pu.C); *motor_currentBUS = convert_to_mA(Motor1.Iphase_pu.Bus); - *hall_state = Motor1.currentHallPattern; - *Spare_byte1 = Motor1.actualDirection; + *hall_state = Motor1.motor_status.currentHallPattern; + *Spare_byte1 = Motor1.motor_status.actualDirection; //*Spare_1 = 0; //*Spare_2 = 0; } @@ -75,27 +54,27 @@ void update_setpoints(void) { return ((float32_t)input/1000.0); } - //Motor1.des_mode = 0; - //Motor1.set = 0; - Motor1.desired_position = *desired_position; - Motor1.desired_speed = *desired_speed; - //Motor1.desired_speed = 1500; - Motor1.desired_torque = convert_int_to_PU(*desired_torque); - //Motor1.controllerParam.I_kp = 0; - //Motor1.controllerParam.I_ki = 0; - //Motor1.controllerParam.V_kp = 0; - //Motor1.controllerParam.V_kd = 0; - //Motor1.controllerParam.V_kd = 0; - //Motor1.controllerParam.P_kp = 0; - //Motor1.controllerParam.P_ki = 0; - //Motor1.reductionRatio = 0; - Motor1.max_velocity = *max_velocity; - Motor1.max_current = convert_int_to_PU(*max_current); - Motor1.max_torque = convert_int_to_PU(*max_torque); - //Motor1.Spare1 = 0; - //Motor1.Spare2 = 0; - //Motor1.Spare3 = 0; - //Motor1.Spare4 = 0; + //Motor1.des_mode = 0; + //Motor1.set = 0; + Motor1.motor_setpoints.desired_position = *desired_position; + Motor1.motor_setpoints.desired_speed = *desired_speed; + //Motor1.desired_speed = 1500; + Motor1.motor_setpoints.desired_torque = convert_int_to_PU(*desired_torque); + //Motor1.controllerParam.I_kp = 0; + //Motor1.controllerParam.I_ki = 0; + //Motor1.controllerParam.V_kp = 0; + //Motor1.controllerParam.V_kd = 0; + //Motor1.controllerParam.V_kd = 0; + //Motor1.controllerParam.P_kp = 0; + //Motor1.controllerParam.P_ki = 0; + //Motor1.reductionRatio = 0; + Motor1.motor_setpoints.max_velocity = *max_velocity; + Motor1.motor_setpoints.max_current = convert_int_to_PU(*max_current); + Motor1.motor_setpoints.max_torque = convert_int_to_PU(*max_torque); + //Motor1.Spare1 = 0; + //Motor1.Spare2 = 0; + //Motor1.Spare3 = 0; + //Motor1.Spare4 = 0; } diff --git a/BLDC_E54/BLDC_E54/interrupt_handlers.h b/BLDC_E54/BLDC_E54/interrupt_handlers.h index bda5cef..42f634c 100644 --- a/BLDC_E54/BLDC_E54/interrupt_handlers.h +++ b/BLDC_E54/BLDC_E54/interrupt_handlers.h @@ -48,7 +48,7 @@ void TC0_Handler(void) /* Overflow Interrupt */ if (TC0->COUNT32.INTFLAG.bit.OVF == 0x01) { TC0->COUNT32.INTFLAG.bit.OVF = 0x01; - Motor1.calc_rpm = 0; + Motor1.motor_status.calc_rpm = 0; //gpio_toggle_pin_level(DEBUG_2); } diff --git a/BLDC_E54/BLDC_E54/main.c b/BLDC_E54/BLDC_E54/main.c index 9499418..4d614dc 100644 --- a/BLDC_E54/BLDC_E54/main.c +++ b/BLDC_E54/BLDC_E54/main.c @@ -19,32 +19,6 @@ // ---------------------------------------------------------------------- //#define DBG_PIN1 GPIO(GPIO_PORTD, 0) -#define DEBUG_1_PORT PORT_PC01 -#define DEBUG_2_PORT PORT_PC03 -#define DEBUG_3_PORT PORT_PC02 -#define DEBUG_4_PORT PORT_PC30 - -inline void tic(const uint8_t pin) -{ - gpio_set_pin_level(pin, true); -} - -inline void toc(const uint8_t pin) -{ - gpio_set_pin_level(pin, false); -} - -inline void tic_port(const uint32_t port) -{ - REG_PORT_OUTSET2 = port; -} - -inline void toc_port(const uint32_t port) -{ - REG_PORT_OUTCLR2 = port; -} - - @@ -76,7 +50,8 @@ inline void CONTROLLER_StateMachine(void) break; case MOTOR_IDLE: applicationStatus.previousstate = applicationStatus.currentstate; - applicationStatus.currentstate = MOTOR_PVI_CTRL_STATE; + //applicationStatus.currentstate = MOTOR_PVI_CTRL_STATE; + applicationStatus.currentstate = MOTOR_V_CTRL_STATE; break; case MOTOR_I_CTRL_STATE: @@ -85,12 +60,14 @@ inline void CONTROLLER_StateMachine(void) case MOTOR_V_CTRL_STATE: switch (Motor1.regulation_loop_count) { case 0: /* PWM FREQ / 25 - 1kHz */ + Motor1.timerflags.motor_telemetry_flag = true; /* Blank */ case 6: /* PWM FREQ / 6.25 - 4kHz */ calculate_motor_speed(); - BLDC_runSpeedCntl(&Motor1, Motor1.calc_rpm, Motor1.desired_speed); + //BLDC_runSpeedCntl(&Motor1, Motor1.motor_status.calc_rpm, Motor1.motor_setpoints.desired_speed); + BLDC_runSpeedCntl(&Motor1, Motor1.motor_status.calc_rpm, 2000); default: /* PWM FREQ - 25kHz */ - select_active_phase(&Motor1, Motor1.currentHallPattern); /* Still measure current */ + select_active_phase(&Motor1, Motor1.motor_status.currentHallPattern); /* Still measure current */ break; } // end switch(regulation_loop_count) @@ -104,12 +81,13 @@ inline void CONTROLLER_StateMachine(void) case MOTOR_VI_CTRL_STATE: switch (Motor1.regulation_loop_count) { case 0: /* PWM FREQ / 25 - 1kHz */ + Motor1.timerflags.motor_telemetry_flag = true; /* Blank */ case 6: /* PWM FREQ / 6.25 - 4kHz */ calculate_motor_speed(); - BLDC_runSpeedCntl(&Motor1, Motor1.calc_rpm, Motor1.desired_speed); + BLDC_runSpeedCntl(&Motor1, Motor1.motor_status.calc_rpm, Motor1.motor_setpoints.desired_speed); default: /* PWM FREQ - 25kHz */ - select_active_phase(&Motor1, Motor1.currentHallPattern); + select_active_phase(&Motor1, Motor1.motor_status.currentHallPattern); BLDC_runCurrentCntl(&Motor1, Motor1.Iphase_pu.Bus, Motor1.controllers.Pi_Idc.Ref_pu); break; } // end switch(regulation_loop_count) @@ -123,7 +101,7 @@ inline void CONTROLLER_StateMachine(void) case 0: /* PWM FREQ / 25 - 1kHz */ Motor1.timerflags.motor_telemetry_flag = true; // Update telemetry flag //tic(DEBUG_4); - BLDC_runPosCntl(&Motor1, Motor1.Num_Steps, Motor1.desired_position); + BLDC_runPosCntl(&Motor1, Motor1.motor_status.Num_Steps, Motor1.motor_setpoints.desired_position); //toc(DEBUG_4); case 5: case 10: case 15: case 20:/* PWM FREQ / 5 - 5kHz */ //tic(DEBUG_3); @@ -132,14 +110,14 @@ inline void CONTROLLER_StateMachine(void) calculate_motor_speed(); //toc(DEBUG_2); //tic(DEBUG_3); - BLDC_runSpeedCntl(&Motor1, Motor1.calc_rpm, Motor1.controllers.Pid_Speed.Ref_pu); + BLDC_runSpeedCntl(&Motor1, Motor1.motor_status.calc_rpm, Motor1.controllers.Pid_Speed.Ref_pu); //toc(DEBUG_3); //toc(DEBUG_4); //toc(DEBUG_3); default: /* PWM FREQ - 25kHz */ //tic(DEBUG_2); //tic(DEBUG_3); - select_active_phase(&Motor1, Motor1.currentHallPattern); + select_active_phase(&Motor1, Motor1.motor_status.currentHallPattern); ////toc(DEBUG_2); ////tic(DEBUG_3); BLDC_runCurrentCntl(&Motor1, Motor1.Iphase_pu.Bus, Motor1.controllers.Pi_Idc.Ref_pu); @@ -203,7 +181,11 @@ int main(void) atmel_start_init(); - BLDC_init(&Motor1); + BldcInitStruct(&Motor1); + BldcInitStruct(&Motor2); + BldcInitStruct(&Motor3); + BldcInitFunctions(); + read_zero_current_offset_value(); configure_tcc_pwm(); config_pins(); @@ -212,14 +194,9 @@ int main(void) ethercat_update(); custom_logic_enable(); configure_TC_CCL_SPEED(); - //ext_irq_register(PIN_PB05, button_on_PB05_pressed); - ext_irq_register(nDRV_RESET, HW_current_limit_detect_callback); - enable_NVIC_IRQ(); // Enable TC_ECAT TC7 Timer - //__enable_irq(); - //gpio_set_pin_level(DRV_RESET, true); - //gpio_set_pin_level(DRV_RST, false); - + ext_irq_register(nDRV_RESET, HW_current_limit_detect_callback); + enable_NVIC_IRQ(); /* Replace with your application code */ while (1) { diff --git a/BLDC_E54/BLDC_E54/motor_params.h b/BLDC_E54/BLDC_E54/motor_params.h index 6855cc7..810e79c 100644 --- a/BLDC_E54/BLDC_E54/motor_params.h +++ b/BLDC_E54/BLDC_E54/motor_params.h @@ -64,63 +64,68 @@ static uint32_t speed_average = 0; static uint8_t count = 1; +// ---------------------------------------------------------------------- +// Commutation Patterns +// ---------------------------------------------------------------------- +/** +** ____|___TCC0_________ ||____TCC1_________| +** W0 | M2_0 | CC0 || __M3_2__ | CC0 | +** W1 | M2_1 | CC1 || - | CC1 | +** W2 | M2_2 | CC2 || M1_0 | CC1 | +** W3 | M2_3 | CC3 || M1_1 | CC1 | +** W4 | __M3_0__ | CC4 || M1_2 | CC1 | +** W5 | __M3_1__ | CC5 || M1_3 | CC1 | +** W6 | M2_4 | CC0 || M1_4 | CC1 | +** W7 | M2_5 | CC1 || M1_5 | CC1 | +*/ static const uint16_t COMMUTATION_PATTERN_M1[] = { - 0x00FC, // invalid state (0) - 0x30F8, // CB_CW (1) - 0x50F4, // CA_CW (2) - 0x60F8, // BA_CW (3) - 0x607C, // AB_CW (4) - 0x507C, // BC_CW (5) - 0x30F4, // AC_CW (6) - 0x00FC, // invalid state - 0x00FC, // invalid state - 0x30F4, // BC_CCW (9) - 0x507C, // AB_CCW (10) - 0x607C, // AC_CCW (11) - 0x60F8, // CA_CCW (12) - 0x50F4, // BA_CCW (13) - 0x30F8, // CB_CCW (14) - 0x00FC + 0x00FF, // (0) invalid state + 0x30F8, // (1) + 0x50F5, // (2) + 0x60F8, // (3) + 0x607D, // (4) + 0x507D, // (5) + 0x30F5, // (6) + 0x00FF, // (7) invalid state + 0x00FF, // (8) invalid state + 0x30F5, // (9) + 0x507D, // (10) + 0x607D, // (11) + 0x60F8, // (12) + 0x50F5, // (13) + 0x30F8, // (14) + 0x00FF // (15) invalid state }; static const uint16_t COMMUTATION_PATTERN_M2[] = { - 0x00FC, // invalid state (0) - 0x30F8, // CB_CW (1) - 0x50F4, // CA_CW (2) - 0x60F8, // BA_CW (3) - 0x607C, // AB_CW (4) - 0x507C, // BC_CW (5) - 0x30F4, // AC_CW (6) - 0x00FC, // invalid state - 0x00FC, // invalid state - 0x30F4, // BC_CCW (9) - 0x507C, // AB_CCW (10) - 0x607C, // AC_CCW (11) - 0x60F8, // CA_CCW (12) - 0x50F4, // BA_CCW (13) - 0x30F8, // CB_CCW (14) - 0x00FC -}; + 0x00FF, // (0) invalid state + 0xC0FB, // (1) + 0x48DD, // (2) + 0x88FB, // (3) + 0x88EE, // (4) + 0x48EE, // (5) + 0xC0DD, // (6) + 0x00FF, // (7) invalid state + 0x00FF, // (8) invalid state + 0xC0DD, // (9) + 0x48EE, // (10) + 0x88EE, // (11) + 0x88FB, // (12) + 0x48DD, // (13) + 0xC0FB, // (14) + 0x00FF // (15) invalid state +}; + +static const uint16_t m3_TCC0_mask = 0x0030; +static const uint16_t m3_TCC0_inv_mask = ~(0x0030); +static const uint16_t m3_TCC1_mask = 0x0001; +static const uint16_t m3_TCC1_inv_mask = ~(0x0001); + + + + -static const uint16_t COMMUTATION_PATTERN_M3[] = { - 0x00FC, // invalid state (0) - 0x30F8, // CB_CW (1) - 0x50F4, // CA_CW (2) - 0x60F8, // BA_CW (3) - 0x607C, // AB_CW (4) - 0x507C, // BC_CW (5) - 0x30F4, // AC_CW (6) - 0x00FC, // invalid state - 0x00FC, // invalid state - 0x30F4, // BC_CCW (9) - 0x507C, // AB_CCW (10) - 0x607C, // AC_CCW (11) - 0x60F8, // CA_CCW (12) - 0x50F4, // BA_CCW (13) - 0x30F8, // CB_CCW (14) - 0x00FC -}; #endif /* MOTOR_PARAMS_H_ */ \ No newline at end of file diff --git a/BLDC_E54/BLDC_E54/utilities.h b/BLDC_E54/BLDC_E54/utilities.h index a58fbf9..1b9be19 100644 --- a/BLDC_E54/BLDC_E54/utilities.h +++ b/BLDC_E54/BLDC_E54/utilities.h @@ -9,6 +9,37 @@ #ifndef UTILITIES_H_ #define UTILITIES_H_ + +#define ENABLED(V...) DO(ENA,&&,V) +#define DISABLED(V...) DO(DIS,&&,V) + + +#define DEBUG_1_PORT PORT_PC01 +#define DEBUG_2_PORT PORT_PC03 +#define DEBUG_3_PORT PORT_PC02 +#define DEBUG_4_PORT PORT_PC30 + +inline void tic(const uint8_t pin) +{ + gpio_set_pin_level(pin, true); +} + +inline void toc(const uint8_t pin) +{ + gpio_set_pin_level(pin, false); +} + +inline void tic_port(const uint32_t port) +{ + REG_PORT_OUTSET2 = port; +} + +inline void toc_port(const uint32_t port) +{ + REG_PORT_OUTCLR2 = port; +} + + //! \brief Saturates the input value between the minimum and maximum values //! \param[in] in The input value //! \param[in] max The maximum value allowed