From 0e1bad417dde8fc40d9d740f300eaaed9ca2fde4 Mon Sep 17 00:00:00 2001
From: Gabri6 <starling1220@gmail.com>
Date: Thu, 7 Mar 2024 12:50:37 +0100
Subject: [PATCH] new playNote function, taking int and giving joint angles to
 the poppy

---
 main.cpp | 87 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 87 insertions(+)

diff --git a/main.cpp b/main.cpp
index 2e2f90a..abd470d 100644
--- a/main.cpp
+++ b/main.cpp
@@ -1,6 +1,8 @@
 #include <iostream>
 #include <chrono>
 #include <thread>
+//#include<windows.h>//for windows
+#include<unistd.h>//for linux
 #include "DynamixelHandler.h"
 
 // Global variables
@@ -135,6 +137,87 @@ int closeGripper()
 	return (int)joint6Torque;
 }
 
+void playNote(int myNote)
+{
+	goToPlayingPosition();
+	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));
+			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));
+			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));
+			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));
+			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));
+			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));
+			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));
+			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));
+			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));
+			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(38.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
+}
+
 int main()
 { 
 	std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
@@ -154,6 +237,10 @@ int main()
 	//int gripperTorque = closeGripper();
 	
 	goToPlayingPosition();
+	goToPlayingPosition();goToPlayingPosition();goToPlayingPosition();goToPlayingPosition();goToPlayingPosition();goToPlayingPosition();
+	currentPos();
+	playNote(1);
+	playNote(2);
 	
 	// wait 1s
 	std::this_thread::sleep_for(std::chrono::milliseconds(1000));