lab fijnisze

main.cpp

@@ -12,6 +12,36 @@ 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);
+}
+
+void gripperControl (float newPos)
+{
+    std::vector<uint16_t> l_vTargetJointPosition;
+    l_vTargetJointPosition.clear();
+    for (int l_joint = 0; l_joint < _nbJoints; l_joint++)
+    {
+        if (l_joint==5){
+            l_vTargetJointPosition.push_back(convertAnglesToJointCmd(newPos));
+        }
+        else {
+            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);
@@ -28,8 +58,48 @@ 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;
+goToHomePosition();
 // wait 1s
+std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+// read current joint position
+l_vCurrentJointPosition.clear();
+_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::vector<uint16_t> l_vCurrentJointTorque;
+// display current joint torque
+int targetTorque = 1080;
+float angularDelta;
+int newPos = 0;
+float epsilon;
+float p = 0.01f;
+for (int samples=0; samples<50;samples++){
+l_vCurrentJointTorque.clear();
+_oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
+std::cout << "vCurrentJointTorque= (" << std::endl;
+for (int l_joint=0; l_joint < l_vCurrentJointTorque.size(); l_joint++)
+std::cout << l_vCurrentJointTorque[l_joint] << ", ";
+std::cout << ")" << std::endl;
 
+epsilon = targetTorque - l_vCurrentJointTorque[5];
+angularDelta = epsilon*p;
+newPos += angularDelta;
+std::cout << newPos << "newPos" << epsilon << " epsilon" << angularDelta << " delate" << std::endl;
+gripperControl(newPos);
+
+// wait 1s
+std::this_thread::sleep_for(std::chrono::milliseconds(50));
+
+
+}
+//#####################################################################
+// wait 1s
 std::this_thread::sleep_for(std::chrono::milliseconds(1000));
 std::cout << "===Closing the Dynamixel Motor communication====" << std::endl;
 _oDxlHandler.enableTorque(false);

main.cpp.autosave

@@ -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;
-}

makefile

@@ -1,12 +1,11 @@
 all : dynamixel main
-	g++ DynamixelHandler.o main.o –L/usr/local/lib -ldxl_x64_cpp -lrt
+	g++ DynamixelHandler.o main.o -L/usr/local/lib -ldxl_x64_cpp -lrt
 dynamixel : DynamixelHandler.cpp
 	g++ -c -I/home/ros/SOFTWARE/toolkit-dynamixel/include /home/ros/SOFTWARE/toolkit-dynamixel/src/DynamixelHandler.cpp
 main : main.cpp
-	g++ -c -I/home/ros/SOFTWARE/toolkit-dynamixel/include main.cpp
+	g++ -c main.cpp -I/home/ros/SOFTWARE/toolkit-dynamixel/
 
 clean :
 	rm * .o
 	rm * .out
 	rm * .exe
-