From 9028e57252fab99aa186beead55368e356f37818 Mon Sep 17 00:00:00 2001
From: Alexandre <alexandre.verot@ecam.fr>
Date: Mon, 13 Mar 2023 15:05:03 +0100
Subject: [PATCH] goes down, grab cube, and lifts it

---
 Robot_arm.cpp | 93 +++++++++++++++++++++++++++++++++++++++++++++------
 develop       |  1 +
 2 files changed, 83 insertions(+), 11 deletions(-)
 create mode 100644 develop

diff --git a/Robot_arm.cpp b/Robot_arm.cpp
index ed3c7b3..234069d 100644
--- a/Robot_arm.cpp
+++ b/Robot_arm.cpp
@@ -23,27 +23,28 @@ int convertAnglesToJointCmd(float fJointAngle)
 	float jointCmd = a * fJointAngle + b;
 	return (int)jointCmd;
 }
-void goToHomePosition()
-{ 
+void goToHomePosition(int grip)
+{
 	vector<uint16_t> l_vTargetJointPosition;
-	for (int l_joint = 0; l_joint < _nbJoints; l_joint++)
+        for (int l_joint = 0; l_joint < (_nbJoints-grip); l_joint++)
         {
 		l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
 	}
         _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
 }
 
-void gripperControl(){
+
+void gripperControltest(){
     while(true){
-        int slot = 2;
-        std::vector<uint16_t> l_vCurrentJointTorque;
+        int slot = 5;//slot 0 is for motor 1 etc
+        vector<uint16_t> l_vCurrentJointTorque;
         _oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
-        int angle = 0;
+        int angle = -10;
         int var = l_vCurrentJointTorque[slot];
         cout<<"initial torque value = "<<var<<endl;
-                while (var < 2000){
-                    int var = l_vCurrentJointTorque[slot];
+                while (angle < 20){
                     vector<uint16_t> l_vTargetJointPosition;
+                    vector<uint16_t> l_vCurrentJointTorque;
                     for (int l_joint = 0; l_joint < (_nbJoints-1); l_joint++)
                     {
                         l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
@@ -51,9 +52,15 @@ void gripperControl(){
                     l_vTargetJointPosition.push_back(convertAnglesToJointCmd(angle));
                     _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
                     std::this_thread::sleep_for(std::chrono::milliseconds(200));
+
                     _oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
                     std::this_thread::sleep_for(std::chrono::milliseconds(200));
+
                     cout<<"torque value: "<<l_vCurrentJointTorque[slot]<<endl;
+                    var = l_vCurrentJointTorque[slot];
+                    if (var >= 1110){
+                        break;
+                    }
                     angle ++;
                 }
                 cout<<"done!!!!!!!!!!!"<<endl;
@@ -62,6 +69,67 @@ void gripperControl(){
 
 }
 
+//troque threshold at around 1110
+
+void grabCube(){
+    while(true){
+
+        int slot = 5;//slot 0 is for motor 1 etc
+        vector<uint16_t> l_vCurrentJointTorque;
+        vector<uint16_t> l_vCurrentJointPosition;
+        _oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
+        _oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
+
+        int angle = -10;
+        int var = l_vCurrentJointTorque[slot];
+        cout<<"initial torque value = "<<var<<endl;
+                while (angle < 20){
+                    vector<uint16_t> l_vTargetJointPosition;
+                    vector<uint16_t> l_vCurrentJointTorque;
+
+                    for (int l_joint = 0; l_joint < (_nbJoints-1); l_joint++)
+                    {
+                        l_vTargetJointPosition.push_back(l_vCurrentJointPosition[l_joint]);
+                    }
+
+                    l_vTargetJointPosition.push_back(convertAnglesToJointCmd(angle));
+                    _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
+                    std::this_thread::sleep_for(std::chrono::milliseconds(200));
+
+                    _oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
+                    std::this_thread::sleep_for(std::chrono::milliseconds(200));
+
+                    cout<<"torque value: "<<l_vCurrentJointTorque[slot]<<endl;
+                    var = l_vCurrentJointTorque[slot];
+                    if (var >= 1118){
+                        break;
+                    }
+                    angle ++;
+                }
+                cout<<"done!!!!!!!!!!!"<<endl;
+                break;
+    }
+}
+
+void trianglePosition(){
+    vector<uint16_t> l_vTargetJointPosition;
+    vector<uint16_t> l_vCurrentJointPosition;
+    int angle = 0;
+    while (angle > -40){
+        vector<uint16_t> l_vTargetJointPosition;
+        l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));//for rotor 1
+        l_vTargetJointPosition.push_back(convertAnglesToJointCmd(angle)); //for user controled rotor 2
+        for (int l_joint = 2; l_joint < (_nbJoints); l_joint++)
+        {
+            l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
+        }
+        _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
+        std::this_thread::sleep_for(std::chrono::milliseconds(200));
+        angle = angle -1;
+    }
+
+}
+
 void inputEndEffector(int Theta){
     while(true){
         cout<<"angle: "<<endl;
@@ -108,11 +176,14 @@ int main()
 	std::this_thread::sleep_for(std::chrono::milliseconds(1000));
 	// display current joint position
         print_position("=========BEFORE MOVING========");
-        goToHomePosition();
+        goToHomePosition(0);
+        trianglePosition();
+        grabCube();
+        goToHomePosition(1);
         print_position("=========AFTER MOVING=========");
         std::this_thread::sleep_for(std::chrono::milliseconds(1000));
         //move only end effector angle
-        gripperControl();
+
 	cout << "===Closing the Dynamixel Motor communication====" << endl;
 	_oDxlHandler.enableTorque(false);
 	_oDxlHandler.closePort();
diff --git a/develop b/develop
new file mode 100644
index 0000000..68e6954
--- /dev/null
+++ b/develop
@@ -0,0 +1 @@
+Branch 'develop' set up to track remote branch 'develop' from 'origin'.