camille.conjat
/
IntroRoboticsLab1
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

lab fijnisze

This commit is contained in:
Camille CONJAT 2023-03-09 12:02:06 +01:00
parent 5670602d22
commit 9930e47577
2 changed files with 58 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
main.cpp
View File

@ -27,6 +27,21 @@ l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
}
void gripperControl (float newPos)
{
std::vector<uint16_t> l_vTargetJointPosition;
l_vTargetJointPosition.clear();
for (int l_joint = 0; l_joint < _nbJoints; l_joint++)
{
if (l_joint==5){
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(newPos));
}
else {
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);
@ -43,6 +58,47 @@ 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;
goToHomePosition();
// wait 1s
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// read current joint position
l_vCurrentJointPosition.clear();
_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
// display current joint position
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;
//#####################################################################
// wait 1s
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
std::vector<uint16_t> l_vCurrentJointTorque;
// display current joint torque
int targetTorque = 1080;
float angularDelta;
int newPos = 0;
float epsilon;
float p = 0.01f;
for (int samples=0; samples<50;samples++){
l_vCurrentJointTorque.clear();
_oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
std::cout << "vCurrentJointTorque= (" << std::endl;
for (int l_joint=0; l_joint < l_vCurrentJointTorque.size(); l_joint++)
std::cout << l_vCurrentJointTorque[l_joint] << ", ";
std::cout << ")" << std::endl;
epsilon = targetTorque - l_vCurrentJointTorque[5];
angularDelta = epsilon*p;
newPos += angularDelta;
std::cout << newPos << "newPos" << epsilon << " epsilon" << angularDelta << " delate" << std::endl;
gripperControl(newPos);
// wait 1s
std::this_thread::sleep_for(std::chrono::milliseconds(50));
}
//#####################################################################
// wait 1s
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
std::cout << "===Closing the Dynamixel Motor communication====" << std::endl;
@ -50,21 +106,4 @@ _oDxlHandler.enableTorque(false);
_oDxlHandler.closePort();
std::cout << std::endl;
return 0;
goToHomePosition();
// 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;
// read current joint position
std::vector<uint16_t> l_vCurrentJointPosition;
_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
// display current joint position
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;
}

5
makefile
View File

@ -1,12 +1,11 @@
all : dynamixel main
g++ DynamixelHandler.o main.o –L/usr/local/lib -ldxl_x64_cpp -lrt
g++ DynamixelHandler.o main.o -L/usr/local/lib -ldxl_x64_cpp -lrt
dynamixel : DynamixelHandler.cpp
g++ -c -I/home/ros/SOFTWARE/toolkit-dynamixel/include /home/ros/SOFTWARE/toolkit-dynamixel/src/DynamixelHandler.cpp
main : main.cpp
g++ -c -I/home/ros/SOFTWARE/toolkit-dynamixel/include main.cpp
g++ -c main.cpp -I/home/ros/SOFTWARE/toolkit-dynamixel/
clean :
rm * .o
rm * .out
rm * .exe