From 9edde290d6573e9bd845e379f4bfa377865ff2fc Mon Sep 17 00:00:00 2001
From: Gabri6 <starling1220@gmail.com>
Date: Thu, 22 Feb 2024 12:57:57 +0100
Subject: [PATCH] move the poppy up and down

---
 main.cpp | 25 ++++++++++++++++++++++++-
 1 file changed, 24 insertions(+), 1 deletion(-)

diff --git a/main.cpp b/main.cpp
index ad5a490..6f60650 100644
--- a/main.cpp
+++ b/main.cpp
@@ -26,6 +26,14 @@ int convertAnglesToJointCmd(float fJointAngle)
 	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;
@@ -34,6 +42,18 @@ void goToHomePosition()
 	_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
 }
 
+void goToPlayingPosition()
+{
+	std::vector<uint16_t> l_vTargetJointPosition;
+	l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
+	l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
+	l_vTargetJointPosition.push_back(convertAnglesToJointCmd(90.0f));
+	l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
+	l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
+	l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
+	_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
+}
+
 void currentPos()
 {
 	// read current joint position
@@ -109,11 +129,14 @@ int gripperControl()
 		//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;
 }
 
+//mettre pince à convertAnglesToJointCmd(38.0f) (à revérifier si ça ne change pas avec un poppy différent)
+
 
 int main()
 { 
@@ -127,7 +150,7 @@ int main()
 	
 	currentPos();
 		
-	goToHomePosition();
+	goToPlayingPosition();
 		
 	currentPos();