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

new playNote function, taking int and giving joint angles to the poppy

This commit is contained in:
Gabri6 2024-03-07 12:50:37 +01:00
parent ea6a0c2b1a
commit 0e1bad417d
1 changed files with 87 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
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
@ -135,6 +137,87 @@ int closeGripper()
return (int)joint6Torque;
}
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()
{
std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
@ -154,6 +237,10 @@ int main()
//int gripperTorque = closeGripper();
goToPlayingPosition();
goToPlayingPosition();goToPlayingPosition();goToPlayingPosition();goToPlayingPosition();goToPlayingPosition();goToPlayingPosition();
currentPos();
playNote(1);
playNote(2);
// wait 1s
std::this_thread::sleep_for(std::chrono::milliseconds(1000));