353 lines
13 KiB
C++
353 lines
13 KiB
C++
#include <iostream>
|
||
#include <chrono>
|
||
#include <thread>
|
||
//#include<windows.h>//for windows
|
||
#include<unistd.h>//for linux
|
||
#include "DynamixelHandler.h"
|
||
#include <array>
|
||
|
||
|
||
// Global variables
|
||
DynamixelHandler _oDxlHandler;
|
||
std::string _poppyDxlPortName = "/dev/ttyUSB0";
|
||
float _poppyDxlProtocol = 2.0;
|
||
int _poppyDxlBaudRate = 1000000;
|
||
int _nbJoints = 6;
|
||
float _minJointCmd = 0;
|
||
float _maxJointCmd = 1023;
|
||
float _minJointAngle = -180.0f;
|
||
float _maxJointAngle = 180.0f;
|
||
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}}};
|
||
//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}};
|
||
|
||
|
||
int convertAnglesToJointCmd(float fJointAngle)
|
||
{ // y = ax + b
|
||
float a = (_maxJointCmd-_minJointCmd) / (_maxJointAngle - _minJointAngle);
|
||
float b = _minJointCmd - a * _minJointAngle;
|
||
float jointCmd = a * fJointAngle + b;
|
||
return (int)jointCmd;
|
||
}
|
||
|
||
int convertJointCmdToAngles(float fJointCmd)
|
||
{ // y = ax + b
|
||
float a = (_maxJointCmd-_minJointCmd) / (_maxJointAngle - _minJointAngle);
|
||
float b = _minJointCmd - a * _minJointAngle;
|
||
float fJointAngle = (fJointCmd - b)/a;
|
||
return (int)fJointAngle;
|
||
}
|
||
|
||
void goToHomePosition()
|
||
{
|
||
std::vector<uint16_t> l_vTargetJointPosition;
|
||
for (int l_joint = 0; l_joint < _nbJoints; l_joint++)
|
||
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
|
||
_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
|
||
}
|
||
|
||
void goToPlayingPosition()
|
||
{
|
||
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(35.0f)); //mettre pince <20> convertAnglesToJointCmd(38.0f) (<28> rev<65>rifier si <20>a ne change pas avec un poppy diff<66>rent)
|
||
_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
|
||
}
|
||
|
||
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;
|
||
|
||
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); //gives Hardware error
|
||
std::cout<<"===Enabling Torque==="<<std::endl;
|
||
|
||
return ;
|
||
}
|
||
|
||
int closeGripper()
|
||
{
|
||
//read current joint 6 torque
|
||
std::vector<uint16_t> l_vCurrentJointTorque;
|
||
_oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
|
||
|
||
//read current joint 6 position
|
||
std::vector<uint16_t> l_vCurrentJointPosition;
|
||
_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
|
||
int joint6Position = l_vCurrentJointPosition[5];
|
||
|
||
// display current joint 6 torque
|
||
std::cout << "CurrentJoint6Torque= ";
|
||
std::cout << l_vCurrentJointTorque[5] << std::endl;
|
||
int joint6Torque = l_vCurrentJointTorque[5];
|
||
|
||
int jointTorqueDiff = joint6Torque - _targetJointTorqueGripper;
|
||
float angleIncrement = -jointTorqueDiff * _proportionnalIncrement;
|
||
std::cout<< "increment= " << angleIncrement<< std::endl;
|
||
float targetJoint6Position = joint6Position + angleIncrement;
|
||
std::cout << "joint torque diff= " <<abs(jointTorqueDiff) << std::endl;
|
||
|
||
while (abs(angleIncrement) > _threshold)
|
||
{
|
||
|
||
//read current joint 6 torque
|
||
std::vector<uint16_t> l_vCurrentJointTorque;
|
||
_oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
|
||
|
||
//read current joint 6 position
|
||
std::vector<uint16_t> l_vCurrentJointPosition;
|
||
_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
|
||
int joint6Position = l_vCurrentJointPosition[5];
|
||
|
||
// display current joint 6 torque
|
||
std::cout << "vCurrentJoint6Torque= ";
|
||
std::cout << l_vCurrentJointTorque[5] << std::endl;
|
||
int joint6Torque = l_vCurrentJointTorque[5];
|
||
|
||
int jointTorqueDiff = joint6Torque - _targetJointTorqueGripper;
|
||
if (jointTorqueDiff < 0)
|
||
{
|
||
float angleIncrement = -jointTorqueDiff * _proportionnalIncrement;
|
||
}
|
||
else{
|
||
float angleIncrement = 0;
|
||
}
|
||
std::cout<< "increment= " << angleIncrement<< std::endl;
|
||
float targetJoint6Position = joint6Position + angleIncrement;
|
||
std::cout << "joint torque diff= " <<jointTorqueDiff << std::endl;
|
||
|
||
//set the position of each servomotor to 0
|
||
std::vector<uint16_t> l_vTargetJointPosition;
|
||
for (int l_joint = 0; l_joint < _nbJoints-1; l_joint++)
|
||
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
|
||
//l_vTargetJointPosition.push_back(convertAnglesToJointCmd(90.0f));
|
||
//l_vTargetJointPosition.push_back(convertAnglesToJointCmd(90.0f));
|
||
//l_vTargetJointPosition.push_back(convertAnglesToJointCmd(-90.0f));
|
||
//l_vTargetJointPosition.push_back(convertAnglesToJointCmd(-90.0f));
|
||
//l_vTargetJointPosition.push_back(convertAnglesToJointCmd(45.0f));
|
||
l_vTargetJointPosition.push_back(targetJoint6Position);
|
||
_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
|
||
std::cout << convertJointCmdToAngles(targetJoint6Position) << std::endl;
|
||
}
|
||
|
||
return (int)joint6Torque;
|
||
}
|
||
|
||
void playNote(int myNote)
|
||
{
|
||
goToPlayingPosition();
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(200));
|
||
std::vector<uint16_t> l_vTargetJointPosition;
|
||
switch(myNote){
|
||
case 1:
|
||
l_vTargetJointPosition.push_back(notes_position[0][0]);
|
||
l_vTargetJointPosition.push_back(notes_position[0][1]);
|
||
l_vTargetJointPosition.push_back(notes_position[0][2]);
|
||
l_vTargetJointPosition.push_back(notes_position[0][3]);
|
||
l_vTargetJointPosition.push_back(notes_position[0][4]);
|
||
break;
|
||
case 2:
|
||
l_vTargetJointPosition.push_back(notes_position[1][0]);
|
||
l_vTargetJointPosition.push_back(notes_position[1][1]);
|
||
l_vTargetJointPosition.push_back(notes_position[1][2]);
|
||
l_vTargetJointPosition.push_back(notes_position[1][3]);
|
||
l_vTargetJointPosition.push_back(notes_position[1][4]);
|
||
break;
|
||
case 3:
|
||
l_vTargetJointPosition.push_back(notes_position[2][0]);
|
||
l_vTargetJointPosition.push_back(notes_position[2][1]);
|
||
l_vTargetJointPosition.push_back(notes_position[2][2]);
|
||
l_vTargetJointPosition.push_back(notes_position[2][3]);
|
||
l_vTargetJointPosition.push_back(notes_position[2][4]);
|
||
break;
|
||
case 4:
|
||
l_vTargetJointPosition.push_back(notes_position[3][0]);
|
||
l_vTargetJointPosition.push_back(notes_position[3][1]);
|
||
l_vTargetJointPosition.push_back(notes_position[3][2]);
|
||
l_vTargetJointPosition.push_back(notes_position[3][3]);
|
||
l_vTargetJointPosition.push_back(notes_position[3][4]);
|
||
break;
|
||
case 5:
|
||
l_vTargetJointPosition.push_back(notes_position[4][0]);
|
||
l_vTargetJointPosition.push_back(notes_position[4][1]);
|
||
l_vTargetJointPosition.push_back(notes_position[4][2]);
|
||
l_vTargetJointPosition.push_back(notes_position[4][3]);
|
||
l_vTargetJointPosition.push_back(notes_position[4][4]);
|
||
break;
|
||
case 6:
|
||
l_vTargetJointPosition.push_back(notes_position[5][0]);
|
||
l_vTargetJointPosition.push_back(notes_position[5][1]);
|
||
l_vTargetJointPosition.push_back(notes_position[5][2]);
|
||
l_vTargetJointPosition.push_back(notes_position[5][3]);
|
||
l_vTargetJointPosition.push_back(notes_position[5][4]);
|
||
break;
|
||
case 7:
|
||
l_vTargetJointPosition.push_back(notes_position[6][0]);
|
||
l_vTargetJointPosition.push_back(notes_position[6][1]);
|
||
l_vTargetJointPosition.push_back(notes_position[6][2]);
|
||
l_vTargetJointPosition.push_back(notes_position[6][3]);
|
||
l_vTargetJointPosition.push_back(notes_position[6][4]);
|
||
break;
|
||
case 8:
|
||
l_vTargetJointPosition.push_back(notes_position[7][0]);
|
||
l_vTargetJointPosition.push_back(notes_position[7][1]);
|
||
l_vTargetJointPosition.push_back(notes_position[7][2]);
|
||
l_vTargetJointPosition.push_back(notes_position[7][3]);
|
||
l_vTargetJointPosition.push_back(notes_position[7][4]);
|
||
break;
|
||
case 9:
|
||
l_vTargetJointPosition.push_back(notes_position[8][0]);
|
||
l_vTargetJointPosition.push_back(notes_position[8][1]);
|
||
l_vTargetJointPosition.push_back(notes_position[8][2]);
|
||
l_vTargetJointPosition.push_back(notes_position[8][3]);
|
||
l_vTargetJointPosition.push_back(notes_position[8][4]);
|
||
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(35.0f)); //mettre pince <20> convertAnglesToJointCmd(38.0f) (<28> rev<65>rifier si <20>a ne change pas avec un poppy diff<66>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;
|
||
_oDxlHandler.setDeviceName(_poppyDxlPortName);
|
||
_oDxlHandler.setProtocolVersion(_poppyDxlProtocol);
|
||
_oDxlHandler.openPort();
|
||
_oDxlHandler.setBaudRate(_poppyDxlBaudRate);
|
||
_oDxlHandler.enableTorque(true);
|
||
std::cout << std::endl;
|
||
|
||
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();
|
||
|
||
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<<"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<<"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
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
|
||
|
||
std::cout << "===Closing the Dynamixel Motor communication====" << std::endl;
|
||
_oDxlHandler.enableTorque(false);
|
||
_oDxlHandler.closePort();
|
||
std::cout << std::endl;
|
||
|
||
return 0;
|
||
} |