gabriel.lucas
/
poppyMusician
1
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

move the poppy up and down

This commit is contained in:
Gabri6 2024-02-22 12:57:57 +01:00
parent 2567cae354
commit 9edde290d6
1 changed files with 24 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
main.cpp
View File

@ -26,6 +26,14 @@ int convertAnglesToJointCmd(float fJointAngle)
return (int)jointCmd;
}
int convertJointCmdToAngles(float fJointCmd)
{ // y = ax + b
float a = (_maxJointCmd-_minJointCmd) / (_maxJointAngle - _minJointAngle);
float b = _minJointCmd - a * _minJointAngle;
float fJointAngle = (fJointCmd - b)/a;
return (int)fJointAngle;
}
void goToHomePosition()
{
std::vector<uint16_t> l_vTargetJointPosition;
@ -34,6 +42,18 @@ void goToHomePosition()
_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
}
void goToPlayingPosition()
{
std::vector<uint16_t> l_vTargetJointPosition;
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(90.0f));
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
}
void currentPos()
{
// read current joint position
@ -109,11 +129,14 @@ int gripperControl()
//l_vTargetJointPosition.push_back(convertAnglesToJointCmd(45.0f));
l_vTargetJointPosition.push_back(targetJoint6Position);
_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
std::cout << convertJointCmdToAngles(targetJoint6Position) << std::endl;
}
return (int)joint6Torque;
}
//mettre pince à convertAnglesToJointCmd(38.0f) (à revérifier si ça ne change pas avec un poppy différent)
int main()
{
@ -127,7 +150,7 @@ int main()
currentPos();
goToHomePosition();
goToPlayingPosition();
currentPos();