From 88281440acc450e05cac35894dd0d9b7d6bb1f45 Mon Sep 17 00:00:00 2001
From: "ly.pechvattana" <ly.pechvattana@ecam.fr>
Date: Fri, 10 Mar 2023 18:08:34 +0700
Subject: [PATCH] still not working

---
 main.cpp | 66 +++++++++++++++++++++++++++++++++-----------------------
 1 file changed, 39 insertions(+), 27 deletions(-)

diff --git a/main.cpp b/main.cpp
index 05cc54d..fe9468f 100644
--- a/main.cpp
+++ b/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;
@@ -51,24 +51,24 @@ void JointPosition()
 	//_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
 //}
 
-void torqueControl()
-{
+//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);
+//	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++;
-	}
-}
+//		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()
 {
@@ -77,16 +77,25 @@ int main()
 	_oDxlHandler.setProtocolVersion(_poppyDxlProtocol);
 	_oDxlHandler.openPort();
 	_oDxlHandler.setBaudRate(_poppyDxlBaudRate);
-	_oDxlHandler.enableTorque(true);
+	_oDxlHandler.enableTorque(false);
 	std::cout << std::endl;
 	
 	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 = 200;
-	float p = 0.4;
+	float targetTorque = 50;
+	float p = -0.03;
 	std::vector<uint16_t> l_vCurrentJointTorque;
 	
 	bool bKeepLooping = true;
@@ -97,7 +106,7 @@ int main()
 		
 		if (l_vCurrentJointTorque.size() == 6)
 		{
-			float currentTorque = l_vCurrentJointTorque[5];
+			float currentTorque = (float)l_vCurrentJointTorque[5];
 			float torqueDiff = targetTorque - currentTorque;
 			currentTorque = torqueDiff * p;
 
@@ -109,24 +118,25 @@ int main()
 			{
 				std::vector<uint16_t> l_vTargetJointPosition;
 				for (int l_joint = 0; l_joint < _nbJoints; l_joint++)
-					l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0));
+					l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
 
-				l_vTargetJointPosition[5]=l_vCurrentJointPosition[5]+currentTorque;
+				l_vTargetJointPosition[5]=(uint16_t)l_vCurrentJointPosition[5]+currentTorque;
 
 				//Print torque different
-				std::cout << torqueDiff << ", " << currentTorque << ", " << l_vTargetJointPosition[5]<< std::endl;
+				std::cout << torqueDiff << ", " << (uint16_t)currentTorque << ", " << l_vTargetJointPosition[5]<< std::endl;
 
-				_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
-
-				std::this_thread::sleep_for(std::chrono::milliseconds(1000));
 
 				//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);
 
-				if (torqueDiff < 100)
+				std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
+				if (l_vTargetJointPosition[5] > 500)
 				{
 					bKeepLooping  = false;
 					// wait 1s
@@ -136,6 +146,8 @@ int main()
 					//_oDxlHandler.closePort();
 					//std::cout << std::endl;
 				}
+				
+				
 			}