IntroRoboticsLab1/Robot_arm.cpp

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

using namespace std;

// Global variables
DynamixelHandler _oDxlHandler;
string _poppyDxlPortName = "/dev/ttyUSB0";
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(int grip)
{
	vector<uint16_t> l_vTargetJointPosition;
        for (int l_joint = 0; l_joint < (_nbJoints-grip); l_joint++)
        {
		l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
	}
        _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
}


void gripperControltest(){
    while(true){
        int slot = 5;//slot 0 is for motor 1 etc
        vector<uint16_t> l_vCurrentJointTorque;
        _oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
        int angle = -10;
        int var = l_vCurrentJointTorque[slot];
        cout<<"initial torque value = "<<var<<endl;
                while (angle < 20){
                    vector<uint16_t> l_vTargetJointPosition;
                    vector<uint16_t> l_vCurrentJointTorque;
                    for (int l_joint = 0; l_joint < (_nbJoints-1); l_joint++)
                    {
                        l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
                    }
                    l_vTargetJointPosition.push_back(convertAnglesToJointCmd(angle));
                    _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
                    std::this_thread::sleep_for(std::chrono::milliseconds(200));

                    _oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
                    std::this_thread::sleep_for(std::chrono::milliseconds(200));

                    cout<<"torque value: "<<l_vCurrentJointTorque[slot]<<endl;
                    var = l_vCurrentJointTorque[slot];
                    if (var >= 1110){
                        break;
                    }
                    angle ++;
                }
                cout<<"done!!!!!!!!!!!"<<endl;
                break;
    }

}

//troque threshold at around 1110

void grabCube(){
    while(true){

        int slot = 5;//slot 0 is for motor 1 etc
        vector<uint16_t> l_vCurrentJointTorque;
        vector<uint16_t> l_vCurrentJointPosition;
        _oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
        _oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);

        int angle = -10;
        int var = l_vCurrentJointTorque[slot];
        cout<<"initial torque value = "<<var<<endl;
                while (angle < 20){
                    vector<uint16_t> l_vTargetJointPosition;
                    vector<uint16_t> l_vCurrentJointTorque;

                    for (int l_joint = 0; l_joint < (_nbJoints-1); l_joint++)
                    {
                        l_vTargetJointPosition.push_back(l_vCurrentJointPosition[l_joint]);
                    }

                    l_vTargetJointPosition.push_back(convertAnglesToJointCmd(angle));
                    _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
                    std::this_thread::sleep_for(std::chrono::milliseconds(200));

                    _oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
                    std::this_thread::sleep_for(std::chrono::milliseconds(200));

                    cout<<"torque value: "<<l_vCurrentJointTorque[slot]<<endl;
                    var = l_vCurrentJointTorque[slot];
                    if (var >= 1118){
                        break;
                    }
                    angle ++;
                }
                cout<<"done!!!!!!!!!!!"<<endl;
                break;
    }
}

void trianglePosition(){
    vector<uint16_t> l_vTargetJointPosition;
    vector<uint16_t> l_vCurrentJointPosition;
    int angle = 0;
    while (angle > -40){
        vector<uint16_t> l_vTargetJointPosition;
        l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));//for rotor 1
        l_vTargetJointPosition.push_back(convertAnglesToJointCmd(angle)); //for user controled rotor 2
        for (int l_joint = 2; l_joint < (_nbJoints); l_joint++)
        {
            l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
        }
        _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
        std::this_thread::sleep_for(std::chrono::milliseconds(200));
        angle = angle -1;
    }

}

void inputEndEffector(int Theta){
    while(true){
        cout<<"angle: "<<endl;
        cin >> Theta;
        if (Theta > -90 && Theta < 30){
            vector<uint16_t> l_vTargetJointPosition;
            for (int l_joint = 0; l_joint < (_nbJoints-1); l_joint++)
            {
                    l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
            }
            l_vTargetJointPosition.push_back(convertAnglesToJointCmd(Theta));
            _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
        }
        else{
            cout<<"angle outisde of range";
        }
    }
}

void print_position(const char* state)
{
	cout<< state << endl;
	std::this_thread::sleep_for(std::chrono::milliseconds(1000));
	// read current joint position
	vector<uint16_t> l_vCurrentJointPosition;
	_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
	cout << "vCurrentJointPosition= (" << endl;
	for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++)
		cout << l_vCurrentJointPosition[l_joint] << ", ";
		cout << ")" << endl;
	// wait 1s
	this_thread::sleep_for(chrono::milliseconds(1000));
}

int main()
{ 
	cout << "===Initialization of the Dynamixel Motor communication====" << endl;
	_oDxlHandler.setDeviceName(_poppyDxlPortName);
	_oDxlHandler.setProtocolVersion(_poppyDxlProtocol);
	_oDxlHandler.openPort();
	_oDxlHandler.setBaudRate(_poppyDxlBaudRate);
	_oDxlHandler.enableTorque(true);
	cout << endl;
	std::this_thread::sleep_for(std::chrono::milliseconds(1000));
	// display current joint position
        print_position("=========BEFORE MOVING========");
        goToHomePosition(0);
        trianglePosition();
        grabCube();
        goToHomePosition(1);
        print_position("=========AFTER MOVING=========");
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        //move only end effector angle

	cout << "===Closing the Dynamixel Motor communication====" << endl;
	_oDxlHandler.enableTorque(false);
	_oDxlHandler.closePort();
	cout << endl;
	return 0;
}