#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 "matrix/PrVector.h"
#include "matrix/PrMatrix.h"
#include "bldc.h"

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

#define HZ  500

#define BUFFER_SIZE		12000
#define BUFFER_COL		10	

#define NUM_JOINT	3
#define NUM_JOINT1	6
#define ADCFACTOR	10.0/4096.0

BLDC::BLDC(ServoToGo *stgPtr)
{
	int i;
//	stg = new ServoToGo(STG_ARM_IRQ, STG_ARM_ADDRESS);
	stg = stgPtr;

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

	initSystem();

	//init variables
	a0 = new PrVector(NUM_JOINT1);
	a2 = new PrVector(NUM_JOINT1);
	a3 = new PrVector(NUM_JOINT1);
	a0->zero(); a2->zero(); a3->zero();
	qgoal = new PrVector(NUM_JOINT1);
	qdes = new PrVector(NUM_JOINT1);
	dqdes = new PrVector(NUM_JOINT1);
	tfinal = new PrVector(NUM_JOINT1);
	qgoal->zero(); qdes->zero(); dqdes->zero(); tfinal->zero();
	q = new PrVector(NUM_JOINT1);
	qOld = new PrVector(NUM_JOINT1);
	dq = new PrVector(NUM_JOINT1);
	dqOld = new PrVector(NUM_JOINT1);
	q->zero();	dq->zero();
	qOld->zero(); dqOld->zero();

	bufferIndex = 0;
	dataBuffer1 = new PrMatrix(BUFFER_SIZE,BUFFER_COL);
	dataBuffer2 = new PrMatrix(BUFFER_SIZE,BUFFER_COL);
	dataBuffer1->zero();
	dataBuffer2->zero();
	printf("Constructor finished \n");

	torqueCommand = new PrVector(NUM_JOINT1);
	torqueCommand->zero();

	xLeft = new PrVector(3);
	xRight = new PrVector(3);
	dxLeft = new PrVector(3);
	dxRight = new PrVector(3);

	for(i=0;i<3;i++)
	{
			uCommand[i] = 0;
			vCommand[i] = 0;
	}

	// JOINT LIMITS FOR ARM
	jointLimitMax[WA1] = 74.63;
//	jointLimitMax[SH1] = 29.86 - 4.44;
	jointLimitMax[SH1] = 29.86;
//	jointLimitMax[SH1] = 24.0;
	jointLimitMax[EL1] = 144.22 - 4.0;
	jointLimitMin[WA1] = -75.3;	//74.77
	jointLimitMin[SH1] = -125.73;
	jointLimitMin[EL1] = 7.73;	// 5.52

	jointIndexAngle[WA1] = -46.37 - 6.0;
	jointIndexAngle[SH1] = -52.22;
	jointIndexAngle[EL1]= 96.70;
}

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

void BLDC::initSystem(void)
{
	stg->EncoderInit();
	stg->EncoderLatch();
	initIndex();
}

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);
}

int BLDC::hitIndex2(int channel)
{
	int check = 1<<channel;
	int result = stg->GetIndexLatches();
	if(stg->GetIndexLatches() & (1<<channel))
	{
//			printf("index: %d, %x, %d, %d \n", result, 
//							result, check, result & check  );
			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; 

}

// UNUSED
void BLDC::initPosition(void)
{
	readPosition();
	for(int i=0;i<8;i++)
	{
		encoderInit[i].Long = 0;
	}
	printf("Init: \n");
	printf("EL1: %d, %d, SH1: %d, %d, WA1: %d, %d \n", ELBOW_MINI, encoder[ELBOW_MINI], 
					SHOULDER_MINI, encoder[SHOULDER_MINI], WAIST_MINI, encoder[WAIST_MINI]);
	printf("\n");
}
void BLDC::openlogLOHET(void)
{	   
	lohet_cal_data = fopen("lohetdata.txt", "w");
}
void BLDC::closelogLOHET(void)
{
	fclose(lohet_cal_data); 
}
void BLDC::displaylogLOHET(void)
{
		int i;

		LOHET[WA1] = (float)(stg->GetADC(WA1))*ADCFACTOR;
		LOHET[SH1] = (float)(stg->GetADC(SH1))*ADCFACTOR;
		LOHET[EL1] = (float)(stg->GetADC(EL1))*ADCFACTOR;

		stg->EncoderLatch();
		stg->EncReadAll(temp);
		
		printf("Encoder DATA: ");
		//for (i = 0; i < 8; i++)            // print out results of the encoders
		printf("6=%8d ", temp[6].Long);
		
		printf("LOHET DATA [WA1]: %15.12f, [SH1]: %15.12f, [EL1]: %15.12f \n", LOHET[WA1], LOHET[SH1], LOHET[EL1]);	

		//Start the logging of the data
		//fprintf(lohet_cal_data,"LOHET with encoder logged data: LOHET_WAIST | LOHET_SHOULDER | LOHET_ELBOW |");
		//fprintf(lohet_cal_data," enc0 | enc1 | enc2 | enc3 | enc4 | enc5 | enc6 | enc7 |");
		//for (i = 0; i < 8; i++)            // print out results of the encoders
		fprintf(lohet_cal_data, "%8d, ", temp[6].Long);						
		fprintf(lohet_cal_data, "%15.12f,%15.12f,%15.12f,\n", LOHET[WA1], LOHET[SH1], LOHET[EL1]);		  
}
void BLDC::displayLOHET(void)
{	
	int i;
	LOHET[WA1] = (float)(stg->GetADC(WA1))*ADCFACTOR;
	LOHET[SH1] = (float)(stg->GetADC(SH1))*ADCFACTOR;
	LOHET[EL1] = (float)(stg->GetADC(EL1))*ADCFACTOR;

	stg->EncoderLatch();
	stg->EncReadAll(temp);
	
	printf("Encoder DATA: ");
	//for (i = 0; i < 8; i++)            // print out results of the encoders
		printf("6=%8d ", temp[6].Long);
		
	printf("LOHET DATA [WA1]: %15.12f, [SH1]: %15.12f, [EL1]: %15.12f \n", LOHET[WA1], LOHET[SH1], LOHET[EL1]);	
}

void BLDC::resetVelocity(void)
{
	int i;
	for(i=0;i<6;i++)
	{
		(*dq)[i] = 0.0;
		(*qOld)[i] = (*q)[i];
	}
}

#define RIGHT_ARM 1
// MUST SET THIS TO ZERO IF DOING LEFT ARM
// Right now I dont have separate left or right arm functions to read joint angle
void BLDC::calcPosition(void)
{
	static int counter = 0;
	int jointSign[3] = {-1, -1, -1};	// RIGHT ARM
//	int jointSign[3] = {-1, -1, -1};	// LEFT ARM
	int i;

	// Calculate mini mechanical and electrical angles and joint angles
	// i = 0,1,2 --> WA1, SH1, EL1
	for(i=0;i<3;i++)
	{
		// calculate mechanical angle of mini encoder
//		mAngle[2*i] = ((float)(encoder[2*i].Long - 
//				encoderInit[2*i].Long)*COUNT_TO_DEG);

		mAngle[2*i] = ((float)(encoder[2*i].Long))*COUNT_TO_DEG;
				
		// calculate electrical angle of mini encoder 
		// 1 mech degree = 6 electrical degrees (RBE1511 and RBE1513 datasheets)
		eAngle[2*i] = mAngle[2*i] * MECH_TO_ELEC;

		// calculate joint angle from CANNON encoder

		// if RIGHT ARM
#if RIGHT_ARM
		if(i==0)
			(*q)[i] = encoder[2*i+1].Long/(float)(CANNON_COUNT_PER_DEG);
		else
		{
		(*q)[i] = encoder[2*i+1].Long*jointSign[i]/(float)(CANNON_COUNT_PER_DEG) 
				+ 2*jointLimitMax[i] ;
		}
#else	// for LEFT ARM
		(*q)[i] = encoder[2*i+1].Long*jointSign[i]/(float)(CANNON_COUNT_PER_DEG) 
				+ 2*jointLimitMax[i] ;
#endif
		if(counter < 1)
		{
			(*dq)[i] = 0.0;
			(*dqOld)[i] = (*dq)[i];
			(*qOld)[i] = (*q)[i];
		}
		else
		{
			// calc velocity from position data
			(*dq)[i] = 1.0*0.9937*(*dqOld)[i] + 6.26*((*q)[i] - (*qOld)[i]);
			(*dqOld)[i] = (*dq)[i];	// store old velocity information
		}

		(*qOld)[i] = (*q)[i];	// store old position information
	}

	if(counter < 1)
		counter++;
}

void BLDC::readPosition(void)
{
	// read encoder counts from right arm STG card
	stg->EncoderLatch();
	stg->EncReadAll(temp);

	// RIGHT ARM
	// assign read encoder counts to corresponding channels
	encoder[ELBOW_MINI].Long = temp[ELBOW_MINI].Long;
	encoder[ELBOW_JOINT].Long = temp[ELBOW_JOINT].Long;
	encoder[SHOULDER_MINI].Long = temp[SHOULDER_MINI].Long;
	encoder[SHOULDER_JOINT].Long = temp[SHOULDER_JOINT].Long;
	encoder[WAIST_MINI].Long = temp[WAIST_MINI].Long;
	encoder[WAIST_JOINT].Long = temp[WAIST_JOINT].Long;
//	encoder[WAIST_JOINT].Long = temp[7].Long;

	//LEFT ARM
	// read encoder counts from left arm STG card
}

// Used for initial calibration of joint angle 
// Bring corresponding joint to its maximum joint limit and set the joint angle
// to the known joint position
void BLDC::setJointAngleMax(int jointIndex)
{
	stg->SetEncoderCounts(2*jointIndex + 1, (long int)(jointLimitMax[jointIndex]*(float)(CANNON_COUNT_PER_DEG)));
	printf("Joint [%d] set to %f \n", jointIndex, jointLimitMax[jointIndex]);
	printf("Count: %d \n",(long int)(jointLimitMax[jointIndex]*(float)(CANNON_COUNT_PER_DEG)));
	printf("%f, %f \n", jointLimitMax[jointIndex], (float)(CANNON_COUNT_PER_DEG));
	printf("%f \n", jointLimitMax[jointIndex]*(float)(CANNON_COUNT_PER_DEG));
}

// Used for initial calibration of joint angle 
// Bring corresponding joint to its maximum joint limit and set the joint angle
// to the known joint position
void BLDC::setJointAngleAtIndex(int jointIndex)
{
	int jointSign[3] = {1, -1, -1};
//	stg->SetEncoderCounts(2*jointIndex + 1, -1*(long int)(jointIndexAngle[jointIndex]*(float)(CANNON_COUNT_PER_DEG)));

	if(jointIndex == 0)
	{
		stg->SetEncoderCounts(2*jointIndex + 1, (long int)(jointIndexAngle[jointIndex]*CANNON_COUNT_PER_DEG));
		// temp hack
//		stg->SetEncoderCounts(7, (long int)(jointIndexAngle[jointIndex]*CANNON_COUNT_PER_DEG));
	}
	else
	{
		stg->SetEncoderCounts(2*jointIndex + 1, (long int)(jointSign[jointIndex]*(jointIndexAngle[jointIndex] - 2*jointLimitMax[jointIndex])
							*(float)(CANNON_COUNT_PER_DEG)));		
	}		
	printf("Joint [%d] set to %f \n", jointIndex, jointIndexAngle[jointIndex]);
	printf("Count: %d \n",(long int)(jointIndexAngle[jointIndex]*(float)(CANNON_COUNT_PER_DEG)));
	printf("%f, %f \n", jointIndexAngle[jointIndex], (float)(CANNON_COUNT_PER_DEG));
	printf("%f \n", jointIndexAngle[jointIndex]*(float)(CANNON_COUNT_PER_DEG));
}
void BLDC::setJointAngleMin(int jointIndex)
{
	stg->SetEncoderCounts(2*jointIndex + 1, (long int)(jointLimitMin[jointIndex]*(float)(CANNON_COUNT_PER_DEG)));
	printf("Joint [%d] set to %f \n", jointIndex, jointLimitMin[jointIndex]);
	printf("Count: %d \n",(long int)(jointLimitMin[jointIndex]*(float)(CANNON_COUNT_PER_DEG)));
	printf("%f, %f \n", jointLimitMin[jointIndex], (float)(CANNON_COUNT_PER_DEG));
	printf("%f \n", jointLimitMin[jointIndex]*(float)(CANNON_COUNT_PER_DEG));
}

// Write DAC count to STG card by writing the corresponding u and v phase of the mini motor
void BLDC::writeTorque(int jointIndex)
{
	//DAC channels allocation:
	//[0]: WA1 mini u-phase
	//[1]: WA1 mini v-phase
	//[2]: SH1 mini u-phase
	//[3]: SH1 mini v-phase
	//[4]: EL mini u-phase
	//[5]: EL1 mini v-phase

	if(jointIndex < 3)
	{
		stg->RawDAC(0+jointIndex*2,uCommand[jointIndex]);
		stg->RawDAC(1+jointIndex*2,vCommand[jointIndex]);
	}
	else
			printf("DAC write channel error \n");
	
}

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

	//Kuan Li: I'm pretty sure these are joint positions (Red Barrel Joint encoders)	
	printf("[WA1]: %7.2f, [SH1]: %7.2f, [EL1]: %7.2f \n", (*q)[WA1], (*q)[SH1], (*q)[EL1]);
#if 0
	if(counter == 100)
	{
		printf("HET: %x \n", HETStatus);
//		printf("c: %8d, %8d \n", uCommand, vCommand);
//		printf("Angle: %5.3f, %5.3f, c:%8d \n", mAngle[ELBOW_MINI],
//				eAngle[ELBOW_MINI], encoder[ELBOW_MINI]);
//		printf("Goal: %6.2f, At: %6.2f, com: %8d \n", goal[ELBOW_JOINT],
//				angle[ELBOW_JOINT], torque);
		counter = 0;
	}
	counter++;
#endif
}

void BLDC::displayAngle(void)
{
	static int counter = 0;
	int index;
	if(counter == 200)
	{
		counter = 0;
		printf("%8d, %8d, %8d, %6.2f %6.2f, %6.2f %6.3f, %6.3f, %6.3f \n", 
						encoder[WAIST_MINI], encoder[SHOULDER_MINI], encoder[ELBOW_MINI], 
						(*q)[WA1], (*q)[SH1], (*q)[EL1], 	
						(*dq)[WA1],(*dq)[SH1],(*dq)[EL1]);
	}
	counter++;
}

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

	for(i=0;i<3;i++)
		position[i] = (*q)[i];
}

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


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 %8d %2d %2d %2d %5d %5d \n", uCommand, vCommand,
//		       mAngle[ELBOW_MINI],eAngle[ELBOW_MINI], encoder[ELBOW_MINI], HAB, HBC, HCA,
//		       uCurrent, vCurrent);
//	fprintf(fdata2,"%8d %8.3f, %8.3f, %8d, %8.3f %8.3f \n", encoder[ELBOW_JOINT], angle[ELBOW_JOINT],
//			velocity[ELBOW_JOINT], torque, (*qdes)[EL1], (*dqdes)[EL1]);

	int jointIndex = SH1;
	int encoderIndex = SHOULDER_MINI;
	if(bufferIndex < BUFFER_SIZE)
	{
		dataBuffer1->elementAt(bufferIndex,0) = (float)(encoder[SHOULDER_MINI].Long);
		dataBuffer1->elementAt(bufferIndex,1) = eAngle[SHOULDER_MINI];
		dataBuffer1->elementAt(bufferIndex,2) = (float)(uCommand[jointIndex]);
		dataBuffer1->elementAt(bufferIndex,3) = (float)(vCommand[jointIndex]);
		dataBuffer1->elementAt(bufferIndex,4) = (float)(uCurrent);
		dataBuffer1->elementAt(bufferIndex,5) = (float)(vCurrent);
		dataBuffer1->elementAt(bufferIndex,6) = (*torqueCommand)[jointIndex];

		dataBuffer2->elementAt(bufferIndex,0) = (float)(encoder[ELBOW_JOINT].Long);
	//	dataBuffer2->elementAt(bufferIndex,1) = (*q)[EL1];
		dataBuffer2->elementAt(bufferIndex,1) = (*q)[SH1];
		dataBuffer2->elementAt(bufferIndex,2) = (*dq)[SH1];
		dataBuffer2->elementAt(bufferIndex,3) = (*qdes)[EL1];
		dataBuffer2->elementAt(bufferIndex,4) = (*dqdes)[EL1];

		bufferIndex++;
	}
}

void BLDC::saveBuffer(void)
{
	int i;
	openDataFile();

	for(i=0;i<bufferIndex;i++)
	{
		fprintf(fdata,"%8.0f %8.3f %6.2f %6.2f %5.0f %5.0f %8.3f \n",
						dataBuffer1->elementAt(i,0),dataBuffer1->elementAt(i,1),
						dataBuffer1->elementAt(i,2),dataBuffer1->elementAt(i,3),
						dataBuffer1->elementAt(i,4),dataBuffer1->elementAt(i,5),
						dataBuffer1->elementAt(i,6));

		fprintf(fdata2,"%8.0f %8.3f %8.3f %8.3f %8.3f \n",
						dataBuffer2->elementAt(i,0),dataBuffer2->elementAt(i,1),
						dataBuffer2->elementAt(i,2),dataBuffer2->elementAt(i,3),
						dataBuffer2->elementAt(i,4));
	}

	closeDataFile();
}	

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

	int miniIndex = 2*jointIndex;

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

	switch(direction)
	{
		case CW:
			uCommand[jointIndex] = (short)(command*sin((eAngle[miniIndex] 
							+ CW_OFFSET)/DEG_TO_RAD));
			vCommand[jointIndex] = (short)(command*sin((eAngle[miniIndex] + CW_OFFSET 
							+ PHASE_SHIFT)/DEG_TO_RAD));
			break;
		case CCW:
			uCommand[jointIndex] = (short)(command*sin((eAngle[miniIndex] 
							+ CCW_OFFSET)/DEG_TO_RAD));
			vCommand[jointIndex] = (short)(command*sin((eAngle[miniIndex] + CCW_OFFSET 
							+ PHASE_SHIFT)/DEG_TO_RAD));
			break;
	}

//	printf("u: %d, v: %d \n", uCommand[jointIndex], vCommand[jointIndex]);
//	printf("[%d]: %f, command: %f, u:%d, v:%d  \n", jointIndex, eAngle[miniIndex], command, uCommand[jointIndex], 
//					vCommand[jointIndex]);
//	printf("miniIndex: %d, %f, %f \n", miniIndex, eAngle[miniIndex], eAngle[SHOULDER_MINI]);
	writeTorque(jointIndex);
}
	
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))
	{
		readPosition();
		valueAtIndex.Long = encoder[ELBOW_MINI].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[ELBOW_MINI].Long;

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

//		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[ELBOW_MINI].Long);
			encoderOld.Long = encoder[ELBOW_MINI].Long;
			readPosition();
		}

//		cin.ignore();
		printf("Stop!!! \n");
		while(!tcischars(1));
		cin.ignore();

		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("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[ELBOW_MINI], eAngle[ELBOW_MINI]);

	}
	
	else if((HETStatus == CW_STEP_FOUR)||(HETStatus == CW_STEP_FIVE)
			||(HETStatus == CW_STEP_SIX))
	{
		readPosition();
		valueAtIndex.Long = encoder[ELBOW_MINI].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[ELBOW_MINI].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[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[ELBOW_MINI].Long);
			encoderOld.Long = encoder[ELBOW_MINI].Long;
			readPosition();
		}

		printf("Stop!!! \n");
		while(!tcischars(1));
		cin.ignore();

		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] = 60.0 + (float)(value2.Long - value1.Long)
			*COUNT_TO_DEG*MECH_TO_ELEC;
		mAngleInit[ELBOW_MINI] = eAngle[ELBOW_MINI]/MECH_TO_ELEC;

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

		//display HET + index calibration value
		printf("Index pulse occurs %7.3f electrical degrees ago \n");


		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 : %8d \n", encoder[ELBOW_MINI]);
		calcPosition();
		printf("Current angles: \n");
		printf("m: %5.2f, e: %5.2f \n", mAngle[ELBOW_MINI], eAngle[ELBOW_MINI]);
	}


}

// 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())
			readPosition();

//	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++)
	for(i=0;i<50;i++)
		stabilizeTimer.wait();

	printf("\n");

	static int counter = 0;
	float time1, time2;
	counter = 0;

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


//	printf("Stop and press any key!!! \n");
//	while(!tcischars(1));
//	cin.ignore();

	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);

	// EL1: 348.8386
	// SH1: 180.41 
	// WA1: 112.47
//	eAngle[ELBOW_MINI] = 348.8386 + angleShift;	// for EL1
//	eAngle[ELBOW_MINI] = 180.41 + angleShift;	// for SH1
//	eAngle[ELBOW_MINI] = 112.47 + angleShift;	// for WA1
//	eAngle[ELBOW_MINI] = 36.8 + angleShift;	// for EL2
//	eAngle[ELBOW_MINI] = 276.8 + angleShift;	// for SH2
	eAngle[ELBOW_MINI] = 290.0 + angleShift;	// for WA2

	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]);
//	}
}

// this is without HET signal
void BLDC::initMiniEncoder(int jointIndex, int armSide)
{
	//jointIndex: 0 = WA; 1 = SH; 2 = EL
	int miniIndex = 2*jointIndex;
	//miniIndex: 0 = WA; 2 = SH; 4 = EL

	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("mini index: %d \n", miniIndex);
	printf("Rotate motor shaft SLOWLY until index pulse is detected \n");

	// wait for index pulse
	while(!hitIndex2(miniIndex))
			readPosition();

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

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

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

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

	printf("\n");

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

	value2.Long = encoder[miniIndex].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);

	// Notice that the index order is WA1, WA2, SH1, SH2, EL1 and EL2
	// this to simplify the indexing calculation
	// CALIBRATION CHART FOR BRUSHLESS MOTORS
	// WA1, WA2, SH1, SH2, EL1, EL2
	//	static float miniIndexPulseAngle[6] = {23.6, 290.1, 180.41, 260.3, 348.84, 36.8};

	static float miniIndexPulseAngleLeft[3] = {290.1, 260.3, 36.8};
	static float miniIndexPulseAngleRight[3] = {23.6, 180.41, 348.84};

	if(armSide == LEFTARM)
		eAngle[miniIndex] = miniIndexPulseAngleLeft[jointIndex] + angleShift;
	else if(armSide == RIGHTARM)
		eAngle[miniIndex] = miniIndexPulseAngleRight[jointIndex] + angleShift;

	printf("Side: %d, Joint index: %d, miniIndex: %d \n", armSide, jointIndex, miniIndex);

	if(armSide == LEFTARM)
		printf("Left Arm, Found index for %d, angle %f \n", jointIndex, miniIndexPulseAngleLeft[jointIndex]);
	else if(armSide == RIGHTARM)
		printf("Right Arm, Found index for %d, angle %f \n", jointIndex, miniIndexPulseAngleRight[jointIndex]);

	printf("Initial eangle: %f \n", eAngle[miniIndex]);

	mAngleInit[miniIndex] = eAngle[miniIndex] / MECH_TO_ELEC;
	
	temp1 = mAngleInit[miniIndex]*COUNT_PER_DEG/360.0; 
	temp2 = (int)(temp1);

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

	calcPosition();
//	printf("Current angles: \n");
//	printf("m: %5.2f, e: %5.2f, c:%8d \n", mAngle[miniIndex], eAngle[miniIndex],
//			encoder[miniIndex]);
//	}
}


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

}

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

}

void main (void)
{
	ServoToGo *stgPtr = new ServoToGo(STG_ARM_IRQ, STG_ARM_ADDRESS);
	BLDC *miniControl = new BLDC(stgPtr);
	PrTimer timer(HZ);
	int direction, choice;
	static int loop = 0;
	float torque = 150.0;
	float elbowGoal;
	float angle[8];
	int i, misTick;
	float time;
	PrVector qgoal(NUM_JOINT);

	STG_LONGBYTE temp[8];

	pid_t pid = getpid();
	setprio(pid, 63);
	printf("priority set to %d \n", PR_PRIORITY_MAX);

//	miniControl->initPosition();

	timer.start();

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

	miniControl->resetIndex();

	printf("BLDC Test Program \n");
	printf("Enter command: \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");
	printf("(7) Calibrate motor enc index with HET \n");
	printf("(8) Synching index with HET \n");
	printf("(10) RIGHT ARM Gravity Compensation Test \n");
	printf("(11) Calibrate Right Arm Brushless Encoders (run once only) \n");
	printf("(12) Set Joint Angle based on Maximum Joint Limit \n");
//	printf("(16) Set Joint Angle based on Minimum Joint Limit \n");
	printf("(13) LEFT ARM Gravity Compensation Test \n");
	printf("(14) Joint Encoder Init using Index Pulse \n");
	printf("(15) Display end-effector position \n");
	printf("(16) Init Cannon Encoders using Index Pulse \n");
	printf("(17) 2008_04 Displaying LoHET Data and logging it \n");
	printf("(19) Displaying joint information and zeroing out the joints.\n");

	printf("(20) Displaying joint information\n");
	cin >> choice;	

	cin.ignore();

	// choose one of these three to be the one to be sinusoidally commuted
//	int jointIndex = EL1;
//	int jointIndex = SH1;
	int jointIndex = WA1;

	switch(choice)
	{
		case 20:
			timer.start();
						while(!tcischars(1))
			{
				stgPtr->EncoderLatch();
				stgPtr->EncReadAll(temp);
		
				//printf("Encoder DATA: ");
				for (int i = 0; i < 8; i++)            // print out results of the encoders
					printf("|%d = %4d", i, temp[i].Long);						


				printf("\n");	
				for(i=0; i<100; i++)
					timer.wait();
			}

			cin.ignore();
			break;
			
		case 19:

			timer.start();

			while(!tcischars(1))
			{
				stgPtr->EncoderLatch();
				stgPtr->EncReadAll(temp);
		
				//printf("Encoder DATA: ");
				for (int i = 0; i < 8; i++)            // print out results of the encoders
					printf("|%d = %4d", i, temp[i].Long);						


				printf("\n");	
				for(i=0; i<100; i++)
					timer.wait();
			}

			cin.ignore();


			miniControl->resetEncoder();  //Resets all the encoders.


			while(!tcischars(1))
			{
				stgPtr->EncoderLatch();
				stgPtr->EncReadAll(temp);
		
				//printf("Encoder DATA: ");
				for (int i = 0; i < 8; i++)            // print out results of the encoders
					printf("|%d = %4d", i, temp[i].Long);						


				printf("\n");	
				for(i=0; i<100; i++)
					timer.wait();
			}

		break;

		case 17:
			miniControl->openlogLOHET();

			printf("Display LOHET and calibration \n");
		
			printf("Press enter when the loHET zeros out and you are ready to log\n");
			//Continously display the data before you press enter to start logging it.
			timer.start();
			while(!tcischars(1))
			{			
				miniControl->displayLOHET();
	
				//wait for a long time.
				for(i=0; i<50; i++)
					timer.wait();
			};
			cin.ignore();

			printf("press any key when you are done logging the data.");
			
			//miniConztrol->resetEncoder();  //resets the encoders to correspond zero with a zero LOHET reading.

			while(!tcischars(1))
			{	
				miniControl->displaylogLOHET();

				//wait for a long time.
				for(i=0; i<50; i++)
					timer.wait();
			}							

			miniControl->closelogLOHET();


			break;
		case 2:
			printf("Sinusoidal Commutation \n");
		
			// calibrate position with index signal (without HET signals)
//			miniControl->initWithIndex2();
			miniControl->initMiniEncoder(jointIndex,RIGHTARM);

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

			timer.start();

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

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

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

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


			break;

		case 7:
			printf("Testing init with index \n");
			miniControl->resetIndex();
			miniControl->initWithIndex();
//			miniControl->initWithIndex2();

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

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


			break;

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

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

			break;


		case 9:
			printf("Synching index with HET \n");
			miniControl->resetIndex();

			while(!tcischars(1))
			{
				while(!miniControl->hitIndex());
				printf("found index \n");
				miniControl->resetIndex();

				miniControl->readHET();
				miniControl->displayInfo();

			}
			break;
		case 5:
			printf("Testing Encoders\n");
			timer.start();

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

			break;

		case 6:
		{
			miniControl->resetEncoder();

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

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

			int exit = 0;

			while(!exit)
			{
				miniControl->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
				{
					miniControl->setGoal(elbowGoal);
					qgoal.zero();
					qgoal[EL1] = elbowGoal;

					// I get my goal position here (qfinal)
					// I should first calculate my time to achieve the motion
					// tfinal = 3(qfinal - qinitial)/(2*dqmax)
					// where I have previously defined my maximum desired joint velocity
					// then calculate my cubic spline parameters a0, a2, a3
					// a0 = qinitial
					// a2 = (3/tfinal^2)*(qfinal - qinitial)
					// a3 = -(2/tfinal^3)*(qfinal - qinitial)
					miniControl->readPosition();
		//			miniControl->resetVelocity();
					miniControl->calcPosition();	
		//			miniControl->getJointPosition(angle);
					miniControl->calcSpline(qgoal);

				}

		//		while(!tcischars(1));
		//		cin.ignore();

				// reset time to 0
				time = 0.0;

				printf("\n");
				timer.start();
	
				while(!tcischars(1) && !exit)
				{
					miniControl->readPosition();
					miniControl->calcPosition();	

					// at each servo tick calculate the desired position qdes(t)
					// and velocity dqdes(t) from the cubic spline equation
					//miniControl->calcPosition();
					miniControl->calcTraj(time);

					// increment time with the servo rate
					time += (1.0/HZ); 

					// calculate control torque based on qdes and dqdes
					// tau = Kp(qdes-q) + Kv(dqdes-dq)
					miniControl->jointControl();
					
					// command torque using sinusoidal com
		//			miniControl->readPosition();
		//			miniControl->calcPosition();	
//					miniControl->servoJoint();
					miniControl->displayAngle();	
			//		miniControl->displayInfo();	
					miniControl->saveInfo();
					misTick = timer.wait();
					if(misTick != 0)	
						printf("missed %d \n", misTick);
				}
			}
			break;
		}	// end of CASE 6

		// GRAVITY COMPENSATION FOR RIGHT ARM
		case 10:
		{
			printf("Release motor shaft and press any key to continue \n");
			while(!tcischars(1));
			cin.ignore();

			int exit = 0;

			timer.start();
			while(!tcischars(1))
			{
				miniControl->readPosition();
				miniControl->calcPosition();	
				miniControl->gravityCompensationRight();	// for RIGHT arml
				miniControl->saveInfo();
				timer.wait();
			}
			break;
		}	// END OF CASE 'A'

		// GRAVITY COMPENSATION FOR LEFT ARM
		case 13:
		{
//	I dont need to do mini init if I already ran the calibration process
//	once (case 11) and the computer is not shut off
#if 0
			miniControl->resetIndex();
			printf("Init EL1 mini \n");
			miniControl->initMiniEncoder(EL1, LEFTARM);
			miniControl->resetIndex();
			printf("Init SH1 mini \n");
			miniControl->initMiniEncoder(SH1, LEFTARM);
			miniControl->resetIndex();
			printf("Init WA1 mini \n");
			miniControl->initMiniEncoder(WA1, LEFTARM);
			miniControl->resetIndex();
#endif
//			miniControl->resetVelocity();

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

			int exit = 0;
			int misTick;
			
			timer.start();
			while(!tcischars(1))
			{
				miniControl->readPosition();
				miniControl->calcPosition();	
				miniControl->gravityCompensationLeft();	// for LEFT arm
				miniControl->saveInfo();
				misTick =timer.wait();
				if(misTick > 0)
						printf("%d\n", misTick);
			}
			break;
		}	// END OF CASE 'A'

		case 11:
		{
		//	int armSide = LEFTARM;
			int armSide = RIGHTARM;

			printf("Brushless Motor Initial Calibration Process\n");
			miniControl->resetIndex();
			printf("Init EL1 mini \n");
			miniControl->initMiniEncoder(EL1, armSide);
			miniControl->resetIndex();
			printf("Init SH1 mini \n");
			miniControl->initMiniEncoder(SH1, armSide);
			miniControl->resetIndex();
			printf("Init WA1 mini \n");
			miniControl->initMiniEncoder(WA1, armSide);
			miniControl->resetIndex();

//			miniControl->resetVelocity();

			printf("Release motor shaft and press any key to continue \n");

			timer.start();

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

				break;
		}	// END OF CASE 11

		case 12:
		{
				int jointSelect;
				printf("Bring the desired joint to its maximum angle \n");
				printf("and enter 0,1 or 2 to set the joint angle: \n");
				printf("[0] WA1 \n");
				printf("[1] SH1 \n");
				printf("[2] EL1 \n");
	
				cin>>jointSelect;

				miniControl->setJointAngleMax(jointSelect);

				break;		// end of case 12
		}
		case 18:
		{
				int jointSelect;
				printf("Bring the desired joint to its minimum angle \n");
				printf("and enter 0,1 or 2 to set the joint angle: \n");
				printf("[0] WA1 \n");
				printf("[1] SH1 \n");
				printf("[2] EL1 \n");
	
				cin>>jointSelect;

				miniControl->setJointAngleMin(jointSelect);

				break;		// end of case 12
		}

		case 14:
		{
				int jointSelect;

				printf("Finding index pulse and displaying corresponding joint angle \n");
				printf("Start with elbow joint \n");
				for(jointSelect = 2;jointSelect > -1 ; jointSelect--)
				{
					while(!miniControl->hitIndex2(2*jointSelect+1));
					miniControl->readPosition();
					miniControl->calcPosition();
					miniControl->displayInfo();
					printf("Next joint ... \n");
				}

				break;		// end of case 14
		}

		case 16:
		{
				int jointSelect;

				timer.start();

				printf("Finding index pulse and displaying corresponding joint angle \n");
				printf("Start with elbow joint \n");
				for(jointSelect = 2;jointSelect > -1 ; jointSelect--)
				{
					// HACK
					#if 0
					if(jointSelect == 0)
					{
						while(!miniControl->hitIndex2(7))
								timer.wait();
					}
					else
					{
						while(!miniControl->hitIndex2(2*jointSelect+1))
								timer.wait();
					}
					#endif
					while(!miniControl->hitIndex2(2*jointSelect+1))
							timer.wait();
					miniControl->setJointAngleAtIndex(jointSelect);
					miniControl->readPosition();
					miniControl->calcPosition();
					miniControl->displayInfo();
					miniControl->resetIndex();
					printf("Next joint ... \n");
				}

			timer.start();
			while(!tcischars(1))
			{
				miniControl->readPosition();
				miniControl->calcPosition();	
				miniControl->displayAngle();	
				miniControl->saveInfo();
				timer.wait();
			}
				break;		// end of case 14
		}
		case 15:
		{
				printf("Displaying End-Effector Position \n");

				timer.start();

				while(!tcischars(1))
				{
					miniControl->readPosition();
					miniControl->calcPosition();
					miniControl->calcX();
					timer.wait();
					
				}

				break;		// end of case 14
		}
	}

	while(!miniControl->shutdownMotors())
	{
		miniControl->saveInfo();
		timer.wait();
	}

	miniControl->saveBuffer();
//	miniControl->closeDataFile();
	delete miniControl;
}

void BLDC::calcSpline(PrVector qfinal)
{
	float dqmax = 25.0;	// max angular velocity in deg/s
	int i;

	// I get my goal position here (qfinal)
	for(i=0;i<NUM_JOINT;i++)
	{
		(*qgoal)[i] = qfinal[i];
		// I should first calculate my time to achieve the motion
		// tfinal = 3(qfinal - qinitial)/(2*dqmax)
		(*tfinal)[i] = 3*((*qgoal)[i] - (*q)[i])/(2*dqmax);
		// calculate cubic spline parameters
		(*a0)[i] = (*q)[i];	// a0 = qinitial 
		(*a2)[i] = (3/((*tfinal)[i]*(*tfinal)[i]))*((*qgoal)[i] - (*q)[i]);
		// a2 = (3/tfinal^2)*(qfinal - qinitial)
		(*a3)[i] = -(2/((*tfinal)[i]*(*tfinal)[i]*(*tfinal)[i]))*((*qgoal)[i] - (*q)[i]);
		// a3 = -(2/tfinal^3)*(qfinal - qinitial)
	}
	printf("q: %f, qgoal: %f \n", (*q)[EL1], (*qgoal)[EL1]);
	printf("tfinal: %f \n", (*tfinal)[EL1]);
	printf("a0: %f, a2: %f, a3: %f \n", (*a0)[EL1],(*a2)[EL1],(*a3)[EL1]);
}

void BLDC::calcTraj(float time)
{
	int i;
  	static int counter = 0;
	// at each servo tick calculate the desired position qdes(t)
	// and velocity dqdes(t) from the cubic spline equation

	// if (t < tfinal)
	// qdes = a0 + a2*t^2 + a3*t^3
	// dqdes = 2*a2*t + 3*a3*t^2

	// else if (t > tfinal)
	// qdes = qgoal
	// dqdes = 0

	for(i=0;i<NUM_JOINT;i++)
	{
		if(time < (*tfinal)[i])
		{
				(*qdes)[i] = (*a0)[i] + (*a2)[i]*time*time + (*a3)[i]*time*time*time;
				(*dqdes)[i] = 2*(*a2)[i]*time + 3*(*a3)[i]*time*time;
		}
		else
		{
				(*qdes)[i] = (*qgoal)[i];
				(*dqdes)[i] = 0.0;
		}
	}


	if(counter == 100)
	{
//		printf("time: %f, qdes: %f, dqdes: %f \n", time, (*qdes)[EL1], (*dqdes)[EL1]);
//		printf("q: %f, dq: %f, angle: %f \n", time, (*q)[EL1], (*dq)[EL1], angle[ELBOW_JOINT]);
		counter = 0;
	}
	counter++;
}

void BLDC::jointControl(void)
{
	float Kp[6] = {50.0,50.0,600.0,50.0,50.0,50.0};
	float Kv[6];
	float LIMIT = 1500.0;
	PrVector command(NUM_JOINT);
	int i;
	static int counter = 0;

	for(i=0;i<NUM_JOINT;i++)
	{
		Kv[i] = 5.0*1.0*2.0*sqrt(Kp[i]);
		command[i] = Kp[i]*((*qdes)[i] - (*q)[i]) + Kv[i]*((*dqdes)[i] - (*dq)[i]);
		if(command[i] > LIMIT)
			command[i] = LIMIT;
		if(command[i] < -LIMIT)
			command[i] = -LIMIT;

	}
	if(counter == 100)
	{
//		printf("command: %f, q: %f, dq: %f \n", command[EL1], (*q)[EL1], (*dq)[EL1]);
		counter = 0;
	}
	counter++;

	sinusoidCom(EL1,command[EL1]);
}

#define GRAVITY	9.8
#define KDAC	0.00244
#define KAMP	1.00	// 01/19/05, PST changed all amp. gain to 1.00

// CALL THIS FUNCTION TO DO GRAVITY COMP IN TESTING CONFIG (STRAIGHT UP ON TABLE)
PrVector BLDC::gravityCompensation (void)
{
	PrVector gravity(3), gravity2(3);
	float cx3 = 0.072, cy3 = 0.017, m3 = 1.194;
	float cx2 = 0.180, cy2 = 0.029, l2x = 0.449, l2y = 0.012, m2 = 3.089;
	float command[3];
	static float command2[3] = {0.0,0.0,0.0};
	float KT[3] = {0.22, 0.22, 0.115};	
	float GEAR[3] = {15.67, 14.28, 10.23};
	static int printCounter = 0;

	static float rampCounter = 0.0, rampTime = 200.0;

	// gravity calculation when SH1 and EL1 are together and SH1 is upright on 
	// the table
	gravity[EL1] = (cx3*cos(((*q)[EL1] + (*q)[SH1])/57.3) 
					- cy3*sin(((*q)[EL1]+(*q)[SH1])/57.3))*m3*GRAVITY; 

	gravity[SH1] = (cx2*cos((*q)[SH1]/57.3) - cy2*sin((*q)[SH1]/57.3))*m2*GRAVITY
					+ (l2x*cos((*q)[SH1]/57.3) - l2y*sin((*q)[SH1]/57.3) 
					+ cx3*cos(((*q)[SH1] + (*q)[EL1])/57.3) 
					- cy3*sin(((*q)[SH1] + (*q)[EL1])/57.3))*m3*GRAVITY;

	if(rampCounter < rampTime)
	{
		gravity2[SH1] = rampCounter*gravity[SH1]/rampTime; 
		gravity2[EL1] = rampCounter*gravity[EL1]/rampTime; 
		rampCounter = rampCounter + 1.0;
  	}
	else
	{
		gravity2[SH1] = gravity[SH1];
		gravity2[EL1] = gravity[EL1];
	}
	//HACK ALERT!!!!
	//I ADDED A FACTOR OF 1.1 SINCE THE SH1 AMPLIFIER SEEMS TO BE UNDERPERFORMING
	command[SH1] = 1.2*gravity2[SH1]/(KDAC*KAMP*KT[SH1]*GEAR[SH1]);	
	command[EL1] = 1.0*gravity[EL1]/(KDAC*KAMP*KT[EL1]*GEAR[EL1]);	

	//	command[SH1] = 1.0*gravity[SH1]/(KDAC*KAMP*KT[SH1]*GEAR[SH1]);	

	//temp hack
	sinusoidCom(SH1, command[SH1]);
	sinusoidCom(EL1, command[EL1]);

	if(printCounter == 100)
	{
		printf("angle: %f, %f, G: %f, %f, %d, %d \n", (*q)[SH1],(*q)[EL1], gravity[SH1], gravity[EL1],
					   (int)(command[SH1]),(int)(command[EL1]));
		printCounter = 0;
	}
	printCounter++;
	
	(*torqueCommand)[WA1] = gravity[WA1];
	(*torqueCommand)[SH1] = gravity[SH1];
	(*torqueCommand)[EL1] = gravity[EL1];
	return(gravity);

}

//static float m1 = 4.3861, m2 = 3.4693, m3 = 1.3684;	// joint mass
static float m1 = 4.3861, m2 = 3.4693, m3 = 1.7214;	// joint mass

// this is translation between each origin of local frame
static const float d2y = -0.109;	// origin of frame 2 from origin of frame 1
static const float d3x = 0.449, d3y = -0.012;	// origin of frame 3 from origin of frame 2

// this is location of center mass from origin of local frame
static const float l1x =  0.0310, l1y = 0.0107, l1z = 0.0108;
static const float l2x = 0.19, l2y = -0.0289, l2z = 0.0065;
//static const float l3x = 0.072, l3y = -0.0168, l3z = 0.0001;
static const float l3x = 0.1216, l3y = -0.0225, l3z = 0.00002;

static const float KT[3] = {0.22, 0.22, 0.115};	
static const float GEAR[3] = {15.67, 14.28, 10.23};

// CALL THIS FUNCTION TO DO GRAVITY COMP IN REAL ROBOT CONFIG (MOUNTED ON BODY)
PrVector BLDC::gravityCompensationRight (void)
{
	//RIGHT ARM GRAVITY COMPENSATION
	PrVector gravity(3), gravity2(3);

	float command[3], qt[3];
	static int printCounter = 0;
	static float rampCounter = 0.0, rampTime = 200.0;

	qt[WA1] = (*q)[WA1]/57.3;
	qt[SH1] = (*q)[SH1]/57.3;
	qt[EL1] = (*q)[EL1]/57.3;

	gravity[EL1] = (-sin(qt[WA1])*sin(qt[SH1] + qt[EL1])*l3x 
					- sin(qt[WA1])*cos(qt[SH1] + qt[EL1])*l3y)*m3*GRAVITY;

	gravity[SH1] = (-sin(qt[WA1])*sin(qt[SH1])*l2x - sin(qt[WA1])*cos(qt[SH1])*l2y)*m2*GRAVITY
					+ (-sin(qt[WA1])*sin(qt[SH1])*d3x - sin(qt[WA1])*cos(qt[SH1])*d3y 
							- sin(qt[WA1])*sin(qt[SH1]+qt[EL1])*l3x 
							- sin(qt[WA1])*cos(qt[SH1]+qt[EL1])*l3y)*m3*GRAVITY;

	gravity[WA1] = (cos(qt[WA1])*l1x - sin(qt[WA1])*l1z)*m1*GRAVITY
					+ (cos(qt[WA1])*cos(qt[SH1])*l2x - cos(qt[WA1])*sin(qt[SH1])*l2y 
							- sin(qt[WA1])*l2z)*m2*GRAVITY
					+ (cos(qt[WA1])*cos(qt[SH1])*d3x - cos(qt[WA1])*sin(qt[SH1])*d3y
							+ cos(qt[WA1])*cos(qt[SH1]+qt[EL1])*l3x
							- cos(qt[WA1])*sin(qt[SH1]+qt[EL1])*l3y
							- sin(qt[WA1])*l3z)*m3*GRAVITY;

	if(rampCounter < rampTime)
	{
		gravity2[WA1] = rampCounter*gravity[WA1]/rampTime; 
		gravity2[SH1] = rampCounter*gravity[SH1]/rampTime; 
		gravity2[EL1] = rampCounter*gravity[EL1]/rampTime; 
		rampCounter = rampCounter + 2.0;
  	}
	else
	{
		gravity2[WA1] = gravity[WA1];
		gravity2[SH1] = gravity[SH1];
		gravity2[EL1] = gravity[EL1];
	}

	// RIGHT ARM
	// need to flip sign for SH1 and EL1 to make CCW of motor to be positive torque
	command[WA1] = 1.00*gravity2[WA1]/(KDAC*KAMP*KT[WA1]*GEAR[WA1]);	
	command[SH1] = -1.00*gravity2[SH1]/(KDAC*KAMP*KT[SH1]*GEAR[SH1]);	
	command[EL1] = -1.0*gravity2[EL1]/(KDAC*KAMP*KT[EL1]*GEAR[EL1]);	

	//temp hack
//	sinusoidCom(WA1, command[WA1]);
	sinusoidCom(SH1, command[SH1]);
//	sinusoidCom(EL1, command[EL1]);

	if(printCounter == 100)
	{
		printf("angle: %6.2f, %6.2f, %6.2f, G: %6.2f, %6.2f, %6.2f, %5d, %5d, %5d \n", 
						(*q)[WA1], (*q)[SH1],(*q)[EL1], 
						gravity2[WA1], gravity2[SH1], gravity2[EL1],
					   (int)(command[WA1]), (int)(command[SH1]),(int)(command[EL1]));
		if((fabs(gravity[WA1]) > 9.5) || (fabs(gravity[SH1]) > 9.5))
				printf("WARNING!!! OVERLOAD MOTOR TORQUE \n");
	
		printCounter = 0;
	}
	printCounter++;
	
	(*torqueCommand)[WA1] = gravity[WA1];
	(*torqueCommand)[SH1] = gravity[SH1];
	(*torqueCommand)[EL1] = gravity[EL1];
	return(gravity);

}

// CALL THIS FUNCTION TO DO GRAVITY COMP OF LEFT ARM IN REAL ROBOT CONFIG (MOUNTED ON BODY)
PrVector BLDC::gravityCompensationLeft (void)
{
	//LEFT ARM GRAVITY COMPENSATION

	PrVector gravity(3), gravity2(3);

	float command[3], qt[3];
	static int printCounter = 0;
	static float rampCounter = 0.0, rampTime = 200.0;

	qt[WA1] = (*q)[WA1]/57.3;
	qt[SH1] = (*q)[SH1]/57.3;
	qt[EL1] = (*q)[EL1]/57.3;

	gravity[EL1] = (-sin(qt[WA1])*sin(qt[SH1] + qt[EL1])*l3x 
					- sin(qt[WA1])*cos(qt[SH1] + qt[EL1])*l3y)*m3*GRAVITY;

	gravity[SH1] = (-sin(qt[WA1])*sin(qt[SH1])*l2x - sin(qt[WA1])*cos(qt[SH1])*l2y)*m2*GRAVITY
					+ (-sin(qt[WA1])*sin(qt[SH1])*d3x - sin(qt[WA1])*cos(qt[SH1])*d3y 
							- sin(qt[WA1])*sin(qt[SH1]+qt[EL1])*l3x 
							- sin(qt[WA1])*cos(qt[SH1]+qt[EL1])*l3y)*m3*GRAVITY;

	gravity[WA1] = (cos(qt[WA1])*l1x + sin(qt[WA1])*l1z)*m1*GRAVITY
					+ (cos(qt[WA1])*cos(qt[SH1])*l2x - cos(qt[WA1])*sin(qt[SH1])*l2y 
							+ sin(qt[WA1])*l2z)*m2*GRAVITY
					+ (cos(qt[WA1])*cos(qt[SH1])*d3x - cos(qt[WA1])*sin(qt[SH1])*d3y
							+ cos(qt[WA1])*cos(qt[SH1]+qt[EL1])*l3x
							- cos(qt[WA1])*sin(qt[SH1]+qt[EL1])*l3y
							+ sin(qt[WA1])*l3z)*m3*GRAVITY;

	if(rampCounter < rampTime)
	{
		gravity2[WA1] = rampCounter*gravity[WA1]/rampTime; 
		gravity2[SH1] = rampCounter*gravity[SH1]/rampTime; 
		gravity2[EL1] = rampCounter*gravity[EL1]/rampTime; 
		rampCounter = rampCounter + 2.0;
  	}
	else
	{
		gravity2[WA1] = gravity[WA1];
		gravity2[SH1] = gravity[SH1];
		gravity2[EL1] = gravity[EL1];
	}

	// LEFT ARM
	// need to flip sign for SH1 and EL1 to make CCW of motor to be positive torque
	command[WA1] = -1.00*gravity2[WA1]/(KDAC*KAMP*KT[WA1]*GEAR[WA1]);	
	command[SH1] = -0.96*gravity2[SH1]/(KDAC*KAMP*KT[SH1]*GEAR[SH1]);	
	command[EL1] = -1.0*gravity2[EL1]/(KDAC*KAMP*KT[EL1]*GEAR[EL1]);	
	// NOTE FROM PST: 5% increase in torque output for SH2 based on gravity compensation test

	//temp hack
	sinusoidCom(WA1, command[WA1]);
	sinusoidCom(SH1, command[SH1]);
	sinusoidCom(EL1, command[EL1]);

	if(printCounter == 100)
	{
		printf("angle: %6.2f, %6.2f, %6.2f, G: %6.2f, %6.2f, %6.2f, %5d, %5d, %5d \n", 
						(*q)[WA1], (*q)[SH1],(*q)[EL1], 
						gravity2[WA1], gravity2[SH1], gravity2[EL1],
					   (int)(command[WA1]), (int)(command[SH1]),(int)(command[EL1]));
		if((fabs(gravity[WA1]) > 9.5) || (fabs(gravity[SH1]) > 9.5))
				printf("WARNING!!! OVERLOAD MOTOR TORQUE \n");
	
		printCounter = 0;
	}
	printCounter++;
	
	(*torqueCommand)[WA1] = gravity[WA1];
	(*torqueCommand)[SH1] = gravity[SH1];
	(*torqueCommand)[EL1] = gravity[EL1];
	return(gravity);

}


int BLDC::shutdownMotors(void)
{
	float KT[3] = {0.22, 0.22, 0.115};	
	float GEAR[3] = {15.67, 14.28, 10.23};
	static float rampCounter = 0.0, rampTime = 100.0;
	float torqueShutdown[3];

	if(rampCounter < rampTime)
	{
			rampCounter = rampCounter + 1.0;
			torqueShutdown[WA1] = (*torqueCommand)[WA1] - (rampCounter/rampTime)*(*torqueCommand)[WA1];
			torqueShutdown[SH1] = (*torqueCommand)[SH1] - (rampCounter/rampTime)*(*torqueCommand)[SH1];
			torqueShutdown[EL1] = (*torqueCommand)[EL1] - (rampCounter/rampTime)*(*torqueCommand)[EL1];

			sinusoidCom(WA1, torqueShutdown[WA1]/(KDAC*KAMP*KT[WA1]*GEAR[WA1]));
			sinusoidCom(SH1, torqueShutdown[SH1]/(KDAC*KAMP*KT[SH1]*GEAR[SH1]));
			sinusoidCom(EL1, torqueShutdown[EL1]/(KDAC*KAMP*KT[EL1]*GEAR[EL1]));
	//		printf("torque: %f, %f, %d \n", torqueShutdown[SH1], (*torqueCommand)[SH1], torque);
			return(0);
	}
	else
			return(1);


}

// position vector of end-effector tip from origin of [ELBOW] defined
// in local frame
static const float eex = 0.36328, eey = -0.03889, eez = 0.0;
// Function to calculate end-effector position of left and right arm
PrVector BLDC::calcX(void)
{
	PrVector xPos(6);
	float qt[3];
	qt[WA1] = (*q)[WA1]/57.3;
	qt[SH1] = (*q)[SH1]/57.3;
	qt[EL1] = (*q)[EL1]/57.3;

	// LEFT ARM END-EFFECTOR X,Y,Z POS
	(*xLeft)[0] = cos(qt[WA1])*cos(qt[SH1])*d3x - cos(qt[WA1])*sin(qt[SH1])*d3y
					+ cos(qt[WA1])*cos(qt[SH1]+qt[EL1])*eex
					- cos(qt[WA1])*sin(qt[SH1]+qt[EL1])*eey
					+ sin(qt[WA1])*eez;

	(*xLeft)[1] = -d2y - sin(qt[SH1])*d3y - cos(qt[SH1])*d3y - sin(qt[SH1]+qt[EL1])*eex
					- cos(qt[SH1]+qt[EL1])*eey;

	(*xLeft)[2] = sin(qt[WA1])*cos(qt[SH1])*d3x - sin(qt[WA1])*sin(qt[SH1])*d3y
					+ sin(qt[WA1])*cos(qt[SH1]+qt[EL1])*eex
					- sin(qt[WA1])*sin(qt[SH1]+qt[EL1])*eey
					- cos(qt[WA1])*eez;

	printf("x: %5.3f, y: %5.3f, z: %5.3f \r", (*xLeft)[0], (*xLeft)[1], (*xLeft)[2]);

	// LEFT ARM END-EFFECTOR X,Y,Z POS
	(*xRight)[0] = cos(qt[WA1])*cos(qt[SH1])*d3x - cos(qt[WA1])*sin(qt[SH1])*d3y
					+ cos(qt[WA1])*cos(qt[SH1]+qt[EL1])*eex
					- cos(qt[WA1])*sin(qt[SH1]+qt[EL1])*eey
					- sin(qt[WA1])*eez;

	(*xRight)[1] = d2y + sin(qt[SH1])*d3y + cos(qt[SH1])*d3y + sin(qt[SH1]+qt[EL1])*eex
					+ cos(qt[SH1]+qt[EL1])*eey;

	(*xRight)[2] = sin(qt[WA1])*cos(qt[SH1])*d3x - sin(qt[WA1])*sin(qt[SH1])*d3y
					+ sin(qt[WA1])*cos(qt[SH1]+qt[EL1])*eex
					- sin(qt[WA1])*sin(qt[SH1]+qt[EL1])*eey
					+ cos(qt[WA1])*eez;

	xPos[0] = (*xRight)[0];
	xPos[1] = (*xRight)[1];
	xPos[2] = (*xRight)[2];
	xPos[3] = (*xLeft)[0];
	xPos[4] = (*xLeft)[1];
	xPos[5] = (*xLeft)[2];
	return(xPos);
}