in process torque guide

main.cpp

@@ -3,6 +3,13 @@
 #include <thread> 
 #include "DynamixelHandler.h" 
  
+// User defined var
+
+int wantedTorque = 515;  
+// Control variables
+int eps;
+int errorMargin=10;
+ 
 // Global variables 
 DynamixelHandler _oDxlHandler; 
 std::string _poppyDxlPortName = "/dev/ttyUSB0"; 
@@ -29,7 +36,7 @@ void goToHomePosition()
  std::vector<uint16_t> l_vTargetJointPosition; 
  for (int l_joint = 0; l_joint < _nbJoints; l_joint++) 
   l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); 
-  
+  //l_vTargetJointPosition[5] = convertAnglesToJointCmd();
  _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition); 
 } 
 
@@ -44,6 +51,30 @@ for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++)
 std::cout << ")" << std::endl; 
 }
 
+void gripperControl() {
+// read current joint torque 
+std::vector<uint16_t> l_vCurrentJointTorque; 
+std::vector<uint16_t> l_vTargetJointPosition; 
+_oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque); 
+// display current joint torque 
+
+eps=wantedTorque-l_vCurrentJointTorque[5];
+while(eps>errorMargin){
+ //Kp=1/(eps*l_vCurrentJointPosition[5]);
+ for (int l_joint = 0; l_joint < _nbJoints; l_joint++) {
+  l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); 
+  l_vTargetJointPosition[5] = convertAnglesToJointCmd(2*(eps/abs(eps)));
+ _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition); 
+ }
+ eps=wantedTorque-l_vCurrentJointTorque[5];
+ 
+ std::cout << "vCurrentJointTorque at 6th joint= ("; 
+ std::cout << l_vCurrentJointTorque[5]; 
+ std::cout << ")" << std::endl; 
+}
+
+}
+
 
 int main() 
 {
@@ -59,17 +90,19 @@ std::cout << std::endl;
  
  // wait 1s 
 std::this_thread::sleep_for(std::chrono::milliseconds(1000)); 
-PosDisp(); 
+gripperControl(); 
+//PosDisp();
 // wait 1s 
 std::this_thread::sleep_for(std::chrono::milliseconds(1000)); 
- goToHomePosition();
+ //goToHomePosition();
  // wait 1s 
 std::this_thread::sleep_for(std::chrono::milliseconds(1000)); 
- PosDisp();
- 
+ //gripperControl();
+ //PosDisp();
  
 // wait 1s 
-std::this_thread::sleep_for(std::chrono::milliseconds(10000)); 
+std::this_thread::sleep_for(std::chrono::milliseconds(1000)); 
+ 
  
  
 std::cout << "===Closing the Dynamixel Motor communication====" << std::endl;