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

fixed some bugs, and added teach instruction when in playing mode

This commit is contained in:
Gabri6 2024-03-20 16:50:01 +01:00
parent a6039b22af
commit 29a1a1bdf1
1 changed files with 97 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

122
main.cpp
View File

@ -21,8 +21,9 @@ int _targetJointTorqueGripper = 1140;
float _proportionnalIncrement = 0.025f;
float _proportionnalIncrementAbove = 0.01f;
float _threshold= 1.9;
std::array<std::array<short unsigned int, 6>,9> notes_position = {{{1, 2, 3, 4, 5, 6}, {2, 2, 3, 4, 5, 6}, {3, 2, 3, 4, 5, 6}, {4, 2, 3, 4, 5, 6}, {5, 2, 3, 4, 5, 6}, {6, 2, 3, 4, 5, 6}, {7, 2, 3, 4, 5, 6}, {8, 2, 3, 4, 5, 6}, {9, 2, 3, 4, 5, 6}}};
std::array<std::array<short unsigned int, 6>,9> notes_position = {{{511, 511, 511, 511, 511, 511}, {511, 511, 511, 511, 511, 511}, {511, 511, 511, 511, 511, 511}, {511, 511, 511, 511, 511, 511}, {511, 511, 511, 511, 511, 511}, {511, 511, 511, 511, 511, 511}, {511, 511, 511, 511, 511, 511}, {511, 511, 511, 511, 511, 511}, {511, 511, 511, 511, 511, 511}}};
//short unsigned int notes_position[54] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54};
std::array<short unsigned int, 6> playingPosition = {{511, 410, 515, 515, 685, 589}}; //511, 310, 485, 515, 665, 592
int convertAnglesToJointCmd(float fJointAngle)
@ -52,31 +53,46 @@ void goToHomePosition()
void goToPlayingPosition()
{
std::vector<uint16_t> l_vTargetJointPosition;
for (int l_joint = 0; l_joint < _nbJoints; l_joint++)
l_vTargetJointPosition.push_back(playingPosition[l_joint]);
_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
/*std::vector<uint16_t> l_vTargetJointPosition;
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(-45.0f));
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(45.0f));
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(38.0f)); //mettre pince à convertAnglesToJointCmd(38.0f) (à revérifier si ça ne change pas avec un poppy différent)
_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(35.0f)); //mettre pince à convertAnglesToJointCmd(38.0f) (à revérifier si ça ne change pas avec un poppy différent)
_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);*/
}
void currentPos()
std::array<short unsigned int,6> currentPos()
{
std::array<short unsigned int, 6> myPosition = {{0, 1, 2, 3, 4, 5}};
// read current joint position
std::vector<uint16_t> l_vCurrentJointPosition;
_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
//store current joint position to return it
for (int l_joint=0; l_joint < myPosition.size(); l_joint++)
{
myPosition[l_joint] = l_vCurrentJointPosition[l_joint];
}
// display current joint position
std::cout << "vCurrentJointPosition= (";
for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++)
std::cout << l_vCurrentJointPosition[l_joint] << ", ";
std::cout << ")" << std::endl;
return myPosition;
}
void teachTarget()
{
_oDxlHandler.enableTorque(false);
std::cout<<"===Disabling Torque==="<<std::endl;
int noteNumber = 333;
@ -86,27 +102,33 @@ void teachTarget()
while(noteNumber != 555)
{
// read current joint position
std::vector<uint16_t> l_vCurrentJointPosition;
_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
for (int l_joint=0; l_joint < notes_position[noteNumber].size(); l_joint++)
{
notes_position[noteNumber][l_joint] = l_vCurrentJointPosition[l_joint];
std::cout<<notes_position[noteNumber][l_joint]<<", ";
std::cout<<noteNumber << ", ";
}
currentPos(); //print position
std::cout << "enter note number (starting from 0):\n"; //allows to wait until user input, and specify which note is given
std::cin >> noteNumber;
while (noteNumber < 0 or (noteNumber > 9 and noteNumber != 555)) // test if the input is good
{ std::cout << "wrong number: enter note number (between 0 and 9), or exit with 555:\n";
{
std::cout << "wrong number: enter note number (between 0 and 9), or exit with 555:\n";
std::cin >> noteNumber;
}
if (noteNumber != 555)
{
// read current joint position
std::vector<uint16_t> l_vCurrentJointPosition;
_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
for (int l_joint=0; l_joint < notes_position[noteNumber].size(); l_joint++)
{
notes_position[noteNumber][l_joint] = l_vCurrentJointPosition[l_joint];
std::cout<<notes_position[noteNumber][l_joint]<<", ";
std::cout<<noteNumber << ", ";
}
currentPos(); //print position
std::cout << "enter note number (starting from 0):\n"; //allows to wait until user input, and specify which note is given
std::cin >> noteNumber;
}
}
_oDxlHandler.enableTorque(true);
_oDxlHandler.enableTorque(true); //gives Hardware error
std::cout<<"===Enabling Torque==="<<std::endl;
return ;
}
int closeGripper()
@ -254,11 +276,20 @@ void playNote(int myNote)
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(20.0f));
break;
}
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(38.0f)); //mettre pince à convertAnglesToJointCmd(38.0f) (à revérifier si ça ne change pas avec un poppy différent)
l_vTargetJointPosition.push_back(playingPosition[5]); //mettre pince à convertAnglesToJointCmd(38.0f) (à revérifier si ça ne change pas avec un poppy différent)
_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
sleep(1); //time period in seconds
}
void playMultipleNotes(std::string myMusic)
{
for (char& c : myMusic)
{
std::cout<<c<<std::endl;
playNote((c - '0')+1);
}
}
int main()
{
std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
@ -269,20 +300,61 @@ int main()
_oDxlHandler.enableTorque(true);
std::cout << std::endl;
currentPos();
std::cout<<"===Initalization of the position==="<<std::endl;
std::array<short unsigned int, 6> myPosition = currentPos(); //check position, to add a test later
goToPlayingPosition();
std::cout<<"===Going to playing position==="<<std::endl;
currentPos();
std::cout<<"===Position: OK==="<<std::endl;
//int gripperTorque = closeGripper();
goToPlayingPosition();
currentPos();
teachTarget();
playNote(1);
playNote(2);
playNote(3);
std::string myMusic = "";
std::cout << std::endl;
std::cout<<"Write the notes you want to play (from 0 to 8), without spaces between them"<<std::endl;
std::cout<<"Write 'stop' to stop"<<std::endl;
std::cin >> myMusic;
while (myMusic != "stop")
{
if (myMusic == "home")
{
goToHomePosition();
std::cout << std::endl;
std::cout<<"Write the notes you want to play (from 0 to 8), without spaces between them"<<std::endl;
std::cout<<"Write 'stop' to stop"<<std::endl;
std::cin >> myMusic;
}
else if (myMusic == "start")
{
goToPlayingPosition();
std::cout << std::endl;
std::cout<<"Write the notes you want to play (from 0 to 8), without spaces between them"<<std::endl;
std::cout<<"Write 'stop' to stop"<<std::endl;
std::cin >> myMusic;
}
else if (myMusic == "teach")
{
teachTarget();
std::cout << std::endl;
std::cout<<"Write the notes you want to play (from 0 to 8), without spaces between them"<<std::endl;
std::cout<<"Write 'stop' to stop"<<std::endl;
std::cin >> myMusic;
}
else
{
playMultipleNotes(myMusic);
std::cout << std::endl;
std::cout<<"Write the notes you want to play (from 0 to 8), without spaces between them"<<std::endl;
std::cout<<"Write 'stop' to stop"<<std::endl;
std::cin >> myMusic;
}
}
goToPlayingPosition();
// wait 1s