loic.delattre
/
IntroRoboticsLab1
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

testing torque

This commit is contained in:
Loïc DELATTRE 2023-03-10 12:00:00 +01:00
parent 701287b250
commit a1bb8e1eb3
1 changed files with 56 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
Robot_arm.cpp
View File

@ -27,10 +27,57 @@ void goToHomePosition()
{ {
vector<uint16_t> l_vTargetJointPosition; 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.0f)); l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
} }
_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition); _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
}
void gripperControl(){
while(true){
std::vector<uint16_t> l_vCurrentJointTorque;
_oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
int angle = 0;
int var = l_vCurrentJointTorque[5];
cout<<"initial torque value = "<<var<<endl;
while (var < 1100){
int var = l_vCurrentJointTorque[5];
vector<uint16_t> l_vTargetJointPosition;
for (int l_joint = 0; l_joint < (_nbJoints-1); l_joint++)
{
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
}
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(angle));
_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
std::this_thread::sleep_for(std::chrono::milliseconds(200));
_oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
std::this_thread::sleep_for(std::chrono::milliseconds(200));
cout<<"torque value: "<<l_vCurrentJointTorque[5]<<endl;
angle ++;
}
cout<<"done!!!!!!!!!!!"<<endl;
break;
}
}
void inputEndEffector(int Theta){
while(true){
cout<<"angle: "<<endl;
cin >> Theta;
if (Theta > -90 && Theta < 30){
vector<uint16_t> l_vTargetJointPosition;
for (int l_joint = 0; l_joint < (_nbJoints-1); l_joint++)
{
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
}
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(Theta));
_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
}
else{
cout<<"angle outisde of range";
}
}
} }
void print_position(const char* state) void print_position(const char* state)
@ -59,14 +106,15 @@ int main()
cout << endl; cout << endl;
std::this_thread::sleep_for(std::chrono::milliseconds(1000)); std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// display current joint position // display current joint position
print_position("=========BEFORE MOVING========"); print_position("=========BEFORE MOVING========");
goToHomePosition(); goToHomePosition();
print_position("=========AFTER MOVING========="); print_position("=========AFTER MOVING=========");
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
std::this_thread::sleep_for(std::chrono::milliseconds(5000)); //move only end effector angle
gripperControl();
cout << "===Closing the Dynamixel Motor communication====" << endl; cout << "===Closing the Dynamixel Motor communication====" << endl;
_oDxlHandler.enableTorque(false); _oDxlHandler.enableTorque(false);
_oDxlHandler.closePort(); _oDxlHandler.closePort();
cout << endl; cout << endl;
return 0; return 0;
} }