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

integrating closed loop sys

This commit is contained in:
Ly PECHVATTANA 2023-03-10 13:56:31 +07:00
parent 844bcd06a2
commit f7fd04552c
1 changed files with 57 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
main.cpp
View File

@ -31,17 +31,8 @@ void goToHomePosition()
_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
}
int main()
void newJointPosition()
{
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;
// read current joint position
std::vector<uint16_t> l_vCurrentJointPosition;
_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
// display current joint position
@ -49,6 +40,62 @@ int main()
for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++)
std::cout << l_vCurrentJointPosition[l_joint] << ", ";
std::cout << ")" << std::endl;
}
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);
}
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;
// 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;
//read the current torque
int targetTorque = 0;
int p = 2;
std::vector<uint16_t> l_vCurrentJointTorque;
for(int l =0;l<20;l++)
{
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));
//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));
}
//goToHomePosition();
//newJointPosition();
// wait 1s
std::this_thread::sleep_for(std::chrono::milliseconds(1000));