julian.leclerc
/
IntroRoboticsLab1
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Home pos robot

This commit is contained in:
Julian LECLERC 2022-03-17 10:55:16 +01:00
parent f168f2e30c
commit 5dc5627f4a
1 changed files with 43 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
main.cpp
View File

@ -14,16 +14,26 @@ float _maxJointCmd = 1023;
float _minJointAngle = -180.0f;
float _maxJointAngle = 180.0f;
int main()
int convertAnglesToJointCmd(float fJointAngle)
{
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;
// 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 PosDisp() {
// read current joint position
std::vector<uint16_t> l_vCurrentJointPosition;
_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
@ -32,9 +42,34 @@ 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;
}
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;
// wait 1s
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
PosDisp();
// wait 1s
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
goToHomePosition();
// wait 1s
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
PosDisp();
// wait 1s
std::this_thread::sleep_for(std::chrono::milliseconds(10000));
std::cout << "===Closing the Dynamixel Motor communication====" << std::endl;