new examples

README.md

@@ -3,3 +3,22 @@
 ##requirements
 
 need a "libraries" folder in the parent folder, in which is located the Dynamixel-sdk library for C++, with the folder toolkit-dynamixel
+
+
+##usage
+
+plug the poppy to the computer and power it
+
+(compile the code if you need. You can use 'make' in Linux.)
+
+After going to playing position, the poppy will disable its torque.
+
+###First phase
+
+You can then move it around and have it register 9 positions (from 0 to 8) by putting the id of the position and pressing enter.
+it will then register its current position
+
+when you are finished, enter 555 and press enter.
+
+###Second phase
+

examples

@@ -0,0 +1,37 @@
+top gun: [c4, d4, e4, f4, g4, a4]
+0a4a4a32321a1a012a123a202a1
+
+interstellar: [c4, d4, e4, f4, g4, a4]
+04004014114124224234334304a04a14a14a24a24a34a34a
+
+hedwige theme: [c4, d4, e4, f4, g4, a4]
+1aa32a1aaa5a4aaaaa2aaaaa1aa32a0aaa2a
+
+frere jacques: [c4, d4, e4, f4, g4, a4]
+01200120234a234a45432a045432a0a010a010
+
+exorcist: [e5, f5, g5, a5, b5, c6]
+0304023050a04503040230304023
+
+star wars: [c4, d4, e4, f4, g4, c5]
+0a4a3215a4a3215a4a3231aaa0a4a3215a4a3215a4a3231
+
+twinkle twinkle little star: [c4, d4, e4, f4, g4, a4]
+0044554a3322110a4433221a4433221a0044554a3322110a
+
+au clair de la lune: [c4, d4, e4, f4, g4, a4]
+33345a4a35443aaa33345a4a45443aaa44441a1a43210aaa33345a4a35443aaa
+
+jingle bells: [c4, d4, e4, f4, g4, a4]
+222a222a24012aaa3333322221121a4a222a222a24012aaa3333322244310aaa
+
+my way: [c4, d4, e4, f4, g4, a4]
+05aaa50505aa50545a054433a205aaa50505aa50545a054433a
+
+gravity falls: [c4, d4, e4, f4, g4, a4]
+0002210a3331a4a2a5a
+
+game of thrones:
+3a0a123a0a123a0a123a0a123a0a423a0a423a0a423a0a423a0a123a0a125a
+
+5553aa4442aa5553334442225553334442aa

main.cpp

@@ -21,9 +21,10 @@ 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}}};
+int waitTime = 100;
+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, 310, 485, 515, 665, 592}};
+std::array<short unsigned int, 6> playingPosition = {{511, 410, 515, 515, 685, 589}}; //511, 310, 485, 515, 665, 592
 
 
 int convertAnglesToJointCmd(float fJointAngle)
@@ -53,13 +54,17 @@ 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(35.0f)); //mettre pince à convertAnglesToJointCmd(38.0f) (à revérifier si ça ne change pas avec un poppy différent)
-	_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
+	_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);*/
 }
 
 std::array<short unsigned int,6> currentPos()
@@ -98,29 +103,32 @@ void teachTarget()
 	
 	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;
 		}
+		if (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;
+		}
 	}
 	
 	_oDxlHandler.enableTorque(true); //gives Hardware error
 	std::cout<<"===Enabling Torque==="<<std::endl;
+	goToPlayingPosition();
 	
 	return ;
 }
@@ -195,84 +203,132 @@ int closeGripper()
 
 void playNote(int myNote)
 {
-	goToPlayingPosition();
+	//goToPlayingPosition();
-	std::this_thread::sleep_for(std::chrono::milliseconds(200));
+	
 	std::vector<uint16_t> l_vTargetJointPosition;
-	switch(myNote){
+	
-		case 1:
+	if (myNote >= 0 && myNote <= 9)
+	{
+		l_vTargetJointPosition.push_back(notes_position[myNote][0]);
+		l_vTargetJointPosition.push_back(notes_position[myNote][1]);
+		l_vTargetJointPosition.push_back(notes_position[myNote][2] + 200);
+		l_vTargetJointPosition.push_back(notes_position[myNote][3]);
+		l_vTargetJointPosition.push_back(notes_position[myNote][4]);
+		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);
+		
+		std::this_thread::sleep_for(std::chrono::milliseconds(75));
+		
+		std::vector<uint16_t> l_vTargetJoint1Position;
+		
+		l_vTargetJoint1Position.push_back(notes_position[myNote][0]);
+		l_vTargetJoint1Position.push_back(notes_position[myNote][1]);
+		l_vTargetJoint1Position.push_back(notes_position[myNote][2]);
+		l_vTargetJoint1Position.push_back(notes_position[myNote][3]);
+		l_vTargetJoint1Position.push_back(notes_position[myNote][4]);
+		l_vTargetJoint1Position.push_back(playingPosition[5]); //mettre pince à convertAnglesToJointCmd(38.0f) (à revérifier si ça ne change pas avec un poppy différent)
+		
+		_oDxlHandler.sendTargetJointPosition(l_vTargetJoint1Position);
+		
+		std::this_thread::sleep_for(std::chrono::milliseconds(waitTime)); //time period in milliseconds
+		
+		std::vector<uint16_t> l_vTargetJoint2Position;
+		
+		l_vTargetJoint2Position.push_back(notes_position[myNote][0]);
+		l_vTargetJoint2Position.push_back(notes_position[myNote][1]);
+		l_vTargetJoint2Position.push_back(notes_position[myNote][2] + 200);
+		l_vTargetJoint2Position.push_back(notes_position[myNote][3]);
+		l_vTargetJoint2Position.push_back(notes_position[myNote][4]);
+		l_vTargetJoint2Position.push_back(playingPosition[5]); //mettre pince à convertAnglesToJointCmd(38.0f) (à revérifier si ça ne change pas avec un poppy différent)
+		
+		_oDxlHandler.sendTargetJointPosition(l_vTargetJoint2Position);
+	}
+	else
+	{
+		goToPlayingPosition();
+		
+		std::this_thread::sleep_for(std::chrono::milliseconds(waitTime * 2)); //time period in milliseconds
+	}
+	/*switch(myNote){
+		case 0:
 			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:
+		case 1:
 			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:
+		case 2:
 			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:
+		case 3:
 			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:
+		case 4:
 			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:
+		case 5:
 			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:
+		case 6:
 			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:
+		case 7:
 			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:
+		case 8:
 			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:
+		case 9:
-			l_vTargetJointPosition.push_back(convertAnglesToJointCmd(25.0f));
+			l_vTargetJointPosition.push_back(notes_position[9][0]);
-			l_vTargetJointPosition.push_back(convertAnglesToJointCmd(-55.0f));
+			l_vTargetJointPosition.push_back(notes_position[9][1]);
-			l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
+			l_vTargetJointPosition.push_back(notes_position[9][2]);
-			l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
+			l_vTargetJointPosition.push_back(notes_position[9][3]);
-			l_vTargetJointPosition.push_back(convertAnglesToJointCmd(20.0f));
+			l_vTargetJointPosition.push_back(notes_position[9][4]);
 			break;
-	}
+		default:
-	l_vTargetJointPosition.push_back(convertAnglesToJointCmd(35.0f)); //mettre pince à convertAnglesToJointCmd(38.0f) (à revérifier si ça ne change pas avec un poppy différent)
+			l_vTargetJointPosition.push_back(playingPosition[0]);
-	_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
+			l_vTargetJointPosition.push_back(playingPosition[1]);
-	sleep(1); //time period in seconds
+			l_vTargetJointPosition.push_back(playingPosition[2]);
+			l_vTargetJointPosition.push_back(playingPosition[3]);
+			l_vTargetJointPosition.push_back(playingPosition[4]);
+			break;
+	}*/
 }
 
 void playMultipleNotes(std::string myMusic)
@@ -280,7 +336,100 @@ void playMultipleNotes(std::string myMusic)
 	for (char& c : myMusic)
 	{
 		std::cout<<c<<std::endl;
-		playNote((c - '0')+1);
+		playNote((c - '0'));
+	}
+	goToPlayingPosition();
+}
+
+void changeWaitTime()
+{
+	int myTime = 0;
+	
+	std::cout << std::endl;
+	std::cout << "write the wait Time between each note in milliseconds" << std::endl;
+	
+	std::cin >> myTime;
+	waitTime = myTime;
+}
+
+void glissendo()
+{
+	//good idea, but not working so much, might need to make a linear displacement instead of a joint one (but too much work for this project)
+	std::string upOrDown = "u";
+	
+	std::cout << std::endl;
+	std::cout << "Do you want a glissendo upwards or downwards ? u/d" << std::endl;
+	std::cin >> upOrDown;
+	
+	if (upOrDown == "u" || upOrDown == "U")
+	{
+		std::vector<uint16_t> Target1JointPosition;
+		Target1JointPosition.push_back(notes_position[0][0]);
+		Target1JointPosition.push_back(notes_position[0][1]);
+		Target1JointPosition.push_back(notes_position[0][2]);
+		Target1JointPosition.push_back(notes_position[0][3]);
+		Target1JointPosition.push_back(notes_position[0][4]);
+		Target1JointPosition.push_back(playingPosition[5]); 
+		
+		_oDxlHandler.sendTargetJointPosition(Target1JointPosition);
+		std::this_thread::sleep_for(std::chrono::milliseconds(150));
+		
+		std::vector<uint16_t> Target2JointPosition;
+		Target2JointPosition.push_back(notes_position[2][0]);
+		Target2JointPosition.push_back(notes_position[2][1]);
+		Target2JointPosition.push_back(notes_position[2][2]);
+		Target2JointPosition.push_back(notes_position[2][3]);
+		Target2JointPosition.push_back(notes_position[2][4]);
+		Target2JointPosition.push_back(playingPosition[5]); 
+		
+		_oDxlHandler.sendTargetJointPosition(Target2JointPosition);
+		
+		std::vector<uint16_t> Target3JointPosition;
+		Target3JointPosition.push_back(notes_position[5][0]);
+		Target3JointPosition.push_back(notes_position[5][1]);
+		Target3JointPosition.push_back(notes_position[5][2]);
+		Target3JointPosition.push_back(notes_position[5][3]);
+		Target3JointPosition.push_back(notes_position[5][4]);
+		Target3JointPosition.push_back(playingPosition[5]); 
+		
+		_oDxlHandler.sendTargetJointPosition(Target3JointPosition);
+	}
+	else if (upOrDown == "d" || upOrDown == "D")
+	{
+		std::vector<uint16_t> Target1JointPosition;
+		Target1JointPosition.push_back(notes_position[5][0]);
+		Target1JointPosition.push_back(notes_position[5][1]);
+		Target1JointPosition.push_back(notes_position[5][2]);
+		Target1JointPosition.push_back(notes_position[5][3]);
+		Target1JointPosition.push_back(notes_position[5][4]);
+		Target1JointPosition.push_back(playingPosition[5]); 
+		
+		_oDxlHandler.sendTargetJointPosition(Target1JointPosition);
+		std::this_thread::sleep_for(std::chrono::milliseconds(150));
+		
+		std::vector<uint16_t> Target2JointPosition;
+		Target2JointPosition.push_back(notes_position[2][0]);
+		Target2JointPosition.push_back(notes_position[2][1]);
+		Target2JointPosition.push_back(notes_position[2][2]);
+		Target2JointPosition.push_back(notes_position[2][3]);
+		Target2JointPosition.push_back(notes_position[2][4]);
+		Target2JointPosition.push_back(playingPosition[5]); 
+		
+		_oDxlHandler.sendTargetJointPosition(Target2JointPosition);
+		
+		std::vector<uint16_t> Target3JointPosition;
+		Target3JointPosition.push_back(notes_position[0][0]);
+		Target3JointPosition.push_back(notes_position[0][1]);
+		Target3JointPosition.push_back(notes_position[0][2]);
+		Target3JointPosition.push_back(notes_position[0][3]);
+		Target3JointPosition.push_back(notes_position[0][4]);
+		Target3JointPosition.push_back(playingPosition[5]); 
+		
+		_oDxlHandler.sendTargetJointPosition(Target3JointPosition);
+	}
+	else
+	{
+		std::cout << "write only u for upwards, or d for downards" << std::endl;
 	}
 }
 
@@ -310,7 +459,7 @@ int main()
 	teachTarget();
 	
 	std::string myMusic = "";
-	std::cout<<" "<<std::endl;
+	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;
@@ -319,6 +468,7 @@ int main()
 			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;
@@ -326,14 +476,48 @@ int main()
 			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 if (myMusic == "wait")
+			{
+				changeWaitTime();
+				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 == "glissendo")
+			{
+				glissendo();
+				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 == "demo")
+			{
+				playMultipleNotes("0a4a4a32321a1a012a123a202a1");
+				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 << 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;