WindowsExample_CartesianControl.cpp

#include <Windows.h>
#include "CommunicationLayerWindows.h"
#include "CommandLayer.h"
#include <conio.h>
#include "KinovaTypes.h"
#include <iostream>


using namespace std;

//A handle to the API.
HINSTANCE commandLayer_handle;

//Function pointers to the functions we need
int(*MyInitAPI)();
int(*MyCloseAPI)();
int(*MySendBasicTrajectory)(TrajectoryPoint command);
int(*MyGetDevices)(KinovaDevice devices[MAX_KINOVA_DEVICE], int &result);
int(*MySetActiveDevice)(KinovaDevice device);
int(*MyMoveHome)();
int(*MyInitFingers)();
int(*MyGetCartesianCommand)(CartesianPosition &);

int main(int argc, char* argv[])
{
	//We load the API.
	commandLayer_handle = LoadLibrary(L"CommandLayerWindows.dll");

	CartesianPosition currentCommand;
	CartesianPosition homePosition;

	int programResult = 0;

	//We load the functions from the library (Under Windows, use GetProcAddress)
	MyInitAPI = (int(*)()) GetProcAddress(commandLayer_handle, "InitAPI");
	MyCloseAPI = (int(*)()) GetProcAddress(commandLayer_handle, "CloseAPI");
	MyMoveHome = (int(*)()) GetProcAddress(commandLayer_handle, "MoveHome");
	MyInitFingers = (int(*)()) GetProcAddress(commandLayer_handle, "InitFingers");
	MyGetDevices = (int(*)(KinovaDevice devices[MAX_KINOVA_DEVICE], int &result)) GetProcAddress(commandLayer_handle, "GetDevices");
	MySetActiveDevice = (int(*)(KinovaDevice devices)) GetProcAddress(commandLayer_handle, "SetActiveDevice");
	MySendBasicTrajectory = (int(*)(TrajectoryPoint)) GetProcAddress(commandLayer_handle, "SendBasicTrajectory");
	MyGetCartesianCommand = (int(*)(CartesianPosition &)) GetProcAddress(commandLayer_handle, "GetCartesianCommand");

	//Verify that all functions has been loaded correctly
	if ((MyInitAPI == NULL) || (MyCloseAPI == NULL) || (MySendBasicTrajectory == NULL) ||
		(MyGetDevices == NULL) || (MySetActiveDevice == NULL) || (MyGetCartesianCommand == NULL) ||
		(MyMoveHome == NULL) || (MyInitFingers == NULL))

	{
		cout << "* * *  E R R O R   D U R I N G   I N I T I A L I Z A T I O N  * * *" << endl;
		programResult = 0;
	}
	else
	{
		cout << "I N I T I A L I Z A T I O N   C O M P L E T E D" << endl << endl;

		int result = (*MyInitAPI)();

		cout << "Initialization's result :" << result << endl;

		KinovaDevice list[MAX_KINOVA_DEVICE];

		int devicesCount = MyGetDevices(list, result);

		

			//Setting the current device as the active device.
			MySetActiveDevice(list[1]);

			TrajectoryPoint pointToSend;
			pointToSend.InitStruct();

			pointToSend.Position.Type = CARTESIAN_POSITION;
			
			MyGetCartesianCommand(currentCommand);

			cout << "Current detected home position:" << endl;
			
			cout << currentCommand.Coordinates.X << "," 
				 << currentCommand.Coordinates.Y << ","
				 << currentCommand.Coordinates.Z << ","
				 << currentCommand.Coordinates.ThetaX << "," 
				 << currentCommand.Coordinates.ThetaY << ","
				 << currentCommand.Coordinates.ThetaZ << endl;

		
			//Set homePosition with desired home that we saved in a text file.
			homePosition.Coordinates.X = 0.212143;
			homePosition.Coordinates.Y = -0.266295;
			homePosition.Coordinates.Z = 0.504676;
			homePosition.Coordinates.ThetaX = 1.64361;
			homePosition.Coordinates.ThetaY = 1.10555;
			homePosition.Coordinates.ThetaZ = 0.133669;

			
			pointToSend.Position.CartesianPosition.X = homePosition.Coordinates.X;
			pointToSend.Position.CartesianPosition.Y = homePosition.Coordinates.Y;
			pointToSend.Position.CartesianPosition.Z = homePosition.Coordinates.Z;
			pointToSend.Position.CartesianPosition.ThetaX = homePosition.Coordinates.ThetaX;
			pointToSend.Position.CartesianPosition.ThetaY = homePosition.Coordinates.ThetaY;
			pointToSend.Position.CartesianPosition.ThetaZ = homePosition.Coordinates.ThetaZ;

			

			//We get the actual angular command of the robot.
			//MyGetCartesianCommand(currentCommand);

			pointToSend.Position.CartesianPosition.X = currentCommand.Coordinates.X;
			pointToSend.Position.CartesianPosition.Y = currentCommand.Coordinates.Y;
			pointToSend.Position.CartesianPosition.Z = currentCommand.Coordinates.Z;
			pointToSend.Position.CartesianPosition.ThetaX = currentCommand.Coordinates.ThetaX;
			pointToSend.Position.CartesianPosition.ThetaY = currentCommand.Coordinates.ThetaY;
			pointToSend.Position.CartesianPosition.ThetaZ = currentCommand.Coordinates.ThetaZ + 1.0;

			cout << "*********************************" << endl;
			cout << "Sending the first point to the robot." << endl;
			MySendBasicTrajectory(pointToSend);

			Sleep(15000);

			pointToSend.Position.CartesianPosition.ThetaZ = 0.133669f;
			cout << "Sending the second point to the robot." << endl;
			MySendBasicTrajectory(pointToSend);

			cout << "*********************************" << endl << endl << endl;
		

		cout << endl << "C L O S I N G   A P I" << endl;
		result = (*MyCloseAPI)();
		programResult = 1;
	}

	FreeLibrary(commandLayer_handle);

	return programResult;

}