
#include <stdio.h>
#include <stdlib.h>
#include <iostream.h>
#include <math.h>
#include <termios.h>
#include <sys/sched.h>
#include <sys/types.h>
#include <unistd.h>

#include "driver/PrTimer.h"
#include "driver/PrSTGDefn.h"

#include "bldc.h"

#define DELAY_1MS		7159		// 1 ms delay if timer crystal is 7.159 MHz

#define HZ  1000

BLDC::BLDC()
{
	stg = new ServoToGo(STG_ARM_IRQ, STG_ARM_ADDRESS);

	for(int i=0;i<8;i++)
	{
		mAngleInit[i] = 0.0;
		angleOld[i] = 0.0;
		velocityOld[i] = 0.0;
	}

	openDataFile();
	initSystem();

}

BLDC::~BLDC()
{
	delete stg;
	closeDataFile();
}

void BLDC::initSystem(void)
{
	stg->EncoderInit();
	stg->EncoderLatch();
	stg->SetEncoderCounts(ELBOW_MINI,0);
	initIndex();
	torque = TORQUE_MAX;
}

void BLDC::initIndex(void)
{
	stg->SelectIndexOrExtLatch(0x00);
	stg->EnableCounterLatchOnIndexOrExt(0xff);
	resetIndex();
}

void BLDC::resetIndex(void)
{
	stg->ResetIndexLatches(0x00);
}

int BLDC::hitIndex(void)
{
	if(stg->GetIndexLatches() && (1<<ELBOW_MINI))
		return(1);
	else
		return(0);
}


void BLDC::openDataFile(void)
{
	fdata = fopen("datamotor.txt","w");
	fdata2 = fopen("datajoint.txt","w");
}

void BLDC::closeDataFile(void)
{
	fclose(fdata);
	fclose(fdata2);
}

void BLDC::readHET(void)
{
	HETStatus = stg->RawDI(PORTD) & HET_MASK;

	HAB = (HETStatus >> 2) & 0x01; 
	HBC = (HETStatus >> 1) & 0x01; 
	HCA = (HETStatus >> 0) & 0x01; 

}

void BLDC::initPosition(void)
{
	readPosition();
	for(int i=0;i<8;i++)
		encoderInit[i].Long = encoder[i].Long;
}

void BLDC::resetVelocity(void)
{
	int i;
	for(i=0;i<8;i++)
	{
		velocity[ELBOW_JOINT] = 0.0;
		velocityOld[ELBOW_JOINT] = 0.0;
	}
}

void BLDC::calcPosition(void)
{
	static int counter = 0;

	mAngle[ELBOW_MINI] = ((float)(encoder[ELBOW_MINI].Long - 
				encoderInit[ELBOW_MINI].Long)*COUNT_TO_DEG);

	int temp;
	temp = (int)(mAngle[ELBOW_MINI]/360.0);
//	mAngle[BLDC_CHANNEL] -= (float)(temp)*360.0;
//	printf("Angle: %5.3f, %5.3f, n: %d \n", mAngle[BLDC_CHANNEL],eAngle[BLDC_CHANNEL], temp);

	eAngle[ELBOW_MINI] = mAngle[ELBOW_MINI] * MECH_TO_ELEC;
//	eAngle[BLDC_CHANNEL] -= (int)(eAngle[BLDC_CHANNEL]/360.0)*360.0;

	angle[ELBOW_JOINT] = encoder[ELBOW_JOINT].Long/(float)(CANNON_COUNT_PER_DEG);

	if(counter < 10)
	{
		velocityOld[ELBOW_JOINT] = velocity[ELBOW_JOINT];
		counter++;
	}
//	printf("e: %5.2f, v: %5.2f, %5.2f\n", goal[ELBOW_JOINT] - angle[ELBOW_JOINT],
//		       velocity[ELBOW_JOINT],velocityOld[ELBOW_JOINT]);	
//	printf("p1: %5.2f, p2: %5.2f \n", angle[ELBOW_JOINT], angleOld[ELBOW_JOINT]);


	velocity[ELBOW_JOINT] = 0.9937*velocityOld[ELBOW_JOINT] + 6.26*(angle[ELBOW_JOINT]
			-angleOld[ELBOW_JOINT]);

	angleOld[ELBOW_JOINT] = angle[ELBOW_JOINT];
	velocityOld[ELBOW_JOINT] = velocity[ELBOW_JOINT];
}
void BLDC::readPosition(void)
{
	stg->EncoderLatch();
	stg->EncReadAll(temp);

	encoder[ELBOW_MINI].Long = temp[ELBOW_MINI].Long;
	encoder[ELBOW_JOINT].Long = temp[ELBOW_JOINT].Long;
	//	printf("Enc: %8d \n", encoder[BLDC_CHANNEL].Long);
}

void BLDC::writeTorque(void)
{
	stg->RawDAC(U_CHANNEL,uCommand);

	stg->RawDAC(V_CHANNEL,vCommand);
}

void BLDC::displayInfo(void)
{
	static int counter = 0;

//	if(counter == HZ)
	if(counter == 20)
	{
		printf("HET: %x \n", HETStatus);
//		printf("c: %8d, %8d \n", uCommand, vCommand);
//		printf("Angle: %5.3f, %5.3f, c:%8d \n", mAngle[BLDC_CHANNEL],
//				eAngle[BLDC_CHANNEL], encoder[BLDC_CHANNEL]);
//		printf("Goal: %6.2f, At: %6.2f, com: %8d \n", goal[ELBOW_JOINT],
//				angle[ELBOW_JOINT], torque);
		counter = 0;
	}
	counter++;
}

void BLDC::displayAngle(void)
{
	static int counter = 0;

	if(counter == 20)
	{
		printf("%8d	%8d %6.2f, %6.3f \n", encoder[ELBOW_MINI], encoder[ELBOW_JOINT],
				angle[ELBOW_JOINT], velocity[ELBOW_JOINT]);
		counter = 0;
	}
	counter++;
}

void BLDC::getJointPosition(float position[])
{
	int i;

	for(i=0;i<8;i++)
		position[i] = angle[i];
}

void BLDC::setGoal(float goalPosition)
{
	goal[ELBOW_JOINT] = goalPosition;
}

void BLDC::servoJoint(void)
{
	float Kp = 80.0;
	float Kv = 0.3*2.0*sqrt(Kp);
	float command;

	float velocityDes = (Kp/Kv)*(goal[ELBOW_JOINT] - angle[ELBOW_JOINT]);
	float VLIMIT = 130.0;

	if(velocityDes > VLIMIT)
		velocityDes = VLIMIT;
	else if(velocityDes < -VLIMIT)
		velocityDes = -VLIMIT;

	command = Kv*(velocityDes - velocity[ELBOW_JOINT]);
	
//	command = Kp*(goal[ELBOW_JOINT] - angle[ELBOW_JOINT]) - Kv*velocity[ELBOW_JOINT];

//	printf("e: %5.2f, vd: %5.2f, %5.2f, %5f c: %5.2f \n", 
//			goal[ELBOW_JOINT] - angle[ELBOW_JOINT],
//		       	velocityDes, Kv*velocityDes, Kv,  command);	
//	printf("p1: %5.2f, p2: %5.2f \n", angle[ELBOW_JOINT], angleOld[ELBOW_JOINT]);
	
	//	command = Kp*(goal[ELBOW_JOINT] - angle[ELBOW_JOINT]);

	float LIMIT = 500.0;

	if(command > LIMIT)
		command = LIMIT;
	if(command < -LIMIT)
		command = -LIMIT;

	sinusoidCom(command);

}


void BLDC::resetEncoder(void)
{
	int i;

	for(i=0;i<8;i++)
		stg->SetEncoderCounts(i,0);
}


void BLDC::saveInfo(void)
{
	readMotorCurrent();
	fprintf(fdata,"%8d %8d %8.3f %8.3f %2d %2d %2d %5d %5d \n", uCommand, vCommand,
		       mAngle[ELBOW_MINI],eAngle[ELBOW_MINI], HAB, HBC, HCA,
		       uCurrent, vCurrent);
	fprintf(fdata2,"%8d %8.3f, %8.3f, %8d \n", encoder[ELBOW_JOINT], angle[ELBOW_JOINT],
			velocity[ELBOW_JOINT], torque);

}

void BLDC::trapezoidCom(void)
{
	torque = 250;

	if(direction == CW)
	{
		switch(HETStatus)
		{
			case CW_STEP_ONE:
				uCommand = torque;
				vCommand = -torque;
				break;
			case CW_STEP_TWO:
				uCommand = torque;
				vCommand = 0;
				break;
			case CW_STEP_THREE:
				uCommand = 0;
				vCommand = torque;
				break;
			case CW_STEP_FOUR:
				uCommand = -torque;
				vCommand = torque;
				break;
			case CW_STEP_FIVE:
				uCommand = -torque;
				vCommand = 0;
				break;
			case CW_STEP_SIX:
				uCommand = 0;
				vCommand = -torque;
				break;
		}	
	}
	else if(direction == CCW)
	{
		switch(HETStatus)
		{
			case CCW_STEP_ONE:
				uCommand = 0;
				vCommand = torque;
				break;
			case CCW_STEP_TWO:
				uCommand = torque;
				vCommand = 0;
				break;
			case CCW_STEP_THREE:
				uCommand = torque;
				vCommand = -torque;
				break;
			case CCW_STEP_FOUR:
				uCommand = 0;
				vCommand = -torque;
				break;
			case CCW_STEP_FIVE:
				uCommand = -torque;
				vCommand = 0;
				break;
			case CCW_STEP_SIX:
				uCommand = -torque;
				vCommand = torque;
				break;
		}	
	}

	writeTorque();

}

void BLDC::sinusoidCom(float command)
{
	static double CW_OFFSET = 0.0;
	static double CCW_OFFSET = 00.0;	// was 50
	float PHASE_SHIFT = 120.0;

	if(command < 0)
		setDirection(CCW);
	else if(command >= 0)
		setDirection(CW);

	torque = (short)(command);

	switch(direction)
	{
		case CW:
			uCommand = (short)(torque*sin((eAngle[ELBOW_MINI] 
							+ CW_OFFSET)/DEG_TO_RAD));
			vCommand = (short)(torque*sin((eAngle[ELBOW_MINI] + CW_OFFSET 
							+ PHASE_SHIFT)/DEG_TO_RAD));
	//		CW_OFFSET += 0.0135*(torque/300.0);	// +0.0128
//			uCommand = (short)((float)(uCommand) * 1.6185);
			break;
		case CCW:
			uCommand = (short)(torque*sin((eAngle[ELBOW_MINI] 
							+ CCW_OFFSET)/DEG_TO_RAD));
			vCommand = (short)(torque*sin((eAngle[ELBOW_MINI] + CCW_OFFSET 
							+ PHASE_SHIFT)/DEG_TO_RAD));
	//		CCW_OFFSET -= 0.0135*(torque/300.0);	// -0.0051
//			vCommand = (short)((float)(vCommand) * 1.964);
			break;
	}
	// this funny offset is so that the uCommand doesnt lag HAB as time goes by.

	writeTorque();
}
void BLDC::sinusoidCom2(void)
{
	static double offset = 0.0;
	float PHASE_SHIFT = -120.0;

//	offset = 90.0;
	torque = 300;
	switch(direction)
	{
		case CW:
			uCommand = (short)(torque*sin((eAngle[ELBOW_MINI]+offset)/DEG_TO_RAD));
			vCommand = (short)(torque*sin((eAngle[ELBOW_MINI]+offset 
							+ PHASE_SHIFT)/DEG_TO_RAD));

			break;
		case CCW:
			uCommand = (short)(-torque*sin((eAngle[ELBOW_MINI]+offset)/DEG_TO_RAD));
			vCommand = (short)(-torque*sin((eAngle[ELBOW_MINI]+offset 
							+ PHASE_SHIFT)/DEG_TO_RAD));

			break;
	}
//	offset -= 0.0011;
	// this funny offset is so that the uCommand doesnt lag HAB as time goes by.

//	writeTorque();
}
/*
void BLDC::initWithIndex(void)
{
	static STG_LONGBYTE encoderOld;
	static STG_LONGBYTE value1, value2, valueAtIndex, valueAtStepTransition;
	unsigned char HETStatusAtIndex;

	printf("Rotate motor shaft SLOWLY until index pulse is detected \n");

	// wait for index pulse
	while(!hitIndex());

	readHET();
	printf("Index pulse detected \n");
	printf("Current HET status: %d \n", HETStatus);	

	if((HETStatus == CW_STEP_ONE)||(HETStatus == CW_STEP_TWO)||(HETStatus == CW_STEP_THREE))
	{
		printf("Index found at HET %d \n", HETStatus);
		printf("Move shaft in the same direction again until said otherwise \n");

		while(HETStatus != CW_STEP_FOUR)
			readHET();

		readPosition();
		value1.Long = encoder[ELBOW_MINI].Long;

		encoderOld.Long = encoder[ELBOW_MINI].Long + 1;

		printf("e: %d, %d \n", encoderOld.Long, encoder[ELBOW_MINI].Long);
		// wait until stop moving
		while(encoderOld.Long != encoder[ELBOW_MINI].Long)
		{
			printf("e: %d, %d \n", encoderOld.Long, encoder[BLDC_CHANNEL].Long);
			encoderOld.Long = encoder[ELBOW_MINI].Long;
			readPosition();
		}


		value2.Long = encoder[ELBOW_MINI].Long;
		printf("Shifted %d counts, %8f deg \n", value2.Long - value1.Long,
				(float)(value2.Long - value1.Long)
				*COUNT_TO_DEG*MECH_TO_ELEC);

		eAngle[ELBOW_MINI] = 240.0 + (float)(value2.Long - value1.Long)
			*COUNT_TO_DEG*MECH_TO_ELEC;
		mAngleInit[ELBOW_MINI] = eAngle[ELBOW_MINI]/MECH_TO_ELEC;
		printf("Stop!!! \n");
		printf("HET init to %3d, eangle: %5.2f, mangle: %5.2f \n", 
				HETStatus, eAngle[ELBOW_MINI], mAngleInit[ELBOW_MINI]);

		float temp1;
		long int temp2;

		temp1 = mAngleInit[ELBOW_MINI]*COUNT_PER_DEG/360.0; 
		temp2 = (int)(temp1);
		printf("temp1: %f, temp2: %d \n", temp1, temp2);
		stg->SetEncoderCounts(ELBOW_MINI, temp2);
		readPosition();
		printf("Encoder init to : %8 \n", encoder[ELBOW_MINI]);
		calcPosition();
		printf("Current angles: \n");
		printf("m: %5.2f, e: %5.2f \n", mAngle[BLDC_CHANNEL], eAngle[BLDC_CHANNEL]);

	}
	
	else if((HETStatus == CW_STEP_FOUR)||(HETStatus == CW_STEP_FIVE)
			||(HETStatus == CW_STEP_SIX))
	{
		readPosition();
		valueAtIndex.Long = encoder[BLDC_CHANNEL].Long;

		printf("Index found at HET %d, count: %8d \n", HETStatus, valueAtIndex.Long);
		printf("Move shaft in the same direction again until said otherwise \n");

		HETStatusAtIndex = HETStatus;

		while(HETStatus == HETStatusAtIndex)
		       readHET();

		readPosition();
		valueAtStepTransition.Long = encoder[BLDC_CHANNEL].Long;

		printf("Next count : %8d \n", valueAtStepTransition.Long);
		printf("Found next step! %d , %8d, %8d \n \n, HETStatus, 
				valueAtIndex.Long,valueAtStepTransition.Long");

		while(HETStatus != CW_STEP_ONE)
			readHET();
		readPosition();
		value1.Long = encoder[BLDC_CHANNEL].Long;

		encoderOld.Long = encoder[BLDC_CHANNEL].Long + 1;

		printf("e: %d, %d \n", encoderOld.Long, encoder[BLDC_CHANNEL].Long);
		// wait until stop moving
		while(encoderOld.Long != encoder[BLDC_CHANNEL].Long)
		{
			printf("e: %d, %d \n", encoderOld.Long, encoder[BLDC_CHANNEL].Long);
			encoderOld.Long = encoder[BLDC_CHANNEL].Long;
			readPosition();
		}

		value2.Long = encoder[BLDC_CHANNEL].Long;
		printf("Shifted %d counts, %8f deg \n", value2.Long - value1.Long,
				(float)(value2.Long - value1.Long)
				*COUNT_TO_DEG*MECH_TO_ELEC);

		eAngle[BLDC_CHANNEL] = 60.0 + (float)(value2.Long - value1.Long)
			*COUNT_TO_DEG*MECH_TO_ELEC;
		mAngleInit[BLDC_CHANNEL] = eAngle[BLDC_CHANNEL]/MECH_TO_ELEC;

		printf("Stop!!! \n");
		printf("HET init to %3d, eangle: %5.2f, mangle: %5.2f \n", 
				HETStatus, eAngle[BLDC_CHANNEL], mAngleInit[BLDC_CHANNEL]);

		float temp1;
		long int temp2;

		temp1 = mAngleInit[BLDC_CHANNEL]*COUNT_PER_DEG/360.0; 
		temp2 = (int)(temp1);

		printf("temp1: %f, temp2: %d \n", temp1, temp2);
		stg->SetEncoderCounts(BLDC_CHANNEL, temp2);
		readPosition();
		printf("Encoder init to : %8d \n", encoder[BLDC_CHANNEL]);
		calcPosition();
		printf("Current angles: \n");
		printf("m: %5.2f, e: %5.2f \n", mAngle[BLDC_CHANNEL], eAngle[BLDC_CHANNEL]);
	}


}
*/
// this is without HET signal
void BLDC::initWithIndex2(void)
{
	static STG_LONGBYTE encoderOld;
	static STG_LONGBYTE value1, value2, valueAtIndex, valueAtStepTransition;
	unsigned char HETStatusAtIndex;
	float temp1, angleShift;
	long int temp2;
	long int i;
	PrTimer stabilizeTimer(HZ);

	printf("Rotate motor shaft SLOWLY until index pulse is detected \n");

	// wait for index pulse
	while(!hitIndex());

//	readHET();
	printf("Index pulse detected \n");
	readPosition();
	value1.Long = encoder[ELBOW_MINI].Long;

	encoderOld.Long = encoder[ELBOW_MINI].Long + 1;

	printf("e: %d, %d \n", encoderOld.Long, encoder[ELBOW_MINI].Long);

//	for(i=0;i<100000;i++)
//		printf("Waiting to stabilize \r");

	stabilizeTimer.start();
	for(i=0;i<2*HZ;i++)
		stabilizeTimer.wait();

	printf("\n");

	// wait until stop moving
	while(encoderOld.Long != encoder[ELBOW_MINI].Long)
	{
		printf("e: %d, %d \n", encoderOld.Long, encoder[ELBOW_MINI].Long);
		encoderOld.Long = encoder[ELBOW_MINI].Long;
		readPosition();
	}

	value2.Long = encoder[ELBOW_MINI].Long;
	angleShift = (float)(value2.Long - value1.Long)*COUNT_TO_DEG*MECH_TO_ELEC;

	printf("Shifted %d counts, %8f deg \n", value2.Long - value1.Long,angleShift);

	eAngle[ELBOW_MINI] = 348.8386 + angleShift;
	mAngleInit[ELBOW_MINI] = eAngle[ELBOW_MINI] / MECH_TO_ELEC;
	
	temp1 = mAngleInit[ELBOW_MINI]*COUNT_PER_DEG/360.0; 
	temp2 = (int)(temp1);

	printf("temp1: %f, temp2: %d \n", temp1, temp2);
	stg->SetEncoderCounts(ELBOW_MINI, temp2);
	readPosition();
	printf("Encoder init to : %8d \n", encoder[ELBOW_MINI]);

//	for(i=0;i<10;i++)
//	{
//		readPosition();
	calcPosition();
	printf("Current angles: \n");
	printf("m: %5.2f, e: %5.2f, c:%8d \n", mAngle[ELBOW_MINI], eAngle[ELBOW_MINI],
			encoder[ELBOW_MINI]);
//	}
}

void BLDC::setDirection(int dir)
{
	direction = dir;

}

void BLDC::readMotorCurrent(void)
{
	uCurrent = stg->GetADC(0);
	vCurrent = stg->GetADC(2);

}

void main (void)
{
	BLDC *motor1 = new BLDC();
	PrTimer timer(HZ);
	int direction, choice;
	static int loop = 0;
	float torque = 400.0;
	float elbowGoal;
	float angle[8];
	int i;

	pid_t pid = getpid();
	setprio(pid, PR_PRIORITY_MAX);

	motor1->initPosition();

	timer.start();
	for(i=0;i<100;i++)
	{
		motor1->readPosition();
		motor1->calcPosition();
		timer.wait();
	}


//	while(1)
//	{
//		motor1->readPosition();
//		motor1->calcPosition();
//		timer.wait();
//	}

//	exit(1);	
	printf("BLDC Test Program \n");
	printf("Enter command: \n");
//	printf("(1) Trapezoid Commutation \n");
	printf("(2) Sinusoid Commutation \n");
	printf("(3) Initialization with Index \n");
	printf("(4) Test HET \n");
	printf("(5) Test Encoders \n");
	printf("(6) Test Position Loop on EL1 \n");




	cin >> choice;	

	cin.ignore();

	switch(choice)
	{
		case 1:
/*			printf("Trapezoidal Commutation \n");
			printf("Select direction: \n");
			printf("(1) CW \n");
			printf("(2) CCW \n");
			cin >> direction;
			motor1->setDirection(direction);
#if 0
			motor1->initWithIndex();
			cin.ignore();
			printf("Release motor shaft and press any key to continue \n");
			while(!tcischars(1));
			cin.ignore();
#endif
			timer.start();

			while(loop<3)
			{
				motor1->setDirection(CW);
				while(!tcischars(1))
				{
					motor1->readHET();
					motor1->readPosition();
					motor1->calcPosition();
					motor1->trapezoidCom();
					motor1->saveInfo();
					timer.wait();
				}
				cin.ignore();
				motor1->setDirection(CCW);
				while(!tcischars(1))
				{
					motor1->readHET();
					motor1->readPosition();
					motor1->calcPosition();
					motor1->trapezoidCom();
					motor1->saveInfo();
					timer.wait();
				}
				cin.ignore();
				loop++;
				printf("Loop: %3d \n", loop);
			}
*/			break;
		case 2:
			printf("Sinusoidal Commutation \n");
//			printf("Select direction: \n");
//			printf("(1) CW \n");
//			printf("(2) CCW \n");
//			cin >> direction;
//			motor1->setDirection(direction);
		
			// calibrate position with index signal (without HET signals)
			motor1->initWithIndex2();

			cin.ignore();
			printf("Release motor shaft and press any key to continue \n");
			while(!tcischars(1));
			cin.ignore();

			timer.start();

			while(loop < 3)
			{
				motor1->setDirection(CW);
				printf("up \n");
				while(!tcischars(1))
				{
					motor1->readHET();
					motor1->readPosition();
					motor1->calcPosition();
					motor1->sinusoidCom(torque+200.0);
			//		motor1->displayInfo();
					motor1->saveInfo();
					timer.wait();
				}
				cin.ignore();
				printf("down \n");
				motor1->setDirection(CCW);
				while(!tcischars(1))
				{
					motor1->readHET();
					motor1->readPosition();
					motor1->calcPosition();
					motor1->sinusoidCom(-torque+200.0);
			//		motor1->displayInfo();
					motor1->saveInfo();
					timer.wait();
				}
				cin.ignore();
				printf("Loop: %2d \n", loop);	
				loop++;
			}
			break;

		case 3:
			printf("Testing init with index \n");
//			motor1->initWithIndex();
			motor1->initWithIndex2();

			printf("Release motor shaft and press any key to continue \n");
			while(!tcischars(1));
			cin.ignore();

			timer.start();
			while(!tcischars(1))
			{
				motor1->readPosition();
				motor1->calcPosition();
				motor1->displayInfo();
				motor1->saveInfo();
				timer.wait();
			}


			break;

		case 4:
			printf("Testing HET sensor \n");
			timer.start();

			while(!tcischars(1))
			{
				motor1->readHET();
				motor1->readPosition();
				motor1->calcPosition();
				motor1->displayInfo();
				motor1->saveInfo();
				timer.wait();
			}

			break;

		case 5:
			motor1->resetEncoder();
			printf("Testing Encoders\n");
			timer.start();

			while(!tcischars(1))
			{
				motor1->readPosition();
				motor1->calcPosition();	
				motor1->displayAngle();	
				motor1->saveInfo();
				timer.wait();
			}

			break;

		case 6:

			motor1->resetEncoder();

			printf("Testing init with index \n");
			motor1->initWithIndex2();
			motor1->resetVelocity();

			printf("Release motor shaft and press any key to continue \n");
			while(!tcischars(1));
			cin.ignore();

			int exit = 0;

			while(!exit)
			{
				motor1->getJointPosition(angle);

				printf("Current position: %6.2f \n", angle[ELBOW_JOINT]);
				printf("Enter elbow joint goal position in degrees: \n");
				printf("Enter 1000 to exit \n");
				printf("> ");
				cin >> elbowGoal;
	
				if(elbowGoal == 1000.0)
						exit = 1;
				else
					motor1->setGoal(elbowGoal);
		
				printf("\n");
				timer.start();
	
				while(!tcischars(1) && !exit)
				{
					motor1->readPosition();
					motor1->calcPosition();	
					motor1->servoJoint();
				//	motor1->displayAngle();	
					motor1->displayInfo();	
					timer.wait();
				}
			}
			break;
	}

	motor1->closeDataFile();
	delete motor1;
}



