From 9d2e99a0e61bd9843bfec490f492e9c3b5adead6 Mon Sep 17 00:00:00 2001
From: "ly.pechvattana" <ly.pechvattana@ecam.fr>
Date: Thu, 9 Mar 2023 16:37:40 +0700
Subject: [PATCH] Adding home position

---
 main.cpp | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/main.cpp b/main.cpp
index e6cce4f..561dd91 100644
--- a/main.cpp
+++ b/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;
 }