AngularVelocityTest.cpp

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


using namespace std;
// Dependencies: CommunicationLayerWindows.dll, CommandLayerWindows.dll
//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"); //This is the oddest syntax ever. Look this up.

	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]; //neato.
		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 )
		/*---------------Snippit of code from kinova.h that describes the different types of positions
		enum POSITION_TYPE
		{
		NOMOVEMENT_POSITION = 0,    /*!< Used for initialisation. 
		CARTESIAN_POSITION = 1,     /*!< A cartesian position described by a translation X, Y, Z and an orientation ThetaX, thetaY and ThetaZ. 
			ANGULAR_POSITION = 2,       /*!< An angular position described by a value for each actuator. 
			RETRACTED = 3,              /*!< The robotic arm is in retracted mode. It may be anywhere between the HOME position and the RETRACTED position. 
			PREDEFINED1 = 4,   			/*!< The robotic arm is moving to the pre defined position #1. 
			PREDEFINED2 = 5,   			/*!< The robotic arm is moving to the pre defined position #2. 
			PREDEFINED3 = 6, 			/*!< The robotic arm is moving to the pre defined position #3. 
			CARTESIAN_VELOCITY = 7,     /*!< A velocity vector used for velocity control. 
			ANGULAR_VELOCITY = 8,       /*!< Used for initialisation. 
			PREDEFINED4 = 9,  			/*!< The robotic arm is moving to the pre defined position #4. 
			PREDEFINED5 = 10,  			/*!< The robotic arm is moving to the pre defined position #5. 
			ANY_TRAJECTORY = 11,        /*!< Not used. 
			TIME_DELAY = 12,            /*!< The robotic arm is on time delay. 
	};
        ---------------------------------*/
		MySetActiveDevice(list[1]); //Right ARM
		//Find out if you can use hardcoded serial numbers so there is no ambiguity
		TrajectoryPoint pointToSend; //important piece
		pointToSend.InitStruct();  // important piece
		pointToSend.Position.Type = ANGULAR_VELOCITY; //Important piece
		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 7 at 100? degrees per second.
		
		
		for (int i = 0; i < 50; i++) {
			MySendBasicTrajectory(pointToSend);
			Sleep(5);
		}
		
		cout << "Moving HOME" << endl;
		//MyMoveHome();
		result = (*MyCloseAPI)();
		programResult = 1;
	}
	FreeLibrary(commandLayer_handle);
	return programResult;
}