From b6246f448fd69cc63785d4f76707211db005f6d5 Mon Sep 17 00:00:00 2001 From: "cagdas-aras.ciblak" Date: Fri, 10 Mar 2023 11:55:50 +0100 Subject: [PATCH] final --- main.cpp | 50 +++++++++++++++++++++++++++++++++++++++++++++++++- main.o | Bin 58608 -> 60504 bytes 2 files changed, 49 insertions(+), 1 deletion(-) diff --git a/main.cpp b/main.cpp index e3efb2c..c12f906 100644 --- a/main.cpp +++ b/main.cpp @@ -27,8 +27,51 @@ l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition); } +float convertJointCmdToAngle(int jointCmd) { + const float jointCmdRange = 65535.0f; // range of the joint command (0 to 65535) + const float angleRange = 360.0f; // range of the joint angle (-180 to 180) + float jointCmdMapped = (jointCmd / jointCmdRange) * angleRange; // map the joint command value to the angle range + if (jointCmdMapped > 180.0f) { + jointCmdMapped -= 360.0f; // ensure that the angle is between -180 and 180 + } + return jointCmdMapped; +} + + +void gripperControl(float targetAngle) +{ + const int jointIndex = 5; // index of the 6th motor (starts from 0) + const float kP = 0.1f; // proportional gain + + // Read current joint torque + std::vector l_vCurrentJointTorque; + _oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque); + + // Calculate error and proportional control + float error = targetAngle - convertJointCmdToAngle(l_vCurrentJointTorque[jointIndex]); + float jointCmd = convertAnglesToJointCmd(convertJointCmdToAngle(l_vCurrentJointTorque[jointIndex]) + kP * error); + + // Set the target joint position for the 6th motor + std::vector l_vTargetJointPosition; + for (int l_joint = 0; l_joint < _nbJoints; l_joint++) { + if (l_joint == jointIndex) { + l_vTargetJointPosition.push_back(jointCmd); + } + else { + l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); + } + } + _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition); +} + + + + + int main() -{ std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl; +{ + + std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl; _oDxlHandler.setDeviceName(_poppyDxlPortName); _oDxlHandler.setProtocolVersion(_poppyDxlProtocol); _oDxlHandler.openPort(); @@ -45,6 +88,11 @@ std::cout << l_vCurrentJointPosition[l_joint] << ", "; std::cout << ")" << std::endl; // wait 1s std::this_thread::sleep_for(std::chrono::milliseconds(1000)); +goToHomePosition(); +std::this_thread::sleep_for(std::chrono::milliseconds(2000)); +gripperControl(l_vCurrentJointPosition[5]); +std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + std::cout << "===Closing the Dynamixel Motor communication====" << std::endl; _oDxlHandler.enableTorque(false); _oDxlHandler.closePort(); diff --git a/main.o b/main.o index c12800143e0e9c0cf22e1face8c76697268d3bf5..30472a83291a173ff3f79114151a9ced657416dc 100644 GIT binary patch delta 10886 zcmZ`<3v^V~)t-Cv9%e`aNiG9PCYgi~VKjjvU<(s4L?+XKQlSJLDp5eNyhR9)FbM%k zh6EEgt6Gf}Tq;z#qL3KrCA)KH+Hg#s-us6~tA|IV3vFI(z=)|&I3`+a-w zv(M|^%MDlFkLc)#XvkGNHn;S>5gDkhxO6o>A~sOp#Z&T>Q*t(!s0o>EDg_)_(b*xB61KaIOnX@ve#Nos0P1 z+M@IQ+l~~i7lCedl34Vrx4vkbxAw?+kwmzC*jqnLT$$8<;bhpv@3h44Y||YOX>{Gy zcz=6*u1sZLpI zS6$H*^;G)Rcz03rwA!KY{#ru=+TrT+O#h-||_SPQc$zF`PMe3fkD8?Qap2q4!<*mJ z3y&Q=I*d-o?RF>JAN60=a#I<8LA5*=fvLNBQ|a0FPK#w~)b1$DYNfu}Ql@?9awse|cbGL4^{jz(jlCf0@q#~UuikN?ksdBO+cbc>-{BLEc9GPvU*LSEg9)* zl1k~WbakLg9>7p!>~blSsmhh5HmS6wU%IlFzH~X&PL=xh>q)0wk;)B8q()Fq7Ca*8 zK7c<0(VR*It$4)2CVDL^OKGF`vz*Flx{&2mEsGr>}cE)!1nIM|OHe2P^b|Kbpy5J z^ihvTQz$E4xkBIOIMsw0>e(k<&5xm<0v?FLID1BAzpwk5f&GrMr4#USr9V>rzUoEZ3=&Qx)pxX#?tAbO3b+ok4w_lslYik2v}rdKeNX zXB{4QNDtKTKn)Mn@VFyOy%8reR8y_ORvl`U`TA*UU#GH+R`qqNO;*f+limZ-NnfJA zK~ep()Qm1z#dLLe7xMLUsZ+XOxzg2@UBqlDuhQmzPW4b1nvFHU?Dumjm&oq!qq=Ra zgWONV<(2BwnXK=m&6ot$GSMMfV#8%(~N-Wo6YMw7ow zZ!M($_i2tzQZ;T(7R&E+^;0UlPb(IWWtXMg^mXg4MaB+*Ut#jbx;!iSN|R65TW(CI%d*1c4U0D=ztZFl`!NZ^{x|oU!ee^t zD=9o}^1szvWr@P!d6S={w_2!Wl%~8RYR6G&^MYw(EbtXNGfFG|UaEDbYHW*qtcuH0 zZ}NuMILW_g@`n9t8sXI(FH7}hQ^ln$AJIIoR-D*f#C*k6AJAKF$!|3IzpLTq(~|$+ z(S1TE(AV4~5$%7+wT_fx&2dPoo6VRACCgRnU#u11g_UzzwwbE2z%wQPs>u)1Tk9nM zy2%@}@dY&^T4aid)@rIHdMi`%J50V%Z!MSnPLm(3w|-Aw7i<0hCe_`hI#F+>Xrh4UduBZ2z$Q9Glp`iTNN+tN`S(rUn5S219-{qIs>e*#STSc$5$(9i8++QrIGC^I#8nBJsR%_F3X=y)rdb+@~2F`zur0sUOp0(KT_ss zt@t}>chJ6!0GS!Fl z)-Nb=jOKU(sVC^?)HFe3kx&&ps8Bb_`fXVsm-R{1JtFZa5rax<-92VcblxKm4H)p) zoY@Pf&R^i0Gh_CG!dX+s%^5NK@tIR+SRQ$(|G>xR&zL)R>iohvvlq;tGjkfvnV@L{ z=1(n~Qux&T`BP_yoyN_XKkuoj^zsCahK)~*oKapht9AbPN7dHL6N1UD<+Cc4K(YR6 zAd-&8?~tDMFK^@9b$YWTl|vFN5Wn# z&W)Sl@*QoMqXlj<^s4 z%ABVKYIIdVCsVgFFR{mrLGEEZk9LCb-2O|Q7a-295l4T9^p*e6`3DoeVS)b1c$KOa zXjJ~Bjp`QIl%e9vF)9KEC}Qre;AaFcp_T<&V4<#xfMP@0@Drti@z-%}c&%X3zGZx! z&Wl_2cZ`3I+5d;}uY~#VN_@tIH<-f>#_yuWC$-{zO#Pmz?=$|tjQ^9)i{XhopD~Qj zMKs4L=5>>)UkSDUMW$M?VGZA}Sw;=at_R5D$ng{ z#z(T=Ut>H!vfFk3_Ob6~_E9WOTbK`+M|rXwgvHX%9C*5aW*vxOKc?GvGM*3n%>NVT zqRxkRI{j{;HSi5H<#Q1J3>RMQXqJ1b{JIq8V;G;w_`f4PW5@lB@jT9`FdvZHi3ciW z4t$ayXZ$tRK^5b92lb5S@z?2m_^uJS^;PCYCh*CW9|6&!m?*v|Z|L^7-!^uzj`@rp zVDUSQ0R-@9Im{gR!8-P14zT}_*~hXa_)O<-Pv|U*!v}UAJjNd$8PD`v)&ajoJH_79 z?fIRmFXQ=bdKlw*9PceY@PqsM=v#v2Yjk96qIEhi)=VrzC4Qp(mf6o>GqIl8Kf(Bo zj1R|=)qPp9fr&w8*R2Y0@H1?W)j!7p5rjRKG}5y3y#B z8tq-wLrJDPD>WsBE`v>^(d8Q5SZt%*avPPD+w56LPZTi?y`bhPX;gvQNt??x)b*7b zb(Z%a_YxajUt$AeU#!`?u!&hhC(5&^Zi&r4SVjuRa?-5Dnxi+foA?9urNtV0+Oar~ zn#*mQjB$}&7Q|>XVbi;8kIun#BVxA)n9rz>2kG3%}euX=rS9fUn)lXucdiZQe{)J zsH#Gv4$K@P+6S?W2FreKz@uSVHn}To)Um=wN0;R*KcnxF!QKj+eF?K4Mt4?db`Q(v z_cEX96*dRY@>Aw>57k#_4)O8QxKoHHhA3j)3TPkH;Y`g$TNI%t(`BdyOudh(!zpd~ z47{_C6w!{rDDqcgt+1kXz?zkEWobxdHZ@oEaPZfm zM`T{%%zWZR{6OVv8!6V7Y%6T`jm#&KF5?t6V;B9AHdJYpTa`^>zm!&GD+N?h^}{(7 zHw;n4Je1HRcn-yWIu4(v6*grcHLk$F9v0i|_p=dZ(E+gHe8!1AjFUBMMV>lVp$`_P z&<(idQYN-GHYPqgr{NahueMQabvD%jhg8~>36zE-u@_geLyWb?`_+O#S`ofXr%XO%~} zqXzb(h+QFmTlMQk#*42hT`6;Q_uj(7_pmL4JmiU8xg zh`1iY_!L3trIhBa)*NL_71<2e)n&8+>Z;q+k-EB`PC#vAYENAeaGI1Ft=JL~ri2kb z^Oz-r@%SWTmf?)Y#~8Crrm`B%QOVQ{Q?ppsQzO(vOm#8!934O=kz$Xdh)l9&D_mtW zzBl8&jL%{GbjqyN95qbMW$Gpx3)T21hv>^VV#4S=Rn=<6vFz2+&3tnie<$O;j5oeU z3V9}-L0@%D&1YWClo-T!a;6So>LnTx)QWBFMP?vV-HbQBp9#5y@y4kX{7l*q)a-Rk z9YXdsc$f!mjvb5@d&?^vP(CJQjaGb(sXq$~vQE-pM8pgZW&A+K4`ci|DqEvD7BIDd zsn1g*)Ll#+!PE{qfK0A4wUDVjV#60AlaY)c!uTS_`zf;_`Ok^Q`#$S39887zuT>M1I zXS}e-Q=#WG7gU<~LLv=*wkt(En?v6>WKx4aj>bG3*ShrC%oHWA^`%XJP}FZM^s6^K zz9k87>L<4pP%5H&x55_@Yqln-SqlB-^}f`; zbseR)fZ7HJU^_7NEp*zFq~swT1a9e0elU;hM@lVt#YJcBEwI`StGANW{^3C0x00w6 zW;JgmDR_d*Byf%)%6($RMklxcMGodN}K6MGBrHZkc9H`VSPr9K@) zAMPDw9WqbuD$5lcb=x;cor(XQu+O8e??S;ag!XyjcIlfJuL=7Hs|_~t?f0m!+b9S) zVxth?6B}9o;8AmzlMnC>G{ErX6au8!$=c>oOQE%S;)dA69ad9g+hBFCokGw)gvkMq zdd*Hgz})2&1Y~rj5Mcdsvc8L8UC9U71r5*u4R9XXL66!24Uiw8;6YDZ*Z6P`Su}RP zQ~8$qAJF0k=v)pZ9vZBc#*^=mM_n3ELBQkyg#a7l$@-p0Ef0_na2*<;X99%)c?o1a z>`~jG0bWa>;9(EdoWQ&vcB=;=9znX52FOn)>oJd7kW4hsm)1N=0Ff`E2tfVnATJ&y6EkPmPjF#%6FCITWmP$T=FO`CT*jfq!UP&eECm3ce`2a_u0j5I(M{UobFefQB^`1bhz-unF2Jk2*S&e1I#^08eI82#`@n*3;OE&;b9= zq#z*9MIpfR(9U>NkBfYOaV`o1w$)KcfQzhu#V)8LA7B?Wz-efJHfU!(>LnNcbeTev z&*rLEpr7@`C1iyUrk75f9UOPPE_~HMiRT8#mF9)7w$a#ggRQZ@2w(Mc(zE9VsWrc$ I(7Bxd1LWmhNB{r; delta 9768 zcmZ`<33yahmacoLguJ9m5>oI|7%E!oJ0iAo?tS;YIl%P&zPjhU|3BNi z%Ui0dv9UwT{tlJNyx^VclsGva|vDGZP!~d-BS8E z98ZSqb2^r?T!-TawuOJ^bTm6_UU0v|U`VF`By{q@8F$B;X`LR3h7D=Xn%#*XFt^{~ zaGXkgH@hmT)na4@MGOzz?2KRMv39*Hal zmi0(t<6%>eI5rD@z#tx9#E0JIC01x)^@mMD&_3=SYIGOC@4?AHc2AL1Xdd2bePI$jh9CN~z zR3C2=2i+hq*~zav;lboY77ufi=S41UCMF9?GfeSEAEE~8HsQoVirP?(6xTf*S|9zGTcQv-1<9hL>+c%CNC zixt3rgwQ9Dgwx^fgVTWo+WsUCmGmz49E1{g;pbbEqDrv%#_-i+vjVPU`Uxb+68HWj|f^t7Ts+ z`ix(2;U+p>FvDTrQ)w}pz-QZdlF~BZ&3kq4-IASi3p)!~w$AoRn=8_0s%_InX+D^O zHseKKygXcFdCs;;Ra&jYm)m%p()LQc!p1X|)-LfCHr`2Tu@NHvbQ`zg^p^PZ@Q>^$ zZ;sSg+BSD8tq5AP^_;bmooTb#N~@N5rHyx1+69TPvhlu33xS0>x_6*U)U(EB|3hg{ zL3NIv^C!s`*=#^*trA~g6SMG!SwXVs!Fz9Ma2vTdwcNVrY-me{yY zY1<`UYU7s0T{xtPjxrm!>`zJjH5-3iX)iL*&3Y)bS!7hp4x(Loo{ExTTcZF4$t1r%6qT^3CYfa?uVK?U5 zAld)4**v9fm-xFjenoA_F3}>|t!MWnB;@Me-z0nMc_l$UvcK~P-%WN*G?uLt7Ut?X zJ0)9Xvk6N3MdEMS_yDCP;Y=qw-m!6O44;K&j0TdevDsXuRZ09q8y~JTcUKW@i;WLd zS{4i&qxYXJ*{wD^QE6{We20xsP}*h0BVkoqwD%Wj@v&`Tt(U%Vc#O^liQb#5i`;d# zjkUxYC3{A)w~kA9$QY|L0~q!X!<4alPGO9Qv)7JeZOsi5|IEe*DD6jy@3(PljP8Kl z7;S-9L_1)!)@;2m@k2Ilt*3JmKWO9Dg)tlw?$^Cp-9)q_Hv6>Fo`U@Q^_<0${n}>x zEA11BH`=(hDO)9e(#F%2))A`j*V!G=jD4=?z2j~dd7ErYtJ)b5m8bV_lI$^?wc`IG z@iR7_p|o*$2bc58Cc(@+J!csnCpymCcGk`1Yl(kr<1>`Ty9@h^Hjax?wj?;5r+bG; z_Oi`dqwp-Se7*b2lD%rPla$tsxVhGT7Mj|pwizm+kdd!5pX^g*KT`IO$$pybH;KL` zY~1$HnlTfnaL9Tx(mAzgV?OOeXkGh+Y59Y)`_f zf7gqjSFBk0e<$obg*y@Z2f-JUXeS81g4nkc{Ex(5EEFsI1`{uKhy*_qVj~H3jo?8N zNUSU?&`yHW1baxFUx@ww8|;h!FVI2aaGf|DB@W*Z{1m}y;&UXoYG zTEN{T&fn0^N;Fd8H=jtJiJNPHF0!d4kON1-3RF&TM({r=EBe-j6K2DLXhI2_h3U>Ct@;`)CP=b*xG7zwckP7nt=5`QIl2XY3#5!^}e&<^H_ z2wF_pH1p8mCO9p$GvXtKjaW4FjANC<4V7Y{y%c`)BD(`xXX9;zuyiR85$u!^gbg7@ z-}f(aJ)m%LGPLMPBn};{=OF3@-Mfmg^a8RThP|M(4}=ZdCiLQ6#IhB0Sj#L_oE$ch zSEwcU5iLUDLOez4@2qfP|1`n91Q%)veng8m@f3#`+-HbGvT_h&D#6nrVUC{BkFd`Y z_HKpWoc)t_x9Yxr|Fc?`HS*&c)%*fM`HHhzO!ghu?Zi>8xum&7Jb^!4`LfFpm zE3(mqjSyD{mA4c0S%QzVgk40~q43BO9Rf>S?so|q4J**66Y=?ku)|>I5}n-x$Clj7 zhco!Gq$?W%Z@jFtRNO4M_?Nn%e5uPz@8yq~^~iy^S8RnFSUQCcF@@f2lKNfZ>i~B- zPViC}`vRhHLuHryad}KHPs6vjH_CN4othMwxikqjVl^%0e(!xGVm}h`KC!W&q1@%B z+jxvz@}Y7Gi*2=bnI8hU_;9hh?IOx7S4X)zR1gvtr+%B=p zaYUB;Ar~duvVVh@u=e?Qgc}7mE_bm3@aA$IV(_AaNA5|azDY0`w~-5(WV|ei2;$N- z2tQeiTJIro!cZoSe%P zXvIv~X#b>)By#Mlh~)S+;K>Suv6+yuQje-c57ESWkZ++dY-Kd8U+F47M3^qp#_U=M z-j(3i+mVQ*6Wptonh@d*F`&491dk#1gH1eKmLwd~;qc06x0jd{LH;Xx@mdl9-zn`z zpZD-x(Qb{z9^dBd)=qHiB~_3y>Ip;8akjO?p`MfA;ww?!3B)FW*vyBhNw}*l?41HuZ92il3bXQ65d1Favs#CW)vlZdf)27m zpu9@(!31}8G!H~g2EhZ+yju6>5q2nH3m^(~ZzW;xA#4>413l*uVMh?Qh2SFz?!oWp zMH7{blA77|gEukPc*0uWObdP%97c8xVMi0+&A`^^IY$YbOW2!GA~aMuLwgcss!#Ab3o;c_3N!de%5VN^@-hlp1b!5=30b%H-a@Ob=PKr~U|qf#@wp(YL&vBJs3W-_rU zfvDH?VH*hhd&1TcTtv5iJK9fhu|pn)gx8`&g$3R|N?*IoVfnhAFk@XT-2R7NQ1yB! zbb3sKSJy^D-s>G}uB{92%0g>Gs&+B%VDM=*-fkcySPW$!1)wVEVP4paNANOWk~e3W&9y&n#N`VRw;{*ect=0|=s z1fSy4P~7^>wX42hoUTh{R2X0Vnm9+m^2YnfVX&CG0TFG?U+zt+tCAM6-wuJ^gvmLGJOZ) zC<{>5qpU*d+=09}Mwz}7k9Q)!)5Ef1@y;PI0*_koXf#$CfY#b*C=>k0XpgcArSoI7 zKv{+|eHX?@S%9)0WtBMIh5WAZP=>Og4sn!KbsqKr?5)H2b?CJlqwGd@w}($+u;hbO zX#F@EMj+I(+k=lFA$#ytg~1*Vn+%Kb$hil(Js2Ei`X`tiWdX{1lvOC5pCJE9Hg9*r zUG)RPi>8|^tWtw{^#gcQ7zFDL?$DsE-q0Ls7j}gyY0;3o*RP3R3q{)pbN3G9XEg}! zHTd7rdat4BYDaW3tv~hip5ajVslmhMKoG^KaA-pjKL;{DGx$@;p~yn+Geav^zR$zY zpAF>rV`}C;gWnbbg(%V^Ac!Jw4z!`b-&8aA8@yx=6r#YNR)Z+kB8Q>@xz7#04>=Sb z7X(p67D3zRhBn$|R^Ailei|Q-yF)x0$z5XNvM*3ZNig?|fqV@H_`=Y(C@>(;%hllL zLyI7@!O+etB3}{}NK7b#V1t2+ZsxvT5OW}gJqQ`7O*jyvJ?A#_%r=b=`1uAjK456g ziol;sagbdH{rnP!KWJ#bDWb&mK*BK>f=CqKW`@Q^ z0QIBLMKi}CKW{{jLk9OnLLrKM^B{=Ah=ev21(A?>7>5`+6lak`u?@MyhSm^i=I$#a z<$mSo?en1UD?_`ch|ZE|K*ALTZC~N|zX+K}4DB!YQ4o2k;tutOqW0qj zyg-ye<^_Z0L+j~Yu=YX>FX;}!3x>8h&hW zEC-2#GH65MPDSLI9^c_cLXYnZZJ8o^njT0TLJuT9P{a(=<04*27DM4h-2HJ7yomLy zHzrlpB|jSpGtc8?>rxCaLd#2rHcy$DcY=mXeyvg=lVniWWk0W644Ib=-Vz6eDBfEP zK@{zA(1xN8xhr@>i8q&!c`2>9;^%>Q@$)eJ7z~FmuO#sFcxXerJZ0yAJHH>OH7+*2 mvSBVB6XJG611H)_86Y4e}T>E}Nc>F6SnXBX7&G*0H