darren.gallois
/
robotarm
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Last commit

This commit is contained in:
Estevan BIAU-LOYER 2023-03-16 20:55:48 +01:00
parent 8360eda477
commit 36053ed29c
1 changed files with 93 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

134
main.cpp
View File

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