final

main.cpp

@@ -27,8 +27,51 @@ l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
 _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
 }
 
+float convertJointCmdToAngle(int jointCmd) {
+    const float jointCmdRange = 65535.0f; // range of the joint command (0 to 65535)
+    const float angleRange = 360.0f; // range of the joint angle (-180 to 180)
+    float jointCmdMapped = (jointCmd / jointCmdRange) * angleRange; // map the joint command value to the angle range
+    if (jointCmdMapped > 180.0f) {
+        jointCmdMapped -= 360.0f; // ensure that the angle is between -180 and 180
+    }
+    return jointCmdMapped;
+}
+
+
+void gripperControl(float targetAngle)
+{
+    const int jointIndex = 5; // index of the 6th motor (starts from 0)
+    const float kP = 0.1f; // proportional gain
+
+    // Read current joint torque
+    std::vector<uint16_t> l_vCurrentJointTorque;
+    _oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
+
+    // Calculate error and proportional control
+    float error = targetAngle - convertJointCmdToAngle(l_vCurrentJointTorque[jointIndex]);
+    float jointCmd = convertAnglesToJointCmd(convertJointCmdToAngle(l_vCurrentJointTorque[jointIndex]) + kP * error);
+
+    // Set the target joint position for the 6th motor
+    std::vector<uint16_t> l_vTargetJointPosition;
+    for (int l_joint = 0; l_joint < _nbJoints; l_joint++) {
+        if (l_joint == jointIndex) {
+            l_vTargetJointPosition.push_back(jointCmd);
+        }
+        else {
+            l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
+        }
+    }
+    _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
+}
+
+
+
+
+
 int main()
-{ std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
+{
+
+    std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
 _oDxlHandler.setDeviceName(_poppyDxlPortName);
 _oDxlHandler.setProtocolVersion(_poppyDxlProtocol);
 _oDxlHandler.openPort();
@@ -45,6 +88,11 @@ std::cout << l_vCurrentJointPosition[l_joint] << ", ";
 std::cout << ")" << std::endl;
 // wait 1s
 std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+goToHomePosition();
+std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+gripperControl(l_vCurrentJointPosition[5]);
+std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+
 std::cout << "===Closing the Dynamixel Motor communication====" << std::endl;
 _oDxlHandler.enableTorque(false);
 _oDxlHandler.closePort();