From d6cc80c17ebe6bdaf362ccde9e4ab88dda9b98a3 Mon Sep 17 00:00:00 2001 From: toki <82812173+tokirobot@users.noreply.github.com> Date: Mon, 19 May 2025 11:32:09 +0900 Subject: [PATCH] fix: compatible with canfd (#7) ## Summary This PR updates the C++ hardware interface to support **CANFD** and confirms successful operation with **MoveIt** on the actual robot hardware. --- ## Changes Made - Migrated from classic CAN to **CANFD** in the motor control interface. - Updated the following modules: - `openarm_hardware/include/openarm_hardware/canbus.hpp` - `openarm_hardware/src/canbus.cpp` - `openarm_hardware/src/motor_control.cpp` - `openarm_hardware/include/openarm_hardware/motor_control.hpp` - `openarm_hardware/src/openarm_hardware.cpp` - Improved CAN socket handling and data frame structure. - Refactored internal motor communication logic for robustness. --- ## Verification - Confirmed joint control and motion execution with real hardware using **MoveIt**. - Controllers were properly loaded and executed planned trajectories without error. - CANFD communication is now functional and stable during runtime. --- .../.openarm_hardware.hpp.swp | Bin 0 -> 20480 bytes .../include/openarm_hardware/canbus.hpp | 24 +- .../include/openarm_hardware/motor.hpp | 78 ++- .../openarm_hardware/motor_control.hpp | 75 ++- .../openarm_hardware/openarm_hardware.hpp | 5 +- .../openarm_hardware/visibility_control.h | 5 +- .../src/.openarm_hardware.cpp.swp | Bin 0 -> 24576 bytes openarm_hardware/src/canbus.cpp | 81 ++- openarm_hardware/src/motor.cpp | 141 +++-- openarm_hardware/src/motor_control.cpp | 502 ++++++++++++++---- openarm_hardware/src/openarm_hardware.cpp | 4 +- 11 files changed, 687 insertions(+), 228 deletions(-) create mode 100644 openarm_hardware/include/openarm_hardware/.openarm_hardware.hpp.swp create mode 100644 openarm_hardware/src/.openarm_hardware.cpp.swp diff --git a/openarm_hardware/include/openarm_hardware/.openarm_hardware.hpp.swp b/openarm_hardware/include/openarm_hardware/.openarm_hardware.hpp.swp new file mode 100644 index 0000000000000000000000000000000000000000..247bab93cf24aafa7f53811445096c8920b0d86a GIT binary patch literal 20480 zcmeI2Uu+{s9mj{Gf8a_fqJmeZIjC}6;(tjlkuME$5+~UpM3LYDq*h2!zO!B@j+0zc zj;csED}DTDXXdxR{m$(C`Rz0^myHsgOXUcT_Yv~?m(O>;{%nK%@B$&yzsl9=lMa<{ zvS8ND$y$A7a@Fo~uRXbvota#DcqTLZh%w#&K^{{X(|*7_TXZeqhq=8!9a}WFZNbvq zbKI!O4krwX4RWSBL2!~|I$bNE73eDv$=2lD-DE14&8QRS#_pr{zQ3;3(F$k;G5oA>=Wz3+BK- z-b=`f;HTgz@I~+-xE*}|ZbAg8fHL?H_}hC3`78JxcnN$Dd>1?kOz=_g&$|eD7CZ$! z&;^@d5qt!E7~Bb7Jxj=+!7sqe;OF2Bc<$YVG{F6UfZx80kgtGw@W(p|c?Jk@4H)1o zc=--Oz6QPuJ_SArYM=-f!3V&fZYSj1;9KA`zyNoFv*7m#gnSM>39f)Q5cv2v_%(PM z*x=J(3cP`!%8$X1z?Z;Pa37%H@6cOm{$fASLTM?=I`})-8ewryGSRXfmd^_&BjH)H7cjalxMbtV zSSyw)>xEj;w5)2`%vnq2jas$5ZdsQN(^xdtjoKBfQeRv*mPW4<1o?c8cNx8(N_?A+px;*SfT%-RhABLZ&Ab4w95}w!=Z%5)L8q!hKw3ck3 zGQ5_L7SHi@ur`h=bhw>!kIOgN!YRd*UB;%ag6OVoj-S6of~etgCr@^XTD4c~WZ-842x}r?aL9Gee|IEd|XJ*uykq=zrNbAYyxL@mg!y~=0SJ7&)Jg9N( zSyFb;f>fJ%;82?x&Y29n$Td4+Z^`$h@Lj8>#?-;kSt>6Vk#{&PpD!6TRTcw#(!oY( z#o3FNT%eBaSzQ|qURb;t?q+tfS!lbH&BdQn@u$*#C#&S%$tvY{va_)+$JuLQlyg{O zFB99##P%|=y-aK`6Whzg_A;aPmP=O2$4C(0+XC&zzt!9!+&z$lk-juNHI1r!t4?Q+ zwtehr2!sB{v%4$|F!7s*S?Q-Xm zU8!`euL$~Z)dJKO)UnaT(UYj?MilnX4&_x-w&5Wr9S>xqh5NbEUE9HK+0n$qJ$gE> zs`TLmE13pv*>1#6V`#`k%Rj}1JD4K4>=YTpxRc)>ut$rG)?0duhrGdEE_;}9B(jeU zRmpspb$!uW*w=Qx|F8|838lcE(H6CO2vZFmWLwr zQ0g)H(U7RiyIhT2n3~fFr3IB8M%VFZdLc`x942GK=V;6C=LrMm@D_$V*Y?_x-Db3{ zrq3RHSM%vE55w3h#8zfDjoMndUZc1Hj2ShfXwq_3^+?Nx8szYAg%&oh(1$QyPEf|- zhETIN!7wP}lDr$ZoHb*Ynp|nrh$L$scAD~#4rjmw-}dNO!K8*cMi&dFVNNKA>Yl4i z3wABy0)`qrY6rN6W2>RqwYSw(?Y3Y@lRhRX@sb30H$g+cCDjISm{FG0(UCI9r_7_}e8U76zS|yplWk}>Y$cW~snKD*3bCJqx}KnynNT%`(KJKH zRVx@v3XNf!)W6du_$YY0Bk8zvp5`(+^v|aB+b*s84IlHLPz@?!Z^3TW4C1F zwZjGrosRg)H@&O!JghT#t5!Ga_Kq@IW%hNrvgW%@SPCbo;W;Uy*8g9?di`~v*8hX| z_pf2i{|E2`@OjVyML@y7u;%{_cn*9PJOMcPIM@IsFaa(AW%o5@TOV2ht$yrE%~Z;R3bjw-8Lc7BNem z-|~4Av4IxC4;?EGq~VPb!gk5K&m=?|hf5=mt5Sg!^Km?KA&EtXv7zCJYCnvCruP5vO!T$aiu+D!TJOO&30X_!q0k2|R z|11>$fbRfxjULd4RzNGD70?Q31+)TM0j+>mKr5gX exV03}EAaj0X|nboEaq>i75L3>{l1w^+5Z9aMBN?$ literal 0 HcmV?d00001 diff --git a/openarm_hardware/include/openarm_hardware/canbus.hpp b/openarm_hardware/include/openarm_hardware/canbus.hpp index 0fe6be1..78f8847 100644 --- a/openarm_hardware/include/openarm_hardware/canbus.hpp +++ b/openarm_hardware/include/openarm_hardware/canbus.hpp @@ -1,6 +1,6 @@ -#ifndef CANBUS_H -#define CANBUS_H +#pragma once +#include #include #include #include @@ -10,17 +10,29 @@ #include #include #include +#include +enum CANMode { + CAN_MODE_CLASSIC = 0, + CAN_MODE_FD = 1 +}; class CANBus { public: - explicit CANBus(const std::string& interface); + explicit CANBus(const std::string& interface, int mode); ~CANBus(); - + int whichCAN(); bool send(uint16_t motor_id, const std::array& data); - struct can_frame recv(); + std::array recv(uint16_t& out_id, uint8_t& out_len); private: + bool sendClassic(uint16_t motor_id, const std::array& data); + bool sendFD(uint16_t motor_id, const std::array& data); + + struct can_frame recvClassic(); + struct canfd_frame recvFD(); + int sock_; + int mode_; }; -#endif // CANBUS_H + diff --git a/openarm_hardware/include/openarm_hardware/motor.hpp b/openarm_hardware/include/openarm_hardware/motor.hpp index 83e7c84..7bdc6e6 100644 --- a/openarm_hardware/include/openarm_hardware/motor.hpp +++ b/openarm_hardware/include/openarm_hardware/motor.hpp @@ -1,5 +1,4 @@ -#ifndef MOTOR_H -#define MOTOR_H +#pragma once #include #include @@ -27,12 +26,51 @@ enum class DM_Motor_Type : uint8_t { }; enum class DM_variable : uint8_t { - UV_Value = 0, KT_Value, OT_Value, OC_Value, ACC, DEC, MAX_SPD, MST_ID, - ESC_ID, TIMEOUT, CTRL_MODE, Damp, Inertia, hw_ver, sw_ver, SN, NPP, Rs, - LS, Flux, Gr, PMAX, VMAX, TMAX, I_BW, KP_ASR, KI_ASR, KP_APR, KI_APR, - OV_Value, GREF, Deta, V_BW, IQ_c1, VL_c1, can_br, sub_ver, - u_off = 50, v_off, k1, k2, m_off, dir, - p_m = 80, xout, + UV_Value = 0, + KT_Value, + OT_Value, + OC_Value, + ACC, + DEC, + MAX_SPD, + MST_ID, + ESC_ID, + TIMEOUT, + CTRL_MODE, + Damp, + Inertia, + hw_ver, + sw_ver, + SN, + NPP, + Rs, + LS, + Flux, + Gr, + PMAX, + VMAX, + TMAX, + I_BW, + KP_ASR, + KI_ASR, + KP_APR, + KI_APR, + OV_Value, + GREF, + Deta, + V_BW, + IQ_c1, + VL_c1, + can_br, + sub_ver, + u_off = 50, + v_off, + k1, + k2, + m_off, + dir, + p_m = 80, + xout, COUNT }; @@ -54,7 +92,7 @@ public: double getVelocity() const; double getTorque() const; int getParam(int RID) const; - + void setTempParam(int RID, int val); uint16_t SlaveID; uint16_t MasterID; bool isEnable; @@ -64,31 +102,45 @@ public: int getStateTmos() const; int getStateTrotor() const; double getGoalPosition() const; + double getGoalVelocity() const; double getGoalTau() const; void setGoalPosition(double pos); + void setGoalVelocity(double vel); void setGoalTau(double tau); void setStateTmos(int tmos); void setStateTrotor(int trotor); private: double Pd, Vd; - double goal_position, goal_tau; + double goal_position,goal_velocity, goal_tau; double state_q, state_dq, state_tau; int state_tmos, state_trotor; std::map temp_param_dict; }; double LIMIT_MIN_MAX(double x, double min, double max); + uint16_t double_to_uint(double x, double x_min, double x_max, int bits); + double uint_to_double(uint16_t x, double min, double max, int bits); -std::array double_to_uint8s(double value); -std::array data_to_uint8s(uint32_t value); + +std::array double_to_uint8s(double value); + +std::array float_to_uint8s(float value); + +float uint8s_to_float(const std::array& bytes); + + +std::array data_to_uint8s(uint32_t value); + uint32_t uint8s_to_uint32(uint8_t byte1, uint8_t byte2, uint8_t byte3, uint8_t byte4); + double uint8s_to_double(uint8_t byte1, uint8_t byte2, uint8_t byte3, uint8_t byte4); + bool is_in_ranges(int number); + void print_hex(const std::vector& data); template T get_enum_by_index(int index); -#endif // MOTOR_H diff --git a/openarm_hardware/include/openarm_hardware/motor_control.hpp b/openarm_hardware/include/openarm_hardware/motor_control.hpp index 534afa7..ab3eac9 100644 --- a/openarm_hardware/include/openarm_hardware/motor_control.hpp +++ b/openarm_hardware/include/openarm_hardware/motor_control.hpp @@ -1,5 +1,4 @@ -#ifndef MOTOR_CONTROL_H -#define MOTOR_CONTROL_H +#pragma once #include #include @@ -11,37 +10,61 @@ #include "canbus.hpp" #include #include +#include +#include class MotorControl { -public: - explicit MotorControl(CANBus& canbus); + public: + explicit MotorControl(CANBus& canbus); + bool addMotor(Motor& motor); + void enable(Motor& motor); + void disable(Motor& motor); + void set_zero_position(Motor& motor); + void controlMIT(Motor& motor, double kp, double kd, double q, double dq, double tau); + void controlMIT2(Motor& motor, double kp, double kd, double q, double dq, double tau); + void sendData(uint16_t motor_id, const std::array& data); + void recv(); - void controlMIT(Motor& motor, double kp, double kd, double q, double dq, double tau); - void controlMIT2(Motor& motor, double kp, double kd, double q, double dq, double tau); + void control_delay(Motor& motor, double kp, double kd, double q, double dq, double tau, double delay); + void controlPosVel(Motor& motor,double q, double dq); + void controlPosVel2(Motor& motor,double q, double dq); + void controlVel(Motor& motor, double dq); + void controlVel2(Motor& motor, double dq); + void controlPosForce(Motor& motor, double q, double vel, double tau); + void controlPosForce2(Motor& motor, double q, double vel, double tau); - void enable(Motor& motor); - void disable(Motor& motor); - void set_zero_position(Motor& motor); - void recv(); - void sendData(uint16_t motor_id, const std::array& data); - bool addMotor(Motor& motor); - void recv_set_param_data(); + bool switchControlMode(Motor& motor, Control_Type controlMode); + void save_motor_param(Motor &motor); + void change_limit_param(); + //void change_limit_param(DM_Motor_Type motor_type, double PMAX, double VMAX, double TMAX); -private: - CANBus& canbus_; + void recv_set_param_data(); - std::map motors_map; + private: + CANBus& canbus_; - static constexpr double Limit_Param[12][3] = { - {12.5, 30, 10}, {12.5, 50, 10}, {12.5, 8, 28}, {12.5, 10, 28}, - {12.5, 45, 20}, {12.5, 45, 40}, {12.5, 45, 54}, {12.5, 25, 200}, - {12.5, 20, 200}, {12.5, 280, 1}, {12.5, 45, 10}, {12.5, 45, 10} - }; + std::map motors_map_; + static constexpr double Limit_Param[12][3] = { + {12.5, 30, 10}, // DM4310 + {12.5, 50, 10}, // DM4310_48V + {12.5, 8, 28}, // DM4340 + {12.5, 10, 28}, // DM4340_48V + {12.5, 45, 20}, // DM6006 + {12.5, 45, 40}, // DM8006 + {12.5, 45, 54}, // DM8009 + {12.5, 25, 200}, // DM10010L + {12.5, 20, 200}, // DM10010 + {12.5, 280, 1}, // DMH3510 + {12.5, 45, 10}, // DMH6215 + {12.5, 45, 10}, // DMG6220 + }; - void processPacket(const can_frame& frame); - void controlCmd(Motor &motor, uint8_t cmd ); - void readRIDParam(Motor& motor, DM_variable RID); - void writeMotorParam(Motor& motor, DM_variable RID, double value); + + + void processPacket(const can_frame& frame); + void processPacketFD(const canfd_frame& frame); + void controlCmd(Motor &motor, uint8_t cmd ); + void readRIDParam(Motor& motor, DM_variable RID); + void writeMotorParam(Motor& motor, DM_variable RID, double value); }; -#endif // MOTOR_CONTROL_H diff --git a/openarm_hardware/include/openarm_hardware/openarm_hardware.hpp b/openarm_hardware/include/openarm_hardware/openarm_hardware.hpp index 7a05571..9ecbd5f 100644 --- a/openarm_hardware/include/openarm_hardware/openarm_hardware.hpp +++ b/openarm_hardware/include/openarm_hardware/openarm_hardware.hpp @@ -13,8 +13,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OPENARM_HARDWARE__OPENARM_HARDWARE_HPP_ -#define OPENARM_HARDWARE__OPENARM_HARDWARE_HPP_ +#pragma once #include #include @@ -32,6 +31,7 @@ #include "motor.hpp" #include "motor_control.hpp" + namespace openarm_hardware { @@ -106,4 +106,3 @@ private: } // namespace openarm_hardware -#endif // OPENARM_HARDWARE__OPENARM_HARDWARE_HPP_ diff --git a/openarm_hardware/include/openarm_hardware/visibility_control.h b/openarm_hardware/include/openarm_hardware/visibility_control.h index 6398cfe..5311acb 100644 --- a/openarm_hardware/include/openarm_hardware/visibility_control.h +++ b/openarm_hardware/include/openarm_hardware/visibility_control.h @@ -13,9 +13,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OPENARM_HARDWARE__VISIBILITY_CONTROL_H_ -#define OPENARM_HARDWARE__VISIBILITY_CONTROL_H_ - +#pragma once // This logic was borrowed (then namespaced) from the examples on the gcc wiki: // https://gcc.gnu.org/wiki/Visibility @@ -47,4 +45,3 @@ #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE #endif -#endif // OPENARM_HARDWARE__VISIBILITY_CONTROL_H_ diff --git a/openarm_hardware/src/.openarm_hardware.cpp.swp b/openarm_hardware/src/.openarm_hardware.cpp.swp new file mode 100644 index 0000000000000000000000000000000000000000..d9c9539a90cb562b62c5d6bae65848e2e890d61c GIT binary patch literal 24576 zcmeI4eUKYv9mf|eS`mbimJaG4PXn#VUEW%#t!eKd%_VKT=^PKOSoa;5d=c}CWb?2)4p1;TW zewlN9*7w}`8e9Se0tEsE0tEsE0tEsE0tEsE0tEsE0tEsE0tL>10!l@a_PkG$4z5Pz z@c+B}|0ge!q#M8j*arUbK}mWXJO;iB?f}<<^T5w8lq3U8f(bAL2EhP$;{r*#AAAkW zfmv`6902>l72tAk4v@g(ACRPPfIC4N41te;bzlv6{d`IK9vB0I;A7wl@bdd5=@IZ# za4)zAd;#17#=&~93P|8zDC=*)ufeatgMi9^HP{N)fpftV(B=E!MxcWu;4l~m8^L4m zm884C*TJ3OD7Y4E0B^!z@<;F!a2GfXRIm$V!3bClehMkhcjRiroa?h9d6tLk$6u#PCqgX*EUrZjkz^?PymJ~^M0_hv^YGWpVh zDBC#8>#|7Sl<{UHvRyhWIhvd(=1cjBapb6}n>{&NK(4B~!NR6`oXZwd*$_)^$7KgA zw+utB=+*6Pa8QjPoM}~3saamObYn+93B1}3_o%;JT98&6#{`bhq>aYzeW?_mZ|a6+ zm!MN+W;h~A*CTfZw_1kAGD^KpwObHhQK3_rOtx6ujuygLBEcvqE{rvFltV7-nq}y9 zIW}x^OFqsGU4~4mrRrLEKm@U?k7^IEkzBagijf@4kPYD}UgpNjAa_}*tJ4OW8neU$ zj(82ORKiBNjw3*+R7q{{a555Q?v0Taf>W-NQI@3E1Th&y_R-kwFmhe3^765Aojc8# zZDfgorom@bz2(&WKq4abt2(c0tV?xWBaNzRt!41=GL>F&S?GnT!Oa=jmOLzai>0*W zY8A;h8j4mibHx>MO|qbadq{$#o|IGQj_d$ zk4mL7MoqM*89G5lyF_mlJ*uG`;c`n;kGA*@@z!uyC@EXVnw&x%jhnk(|qNzgkQC+9b96Y1N*I$8Ax zOS-|jO6UzUGA0WV$|zp{UjHR53rkOWsw;PflhFkp;@E9g#Z}_%x+_ zyC39#8ePd3LOm+7-1iCUBad-yrRm{KH4p~ zy{y7?a}0w>qrGfOr7&x#nrewQA7Sf<7MqqMU9(ufh<7`OW%CdP(;2nO|6l?WS;2z6 zvzBQ88IGV4S9n!v)h!HMG>NEXXId2_t7#cHSS63;w|d4Nwi87f$o@1EG(Js z9Qkz%gC)PJmFukvXCZrx3J#z zrQy$A@A+f#LMK-{?B(8Y*PXiBP%U#_$a>xx!uDK*Y&`Ybmz2*aA(XsD)%#g54^@l!PW@}=DqQzf=9T_~i- zOZjY(O%zyWVthm_Zt*S0(&Go%p8WVolyMbF7z_(WtQbsMZ>d=KtGptzgfR(qI^D8( zr8m<|HCcB6T|*XFC|zXvVu5{bFFxwwL6G{fV%QA;2;($P$e<}=yx zVm7upo^)P1pto2FsvfNsi{d}#1l4qlwM?oF6tSuzmwdj=n^^K| zc$@T5G&JBjXxAN!kj$ykZoSeXpKe`8*Kr|hTEWyV8f&UnORJgF+$_%+kKUslu_B0F?BXmQ zq2l$6w7`|)$jWZLUO`M|l;yQ@T%!H|1=v@A5zzkM_0PYBJ^!2FX>c3389ax*{tv+2 zAPlYm7lD)5=U)S!#{T^XsDV#_t>EKeEm#9yL>e!EQ%-ufQ<}jiP#{nsP#{nsP#{ns zP#{nsP~iVm0oJooPNmv=^8OAMpN%0RO^#0O%8!rccgorQlM{uKoEc3Qi?C65cHGT+ z3tMfN+7sovVw$j!n-&a^W&9x5-Wp5S3w!=j`**UAIJS}A&2}nfKN6d66k!iphHQI^ z-+Fz}F*(ZEqGtCWVhQHnC}eZlLUufpm3L*+1-XzO$xjvKF*eW+A?}al3)u|0a^$h8 z(NcbLG!OfwZHe-jw$DgHQ8E-U7V{Xs&tyton`~^@vCfX=OC5u_E1<1tl?=?8DXc~9fxPH4o(5d zj%Zt&g?&w#Hp70!SWJl6i7di~8627NwNqo0a%o~T3nLJ^3wjFqCHp%p(uFa3WFlvK zFJa~v`+ssVpCZ38_WyNf>}_Jd|4;1gUj?s#KY*veli)a*2T8CRtO9lH|DVDhe>-sZ z{g>hIb>LF)8ut2sclQ2Y#+CN|w}D&17$E=tTJSu4`A>k8;1=*Xa1+qM6xaY>fDiux z@J(%fcf+mmnqmw^2GcY&{f3>XI6zy@$JI3GyhukhtR2<`_z2KRx_fCX>> z>;rqjW#BLH?f(fp0ZxJw;3n`{@M-WVa1acD=i%S~Gx#~U9UKHP5CLnz2?+l>{Pssc z4O9UKTfr7^6*w3C0)G96z}?^mPy!c#XHaKP0a15Oy}5mLr9H+Tq^S`ux0l1sD142+ zf1C#0zxlNtU&4ECH+q<>96zU>wCy2hFjbLpkbJ=80Py-uWcXU>ZH#|yPp|DB7VcdO ze&T@yynCL8Z0)8UMV}0ct?n(ioVj* zEv{Id(S1dyW0?4ub7yRkEIGzV**Ax7SEyJgZKj8jD7G?0+m4UOhS zAAkMzTbee+;_#rjjEIYKT%p})R}fZm3+rz{_`gI! z{1x_pk*Xbcw_q#mXC#*`)fqlS<{L5WChQlYrzdrKi|?=Hjgkw}tgcqN*ERl8(=&8C zt-s@BH{soPE3NmT_s~(lNVw_$V!kh798-$7R=51ym0 zXC+_0`byEagYm8v&q|Jbdl!F>-pQRy(X+}9 zeyZpE!VwOd;HP>^|LidMsa~%EIQXfa^Q%Jd_C7iPF8@@o^Yh1y{j=WT@PPA^J=i1u E4GpNakN^Mx literal 0 HcmV?d00001 diff --git a/openarm_hardware/src/canbus.cpp b/openarm_hardware/src/canbus.cpp index 36327b7..f0f7e30 100644 --- a/openarm_hardware/src/canbus.cpp +++ b/openarm_hardware/src/canbus.cpp @@ -1,7 +1,7 @@ #include "openarm_hardware/canbus.hpp" -#include -CANBus::CANBus(const std::string& interface) { +CANBus::CANBus(const std::string& interface, int mode) + : mode_(mode) { struct ifreq ifr; struct sockaddr_can addr; @@ -21,6 +21,14 @@ CANBus::CANBus(const std::string& interface) { addr.can_family = AF_CAN; addr.can_ifindex = ifr.ifr_ifindex; + if (mode_ == CAN_MODE_FD) { + int enable_canfd = 1; + if (setsockopt(sock_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_canfd, sizeof(enable_canfd)) < 0) { + perror("CAN FD setsockopt failed"); + exit(EXIT_FAILURE); + } + } + if (bind(sock_, (struct sockaddr*)&addr, sizeof(addr)) < 0) { perror("Error in CAN socket bind"); exit(EXIT_FAILURE); @@ -31,7 +39,34 @@ CANBus::~CANBus() { close(sock_); } +int CANBus::whichCAN(){ + return mode_; +} + bool CANBus::send(uint16_t motor_id, const std::array& data) { + if (mode_ == CAN_MODE_FD) + return sendFD(motor_id, data); + else + return sendClassic(motor_id, data); +} + +std::array CANBus::recv(uint16_t& out_id, uint8_t& out_len) { + std::array buffer = {}; + if (mode_ == CAN_MODE_FD) { + auto frame = recvFD(); + out_id = frame.can_id; + out_len = frame.len; + std::copy(frame.data, frame.data + frame.len, buffer.begin()); + } else { + auto frame = recvClassic(); + out_id = frame.can_id; + out_len = frame.can_dlc; + std::copy(frame.data, frame.data + frame.can_dlc, buffer.begin()); + } + return buffer; +} + +bool CANBus::sendClassic(uint16_t motor_id, const std::array& data) { struct can_frame frame; std::memset(&frame, 0, sizeof(frame)); @@ -42,27 +77,45 @@ bool CANBus::send(uint16_t motor_id, const std::array& data) { if (write(sock_, &frame, sizeof(frame)) != sizeof(frame)) { perror("Error sending CAN frame"); return false; - } else { - // std::cout << "Sent CAN frame to motor ID " << motor_id << std::endl; - return true; } + return true; } -struct can_frame CANBus::recv() { +bool CANBus::sendFD(uint16_t motor_id, const std::array& data) { + struct canfd_frame frame; + std::memset(&frame, 0, sizeof(frame)); + + frame.can_id = motor_id; + frame.len = data.size(); + frame.flags = CANFD_BRS; + std::copy(data.begin(), data.end(), frame.data); + + if (write(sock_, &frame, sizeof(frame)) != sizeof(frame)) { + perror("Error sending CAN FD frame"); + return false; + } + return true; +} + +struct can_frame CANBus::recvClassic() { struct can_frame frame; std::memset(&frame, 0, sizeof(frame)); - // change socket into non blocking mode - // int flags = fcntl(sock_, F_GETFL, 0); - // fcntl(sock_, F_SETFL, flags | O_NONBLOCK); - int nbytes = read(sock_, &frame, sizeof(struct can_frame)); - if (nbytes < 0) { perror("CAN read error"); - } else { - // std::cout << "Received CAN frame from motor ID " << frame.can_id << std::endl; } - return frame; } + +struct canfd_frame CANBus::recvFD() { + struct canfd_frame frame; + std::memset(&frame, 0, sizeof(frame)); + + int nbytes = read(sock_, &frame, sizeof(struct canfd_frame)); + if (nbytes < 0) { + perror("CAN FD read error"); + } + return frame; +} + diff --git a/openarm_hardware/src/motor.cpp b/openarm_hardware/src/motor.cpp index 0d915ef..e9b86e2 100644 --- a/openarm_hardware/src/motor.cpp +++ b/openarm_hardware/src/motor.cpp @@ -1,19 +1,22 @@ +#ifndef OPENARM_MOTOR_H_ +#define OPENARM_MOTOR_H_ + #include "openarm_hardware/motor.hpp" Motor::Motor(DM_Motor_Type motorType, uint16_t slaveID, uint16_t masterID) - : MotorType(motorType), SlaveID(slaveID), MasterID(masterID), - Pd(0.0), Vd(0.0), goal_position(0.0), goal_tau(0.0), - state_q(0.0), state_dq(0.0), state_tau(0.0), - state_tmos(0), state_trotor(0), - isEnable(false), NowControlMode(Control_Type::MIT) {} + : MotorType(motorType), SlaveID(slaveID), MasterID(masterID), + Pd(0.0), Vd(0.0), goal_position(0.0), goal_tau(0.0), + state_q(0.0), state_dq(0.0), state_tau(0.0), + state_tmos(0), state_trotor(0), + isEnable(false), NowControlMode(Control_Type::MIT) {} -void Motor::recv_data(double q, double dq, double tau, int tmos, int trotor) { - state_q = q; - state_dq = dq; - state_tau = tau; - state_tmos = tmos; - state_trotor = trotor; -} + void Motor::recv_data(double q, double dq, double tau, int tmos, int trotor) { + state_q = q; + state_dq = dq; + state_tau = tau; + state_tmos = tmos; + state_trotor = trotor; + } double Motor::getPosition() const { return state_q; } double Motor::getVelocity() const { return state_dq; } @@ -21,100 +24,128 @@ double Motor::getTorque() const { return state_tau; } double Motor::getGoalPosition() const { - return goal_position; + return goal_position; } void Motor::setGoalPosition(double pos) { - goal_position = pos; + goal_position = pos; +} + +double Motor::getGoalVelocity() const { + return goal_velocity; +} + +void Motor::setGoalVelocity(double velocity) { + goal_velocity = velocity; } double Motor::getGoalTau() const { - return goal_tau; + return goal_tau; } void Motor::setGoalTau(double tau) { - goal_tau = tau; + goal_tau = tau; } int Motor::getStateTmos() const { - return state_tmos; + return state_tmos; } void Motor::setStateTmos(int tmos) { - state_tmos = tmos; + state_tmos = tmos; } int Motor::getStateTrotor() const { - return state_trotor; + return state_trotor; } void Motor::setStateTrotor(int trotor) { - state_trotor = trotor; + state_trotor = trotor; } int Motor::getParam(int RID) const { - auto it = temp_param_dict.find(RID); - return (it != temp_param_dict.end()) ? it->second : -1; + auto it = temp_param_dict.find(RID); + return (it != temp_param_dict.end()) ? it->second : -1; +} + +void Motor::setTempParam(int RID, int value) { + temp_param_dict[RID] = value; } double LIMIT_MIN_MAX(double x, double min, double max) { - return std::max(min, std::min(x, max)); + return std::max(min, std::min(x, max)); } uint16_t double_to_uint(double x, double x_min, double x_max, int bits) { - x = LIMIT_MIN_MAX(x, x_min, x_max); - double span = x_max - x_min; - double data_norm = (x - x_min) / span; - return static_cast(data_norm * ((1 << bits) - 1)); + x = LIMIT_MIN_MAX(x, x_min, x_max); + double span = x_max - x_min; + double data_norm = (x - x_min) / span; + return static_cast(data_norm * ((1 << bits) - 1)); } double uint_to_double(uint16_t x, double min, double max, int bits) { - double span = max - min; - double data_norm = static_cast(x) / ((1 << bits) - 1); - return data_norm * span + min; + double span = max - min; + double data_norm = static_cast(x) / ((1 << bits) - 1); + return data_norm * span + min; } -std::array double_to_uint8s(double value) { - std::array bytes; - std::memcpy(bytes.data(), &value, sizeof(double)); - return bytes; +std::array double_to_uint8s(double value) { + std::array bytes; + std::memcpy(bytes.data(), &value, sizeof(double)); + return bytes; } -std::array data_to_uint8s(uint32_t value) { - std::array bytes; - std::memcpy(bytes.data(), &value, sizeof(uint32_t)); - return bytes; +std::array float_to_uint8s(float value) { + std::array bytes{}; + std::memcpy(bytes.data(), &value, sizeof(float)); + return bytes; +} + +float uint8s_to_float(const std::array& bytes) { + float value; + std::memcpy(&value, bytes.data(), sizeof(float)); + return value; +} + +std::array data_to_uint8s(uint32_t value) { + std::array bytes; + std::memcpy(bytes.data(), &value, sizeof(uint32_t)); + return bytes; } uint32_t uint8s_to_uint32(uint8_t byte1, uint8_t byte2, uint8_t byte3, uint8_t byte4) { - uint32_t value; - uint8_t bytes[4] = {byte1, byte2, byte3, byte4}; - std::memcpy(&value, bytes, sizeof(uint32_t)); - return value; + uint32_t value; + uint8_t bytes[4] = {byte1, byte2, byte3, byte4}; + std::memcpy(&value, bytes, sizeof(uint32_t)); + return value; } double uint8s_to_double(uint8_t byte1, uint8_t byte2, uint8_t byte3, uint8_t byte4) { - double value; - uint8_t bytes[4] = {byte1, byte2, byte3, byte4}; - std::memcpy(&value, bytes, sizeof(double)); - return value; + double value; + uint8_t bytes[4] = {byte1, byte2, byte3, byte4}; + std::memcpy(&value, bytes, sizeof(double)); + return value; } bool is_in_ranges(int number) { - return (7 <= number && number <= 10) || (13 <= number && number <= 16) || (35 <= number && number <= 36); + return (7 <= number && number <= 10) || (13 <= number && number <= 16) || (35 <= number && number <= 36); } void print_hex(const std::vector& data) { - for (auto byte : data) { - std::cout << std::hex << std::uppercase << (int)byte << " "; - } - std::cout << std::dec << std::endl; + for (auto byte : data) { + std::cout << std::hex << std::uppercase << (int)byte << " "; + } + std::cout << std::dec << std::endl; } template T get_enum_by_index(int index) { - if (index >= 0 && index < static_cast(T::COUNT)) { - return static_cast(index); - } - return static_cast(-1); + if (index >= 0 && index < static_cast(T::COUNT)) { + return static_cast(index); + } + return static_cast(-1); + } + + +#endif // OPENARM_MOTOR_H_ diff --git a/openarm_hardware/src/motor_control.cpp b/openarm_hardware/src/motor_control.cpp index 02eedf5..5b9c885 100644 --- a/openarm_hardware/src/motor_control.cpp +++ b/openarm_hardware/src/motor_control.cpp @@ -3,161 +3,453 @@ MotorControl::MotorControl(CANBus& canbus) : canbus_(canbus) {} + bool MotorControl::addMotor(Motor& motor) { - motors_map[motor.SlaveID] = &motor; - if (motor.MasterID != 0) { - motors_map[motor.MasterID] = &motor; - } - return true; + motors_map_[motor.SlaveID] = &motor; + if (motor.MasterID != 0) { + motors_map_[motor.MasterID] = &motor; + } + return true; } void MotorControl::enable(Motor& motor) { - controlCmd(motor, 0xFC); - sleep(0.3); + controlCmd(motor, 0xFC); + sleep(0.3); } void MotorControl::disable(Motor& motor) { - controlCmd(motor, 0xFD); - sleep(0.3); + controlCmd(motor, 0xFD); + sleep(0.3); } void MotorControl::set_zero_position(Motor& motor){ - controlCmd(motor, 0xFE); - sleep(0.3); - recv(); + controlCmd(motor, 0xFE); + sleep(0.3); + recv(); } void MotorControl::controlMIT(Motor& motor, double kp, double kd, double q, double dq, double tau) { - controlMIT2(motor, kp, kd, q, dq, tau); - recv(); + if (motors_map_.find(motor.SlaveID) == motors_map_.end()) { + std::cerr << "controlMIT ERROR: Motor ID not found" << std::endl; + return; + } + + uint16_t kp_uint = double_to_uint(kp, 0, 500, 12); + uint16_t kd_uint = double_to_uint(kd, 0, 5, 12); + + int motor_index = static_cast(motor.MotorType); + double Q_MAX = Limit_Param[motor_index][0]; + double DQ_MAX = Limit_Param[motor_index][1]; + double TAU_MAX = Limit_Param[motor_index][2]; + + uint16_t q_uint = double_to_uint(q, -Q_MAX, Q_MAX, 16); + uint16_t dq_uint = double_to_uint(dq, -DQ_MAX, DQ_MAX, 12); + uint16_t tau_uint = double_to_uint(tau, -TAU_MAX, TAU_MAX, 12); + + std::array data = { + static_cast((q_uint >> 8) & 0xFF), + static_cast(q_uint & 0xFF), + static_cast(dq_uint >> 4), + static_cast(((dq_uint & 0xF) << 4) | ((kp_uint >> 8) & 0xF)), + static_cast(kp_uint & 0xFF), + static_cast(kd_uint >> 4), + static_cast(((kd_uint & 0xF) << 4) | ((tau_uint >> 8) & 0xF)), + static_cast(tau_uint & 0xFF) + }; + + sendData(motor.SlaveID, data); + recv(); } void MotorControl::controlMIT2(Motor& motor, double kp, double kd, double q, double dq, double tau) { - - if (motors_map.find(motor.SlaveID) == motors_map.end()) { - std::cerr << "controlMIT ERROR: Motor ID not found" << std::endl; - return; - } - uint16_t kp_uint = double_to_uint(kp, 0, 500, 12); - uint16_t kd_uint = double_to_uint(kd, 0, 5, 12); + if (motors_map_.find(motor.SlaveID) == motors_map_.end()) { + std::cerr << "controlMIT ERROR: Motor ID not found" << std::endl; + return; + } - int motor_index = static_cast(motor.MotorType); - double Q_MAX = Limit_Param[motor_index][0]; - double DQ_MAX = Limit_Param[motor_index][1]; - double TAU_MAX = Limit_Param[motor_index][2]; + uint16_t kp_uint = double_to_uint(kp, 0, 500, 12); + uint16_t kd_uint = double_to_uint(kd, 0, 5, 12); - uint16_t q_uint = double_to_uint(q, -Q_MAX, Q_MAX, 16); - uint16_t dq_uint = double_to_uint(dq, -DQ_MAX, DQ_MAX, 12); - uint16_t tau_uint = double_to_uint(tau, -TAU_MAX, TAU_MAX, 12); + int motor_index = static_cast(motor.MotorType); + double Q_MAX = Limit_Param[motor_index][0]; + double DQ_MAX = Limit_Param[motor_index][1]; + double TAU_MAX = Limit_Param[motor_index][2]; - std::array data = { - static_cast((q_uint >> 8) & 0xFF), - static_cast(q_uint & 0xFF), - static_cast(dq_uint >> 4), - static_cast(((dq_uint & 0xF) << 4) | ((kp_uint >> 8) & 0xF)), - static_cast(kp_uint & 0xFF), - static_cast(kd_uint >> 4), - static_cast(((kd_uint & 0xF) << 4) | ((tau_uint >> 8) & 0xF)), - static_cast(tau_uint & 0xFF) - }; + uint16_t q_uint = double_to_uint(q, -Q_MAX, Q_MAX, 16); + uint16_t dq_uint = double_to_uint(dq, -DQ_MAX, DQ_MAX, 12); + uint16_t tau_uint = double_to_uint(tau, -TAU_MAX, TAU_MAX, 12); - sendData(motor.SlaveID, data); + std::array data = { + static_cast((q_uint >> 8) & 0xFF), + static_cast(q_uint & 0xFF), + static_cast(dq_uint >> 4), + static_cast(((dq_uint & 0xF) << 4) | ((kp_uint >> 8) & 0xF)), + static_cast(kp_uint & 0xFF), + static_cast(kd_uint >> 4), + static_cast(((kd_uint & 0xF) << 4) | ((tau_uint >> 8) & 0xF)), + static_cast(tau_uint & 0xFF) + }; + + sendData(motor.SlaveID, data); } void MotorControl::sendData(uint16_t motor_id, const std::array& data) { - canbus_.send(motor_id, data); + canbus_.send(motor_id, data); } void MotorControl::recv() { - can_frame frame = canbus_.recv(); - processPacket(frame); + uint16_t id; + uint8_t len; + std::array data = canbus_.recv(id, len); + + if (canbus_.whichCAN() == CAN_MODE_CLASSIC) { + can_frame frame; + std::memset(&frame, 0, sizeof(frame)); + frame.can_id = id; + frame.can_dlc = len; + std::memcpy(frame.data, data.data(), len); + + processPacket(frame); + } + else if (canbus_.whichCAN() == CAN_MODE_FD) { + canfd_frame fd_frame; + std::memset(&fd_frame, 0, sizeof(fd_frame)); + fd_frame.can_id = id; + fd_frame.len = len; + std::memcpy(fd_frame.data, data.data(), len); + + processPacketFD(fd_frame); + } +} + +void MotorControl::control_delay(Motor& motor, double kp, double kd, double q, double dq, double tau, double delay){ + controlMIT(motor, kp, kd, q, dq, tau); + std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(delay))); +} + +void MotorControl::controlPosVel(Motor& motor, double pos, double vel) { + if (motors_map_.find(motor.SlaveID) == motors_map_.end()) { + std::cerr << "controlPosVel ERROR: Motor ID not found" << std::endl; + return; + } + + uint16_t motor_id = 0x100 + motor.SlaveID; + std::array data_buf = {0}; + + auto vel_buf = float_to_uint8s(static_cast(vel)); + auto pos_buf = float_to_uint8s(static_cast(pos)); + + for (int i = 0; i < 4; ++i) { + data_buf[i] = pos_buf[i]; + data_buf[i + 4] = vel_buf[i]; + } + + sendData(motor_id, data_buf); + + + recv(); +} + + +void MotorControl::controlPosVel2(Motor& motor, double pos, double vel) { + if (motors_map_.find(motor.SlaveID) == motors_map_.end()) { + std::cerr << "controlPosVel2 ERROR: Motor ID not found" << std::endl; + return; + } + + uint16_t motor_id = 0x100 + motor.SlaveID; + std::array data_buf = {0}; + + auto pos_buf = float_to_uint8s(static_cast(pos)); + auto vel_buf = float_to_uint8s(static_cast(vel)); + + for (int i = 0; i < 4; ++i) { + data_buf[i] = pos_buf[i]; + data_buf[i + 4] = vel_buf[i]; + } + + sendData(motor_id, data_buf); +} + + +void MotorControl::controlVel(Motor& motor, double vel) { + if (motors_map_.find(motor.SlaveID) == motors_map_.end()) { + std::cerr << "controlVel ERROR: Motor ID not found" << std::endl; + return; + } + + uint16_t motor_id = 0x200 + motor.SlaveID; + std::array data_buf = {0}; + + auto vel_buf = float_to_uint8s(static_cast(vel)); + for (int i = 0; i < 4; ++i) { + data_buf[i] = vel_buf[i]; + } + + sendData(motor_id, data_buf); + recv(); +} + + +void MotorControl::controlVel2(Motor& motor, double vel) { + if (motors_map_.find(motor.SlaveID) == motors_map_.end()) { + std::cerr << "controlVel2 ERROR: Motor ID not found" << std::endl; + return; + } + + uint16_t motor_id = 0x200 + motor.SlaveID; + std::array data_buf = {0}; + + auto vel_buf = float_to_uint8s(static_cast(vel)); + for (int i = 0; i < 4; ++i) { + data_buf[i] = vel_buf[i]; + } + + sendData(motor_id, data_buf); + +} + +void MotorControl::controlPosForce(Motor& motor, double pos, double vel, double tau) { + if (motors_map_.find(motor.SlaveID) == motors_map_.end()) { + std::cerr << "controlPosForce ERROR: Motor ID not found" << std::endl; + return; + } + + uint16_t motor_id = 0x300 + motor.SlaveID; + std::array data_buf = {0}; + + auto pos_buf = float_to_uint8s(static_cast(pos)); + auto vel_buf = float_to_uint8s(static_cast(vel)); + auto tau_buf = float_to_uint8s(static_cast(tau)); + + for (int i = 0; i < 4; ++i) { + data_buf[i] = pos_buf[i]; + } + + data_buf[4] = vel_buf[0]; + data_buf[5] = vel_buf[1]; + + data_buf[6] = tau_buf[0]; + data_buf[7] = tau_buf[1]; + + sendData(motor_id, data_buf); + recv(); +} + +void MotorControl::controlPosForce2(Motor& motor, double pos, double vel, double tau) { + if (motors_map_.find(motor.SlaveID) == motors_map_.end()) { + std::cerr << "controlPosForce ERROR: Motor ID not found" << std::endl; + return; + } + + uint16_t motor_id = 0x300 + motor.SlaveID; + std::array data_buf = {0}; + + auto pos_buf = float_to_uint8s(static_cast(pos)); + auto vel_buf = float_to_uint8s(static_cast(vel)); + auto tau_buf = float_to_uint8s(static_cast(tau)); + + for (int i = 0; i < 4; ++i) { + data_buf[i] = pos_buf[i]; + } + + data_buf[4] = vel_buf[0]; + data_buf[5] = vel_buf[1]; + + data_buf[6] = tau_buf[0]; + data_buf[7] = tau_buf[1]; + + sendData(motor_id, data_buf); +} + + +bool MotorControl::switchControlMode(Motor& motor, Control_Type control_mode) { + const int max_retries = 20; + const double retry_interval = 0.1; + DM_variable RID = DM_variable::CTRL_MODE; + + writeMotorParam(motor, RID, static_cast(control_mode)); + + for (int i = 0; i < max_retries; ++i) { + usleep(static_cast(retry_interval * 1e6)); + recv_set_param_data(); + if (motor.getParam(static_cast(RID)) == static_cast(control_mode)) { + return true; + } + } + return false; +} +void MotorControl::save_motor_param(Motor& motor) { + std::array data = { + static_cast(motor.SlaveID & 0xFF), + static_cast((motor.SlaveID >> 8) & 0xFF), + 0xAA, 0x00, 0x00, 0x00, 0x00, 0x00 + }; + disable(motor); + canbus_.send(0x7FF, data); + usleep(1000); +} +//void MotorControl::change_limit_param(DM_Motor_Type motor_type, double PMAX, double VMAX, double TMAX) { +//int index = static_cast(motor_type); +//Limit_Param[index][0] = PMAX; +//Limit_Param[index][1] = VMAX; +//Limit_Param[index][2] = TMAX; +//} + +void MotorControl::recv_set_param_data() { + uint16_t id; + uint8_t len; + std::array data = canbus_.recv(id, len); + + uint8_t cmd = 0x11; + + if (len >= 8) { + std::cout << "CANID: 0x" << std::hex << id << ", CMD: 0x" << static_cast(cmd) << std::dec << std::endl; + for (int i = 0; i < 8; ++i) { + std::cout << "0x" << std::hex << static_cast(data[i]) << " "; + } + std::cout << std::dec << std::endl; + } } void MotorControl::processPacket(const can_frame& frame) { - uint16_t motorID = frame.data[0]; - uint8_t cmd = 0x11; + uint16_t motorID = frame.data[0]; + uint8_t cmd = 0x11; // someday fix - if (cmd == 0x11) { - if (motorID != 0x00) { - auto it = motors_map.find(motorID); - if (it != motors_map.end() && it->second) { - Motor* motor = it->second; + if (cmd == 0x11) { + if (motorID != 0x00) { + auto it = motors_map_.find(motorID); + if (it != motors_map_.end() && it->second) { + Motor* motor = it->second; - uint16_t q_uint = (frame.data[1] << 8) | frame.data[2]; - uint16_t dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4); - uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5]; - int t_mos = frame.data[6]; - int t_rotor = frame.data[7]; + uint16_t q_uint = (frame.data[1] << 8) | frame.data[2]; + uint16_t dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4); + uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5]; + int t_mos = frame.data[6]; + int t_rotor = frame.data[7]; - double Q_MAX = Limit_Param[static_cast(motor->MotorType)][0]; - double DQ_MAX = Limit_Param[static_cast(motor->MotorType)][1]; - double TAU_MAX = Limit_Param[static_cast(motor->MotorType)][2]; + double Q_MAX = Limit_Param[static_cast(motor->MotorType)][0]; + double DQ_MAX = Limit_Param[static_cast(motor->MotorType)][1]; + double TAU_MAX = Limit_Param[static_cast(motor->MotorType)][2]; - double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16); - double recv_dq = uint_to_double(dq_uint, -DQ_MAX, DQ_MAX, 12); - double recv_tau = uint_to_double(tau_uint, -TAU_MAX, TAU_MAX, 12); + double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16); + double recv_dq = uint_to_double(dq_uint, -DQ_MAX, DQ_MAX, 12); + double recv_tau = uint_to_double(tau_uint, -TAU_MAX, TAU_MAX, 12); - motor->recv_data(recv_q, recv_dq, recv_tau, t_mos, t_rotor); - } - } else { - uint16_t MasterID = frame.data[0] & 0x0F; - auto it = motors_map.find(MasterID); - if (it != motors_map.end() && it->second) { - Motor* motor = it->second; + motor->recv_data(recv_q, recv_dq, recv_tau, t_mos, t_rotor); + } + } else { + uint16_t MasterID = frame.data[0] & 0x0F; + auto it = motors_map_.find(MasterID); + if (it != motors_map_.end() && it->second) { + Motor* motor = it->second; - uint16_t q_uint = (frame.data[1] << 8) | frame.data[2]; - uint16_t dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4); - uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5]; - int t_mos = frame.data[6]; - int t_rotor = frame.data[7]; + uint16_t q_uint = (frame.data[1] << 8) | frame.data[2]; + uint16_t dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4); + uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5]; + int t_mos = frame.data[6]; + int t_rotor = frame.data[7]; - double Q_MAX = Limit_Param[static_cast(motor->MotorType)][0]; - double DQ_MAX = Limit_Param[static_cast(motor->MotorType)][1]; - double TAU_MAX = Limit_Param[static_cast(motor->MotorType)][2]; + double Q_MAX = Limit_Param[static_cast(motor->MotorType)][0]; + double DQ_MAX = Limit_Param[static_cast(motor->MotorType)][1]; + double TAU_MAX = Limit_Param[static_cast(motor->MotorType)][2]; - double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16); - double recv_dq = uint_to_double(dq_uint, -DQ_MAX, DQ_MAX, 12); - double recv_tau = uint_to_double(tau_uint, -TAU_MAX, TAU_MAX, 12); + double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16); + double recv_dq = uint_to_double(dq_uint, -DQ_MAX, DQ_MAX, 12); + double recv_tau = uint_to_double(tau_uint, -TAU_MAX, TAU_MAX, 12); - motor->recv_data(recv_q, recv_dq, recv_tau, t_mos, t_rotor); - } + motor->recv_data(recv_q, recv_dq, recv_tau, t_mos, t_rotor); + } + } } - } } +void MotorControl::processPacketFD(const canfd_frame& frame) { + uint16_t motorID = frame.data[0]; + uint8_t cmd = 0x11; // someday fix + + if (cmd == 0x11) { + if (motorID != 0x00) { + auto it = motors_map_.find(motorID); + if (it != motors_map_.end() && it->second) { + Motor* motor = it->second; + + uint16_t q_uint = (frame.data[1] << 8) | frame.data[2]; + uint16_t dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4); + uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5]; + int t_mos = frame.data[6]; + int t_rotor = frame.data[7]; + + double Q_MAX = Limit_Param[static_cast(motor->MotorType)][0]; + double DQ_MAX = Limit_Param[static_cast(motor->MotorType)][1]; + double TAU_MAX = Limit_Param[static_cast(motor->MotorType)][2]; + + double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16); + double recv_dq = uint_to_double(dq_uint, -DQ_MAX, DQ_MAX, 12); + double recv_tau = uint_to_double(tau_uint, -TAU_MAX, TAU_MAX, 12); + + motor->recv_data(recv_q, recv_dq, recv_tau, t_mos, t_rotor); + } + } else { + uint16_t MasterID = frame.data[0] & 0x0F; + auto it = motors_map_.find(MasterID); + if (it != motors_map_.end() && it->second) { + Motor* motor = it->second; + + uint16_t q_uint = (frame.data[1] << 8) | frame.data[2]; + uint16_t dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4); + uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5]; + int t_mos = frame.data[6]; + int t_rotor = frame.data[7]; + + double Q_MAX = Limit_Param[static_cast(motor->MotorType)][0]; + double DQ_MAX = Limit_Param[static_cast(motor->MotorType)][1]; + double TAU_MAX = Limit_Param[static_cast(motor->MotorType)][2]; + + double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16); + double recv_dq = uint_to_double(dq_uint, -DQ_MAX, DQ_MAX, 12); + double recv_tau = uint_to_double(tau_uint, -TAU_MAX, TAU_MAX, 12); + + motor->recv_data(recv_q, recv_dq, recv_tau, t_mos, t_rotor); + } + } + } +} + + void MotorControl::controlCmd(Motor& motor, uint8_t cmd) { - std::array data_buf = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, cmd}; - sendData(motor.SlaveID, data_buf); + std::array data_buf = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, cmd}; + sendData(motor.SlaveID, data_buf); } void MotorControl::readRIDParam(Motor& motor, DM_variable RID) { - std::array data = { - static_cast(motor.SlaveID & 0xFF), - static_cast((motor.SlaveID >> 8) & 0xFF), - 0x33, - static_cast(RID), - 0x00, 0x00, 0x00, 0x00 - }; - canbus_.send(0x7FF, data); + std::array data = { + static_cast(motor.SlaveID & 0xFF), + static_cast((motor.SlaveID >> 8) & 0xFF), + 0x33, + static_cast(RID), + 0x00, 0x00, 0x00, 0x00 + }; + canbus_.send(0x7FF, data); } void MotorControl::writeMotorParam(Motor& motor, DM_variable RID, double value) { - std::array data = { - static_cast(motor.SlaveID & 0xFF), - static_cast((motor.SlaveID >> 8) & 0xFF), - 0x55, - static_cast(RID) - }; + std::array data = { + static_cast(motor.SlaveID & 0xFF), + static_cast((motor.SlaveID >> 8) & 0xFF), + 0x55, + static_cast(RID) + }; - if (is_in_ranges(static_cast(RID))) { - auto intData = data_to_uint8s(static_cast(value)); - std::copy(intData.begin(), intData.end(), data.begin() + 4); - } else { - auto doubleData = double_to_uint8s(value); - std::copy(doubleData.begin(), doubleData.end(), data.begin() + 4); - } + if (is_in_ranges(static_cast(RID))) { + auto intData = data_to_uint8s(static_cast(value)); + std::copy(intData.begin(), intData.end(), data.begin() + 4); + } else { + auto doubleData = double_to_uint8s(value); + std::copy(doubleData.begin(), doubleData.end(), data.begin() + 4); + } - canbus_.send(0x7FF, data); + canbus_.send(0x7FF, data); } diff --git a/openarm_hardware/src/openarm_hardware.cpp b/openarm_hardware/src/openarm_hardware.cpp index 42b38d3..c841065 100644 --- a/openarm_hardware/src/openarm_hardware.cpp +++ b/openarm_hardware/src/openarm_hardware.cpp @@ -57,8 +57,8 @@ hardware_interface::CallbackReturn OpenArmHW::on_init( disable_torque_ = it->second == "true"; } - - canbus_ = std::make_unique(info.hardware_parameters.at("can_device")); + //temp CANFD + canbus_ = std::make_unique(info.hardware_parameters.at("can_device"),CAN_MODE_FD); motor_control_ = std::make_unique(*canbus_); if(USING_GRIPPER){