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

f*** another mistake

This commit is contained in:
Gabri6 2024-03-21 09:11:38 +01:00
parent 20522b0a2f
commit 1758e05d11
1 changed files with 133 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

141
main.cpp
View File

@ -21,9 +21,10 @@ 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}}};
int waitTime = 0;
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, 310, 485, 515, 665, 592}};
std::array<short unsigned int, 6> 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<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(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<short unsigned int,6> currentPos()
@ -97,6 +102,13 @@ void teachTarget()
std::cin >> noteNumber;
while(noteNumber != 555)
{
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<uint16_t> l_vCurrentJointPosition;
@ -111,11 +123,6 @@ void teachTarget()
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::cin >> noteNumber;
}
}
@ -196,6 +203,7 @@ int closeGripper()
void playNote(int myNote)
{
goToPlayingPosition();
std::this_thread::sleep_for(std::chrono::milliseconds(200));
std::vector<uint16_t> 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<<c<<std::endl;
playNote((c - '0')+1);
}
goToPlayingPosition();
}
void changeWaitTime()
{
int myTime = 0;
std::cout << std::endl;
std::cout << "write the wait Time between each note in milliseconds" << std::endl;
std::cin >> 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<uint16_t> 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<uint16_t> 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<uint16_t> 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<uint16_t> 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<<" "<<std::endl;
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;
@ -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"<<std::endl;
std::cout<<"Write 'stop' to stop"<<std::endl;
std::cin >> 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"<<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 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"<<std::endl;
std::cout<<"Write 'stop' to stop"<<std::endl;
std::cin >> 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"<<std::endl;
std::cout<<"Write 'stop' to stop"<<std::endl;
std::cin >> myMusic;
@ -333,7 +440,7 @@ int main()
else
{
playMultipleNotes(myMusic);
std::cout<<" "<<std::endl;
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;