Difference between revisions of "BHand library"
Line 7: | Line 7: | ||
<br> | <br> | ||
− | Include BHand library. | + | ====Edit main.cpp==== |
+ | - Include BHand library. | ||
<syntaxhighlight lang="cpp"> | <syntaxhighlight lang="cpp"> | ||
#include <BHand/BHand.h> | #include <BHand/BHand.h> | ||
</syntaxhighlight> | </syntaxhighlight> | ||
− | Variable declaration for BHand library | + | - Variable declaration for BHand library |
<syntaxhighlight lang="cpp"> | <syntaxhighlight lang="cpp"> | ||
// USER HAND CONFIGURATION | // USER HAND CONFIGURATION | ||
Line 24: | Line 25: | ||
double tau_des[MAX_DOF]; // desired joint torque | double tau_des[MAX_DOF]; // desired joint torque | ||
double cur_des[MAX_DOF]; // current joint torque | double cur_des[MAX_DOF]; // current joint torque | ||
+ | const double tau_cov_const_v4 = 1200.0; // 1200.0 for SAH040xxxxx | ||
+ | </syntaxhighlight> | ||
+ | |||
+ | - | ||
+ | <syntaxhighlight lang="cpp"> | ||
+ | |||
+ | </syntaxhighlight> | ||
+ | |||
+ | - Additional function declarations | ||
+ | <syntaxhighlight lang="cpp"> | ||
+ | // functions declarations | ||
+ | char Getch(); | ||
+ | void MainLoop(); | ||
+ | bool OpenCAN(); | ||
+ | void CloseCAN(); | ||
+ | bool CreateBHandAlgorithm(); // create | ||
+ | void DestroyBHandAlgorithm(); // destroy | ||
+ | void ComputeTorque(); // torque compute | ||
+ | </syntaxhighlight> | ||
+ | |||
+ | - | ||
+ | <syntaxhighlight lang="cpp"> | ||
+ | case ID_RTR_FINGER_POSE_1: | ||
+ | case ID_RTR_FINGER_POSE_2: | ||
+ | case ID_RTR_FINGER_POSE_3: | ||
+ | case ID_RTR_FINGER_POSE_4: | ||
+ | { | ||
+ | int findex = (id & 0x00000007); | ||
+ | |||
+ | vars.enc_actual[findex*4 + 0] = (short)(data[0] | (data[1] << 8)); | ||
+ | vars.enc_actual[findex*4 + 1] = (short)(data[2] | (data[3] << 8)); | ||
+ | vars.enc_actual[findex*4 + 2] = (short)(data[4] | (data[5] << 8)); | ||
+ | vars.enc_actual[findex*4 + 3] = (short)(data[6] | (data[7] << 8)); | ||
+ | data_return |= (0x01 << (findex)); | ||
+ | |||
+ | if (data_return == (0x01 | 0x02 | 0x04 | 0x08)) | ||
+ | { | ||
+ | // convert encoder count to joint angle | ||
+ | for (i=0; i<MAX_DOF; i++) | ||
+ | { | ||
+ | q[i] = (double)(vars.enc_actual[i])*(333.3/65536.0)*DEG2RAD; | ||
+ | } | ||
+ | |||
+ | ComputeTorque(); | ||
+ | |||
+ | // convert desired torque to desired current and PWM count | ||
+ | for (int i=0; i<MAX_DOF; i++) | ||
+ | { | ||
+ | cur_des[i] = tau_des[i]; | ||
+ | // set limit | ||
+ | if (cur_des[i] > 1.0) cur_des[i] = 1.0; | ||
+ | else if (cur_des[i] < -1.0) cur_des[i] = -1.0; | ||
+ | } | ||
+ | for (int i=0; i<4;i++) | ||
+ | { | ||
+ | vars.pwm_demand[i*4+0] = (short)(cur_des[i*4+0]*tau_cov_const_v4); | ||
+ | vars.pwm_demand[i*4+1] = (short)(cur_des[i*4+1]*tau_cov_const_v4); | ||
+ | vars.pwm_demand[i*4+2] = (short)(cur_des[i*4+2]*tau_cov_const_v4); | ||
+ | vars.pwm_demand[i*4+3] = (short)(cur_des[i*4+3]*tau_cov_const_v4); | ||
+ | |||
+ | command_set_torque(CAN_Ch, i, &vars.pwm_demand[4*i]); | ||
+ | } | ||
+ | data_return = 0; | ||
+ | } | ||
+ | break; | ||
+ | } | ||
</syntaxhighlight> | </syntaxhighlight> |
Revision as of 16:46, 3 July 2019
BHand Library
You will make....
Please download linux_BHand_tutorial.zip file and follow below steps. File:Linux BHand tutorial.zip
Also, you need to install BHand library. "libBHand": Grasping_Library_for_Linux
Also, you need to install BHand library. "libBHand": Grasping_Library_for_Linux
Edit main.cpp
- Include BHand library.
#include <BHand/BHand.h>
- Variable declaration for BHand library
// USER HAND CONFIGURATION const bool RIGHT_HAND = true; const int HAND_VERSION = 4; // for BHand library BHand* pBHand = NULL; double q[MAX_DOF]; // joint position double q_des[MAX_DOF]; // desired joint position used in joint pd control motion double tau_des[MAX_DOF]; // desired joint torque double cur_des[MAX_DOF]; // current joint torque const double tau_cov_const_v4 = 1200.0; // 1200.0 for SAH040xxxxx
-
- Additional function declarations
// functions declarations char Getch(); void MainLoop(); bool OpenCAN(); void CloseCAN(); bool CreateBHandAlgorithm(); // create void DestroyBHandAlgorithm(); // destroy void ComputeTorque(); // torque compute
-
case ID_RTR_FINGER_POSE_1: case ID_RTR_FINGER_POSE_2: case ID_RTR_FINGER_POSE_3: case ID_RTR_FINGER_POSE_4: { int findex = (id & 0x00000007); vars.enc_actual[findex*4 + 0] = (short)(data[0] | (data[1] << 8)); vars.enc_actual[findex*4 + 1] = (short)(data[2] | (data[3] << 8)); vars.enc_actual[findex*4 + 2] = (short)(data[4] | (data[5] << 8)); vars.enc_actual[findex*4 + 3] = (short)(data[6] | (data[7] << 8)); data_return |= (0x01 << (findex)); if (data_return == (0x01 | 0x02 | 0x04 | 0x08)) { // convert encoder count to joint angle for (i=0; i<MAX_DOF; i++) { q[i] = (double)(vars.enc_actual[i])*(333.3/65536.0)*DEG2RAD; } ComputeTorque(); // convert desired torque to desired current and PWM count for (int i=0; i<MAX_DOF; i++) { cur_des[i] = tau_des[i]; // set limit if (cur_des[i] > 1.0) cur_des[i] = 1.0; else if (cur_des[i] < -1.0) cur_des[i] = -1.0; } for (int i=0; i<4;i++) { vars.pwm_demand[i*4+0] = (short)(cur_des[i*4+0]*tau_cov_const_v4); vars.pwm_demand[i*4+1] = (short)(cur_des[i*4+1]*tau_cov_const_v4); vars.pwm_demand[i*4+2] = (short)(cur_des[i*4+2]*tau_cov_const_v4); vars.pwm_demand[i*4+3] = (short)(cur_des[i*4+3]*tau_cov_const_v4); command_set_torque(CAN_Ch, i, &vars.pwm_demand[4*i]); } data_return = 0; } break; }
Whos here now: Members 0 Guests 0 Bots & Crawlers 1 |