//////////////////////////////////////////////////////
///////  OWI Robot Navigation and Control Kit  ///////
///////     Version 0.1, December 28, 1999     ///////
///////             Alan Mishchenko            ///////
//////////////////////////////////////////////////////

#include 
#include 

#include "Navigation.h"
#include "control.h"

main()
{
	StopMotion();
	
	Calibrate();

	CARTESIAN StartPosition = { 0,  28, 26 };
	CARTESIAN StopPosition  = { 20, 20,  8 };

	if ( Move( StartPosition, StopPosition ) )
		printf( "\n\nThe destination point is outside the arm's reach!\n\n" ); 

	SetMotion( 'G', 1 );
	ContinueMotion( 3000 );
	StopMotion();

	CARTESIAN StartPosition2 = { 20, 20,  8 };
	CARTESIAN StopPosition2  = { 20, 20,  2 };

	if ( Move( StartPosition2, StopPosition2 ) )
		printf( "\n\nThe destination point is outside the arm's reach!\n\n" ); 

	SetMotion( 'G', 0 );
	ContinueMotion( 3000 );
	StopMotion();

	CARTESIAN StartPosition3 = { 20, 20,  2 };
	CARTESIAN StopPosition3  = {-20, 20, 26 };

	if ( Move( StartPosition3, StopPosition3 ) )
		printf( "\n\nThe destination point is outside the arm's reach!\n\n" ); 

	SetMotion( 'G', 1 );
	ContinueMotion( 500 );
	StopMotion();

	SetMotion( 'G', 0 );
	ContinueMotion( 3000 );
	StopMotion();

	CARTESIAN StartPosition4 = {-20, 20, 26 };
	CARTESIAN StopPosition4  = { 0,  28, 26 };

	if ( Move( StartPosition4, StopPosition4 ) )
		printf( "\n\nThe destination point is outside the arm's reach!\n\n" ); 

	return 0;
}