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

159
main.cpp
View File

@ -21,9 +21,10 @@ int _targetJointTorqueGripper = 1140;
float _proportionnalIncrement = 0.025f; float _proportionnalIncrement = 0.025f;
float _proportionnalIncrementAbove = 0.01f; float _proportionnalIncrementAbove = 0.01f;
float _threshold= 1.9; 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}; //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) int convertAnglesToJointCmd(float fJointAngle)
@ -53,13 +54,17 @@ void goToHomePosition()
void goToPlayingPosition() void goToPlayingPosition()
{ {
std::vector<uint16_t> l_vTargetJointPosition; 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(0.0f));
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(-45.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(0.0f)); l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(45.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) 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() std::array<short unsigned int,6> currentPos()
@ -98,25 +103,27 @@ void teachTarget()
while(noteNumber != 555) 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 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; 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); //gives Hardware error _oDxlHandler.enableTorque(true); //gives Hardware error
@ -196,6 +203,7 @@ int closeGripper()
void playNote(int myNote) void playNote(int myNote)
{ {
goToPlayingPosition(); goToPlayingPosition();
std::this_thread::sleep_for(std::chrono::milliseconds(200)); std::this_thread::sleep_for(std::chrono::milliseconds(200));
std::vector<uint16_t> l_vTargetJointPosition; std::vector<uint16_t> l_vTargetJointPosition;
switch(myNote){ switch(myNote){
@ -263,16 +271,16 @@ void playNote(int myNote)
l_vTargetJointPosition.push_back(notes_position[8][4]); l_vTargetJointPosition.push_back(notes_position[8][4]);
break; break;
default: default:
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(25.0f)); l_vTargetJointPosition.push_back(playingPosition[0]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(-55.0f)); l_vTargetJointPosition.push_back(playingPosition[1]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); l_vTargetJointPosition.push_back(playingPosition[2]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); l_vTargetJointPosition.push_back(playingPosition[3]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(20.0f)); l_vTargetJointPosition.push_back(playingPosition[4]);
break; 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); _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) void playMultipleNotes(std::string myMusic)
@ -282,6 +290,79 @@ void playMultipleNotes(std::string myMusic)
std::cout<<c<<std::endl; std::cout<<c<<std::endl;
playNote((c - '0')+1); 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() int main()
@ -310,7 +391,7 @@ int main()
teachTarget(); teachTarget();
std::string myMusic = ""; 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 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::cout<<"Write 'stop' to stop"<<std::endl;
std::cin >> myMusic; std::cin >> myMusic;
@ -319,6 +400,7 @@ int main()
if (myMusic == "home") if (myMusic == "home")
{ {
goToHomePosition(); 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 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::cout<<"Write 'stop' to stop"<<std::endl;
std::cin >> myMusic; std::cin >> myMusic;
@ -326,6 +408,31 @@ int main()
else if (myMusic == "start") else if (myMusic == "start")
{ {
goToPlayingPosition(); 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 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::cout<<"Write 'stop' to stop"<<std::endl;
std::cin >> myMusic; std::cin >> myMusic;
@ -333,7 +440,7 @@ int main()
else else
{ {
playMultipleNotes(myMusic); 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 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::cout<<"Write 'stop' to stop"<<std::endl;
std::cin >> myMusic; std::cin >> myMusic;