diff --git a/main.cpp b/main.cpp
index 0362a14..471794a 100644
--- a/main.cpp
+++ b/main.cpp
@@ -12,6 +12,21 @@ float _minJointCmd = 0;
 float _maxJointCmd = 1023;
 float _minJointAngle = -180.0f;
 float _maxJointAngle = 180.0f;
+
+nt convertAnglesToJointCmd(float fJointAngle)
+{ // y = ax + b
+float a = (_maxJointCmd-_minJointCmd) / (_maxJointAngle - _minJointAngle);
+float b = _minJointCmd - a * _minJointAngle;
+float jointCmd = a * fJointAngle + b;
+return (int)jointCmd;
+}
+void goToHomePosition()
+{ std::vector<uint16_t> l_vTargetJointPosition;
+for (int l_joint = 0; l_joint < _nbJoints; 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);