From 2567cae35422bcb6127c2b39730ba048c8068176 Mon Sep 17 00:00:00 2001 From: Gabri6 Date: Thu, 22 Feb 2024 12:22:08 +0100 Subject: [PATCH] initial commit --- README.md | 3 ++ main.cpp | 147 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ makefile | 12 +++++ 3 files changed, 162 insertions(+) create mode 100644 main.cpp create mode 100644 makefile diff --git a/README.md b/README.md index 204128e..4d8205e 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,5 @@ # poppyMusician +##requirements + +need a "libraries" folder in the parent folder, in which is located the Dynamixel-sdk library for C++, with the folder toolkit-dynamixel diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..ad5a490 --- /dev/null +++ b/main.cpp @@ -0,0 +1,147 @@ +#include +#include +#include +#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 _targetJointTorqueGripper = 1140; +float _proportionnalIncrement = 0.025f; +float _proportionnalIncrementAbove = 0.01f; +float _threshold= 1.9; + +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 l_vTargetJointPosition; + for (int l_joint = 0; l_joint < _nbJoints; l_joint++) + l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); + _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition); +} + +void currentPos() +{ + // read current joint position + std::vector l_vCurrentJointPosition; + _oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition); + + // display current joint position + //std::cout << "vCurrentJointPosition= ("; + //for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++) + // std::cout << l_vCurrentJointPosition[l_joint] << ", "; + //std::cout << ")" << std::endl; +} + +int gripperControl() +{ + //read current joint 6 torque + std::vector l_vCurrentJointTorque; + _oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque); + + //read current joint 6 position + std::vector l_vCurrentJointPosition; + _oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition); + int joint6Position = l_vCurrentJointPosition[5]; + + // display current joint 6 torque + std::cout << "CurrentJoint6Torque= "; + std::cout << l_vCurrentJointTorque[5] << std::endl; + int joint6Torque = l_vCurrentJointTorque[5]; + + int jointTorqueDiff = joint6Torque - _targetJointTorqueGripper; + float angleIncrement = -jointTorqueDiff * _proportionnalIncrement; + std::cout<< "increment= " << angleIncrement<< std::endl; + float targetJoint6Position = joint6Position + angleIncrement; + std::cout << "joint torque diff= " < _threshold) + { + + //read current joint 6 torque + std::vector l_vCurrentJointTorque; + _oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque); + + //read current joint 6 position + std::vector l_vCurrentJointPosition; + _oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition); + int joint6Position = l_vCurrentJointPosition[5]; + + // display current joint 6 torque + std::cout << "vCurrentJoint6Torque= "; + std::cout << l_vCurrentJointTorque[5] << std::endl; + int joint6Torque = l_vCurrentJointTorque[5]; + + int jointTorqueDiff = joint6Torque - _targetJointTorqueGripper; + if (jointTorqueDiff < 0) + { + float angleIncrement = -jointTorqueDiff * _proportionnalIncrement; + } + else{ + float angleIncrement = 0; + } + std::cout<< "increment= " << angleIncrement<< std::endl; + float targetJoint6Position = joint6Position + angleIncrement; + std::cout << "joint torque diff= " < l_vTargetJointPosition; + for (int l_joint = 0; l_joint < _nbJoints-1; l_joint++) + l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); + //l_vTargetJointPosition.push_back(convertAnglesToJointCmd(90.0f)); + //l_vTargetJointPosition.push_back(convertAnglesToJointCmd(90.0f)); + //l_vTargetJointPosition.push_back(convertAnglesToJointCmd(-90.0f)); + //l_vTargetJointPosition.push_back(convertAnglesToJointCmd(-90.0f)); + //l_vTargetJointPosition.push_back(convertAnglesToJointCmd(45.0f)); + l_vTargetJointPosition.push_back(targetJoint6Position); + _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition); + } + + return (int)joint6Torque; +} + + +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; + + currentPos(); + + goToHomePosition(); + + currentPos(); + + int gripperTorque = gripperControl(); + + 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; + + return 0; +} \ No newline at end of file diff --git a/makefile b/makefile new file mode 100644 index 0000000..721bbd0 --- /dev/null +++ b/makefile @@ -0,0 +1,12 @@ +all: dynamixel main + g++ DynamixelHandler.o main.o -L/usr/local/lib -ldxl_x64_cpp -lrt + +main: main.cpp + g++ -c -I./../libraries/toolkit-dynamixel/include main.cpp + +dynamixel: ../libraries/toolkit-dynamixel/src/DynamixelHandler.cpp + g++ -c -I./../libraries/toolkit-dynamixel/include ../libraries/toolkit-dynamixel/src/DynamixelHandler.cpp + +clean: + rm *.o + rm *.out \ No newline at end of file