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

added a function allowing the poppy to press each note individually when given the number of the note

This commit is contained in:
Gabri6 2024-03-11 13:41:44 +01:00
parent 9edde290d6
commit 83ee7d0e71
1 changed files with 96 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

108
main.cpp
View File

@ -1,6 +1,8 @@
#include <iostream>
#include <chrono>
#include <thread>
//#include<windows.h>//for windows
#include<unistd.h>//for linux
#include "DynamixelHandler.h"
// Global variables
@ -46,11 +48,11 @@ 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(-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);
}
@ -61,13 +63,13 @@ void currentPos()
_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
// 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;
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;
}
int gripperControl()
int closeGripper()
{
//read current joint 6 torque
std::vector<uint16_t> l_vCurrentJointTorque;
@ -135,8 +137,86 @@ int gripperControl()
return (int)joint6Torque;
}
//mettre pince à convertAnglesToJointCmd(38.0f) (à revérifier si ça ne change pas avec un poppy différent)
void playNote(int myNote)
{
goToPlayingPosition();
std::vector<uint16_t> l_vTargetJointPosition;
switch(myNote){
case 1:
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));
break;
case 2:
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(-35.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));
break;
case 3:
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));
break;
case 4:
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));
break;
case 5:
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));
break;
case 6:
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));
break;
case 7:
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));
break;
case 8:
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));
break;
case 9:
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));
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));
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)
_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
sleep(1); //time period in seconds
}
int main()
{
@ -154,9 +234,13 @@ int main()
currentPos();
int gripperTorque = gripperControl();
//int gripperTorque = closeGripper();
goToHomePosition();
goToPlayingPosition();
goToPlayingPosition();goToPlayingPosition();goToPlayingPosition();goToPlayingPosition();goToPlayingPosition();goToPlayingPosition();
currentPos();
playNote(1);
playNote(2);
// wait 1s
std::this_thread::sleep_for(std::chrono::milliseconds(1000));