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

52
Robot_arm.cpp
View File

@ -33,6 +33,53 @@ void goToHomePosition()
_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)
{ {
cout<< state << endl; cout<< state << endl;
@ -62,8 +109,9 @@ int main()
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();