42 lines
1.3 KiB
C++
42 lines
1.3 KiB
C++
#include <iostream>
|
|
#include <chrono>
|
|
#include <thread>
|
|
#include "DynamixelHandler.h"
|
|
|
|
using namespcae std
|
|
|
|
// Global variables
|
|
DynamixelHandler _oDxlHandler;
|
|
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()
|
|
{
|
|
cout << "===Initialization of the Dynamixel Motor communication====" << endl;
|
|
_ oDxlHandler.setDeviceName(_poppyDxlPortName);
|
|
_ oDxlHandler.setProtocolVersion(_poppyDxlProtocol);
|
|
_ oDxlHandler.openPort();
|
|
_ oDxlHandler.setBaudRate(_poppyDxlBaudRate);
|
|
_ oDxlHandler.enableTorque(true);
|
|
cout << endl;
|
|
// read current joint position
|
|
vector<uint16_t> l_vCurrentJointPosition;
|
|
_ oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
|
|
// display current joint position
|
|
cout << "vCurrentJointPosition= (" << endl;
|
|
for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++)
|
|
cout << l_vCurrentJointPosition[l_joint] << ", ";
|
|
cout << ")" << endl;
|
|
// wait 1s
|
|
this_thread::sleep_for(chrono::milliseconds(1000));
|
|
cout << "===Closing the Dynamixel Motor communication====" << endl;
|
|
_ oDxlHandler.enableTorque(false);
|
|
_ oDxlHandler.closePort();
|
|
cout << endl;
|
|
return 0;
|
|
} |