integrating closed loop sys

main.cpp

@@ -31,17 +31,8 @@ void goToHomePosition()
 	_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
 }
 
-int main()
+void newJointPosition()
 {
-	std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
-	_oDxlHandler.setDeviceName(_poppyDxlPortName);
-	_oDxlHandler.setProtocolVersion(_poppyDxlProtocol);
-	_oDxlHandler.openPort();
-	_oDxlHandler.setBaudRate(_poppyDxlBaudRate);
-	_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
@@ -49,6 +40,62 @@ int main()
 	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);
+}
+
+int main()
+{
+	std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
+	_oDxlHandler.setDeviceName(_poppyDxlPortName);
+	_oDxlHandler.setProtocolVersion(_poppyDxlProtocol);
+	_oDxlHandler.openPort();
+	_oDxlHandler.setBaudRate(_poppyDxlBaudRate);
+	//_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;
+	
+	//read the current torque
+	int targetTorque = 0;
+	int p = 2;
+	std::vector<uint16_t> l_vCurrentJointTorque;
+	for(int l =0;l<20;l++)
+	{
+		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));
+		
+		
+		//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));