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