still not working

main.cpp

@@ -5,7 +5,7 @@
 
 // Global variables
 DynamixelHandler _oDxlHandler;
-std::string _poppyDxlPortName = "/dev/ttyUSB0";
+std::string _poppyDxlPortName = "/dev/ttyUSB1";
 float _poppyDxlProtocol = 2.0;
 int _poppyDxlBaudRate = 1000000;
 int _nbJoints = 6;
@@ -31,19 +31,45 @@ void goToHomePosition()
 	_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
 }
 
-void readndisPosition()
+void JointPosition()
 {
 	// read current joint position
 	std::vector<uint16_t> l_vCurrentJointPosition;
 	_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
 	// display current joint position
-	std::cout <<"newCurrentJointPosition= (" << std::endl;
+	std::cout <<"CurrentJointPosition= (" << std::endl;
 	for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++)
 		std::cout << l_vCurrentJointPosition[l_joint] << ", ";
-	std::cout << ")" << std::endl;
 }
 
 
+//void gripperControl()
+//{
+	//std::vector<uint16_t> l_vTargetJointPosition;
+	//for (int l_joint = 5; l_joint;)
+	//l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
+	//_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
+//}
+
+//void torqueControl()
+//{
+	//read the current torque
+//	std::vector<uint16_t> l_vCurrentJointTorque;
+//	int i =0;
+//	while (i<30)
+//	{
+//		l_vCurrentJointTorque.clear();
+//		_oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
+		
+		//display torque
+//		std::cout <<"CurrentJointTorque= (" << std::endl;
+//		std::cout << l_vCurrentJointTorque[5] << std::endl;
+//		std::cout << ")" << std::endl;
+//		std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+//		i++;
+//	}
+//}
+
 int main()
 {
 	std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
@@ -51,23 +77,88 @@ int main()
 	_oDxlHandler.setProtocolVersion(_poppyDxlProtocol);
 	_oDxlHandler.openPort();
 	_oDxlHandler.setBaudRate(_poppyDxlBaudRate);
-	_oDxlHandler.enableTorque(true);
+	_oDxlHandler.enableTorque(false);
 	std::cout << std::endl;
 	
-	// read current joint position
-	std::vector<uint16_t> l_vCurrentJointPosition;
-	_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
-	// display current joint position
-	std::cout <<"vCurrentJointPosition= (" << std::endl;
-	for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++)
-		std::cout << l_vCurrentJointPosition[l_joint] << ", ";
-	std::cout << ")" << std::endl;
-	
-	goToHomePosition();
-	readndisPosition();
-	
-	// wait 1s
 	std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+	
+	//JointPosition();
+	//torqueControl();
+	
+	//read angle
+	//std::vector<uint16_t> l_vCurrentJointPosition;
+	//_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
+	//std::cout << l_vCurrentJointPosition[5] << std::endl;
+	//read torque
+	//std::vector<uint16_t> l_vCurrentJointTorque;
+	//_oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
+	//std::cout << l_vCurrentJointTorque[5] << std::endl;
+		
+	float targetTorque = 50;
+	float p = -0.03;
+	std::vector<uint16_t> l_vCurrentJointTorque;
+	
+	bool bKeepLooping = true;
+	while (bKeepLooping)
+	{
+		l_vCurrentJointTorque.clear();
+		_oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
+		
+		if (l_vCurrentJointTorque.size() == 6)
+		{
+			float currentTorque = (float)l_vCurrentJointTorque[5];
+			float torqueDiff = targetTorque - currentTorque;
+			currentTorque = torqueDiff * p;
+
+
+			std::vector<uint16_t> l_vCurrentJointPosition;
+			_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
+			
+			if (l_vCurrentJointPosition.size() == 6)
+			{
+				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]=(uint16_t)l_vCurrentJointPosition[5]+currentTorque;
+
+				//Print torque different
+				std::cout << torqueDiff << ", " << (uint16_t)currentTorque << ", " << l_vTargetJointPosition[5]<< std::endl;
+
+
+				//display torque
+				//std::cout <<"CurrentJointTorque= (" << std::endl;
+				//std::cout << l_vCurrentJointTorque[5] << std::endl;
+				//std::cout << ")" << std::endl;
+				//std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+				
+				_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
+
+				std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
+				if (l_vTargetJointPosition[5] > 500)
+				{
+					bKeepLooping  = false;
+					// wait 1s
+					//std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+					//std::cout << "===Closing the Dynamixel Motor communication====" << std::endl;
+					//_oDxlHandler.enableTorque(false);
+					//_oDxlHandler.closePort();
+					//std::cout << std::endl;
+				}
+				
+				
+			}
+			
+
+			
+		}
+		
+	}
+
+	//goToHomePosition();
+	
+	std::this_thread::sleep_for(std::chrono::milliseconds(5000));
 	std::cout << "===Closing the Dynamixel Motor communication====" << std::endl;
 	_oDxlHandler.enableTorque(false);
 	_oDxlHandler.closePort();