110 lines
3.7 KiB
C++
110 lines
3.7 KiB
C++
#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 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);
|
|
_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;
|
|
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);
|
|
_oDxlHandler.closePort();
|
|
std::cout << std::endl;
|
|
return 0;
|
|
}
|