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

added feature to teach points to the poppy, with torque disabled, before going to taught positions

This commit is contained in:
Gabri6 2024-03-14 13:00:31 +01:00
parent 0e1bad417d
commit a6039b22af
1 changed files with 90 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

137
main.cpp
View File

@ -4,6 +4,8 @@
//#include<windows.h>//for windows //#include<windows.h>//for windows
#include<unistd.h>//for linux #include<unistd.h>//for linux
#include "DynamixelHandler.h" #include "DynamixelHandler.h"
#include <array>
// Global variables // Global variables
DynamixelHandler _oDxlHandler; DynamixelHandler _oDxlHandler;
@ -19,6 +21,9 @@ 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}}};
//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};
int convertAnglesToJointCmd(float fJointAngle) int convertAnglesToJointCmd(float fJointAngle)
{ // y = ax + b { // y = ax + b
@ -69,6 +74,41 @@ void currentPos()
std::cout << ")" << std::endl; std::cout << ")" << std::endl;
} }
void teachTarget()
{
_oDxlHandler.enableTorque(false);
int noteNumber = 333;
std::cout << "enter 555 when all the notes have been taught\n";
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 != 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
{ std::cout << "wrong number: enter note number (between 0 and 9), or exit with 555:\n";
std::cin >> noteNumber;
}
}
_oDxlHandler.enableTorque(true);
}
int closeGripper() int closeGripper()
{ {
//read current joint 6 torque //read current joint 6 torque
@ -140,70 +180,71 @@ int closeGripper()
void playNote(int myNote) void playNote(int myNote)
{ {
goToPlayingPosition(); goToPlayingPosition();
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){
case 1: case 1:
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(25.0f)); l_vTargetJointPosition.push_back(notes_position[0][0]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(-55.0f)); l_vTargetJointPosition.push_back(notes_position[0][1]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); l_vTargetJointPosition.push_back(notes_position[0][2]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); l_vTargetJointPosition.push_back(notes_position[0][3]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(20.0f)); l_vTargetJointPosition.push_back(notes_position[0][4]);
break; break;
case 2: case 2:
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(-35.0f)); l_vTargetJointPosition.push_back(notes_position[1][0]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(-55.0f)); l_vTargetJointPosition.push_back(notes_position[1][1]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); l_vTargetJointPosition.push_back(notes_position[1][2]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); l_vTargetJointPosition.push_back(notes_position[1][3]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(20.0f)); l_vTargetJointPosition.push_back(notes_position[1][4]);
break; break;
case 3: case 3:
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(25.0f)); l_vTargetJointPosition.push_back(notes_position[2][0]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(-55.0f)); l_vTargetJointPosition.push_back(notes_position[2][1]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); l_vTargetJointPosition.push_back(notes_position[2][2]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); l_vTargetJointPosition.push_back(notes_position[2][3]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(20.0f)); l_vTargetJointPosition.push_back(notes_position[2][4]);
break; break;
case 4: case 4:
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(25.0f)); l_vTargetJointPosition.push_back(notes_position[3][0]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(-55.0f)); l_vTargetJointPosition.push_back(notes_position[3][1]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); l_vTargetJointPosition.push_back(notes_position[3][2]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); l_vTargetJointPosition.push_back(notes_position[3][3]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(20.0f)); l_vTargetJointPosition.push_back(notes_position[3][4]);
break; break;
case 5: case 5:
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(25.0f)); l_vTargetJointPosition.push_back(notes_position[4][0]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(-55.0f)); l_vTargetJointPosition.push_back(notes_position[4][1]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); l_vTargetJointPosition.push_back(notes_position[4][2]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); l_vTargetJointPosition.push_back(notes_position[4][3]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(20.0f)); l_vTargetJointPosition.push_back(notes_position[4][4]);
break; break;
case 6: case 6:
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(25.0f)); l_vTargetJointPosition.push_back(notes_position[5][0]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(-55.0f)); l_vTargetJointPosition.push_back(notes_position[5][1]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); l_vTargetJointPosition.push_back(notes_position[5][2]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); l_vTargetJointPosition.push_back(notes_position[5][3]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(20.0f)); l_vTargetJointPosition.push_back(notes_position[5][4]);
break; break;
case 7: case 7:
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(25.0f)); l_vTargetJointPosition.push_back(notes_position[6][0]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(-55.0f)); l_vTargetJointPosition.push_back(notes_position[6][1]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); l_vTargetJointPosition.push_back(notes_position[6][2]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); l_vTargetJointPosition.push_back(notes_position[6][3]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(20.0f)); l_vTargetJointPosition.push_back(notes_position[6][4]);
break; break;
case 8: case 8:
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(25.0f)); l_vTargetJointPosition.push_back(notes_position[7][0]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(-55.0f)); l_vTargetJointPosition.push_back(notes_position[7][1]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); l_vTargetJointPosition.push_back(notes_position[7][2]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); l_vTargetJointPosition.push_back(notes_position[7][3]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(20.0f)); l_vTargetJointPosition.push_back(notes_position[7][4]);
break; break;
case 9: case 9:
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(25.0f)); l_vTargetJointPosition.push_back(notes_position[8][0]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(-55.0f)); l_vTargetJointPosition.push_back(notes_position[8][1]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); l_vTargetJointPosition.push_back(notes_position[8][2]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f)); l_vTargetJointPosition.push_back(notes_position[8][3]);
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(20.0f)); l_vTargetJointPosition.push_back(notes_position[8][4]);
break; break;
default: default:
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(25.0f)); l_vTargetJointPosition.push_back(convertAnglesToJointCmd(25.0f));
@ -219,7 +260,7 @@ void playNote(int myNote)
} }
int main() int main()
{ {
std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl; std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
_oDxlHandler.setDeviceName(_poppyDxlPortName); _oDxlHandler.setDeviceName(_poppyDxlPortName);
_oDxlHandler.setProtocolVersion(_poppyDxlProtocol); _oDxlHandler.setProtocolVersion(_poppyDxlProtocol);
@ -237,10 +278,12 @@ int main()
//int gripperTorque = closeGripper(); //int gripperTorque = closeGripper();
goToPlayingPosition(); goToPlayingPosition();
goToPlayingPosition();goToPlayingPosition();goToPlayingPosition();goToPlayingPosition();goToPlayingPosition();goToPlayingPosition();
currentPos(); currentPos();
teachTarget();
playNote(1); playNote(1);
playNote(2); playNote(2);
playNote(3);
goToPlayingPosition();
// wait 1s // wait 1s
std::this_thread::sleep_for(std::chrono::milliseconds(1000)); std::this_thread::sleep_for(std::chrono::milliseconds(1000));