diff --git a/main.cpp b/main.cpp
index 258489a..05cc54d 100644
--- a/main.cpp
+++ b/main.cpp
@@ -31,23 +31,43 @@ void goToHomePosition()
 	_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
 }
 
-void newJointPosition()
+void JointPosition()
 {
+	// read current joint position
 	std::vector<uint16_t> l_vCurrentJointPosition;
 	_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
 	// display current joint position
-	std::cout <<"vCurrentJointPosition= (" << 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()
+
+//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()
 {
-	std::vector<uint16_t> l_vTargetJointPosition;
-	for (int l_joint = 5; l_joint;)
-	l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
-	_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
+	//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()
@@ -57,53 +77,83 @@ int main()
 	_oDxlHandler.setProtocolVersion(_poppyDxlProtocol);
 	_oDxlHandler.openPort();
 	_oDxlHandler.setBaudRate(_poppyDxlBaudRate);
-	//_oDxlHandler.enableTorque(true);
+	_oDxlHandler.enableTorque(true);
 	std::cout << std::endl;
 	
-	// read current joint position
-	std::vector<uint16_t> l_vCurrentJointPosition;
-	_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
-	// display current joint position
-	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;
+	std::this_thread::sleep_for(std::chrono::milliseconds(1000));
 	
-	//read the current torque
-	int targetTorque = 0;
-	int p = 2;
+	//JointPosition();
+	//torqueControl();
+		
+	float targetTorque = 200;
+	float p = 0.4;
 	std::vector<uint16_t> l_vCurrentJointTorque;
-	for(int l =0;l<20;l++)
+	
+	bool bKeepLooping = true;
+	while (bKeepLooping)
 	{
 		l_vCurrentJointTorque.clear();
 		_oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
-		float currentTorque = l_vCurrentJointTorque[5];
-		float torqueDiff = targetTorque - currentTorque;
-		float newAngle = torqueDiff * p
 		
-		std::vector<uint16_t> l_vTargetJointPosition;
-		for (int l_joint = 5, l_joint;)
-		l_vTargetJointPosition.push_back(convertAnglesToJointCmd(newAngle));
+		if (l_vCurrentJointTorque.size() == 6)
+		{
+			float currentTorque = 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.0));
+
+				l_vTargetJointPosition[5]=l_vCurrentJointPosition[5]+currentTorque;
+
+				//Print torque different
+				std::cout << torqueDiff << ", " << 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));
+
+				if (torqueDiff < 100)
+				{
+					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;
+				}
+			}
+			
+
+			
+		}
 		
-		
-		//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));
 	}
-	
-	
+
 	//goToHomePosition();
-	//newJointPosition();
 	
-	// wait 1s
-	std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+	std::this_thread::sleep_for(std::chrono::milliseconds(5000));
 	std::cout << "===Closing the Dynamixel Motor communication====" << std::endl;
 	_oDxlHandler.enableTorque(false);
 	_oDxlHandler.closePort();
 	std::cout << std::endl;
 	
 	
+	
+	
 	return 0;
 }