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

Gripper does not stop

This commit is contained in:
Ly PECHVATTANA 2023-03-10 16:49:12 +07:00
parent f7fd04552c
commit 277ff5bf2c
1 changed files with 88 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

122
main.cpp
View File

@ -31,23 +31,43 @@ void goToHomePosition()
_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
}
void newJointPosition()
void JointPosition()
{
// read current joint position
std::vector<uint16_t> l_vCurrentJointPosition;
_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
// display current joint position
std::cout <<"vCurrentJointPosition= (" << std::endl;
std::cout <<"CurrentJointPosition= (" << std::endl;
for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++)
std::cout << l_vCurrentJointPosition[l_joint] << ", ";
std::cout << ")" << std::endl;
}
void gripperControl()
//void gripperControl()
//{
//std::vector<uint16_t> l_vTargetJointPosition;
//for (int l_joint = 5; l_joint;)
//l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
//_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
//}
void torqueControl()
{
std::vector<uint16_t> l_vTargetJointPosition;
for (int l_joint = 5; l_joint;)
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
//read the current torque
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++;
}
}
int main()
@ -57,53 +77,83 @@ int main()
_oDxlHandler.setProtocolVersion(_poppyDxlProtocol);
_oDxlHandler.openPort();
_oDxlHandler.setBaudRate(_poppyDxlBaudRate);
//_oDxlHandler.enableTorque(true);
_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 <<"CurrentJointPosition= (" << 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));
//read the current torque
int targetTorque = 0;
int p = 2;
//JointPosition();
//torqueControl();
float targetTorque = 200;
float p = 0.4;
std::vector<uint16_t> l_vCurrentJointTorque;
for(int l =0;l<20;l++)
bool bKeepLooping = true;
while (bKeepLooping)
{
l_vCurrentJointTorque.clear();
_oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
float currentTorque = l_vCurrentJointTorque[5];
float torqueDiff = targetTorque - currentTorque;
float newAngle = torqueDiff * p
std::vector<uint16_t> l_vTargetJointPosition;
for (int l_joint = 5, l_joint;)
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(newAngle));
if (l_vCurrentJointTorque.size() == 6)
{
float currentTorque = l_vCurrentJointTorque[5];
float torqueDiff = targetTorque - currentTorque;
currentTorque = torqueDiff * p;
//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));
std::vector<uint16_t> l_vCurrentJointPosition;
_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
if (l_vCurrentJointPosition.size() == 6)
{
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[5]=l_vCurrentJointPosition[5]+currentTorque;
//Print torque different
std::cout << torqueDiff << ", " << 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));
if (torqueDiff < 100)
{
bKeepLooping = false;
// wait 1s
//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;
}
}
}
}
//goToHomePosition();
//newJointPosition();
// wait 1s
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
std::this_thread::sleep_for(std::chrono::milliseconds(5000));
std::cout << "===Closing the Dynamixel Motor communication====" << std::endl;
_oDxlHandler.enableTorque(false);
_oDxlHandler.closePort();
std::cout << std::endl;
return 0;
}