
#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 "motorControl.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
motorControl::motorControl()
{
	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;
	}

	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);
	dq = new PrVector(NUM_JOINT1);
	q->zero();	dq->zero();

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

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

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

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

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

int motorControl::hitIndex(int channel)
{
	if(stg->GetIndexLatches() && (1<<channel))
		return(1);
	else
		return(0);
}


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

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

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

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

}

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

void motorControl::resetVelocity(void)
{
	int i;
	for(i=0;i<8;i++)
	{
	//	velocity[ELBOW_JOINT] = 0.0;
	//	velocityOld[ELBOW_JOINT] = 0.0;
	//	angleOld[ELBOW_JOINT] = encoder[ELBOW_JOINT].Long/(float)(CANNON_COUNT_PER_DEG);
		velocity[i] = 0.0;
		velocityOld[i] = 0.0;
		angleOld[i] = encoder[ELBOW_JOINT].Long/(float)(CANNON_COUNT_PER_DEG);
		
		if(i<6)
			(*dq)[i] = 0.0;

	}
}

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

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

	eAngle[ELBOW_MINI] = mAngle[ELBOW_MINI] * MECH_TO_ELEC;

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

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

	eAngle[SHOULDER_MINI] = mAngle[SHOULDER_MINI] * MECH_TO_ELEC;

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

	if(counter < 10)
	{
		velocityOld[ELBOW_JOINT] = velocity[ELBOW_JOINT];
		velocityOld[SHOULDER_JOINT] = velocity[SHOULDER_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];

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

	angleOld[SHOULDER_JOINT] = angle[SHOULDER_JOINT];
	velocityOld[SHOULDER_JOINT] = velocity[SHOULDER_JOINT];

	int i,jointIndex;
	for(i=0;i<NUM_JOINT;i++)
	{
		jointIndex = 2*i + 1;
//		printf("i: %d, j: %d \n", i, jointIndex);
		(*q)[i] = angle[jointIndex];
		(*dq)[i] = velocity[jointIndex];
	}

//	printf("q: %f, dq: %f \n", (*q)[2], (*dq)[2]);
}
void motorControl::readPosition(void)
{
	stg->EncoderLatch();
	stg->EncReadAll(encoder);

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

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

	stg->RawDAC(V_CHANNEL,vCommand);
}

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

//	if(counter == HZ)
	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++;
}

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

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

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

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

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

void motorControl::servoJoint(void)
{
//	float Kp = 80.0;
	float Kp = 50.0;
	float Kv = 1.0*2.0*sqrt(Kp);
	float command;

	float velocityDes = (Kp/Kv)*(goal[ELBOW_JOINT] - angle[ELBOW_JOINT]);
	//float VLIMIT = 130.0;
	float VLIMIT = 70.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 = 800.0;

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

	sinusoidCom(command);

}


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

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


void motorControl::saveInfo(void)
{
	static long int index = 0;
	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]);

	if(index < BUFFER_SIZE)
	{
		dataBuffer1->elementAt(index,0) = (float)(encoder[ELBOW_MINI].Long);
		dataBuffer1->elementAt(index,1) = eAngle[ELBOW_MINI];
		dataBuffer1->elementAt(index,2) = (float)(uCommand);
		dataBuffer1->elementAt(index,3) = (float)(vCommand);
		dataBuffer1->elementAt(index,4) = (float)(uCurrent);
		dataBuffer1->elementAt(index,5) = (float)(vCurrent);
		dataBuffer1->elementAt(index,6) = torque;

		dataBuffer2->elementAt(index,0) = (float)(encoder[ELBOW_JOINT].Long);
		dataBuffer2->elementAt(index,1) = angle[ELBOW_JOINT];
		dataBuffer2->elementAt(index,2) = velocity[ELBOW_JOINT];
		dataBuffer2->elementAt(index,3) = (*qdes)[EL1];
		dataBuffer2->elementAt(index,4) = (*dqdes)[EL1];

//	printf("%8d , %f \n", encoder[ELBOW_MINI].Long, dataBuffer1->elementAt(index,0));	
		index++;
	}
}

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

	for(i=0;i<BUFFER_SIZE;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 motorControl::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 motorControl::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 motorControl::initMiniEncoder(int jointIndex)
{
	//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("Rotate motor shaft SLOWLY until index pulse is detected \n");

	// wait for index pulse
	while(!hitIndex(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);

	// EL1: 348.8386
	// SH1: 180.41 
	// WA1: 112.47
	static float miniIndexPulseAngle[6] = {112.47, 0.0, 180.41, 276.8, 348.84, 36.8};
	eAngle[miniIndex] = miniIndexPulseAngle[jointIndex] + angleShift;

//	eAngle[miniIndex] = 348.8386 + angleShift;	// for EL1
//	eAngle[miniIndex] = 180.41 + angleShift;	// for SH1
//	eAngle[miniIndex] = 112.47 + angleShift;	// for WA1
//	eAngle[miniIndex] = 36.8 + angleShift;	// for EL2
//	eAngle[miniIndex] = 276.8 + angleShift;	// for SH2

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

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

void motorControl::initJointEncoder(int jointIndex)
{
	// look for index pules from joint encoder
	printf("waiting for joint encoder index \n");
	while(!hitIndex(2*jointIndex + 1));
	printf("index found \n");
	
	readPosition();
	printf("Joint index hit \n");

}

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

}

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

}


void motorControl::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 motorControl::setSplineParameters(float a0_[], float a2_[], float a3_[], float tfinal_)
{
	int i = 0;

	for(i=0;i<3;i++)
	{
		(*a0)[i] = a0_[i];
		(*a2)[i] = a2_[i];
		(*a3)[i] = a3_[i];
		(*tfinal)[i] = tfinal_;
	}

}
void motorControl::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 motorControl::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;

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

	sinusoidCom(command[EL1]);
}
