fixed some bugs, and added teach instruction when in playing mode
This commit is contained in:
Gabri6
parent
a6039b22af
commit
29a1a1bdf1
100
main.cpp
100
main.cpp
|
|
@ -21,8 +21,9 @@ 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}}};
|
||||
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, 410, 515, 515, 685, 589}}; //511, 310, 485, 515, 665, 592
|
||||
|
||||
|
||||
int convertAnglesToJointCmd(float fJointAngle)
|
||||
|
|
@ -52,31 +53,46 @@ 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(38.0f)); //mettre pince à convertAnglesToJointCmd(38.0f) (à revérifier si ça ne change pas avec un poppy différent)
|
||||
_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
|
||||
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);*/
|
||||
}
|
||||
|
||||
void currentPos()
|
||||
std::array<short unsigned int,6> currentPos()
|
||||
{
|
||||
std::array<short unsigned int, 6> myPosition = {{0, 1, 2, 3, 4, 5}};
|
||||
|
||||
// read current joint position
|
||||
std::vector<uint16_t> l_vCurrentJointPosition;
|
||||
_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
|
||||
|
||||
//store current joint position to return it
|
||||
for (int l_joint=0; l_joint < myPosition.size(); l_joint++)
|
||||
{
|
||||
myPosition[l_joint] = l_vCurrentJointPosition[l_joint];
|
||||
}
|
||||
|
||||
// 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;
|
||||
|
||||
return myPosition;
|
||||
}
|
||||
|
||||
void teachTarget()
|
||||
{
|
||||
_oDxlHandler.enableTorque(false);
|
||||
std::cout<<"===Disabling Torque==="<<std::endl;
|
||||
|
||||
int noteNumber = 333;
|
||||
|
||||
|
|
@ -85,6 +101,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;
|
||||
|
|
@ -99,14 +122,13 @@ 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;
|
||||
}
|
||||
}
|
||||
|
||||
_oDxlHandler.enableTorque(true);
|
||||
_oDxlHandler.enableTorque(true); //gives Hardware error
|
||||
std::cout<<"===Enabling Torque==="<<std::endl;
|
||||
|
||||
return ;
|
||||
}
|
||||
|
||||
int closeGripper()
|
||||
|
|
@ -254,11 +276,20 @@ void playNote(int myNote)
|
|||
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)
|
||||
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
|
||||
}
|
||||
|
||||
void playMultipleNotes(std::string myMusic)
|
||||
{
|
||||
for (char& c : myMusic)
|
||||
{
|
||||
std::cout<<c<<std::endl;
|
||||
playNote((c - '0')+1);
|
||||
}
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
|
||||
|
|
@ -269,20 +300,61 @@ int main()
|
|||
_oDxlHandler.enableTorque(true);
|
||||
std::cout << std::endl;
|
||||
|
||||
currentPos();
|
||||
std::cout<<"===Initalization of the position==="<<std::endl;
|
||||
std::array<short unsigned int, 6> myPosition = currentPos(); //check position, to add a test later
|
||||
|
||||
goToPlayingPosition();
|
||||
|
||||
std::cout<<"===Going to playing position==="<<std::endl;
|
||||
currentPos();
|
||||
std::cout<<"===Position: OK==="<<std::endl;
|
||||
|
||||
//int gripperTorque = closeGripper();
|
||||
|
||||
goToPlayingPosition();
|
||||
currentPos();
|
||||
teachTarget();
|
||||
playNote(1);
|
||||
playNote(2);
|
||||
playNote(3);
|
||||
|
||||
std::string myMusic = "";
|
||||
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;
|
||||
while (myMusic != "stop")
|
||||
{
|
||||
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;
|
||||
}
|
||||
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
|
||||
{
|
||||
playMultipleNotes(myMusic);
|
||||
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;
|
||||
}
|
||||
}
|
||||
goToPlayingPosition();
|
||||
|
||||
// wait 1s
|
||||
|
|
|
|||
Loading…
Reference in New Issue