ly.pechvattana
/
roboticlab1
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

still not working

This commit is contained in:
Ly PECHVATTANA 2023-03-10 18:08:34 +07:00
parent 277ff5bf2c
commit 88281440ac
1 changed files with 39 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
main.cpp
View File

@ -5,7 +5,7 @@
// Global variables // Global variables
DynamixelHandler _oDxlHandler; DynamixelHandler _oDxlHandler;
std::string _poppyDxlPortName = "/dev/ttyUSB0"; std::string _poppyDxlPortName = "/dev/ttyUSB1";
float _poppyDxlProtocol = 2.0; float _poppyDxlProtocol = 2.0;
int _poppyDxlBaudRate = 1000000; int _poppyDxlBaudRate = 1000000;
int _nbJoints = 6; int _nbJoints = 6;
@ -51,24 +51,24 @@ void JointPosition()
//_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition); //_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
//} //}
void torqueControl() //void torqueControl()
{ //{
//read the current torque //read the current torque
std::vector<uint16_t> l_vCurrentJointTorque; // std::vector<uint16_t> l_vCurrentJointTorque;
int i =0; // int i =0;
while (i<30) // while (i<30)
{ // {
l_vCurrentJointTorque.clear(); // l_vCurrentJointTorque.clear();
_oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque); // _oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
//display torque //display torque
std::cout <<"CurrentJointTorque= (" << std::endl; // std::cout <<"CurrentJointTorque= (" << std::endl;
std::cout << l_vCurrentJointTorque[5] << std::endl; // std::cout << l_vCurrentJointTorque[5] << std::endl;
std::cout << ")" << std::endl; // std::cout << ")" << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // std::this_thread::sleep_for(std::chrono::milliseconds(1000));
i++; // i++;
} // }
} //}
int main() int main()
{ {
@ -77,16 +77,25 @@ int main()
_oDxlHandler.setProtocolVersion(_poppyDxlProtocol); _oDxlHandler.setProtocolVersion(_poppyDxlProtocol);
_oDxlHandler.openPort(); _oDxlHandler.openPort();
_oDxlHandler.setBaudRate(_poppyDxlBaudRate); _oDxlHandler.setBaudRate(_poppyDxlBaudRate);
_oDxlHandler.enableTorque(true); _oDxlHandler.enableTorque(false);
std::cout << std::endl; std::cout << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(1000)); std::this_thread::sleep_for(std::chrono::milliseconds(1000));
//JointPosition(); //JointPosition();
//torqueControl(); //torqueControl();
//read angle
//std::vector<uint16_t> l_vCurrentJointPosition;
//_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
//std::cout << l_vCurrentJointPosition[5] << std::endl;
//read torque
//std::vector<uint16_t> l_vCurrentJointTorque;
//_oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
//std::cout << l_vCurrentJointTorque[5] << std::endl;
float targetTorque = 200; float targetTorque = 50;
float p = 0.4; float p = -0.03;
std::vector<uint16_t> l_vCurrentJointTorque; std::vector<uint16_t> l_vCurrentJointTorque;
bool bKeepLooping = true; bool bKeepLooping = true;
@ -97,7 +106,7 @@ int main()
if (l_vCurrentJointTorque.size() == 6) if (l_vCurrentJointTorque.size() == 6)
{ {
float currentTorque = l_vCurrentJointTorque[5]; float currentTorque = (float)l_vCurrentJointTorque[5];
float torqueDiff = targetTorque - currentTorque; float torqueDiff = targetTorque - currentTorque;
currentTorque = torqueDiff * p; currentTorque = torqueDiff * p;
@ -109,24 +118,25 @@ int main()
{ {
std::vector<uint16_t> l_vTargetJointPosition; std::vector<uint16_t> l_vTargetJointPosition;
for (int l_joint = 0; l_joint < _nbJoints; l_joint++) for (int l_joint = 0; l_joint < _nbJoints; l_joint++)
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0)); l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
l_vTargetJointPosition[5]=l_vCurrentJointPosition[5]+currentTorque; l_vTargetJointPosition[5]=(uint16_t)l_vCurrentJointPosition[5]+currentTorque;
//Print torque different //Print torque different
std::cout << torqueDiff << ", " << currentTorque << ", " << l_vTargetJointPosition[5]<< std::endl; std::cout << torqueDiff << ", " << (uint16_t)currentTorque << ", " << l_vTargetJointPosition[5]<< std::endl;
_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
//display torque //display torque
//std::cout <<"CurrentJointTorque= (" << std::endl; //std::cout <<"CurrentJointTorque= (" << std::endl;
//std::cout << l_vCurrentJointTorque[5] << std::endl; //std::cout << l_vCurrentJointTorque[5] << std::endl;
//std::cout << ")" << std::endl; //std::cout << ")" << std::endl;
//std::this_thread::sleep_for(std::chrono::milliseconds(1000)); //std::this_thread::sleep_for(std::chrono::milliseconds(1000));
_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
if (torqueDiff < 100) std::this_thread::sleep_for(std::chrono::milliseconds(1000));
if (l_vTargetJointPosition[5] > 500)
{ {
bKeepLooping = false; bKeepLooping = false;
// wait 1s // wait 1s
@ -136,6 +146,8 @@ int main()
//_oDxlHandler.closePort(); //_oDxlHandler.closePort();
//std::cout << std::endl; //std::cout << std::endl;
} }
} }