roboticlab1/main.cpp

#include <iostream>
#include <chrono>
#include <thread>
#include "DynamixelHandler.h"

// Global variables
DynamixelHandler _oDxlHandler;
std::string _poppyDxlPortName = "/dev/ttyUSB1";
float _poppyDxlProtocol = 2.0;
int _poppyDxlBaudRate = 1000000;
int _nbJoints = 6;
float _minJointCmd = 0;
float _maxJointCmd = 1023;
float _minJointAngle = -180.0f;
float _maxJointAngle = 180.0f;

int convertAnglesToJointCmd(float fJointAngle)
{ 
	// y = ax + b
	float a = (_maxJointCmd-_minJointCmd) / (_maxJointAngle - _minJointAngle);
	float b = _minJointCmd - a * _minJointAngle;
	float jointCmd = a * fJointAngle + b;
	return (int)jointCmd;
}

void goToHomePosition()
{ 
	std::vector<uint16_t> l_vTargetJointPosition;
	for (int l_joint = 0; l_joint < _nbJoints; l_joint++)
	l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
	_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
}

void JointPosition()
{
	// read current joint position
	std::vector<uint16_t> l_vCurrentJointPosition;
	_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
	// display current joint position
	std::cout <<"CurrentJointPosition= (" << std::endl;
	for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++)
		std::cout << l_vCurrentJointPosition[l_joint] << ", ";
}


//void gripperControl()
//{
	//std::vector<uint16_t> l_vTargetJointPosition;
	//for (int l_joint = 5; l_joint;)
	//l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
	//_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
//}

//void torqueControl()
//{
	//read the current torque
//	std::vector<uint16_t> l_vCurrentJointTorque;
//	int i =0;
//	while (i<30)
//	{
//		l_vCurrentJointTorque.clear();
//		_oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
		
		//display torque
//		std::cout <<"CurrentJointTorque= (" << std::endl;
//		std::cout << l_vCurrentJointTorque[5] << std::endl;
//		std::cout << ")" << std::endl;
//		std::this_thread::sleep_for(std::chrono::milliseconds(1000));
//		i++;
//	}
//}

int main()
{
	std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
	_oDxlHandler.setDeviceName(_poppyDxlPortName);
	_oDxlHandler.setProtocolVersion(_poppyDxlProtocol);
	_oDxlHandler.openPort();
	_oDxlHandler.setBaudRate(_poppyDxlBaudRate);
	_oDxlHandler.enableTorque(false);
	std::cout << std::endl;
	
	std::this_thread::sleep_for(std::chrono::milliseconds(1000));
	
	//JointPosition();
	//torqueControl();
	
	//read angle
	//std::vector<uint16_t> l_vCurrentJointPosition;
	//_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
	//std::cout << l_vCurrentJointPosition[5] << std::endl;
	//read torque
	//std::vector<uint16_t> l_vCurrentJointTorque;
	//_oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
	//std::cout << l_vCurrentJointTorque[5] << std::endl;
		
	float targetTorque = 50;
	float p = -0.03;
	std::vector<uint16_t> l_vCurrentJointTorque;
	
	bool bKeepLooping = true;
	while (bKeepLooping)
	{
		l_vCurrentJointTorque.clear();
		_oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
		
		if (l_vCurrentJointTorque.size() == 6)
		{
			float currentTorque = (float)l_vCurrentJointTorque[5];
			float torqueDiff = targetTorque - currentTorque;
			currentTorque = torqueDiff * p;


			std::vector<uint16_t> l_vCurrentJointPosition;
			_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
			
			if (l_vCurrentJointPosition.size() == 6)
			{
				std::vector<uint16_t> l_vTargetJointPosition;
				for (int l_joint = 0; l_joint < _nbJoints; l_joint++)
					l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));

				l_vTargetJointPosition[5]=(uint16_t)l_vCurrentJointPosition[5]+currentTorque;

				//Print torque different
				std::cout << torqueDiff << ", " << (uint16_t)currentTorque << ", " << l_vTargetJointPosition[5]<< std::endl;


				//display torque
				//std::cout <<"CurrentJointTorque= (" << std::endl;
				//std::cout << l_vCurrentJointTorque[5] << std::endl;
				//std::cout << ")" << std::endl;
				//std::this_thread::sleep_for(std::chrono::milliseconds(1000));
				
				_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);

				std::this_thread::sleep_for(std::chrono::milliseconds(1000));

				if (l_vTargetJointPosition[5] > 500)
				{
					bKeepLooping  = false;
					// wait 1s
					//std::this_thread::sleep_for(std::chrono::milliseconds(1000));
					//std::cout << "===Closing the Dynamixel Motor communication====" << std::endl;
					//_oDxlHandler.enableTorque(false);
					//_oDxlHandler.closePort();
					//std::cout << std::endl;
				}
				
				
			}
			

			
		}
		
	}

	//goToHomePosition();
	
	std::this_thread::sleep_for(std::chrono::milliseconds(5000));
	std::cout << "===Closing the Dynamixel Motor communication====" << std::endl;
	_oDxlHandler.enableTorque(false);
	_oDxlHandler.closePort();
	std::cout << std::endl;
	
	
	
	
	return 0;
}