From 36053ed29cac0af1cffdf022fc5ac99b45fae656 Mon Sep 17 00:00:00 2001
From: "estevan.biau-loyer" <estevan.biau-loyer@ecam.fr>
Date: Thu, 16 Mar 2023 20:55:48 +0100
Subject: [PATCH] Last commit

---
 main.cpp | 134 ++++++++++++++++++++++++++++++++++++++-----------------
 1 file changed, 93 insertions(+), 41 deletions(-)

diff --git a/main.cpp b/main.cpp
index a72c5c9..91b7ea5 100644
--- a/main.cpp
+++ b/main.cpp
@@ -2,56 +2,108 @@
 #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;
+int i = 0;
 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);
-_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
-goToHomePosition();
-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;
 
-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;
+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() // This function sets all the servos to their home position except the gripper servo
+{ 
+	std::vector<uint16_t> l_vTargetJointPosition;
+	for (int l_joint = 1; l_joint < _nbJoints; l_joint++)
+	{
+	l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
+	}
+	int l_joint=6;
+	if (l_joint= 6)	
+	{
+		l_vTargetJointPosition.push_back(convertAnglesToJointCmd(30.0f));
+	}
+    	_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
+}
+
+void gripperControl() // This function sets all the servos to their current position(meaning they wont move from whichever position they were in) And closes the gripper to hold the cube
+{
+	std::vector<uint16_t> l_vCurrentJointPosition;
+	_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
+	// display current joint position
+	// Calculate the target joint command based on the desired angle
+	std::vector<uint16_t> l_vTargetJointPosition;
+	for (int l_joint = 1; l_joint < _nbJoints; l_joint++)
+	{
+		l_vTargetJointPosition.push_back(convertAnglesToJointCmd(l_vCurrentJointPosition[l_joint]));
+	}
+	int l_joint=6;
+	if (l_joint= 6)
+	{
+		l_vTargetJointPosition.push_back(convertAnglesToJointCmd(30.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 << "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;
+
+
+
+	std::vector<uint16_t> l_vTargetJointPosition;
+	_oDxlHandler.readCurrentJointPosition(l_vTargetJointPosition);
+	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;
+
+
+	
+	gripperControl(); // Catch the cube
+
+	std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+	goToHomePosition(); //Take the cube up to home position
+	
+		// wait 1s
+	std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+	std::cout << "===Closing the Dynamixel Motor communication====" << std::endl;
+	_oDxlHandler.enableTorque(false);
+	_oDxlHandler.closePort();
+	std::cout << std::endl;
+	return 0;
+}
+