From 96b584abfca2bc939a29ad9fa129df3a3a5de66f Mon Sep 17 00:00:00 2001
From: Gabri6 <gabriel.lucas.64100@gmail.com>
Date: Tue, 19 Mar 2024 16:48:39 +0100
Subject: [PATCH] new function to chose notes to play with the terminal

---
 main.cpp | 205 +++++++++++++++++++++++++++++++++++++++++--------------
 1 file changed, 152 insertions(+), 53 deletions(-)

diff --git a/main.cpp b/main.cpp
index abd470d..bfe1eb7 100644
--- a/main.cpp
+++ b/main.cpp
@@ -4,6 +4,8 @@
 //#include<windows.h>//for windows
 #include<unistd.h>//for linux
 #include "DynamixelHandler.h"
+#include <array>
+
 
 // Global variables
 DynamixelHandler _oDxlHandler;
@@ -19,6 +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}}};
+//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
@@ -52,21 +58,71 @@ void goToPlayingPosition()
 	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)
+	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;
+	
+	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()
@@ -140,70 +196,71 @@ int closeGripper()
 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(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));
+			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(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));
+			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(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));
+			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(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));
+			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(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));
+			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(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));
+			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(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));
+			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(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));
+			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(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));
+			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));
@@ -213,13 +270,22 @@ 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(convertAnglesToJointCmd(35.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
 }
 
+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);
@@ -228,19 +294,52 @@ 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();
-	goToPlayingPosition();goToPlayingPosition();goToPlayingPosition();goToPlayingPosition();goToPlayingPosition();goToPlayingPosition();
 	currentPos();
-	playNote(1);
-	playNote(2);
+	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));