diff --git a/main.cpp b/main.cpp index bfe1eb7..fcd27d3 100644 --- a/main.cpp +++ b/main.cpp @@ -21,9 +21,10 @@ int _targetJointTorqueGripper = 1140; float _proportionnalIncrement = 0.025f; float _proportionnalIncrementAbove = 0.01f; float _threshold= 1.9; -std::array,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}}}; +int waitTime = 0; +std::array,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 playingPosition = {{511, 310, 485, 515, 665, 592}}; +std::array playingPosition = {{511, 410, 515, 515, 685, 589}}; //511, 310, 485, 515, 665, 592 int convertAnglesToJointCmd(float fJointAngle) @@ -53,13 +54,17 @@ void goToHomePosition() void goToPlayingPosition() { std::vector 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 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(35.0f)); //mettre pince à convertAnglesToJointCmd(38.0f) (à revérifier si ça ne change pas avec un poppy différent) - _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition); + _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);*/ } std::array currentPos() @@ -98,25 +103,27 @@ void teachTarget() while(noteNumber != 555) { - // read current joint position - std::vector 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<> 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::cin >> noteNumber; } + if (noteNumber != 555) + { + // read current joint position + std::vector 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<> noteNumber; + } } _oDxlHandler.enableTorque(true); //gives Hardware error @@ -196,6 +203,7 @@ int closeGripper() void playNote(int myNote) { goToPlayingPosition(); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); std::vector l_vTargetJointPosition; switch(myNote){ @@ -263,16 +271,16 @@ void playNote(int myNote) l_vTargetJointPosition.push_back(notes_position[8][4]); break; default: - l_vTargetJointPosition.push_back(convertAnglesToJointCmd(25.0f)); - l_vTargetJointPosition.push_back(convertAnglesToJointCmd(-55.0f)); - l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); - l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); - l_vTargetJointPosition.push_back(convertAnglesToJointCmd(20.0f)); + l_vTargetJointPosition.push_back(playingPosition[0]); + l_vTargetJointPosition.push_back(playingPosition[1]); + l_vTargetJointPosition.push_back(playingPosition[2]); + l_vTargetJointPosition.push_back(playingPosition[3]); + l_vTargetJointPosition.push_back(playingPosition[4]); break; } - l_vTargetJointPosition.push_back(convertAnglesToJointCmd(35.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 + std::this_thread::sleep_for(std::chrono::milliseconds(waitTime)); //time period in milliseconds } void playMultipleNotes(std::string myMusic) @@ -282,6 +290,79 @@ void playMultipleNotes(std::string myMusic) std::cout<> myTime; + waitTime = myTime; +} + +void glissendo() +{ + //good idea, but not working so much, might need to make a linear displacement instead of a joint one (but too much work for this project) + std::string upOrDown = "u"; + + std::cout << std::endl; + std::cout << "Do you want a glissendo upwards or downwards ? u/d" << std::endl; + std::cin >> upOrDown; + + if (upOrDown == "u" || upOrDown == "U") + { + std::vector Target1JointPosition; + Target1JointPosition.push_back(notes_position[0][0]); + Target1JointPosition.push_back(notes_position[0][1]); + Target1JointPosition.push_back(notes_position[0][2]); + Target1JointPosition.push_back(notes_position[0][3]); + Target1JointPosition.push_back(notes_position[0][4]); + Target1JointPosition.push_back(playingPosition[5]); + + _oDxlHandler.sendTargetJointPosition(Target1JointPosition); + std::this_thread::sleep_for(std::chrono::milliseconds(150)); + + std::vector Target2JointPosition; + Target2JointPosition.push_back(notes_position[5][0]); + Target2JointPosition.push_back(notes_position[5][1]); + Target2JointPosition.push_back(notes_position[5][2]); + Target2JointPosition.push_back(notes_position[5][3]); + Target2JointPosition.push_back(notes_position[5][4]); + Target2JointPosition.push_back(playingPosition[5]); + + _oDxlHandler.sendTargetJointPosition(Target2JointPosition); + } + else if (upOrDown == "d" || upOrDown == "D") + { + std::vector Target1JointPosition; + Target1JointPosition.push_back(notes_position[5][0]); + Target1JointPosition.push_back(notes_position[5][1]); + Target1JointPosition.push_back(notes_position[5][2]); + Target1JointPosition.push_back(notes_position[5][3]); + Target1JointPosition.push_back(notes_position[5][4]); + Target1JointPosition.push_back(playingPosition[5]); + + _oDxlHandler.sendTargetJointPosition(Target1JointPosition); + std::this_thread::sleep_for(std::chrono::milliseconds(150)); + + std::vector Target2JointPosition; + Target2JointPosition.push_back(notes_position[0][0]); + Target2JointPosition.push_back(notes_position[0][1]); + Target2JointPosition.push_back(notes_position[0][2]); + Target2JointPosition.push_back(notes_position[0][3]); + Target2JointPosition.push_back(notes_position[0][4]); + Target2JointPosition.push_back(playingPosition[5]); + + _oDxlHandler.sendTargetJointPosition(Target2JointPosition); + } + else + { + std::cout << "write only u for upwards, or d for downards" << std::endl; + } } int main() @@ -310,7 +391,7 @@ int main() teachTarget(); std::string myMusic = ""; - std::cout<<" "<> myMusic; @@ -319,6 +400,7 @@ int main() 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"<> myMusic; @@ -326,6 +408,31 @@ int main() 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"<> 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"<> myMusic; + } + else if (myMusic == "wait") + { + changeWaitTime(); + std::cout << std::endl; + std::cout<<"Write the notes you want to play (from 0 to 8), without spaces between them"<> myMusic; + } + else if (myMusic == "glissendo") + { + glissendo(); + std::cout << std::endl; std::cout<<"Write the notes you want to play (from 0 to 8), without spaces between them"<> myMusic; @@ -333,7 +440,7 @@ int main() else { playMultipleNotes(myMusic); - std::cout<<" "<> myMusic;