Adding home position

main.cpp

@@ -31,6 +31,19 @@ void goToHomePosition()
 	_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
 }
 
+void readndisPosition()
+{
+	// read current joint position
+	std::vector<uint16_t> l_vCurrentJointPosition;
+	_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
+	// display current joint position
+	std::cout <<"newCurrentJointPosition= (" << std::endl;
+	for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++)
+		std::cout << l_vCurrentJointPosition[l_joint] << ", ";
+	std::cout << ")" << std::endl;
+}
+
+
 int main()
 {
 	std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
@@ -50,6 +63,9 @@ int main()
 		std::cout << l_vCurrentJointPosition[l_joint] << ", ";
 	std::cout << ")" << std::endl;
 	
+	goToHomePosition();
+	readndisPosition();
+	
 	// wait 1s
 	std::this_thread::sleep_for(std::chrono::milliseconds(1000));
 	std::cout << "===Closing the Dynamixel Motor communication====" << std::endl;
@@ -58,5 +74,7 @@ int main()
 	std::cout << std::endl;
 	
 	
+	
+	
 	return 0;
 }