WindowsExample_AngularControl.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(*MyGetAngularCommand)(AngularPosition &);

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

	int programResult = 0;
	//Initialise the function pointer from the API
	MyInitAPI = (int(*)()) GetProcAddress(commandLayer_handle, "InitAPI");
	MyCloseAPI = (int(*)()) GetProcAddress(commandLayer_handle, "CloseAPI");
	MyGetDevices = (int(*)(KinovaDevice[MAX_KINOVA_DEVICE], int&)) GetProcAddress(commandLayer_handle, "GetDevices");
	MySetActiveDevice = (int(*)(KinovaDevice)) GetProcAddress(commandLayer_handle, "SetActiveDevice");
	MySendBasicTrajectory = (int(*)(TrajectoryPoint)) GetProcAddress(commandLayer_handle, "SendBasicTrajectory");
	MyGetAngularCommand = (int(*)(AngularPosition &)) GetProcAddress(commandLayer_handle, "GetAngularCommand");
	MyMoveHome = (int(*)()) GetProcAddress(commandLayer_handle, "MoveHome");
	MyInitFingers = (int(*)()) GetProcAddress(commandLayer_handle, "InitFingers");

	//Verify that all functions has been loaded correctly
	if ((MyInitAPI == NULL) || (MyCloseAPI == NULL) || (MySendBasicTrajectory == NULL) ||
		(MyGetDevices == NULL) || (MySetActiveDevice == NULL) || (MyGetAngularCommand == 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)();		
		AngularPosition currentCommand;

		cout << "Initialization's result :" << result << endl;
		
		KinovaDevice list[MAX_KINOVA_DEVICE];
		int devicesCount = MyGetDevices(list, result);

		cout << "Found a robot on the USB bus (Left ARM: " << list[0].SerialNumber <<
			" Right ARM: " << list[1].SerialNumber << ")" << endl; //See if it holds true that the list is 
		//always instantiated in this order: (Left ARM: PJ00650019161750001 Right ARM: PJ00900006020921-0 )
		
		MySetActiveDevice(list[1]); //Right ARM
		TrajectoryPoint pointToSend;
		MyMoveHome();
		pointToSend.InitStruct();
		pointToSend.Position.Type = ANGULAR_VELOCITY;
		pointToSend.Position.Actuators.Actuator1 = 0;
		pointToSend.Position.Actuators.Actuator2 = 0;
		pointToSend.Position.Actuators.Actuator3 = 0;
		pointToSend.Position.Actuators.Actuator4 = 0;
		pointToSend.Position.Actuators.Actuator5 = 0;
		pointToSend.Position.Actuators.Actuator6 = 0;
		pointToSend.Position.Actuators.Actuator7 = 100; //joint 6 at 48 degrees per second.	
		for (int i = 0; i < 300; i++) {
			MySendBasicTrajectory(pointToSend);
			Sleep(5);
	}
		result = (*MyCloseAPI)();
		programResult = 1;
	}
	FreeLibrary(commandLayer_handle);
	return programResult;
}