From 5670602d22d5bef80158eb9c6163d83035496d5c Mon Sep 17 00:00:00 2001
From: ros <camille.conjat@ecam.fr>
Date: Thu, 9 Mar 2023 09:29:48 +0100
Subject: [PATCH] update reading position

---
 main.cpp          | 33 ++++++++++++++++++++++++++++++++-
 main.cpp.autosave | 38 --------------------------------------
 2 files changed, 32 insertions(+), 39 deletions(-)
 delete mode 100644 main.cpp.autosave

diff --git a/main.cpp b/main.cpp
index 5d228d7..e98df88 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;
+
+int 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);
@@ -29,11 +44,27 @@ for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++)
 std::cout << l_vCurrentJointPosition[l_joint] << ", ";
 std::cout << ")" << std::endl;
 // 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;
 return 0;
+goToHomePosition();
+// 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;
+// read current joint position
+std::vector<uint16_t> l_vCurrentJointPosition;
+_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
+// display current joint position
+std::cout << "vCurrentJointPosition= (" << std::endl;
+for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++)
+std::cout << l_vCurrentJointPosition[l_joint] << ", ";
+std::cout << ")" << std::endl;
+
+
 }
diff --git a/main.cpp.autosave b/main.cpp.autosave
deleted file mode 100644
index 0362a14..0000000
--- a/main.cpp.autosave
+++ /dev/null
@@ -1,38 +0,0 @@
-#include <iostream>
-#include <chrono>
-#include <thread>
-#include "DynamixelHandler.h"
-// Global variables
-DynamixelHandler _oDxlHandler;
-std::string _poppyDxlPortName = "/dev/ttyUSB0";
-float _poppyDxlProtocol = 2.0;
-int _poppyDxlBaudRate = 1000000;
-int _nbJoints = 6;
-float _minJointCmd = 0;
-float _maxJointCmd = 1023;
-float _minJointAngle = -180.0f;
-float _maxJointAngle = 180.0f;
-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 << "vCurrentJointPosition= (" << std::endl;
-for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++)
-std::cout << l_vCurrentJointPosition[l_joint] << ", ";
-std::cout << ")" << std::endl;
-// 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;
-return 0;
-}