/*
	control.cpp
	-----------

	Module that handles low-level control of base and mini actuators. Include
	SEA control and all hardware interface.

   */

#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/PrVector3.h"
#include "matrix/PrMatrix.h"
#include "control.h"
#include "max547Driver.h"	// custom MAX547 interface driver
#include "Definitions.h"
#include "sysID.h"

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

#define HZ  1000

#define BUFFER_SIZE		120*HZ	// buffer to hold 30 seconds of data	
#define BUFFER_COL		10	

#define NUM_JOINT	3
#define NUM_JOINT1	6

// BASE MOTOR CONSTANTS
const float KDAC_BASE[3] = {10.0/4096.0, 10.0/4096.0, 5.0/4096.0};	// DAC gain, V/count
const float KAMP_BASE[3] =	{2.0, 2.0, 2.0};		// amplifier gain, A/V
#define KTORQUE_BASE	0.0727	// motor torque constant, N.m./A
#define limitDAC2	1500
#define ADCFACTOR	10.0/4096.0
#define EL1BIAS		0
float gearRatioBase[3] = {120.0,100.0,50.0};

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


// CONSTRUCTOR FUNCTION
dm2Control::dm2Control(ServoToGo *stgPtr)
{
	int i;
	stgRight = stgPtr;

	initSystem();
	initVariables();	// create and init all data objects
	initDataBuffer();	// create data buffer matrices

	initTorqueMeasFromEncoder();
	initActuatorJacobian();
	initJointLimit();
	initInertiaMatrices();

	dacRight = new max547Driver(stgRight);
}

// DESTRUCTOR FUNCTION
dm2Control::~dm2Control()
{
	delete dacRight;
	delete stgRight;
	closeDataFile();
}

// -------------------------------------------------------------------------- //
// PUT ALL INITIALIZING FUNCTIONS HERE //

// INIT FUNCTIONS
void dm2Control::initSystem(void)
{
//	stgRight->CalADC();
	stgRight->Timer2Delay(DELAY_1MS);
	stgRight->EncoderInit();
	stgRight->EncoderLatch();
	initIndex();
}

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

void dm2Control::initVariables(void)
{
	int i;

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

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

	J = new PrMatrix(3,3);
	Jw = new PrMatrix(3,3);
	// Init linear and angular velocity jacobians for used in A matrix calc.
	Jv1 = new PrMatrix(3,3); Jv1->zero();
	Jv2 = new PrMatrix(3,3); Jv2->zero();
	Jv3 = new PrMatrix(3,3); Jv3->zero();
	Jw1 = new PrMatrix(3,3); Jw1->zero();
	Jw2 = new PrMatrix(3,3); Jw2->zero();
	Jw3 = new PrMatrix(3,3); Jw3->zero();

	AMatrix = new PrMatrix(3,3);
	AMatrixInv = new PrMatrix(3,3);
	Lambda = new PrMatrix(3,3);
	LambdaInv = new PrMatrix(3,3);

	tjspace = new PrVector(NUM_JOINT1);
	baseTorqueDes = new PrVector(3);
	jointTorqueDes = new PrVector(3);
	torqueCommand = new PrVector(NUM_JOINT1);
	torqueCommand->zero();

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

	// end-effector cartesian coordinates
	xLeft = new PrVector(3);
	xRight = new PrVector(3);
	dxLeft = new PrVector(3);
	dxRight = new PrVector(3);

	dxLeft->zero(); dxRight->zero();	
}

// Function to create and populate the inertia matrices
void dm2Control::initInertiaMatrices(void)
{
	I1 = new PrMatrix(3,3); I2 = new PrMatrix(3,3); I3 = new PrMatrix(3,3);

	I1->zero(); I2->zero(); I3->zero();
	
	const float Ixx1 = 0.0273, Iyy1 = 0.0290, Izz1 = 0.0304;
	const float Ixx2 = 0.0112, Iyy2 = 0.2319, Izz2 = 0.2363, Ixy2 = -0.0284;
	const float Ixx3 = 0.0035, Iyy3 = 0.0546, Izz3 = 0.0564;

	I1->elementAt(0,0) = Ixx1;
	I1->elementAt(1,1) = Iyy1;
	I1->elementAt(2,2) = Izz1;

	I2->elementAt(0,0) = Ixx2;
	I2->elementAt(1,1) = Iyy2;
	I2->elementAt(2,2) = Izz2;
	I2->elementAt(0,1) = Ixy2;
	I2->elementAt(1,0) = Ixy2;

	I3->elementAt(0,0) = Ixx3;
	I3->elementAt(1,1) = Iyy3;
	I3->elementAt(2,2) = Izz3;
}

void dm2Control::initActuatorJacobian(void)
{
	// The actuator jacobian exists due to the kinematic coupling caused
	// by the use of cable drive which couples the motion of joint 3 to the
	// motion of joint 2 and the motion of joint 2 to the motion of joint 1.
	
	// In terms of torque transmission, the torque at joint 1 is a linear combination
	// of actuator torques at 1,2 and 3. Torque at joint 2 is a linear combination of
	// actuator torques at 2 and 3.
	J1 = new PrMatrix(3,3);
	J2 = new PrMatrix(3,3);

	J1->zero(); J2->zero();

	//J1 is the transpose of the actuator jacobian
	J1->elementAt(0,0) = 1.0;
	J1->elementAt(0,1) = -0.4933;
	J1->elementAt(0,2) = -0.3553;
	J1->elementAt(1,1) = 1.0;
	J1->elementAt(1,2) = -1.0;
	J1->elementAt(2,2) = 1.0;

	//J2 is the inverse of J1
	(*J2) = J1->operator~();

	// 04/07/05
	// PST added factor of 0.8 to this since with 1.0 tb1 is overcompensating
	// for closed-loop G
	J1->elementAt(0,1) = 0.8*-0.4933;

	J1->display("J1");
	J2->display("J2");

}

void dm2Control::initJointLimit(void)
{
	// JOINT LIMITS FOR ARM
	jointLimitMax[WA1] = 74.63;
//	jointLimitMax[SH1] = 29.86 - 4.44;
	jointLimitMax[SH1] = 29.86;
	jointLimitMax[EL1] = 144.22 - 4.0;
	jointLimitMin[WA1] = -74.77;
	jointLimitMin[SH1] = -125.73;
	jointLimitMin[EL1] = 5.52;
}

void dm2Control::initDataBuffer(void)
{
	// create and init data buffer matrices
	bufferIndex = 0;
	dataBuffer1 = new PrMatrix(BUFFER_SIZE,BUFFER_COL);
	dataBuffer2 = new PrMatrix(BUFFER_SIZE,BUFFER_COL);
	dataBuffer3 = new PrMatrix(BUFFER_SIZE,BUFFER_COL);
	dataBuffer4 = new PrMatrix(BUFFER_SIZE,BUFFER_COL);
	dataBuffer5 = new PrMatrix(BUFFER_SIZE,BUFFER_COL);
	dataBuffer6 = new PrMatrix(BUFFER_SIZE,BUFFER_COL);
	dataBuffer7 = new PrMatrix(BUFFER_SIZE,BUFFER_COL);
	dataBuffer1->zero();dataBuffer2->zero();dataBuffer3->zero();
	dataBuffer4->zero();dataBuffer5->zero();dataBuffer6->zero();
}

void dm2Control::initTorqueMeasFromEncoder(void)
{
	readPosition();
	calcPosition();
	calcPreloadSpringTorque();

	int i;

	for(i=0;i<3;i++)
	{
		initBaseOutputAngle[i] = baseOutputAngle[i];
		initOutputShaftAngle[i] = (*q)[i];
	}

//	printf("WA: init base output: %f, init base load: %f \n", initBaseOutputAngle[WA1],
//					initOutputShaftAngle[WA1]);
}

// END OF INIT FUNCTIONS
// ------------------------------------------------------------------------------- //

void dm2Control::resetIndex(void)
{
	stgRight->ResetIndexLatches(0x00);
}

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

int dm2Control::hitIndex2(int channel)
{
	int check = 1<<channel;
	int result = stgRight->GetIndexLatches();
	if(stgRight->GetIndexLatches() & (1<<channel))
			return(1);
	else
		return(0);
}

// ------------------------------------------------------------------------------- //
// DATA BUFFER AND DATA FILE RELATED FUNCTIONS

void dm2Control::openDataFile(void)
{
	fdata = fopen("datamotor.txt","w");
//	fdata2 = fopen("datatest.txt","w");
	fdata3 = fopen("dataelbow.txt","w");
	fdata4 = fopen("datashoulder.txt","w");
	fdata5 = fopen("datamini.txt","w");
	fdata6 = fopen("dataee.txt","w");
	fdata7 = fopen("datajcontrol.txt","w");
}

void dm2Control::closeDataFile(void)
{
	fclose(fdata);
//	fclose(fdata2);
	fclose(fdata3);
	fclose(fdata4);
	fclose(fdata5);
	fclose(fdata6);
	fclose(fdata7);
}

void dm2Control::saveInfo(void)
{
	if(bufferIndex < BUFFER_SIZE)
	{
		dataBuffer1->elementAt(bufferIndex,0) = (*q)[WA1];	// waist joint position
		dataBuffer1->elementAt(bufferIndex,1) = (*jointTorqueDes)[WA1];
		dataBuffer1->elementAt(bufferIndex,2) = (*baseTorqueDes)[WA1];
		dataBuffer1->elementAt(bufferIndex,3) = baseTorqueCommand[WA1];
		dataBuffer1->elementAt(bufferIndex,4) = baseSpringTorque[WA1];	// measured spring torque
		dataBuffer1->elementAt(bufferIndex,5) = LOHET[WA1];
		dataBuffer1->elementAt(bufferIndex,6) = baseJointTorque[WA1];	// measured spring torque
		dataBuffer1->elementAt(bufferIndex,7) = baseSpringTorqueEnc[WA1];	// measured spring torque
//		dataBuffer1->elementAt(bufferIndex,7) = baseOutputAngle[WA1];	// harmonic drive output angle
		dataBuffer1->elementAt(bufferIndex,8) = (float)(baseCommand[WA1]);		// waist base motor command 
		
		dataBuffer3->elementAt(bufferIndex,0) = (*q)[EL1];	// elbow joint position
		dataBuffer3->elementAt(bufferIndex,1) = (*jointTorqueDes)[EL1];
		dataBuffer3->elementAt(bufferIndex,2) = (*baseTorqueDes)[EL1];
		dataBuffer3->elementAt(bufferIndex,3) = baseTorqueCommand[EL1];
		dataBuffer3->elementAt(bufferIndex,4) = baseSpringTorque[EL1];	// measured spring torque
		dataBuffer3->elementAt(bufferIndex,5) = LOHET[EL1];
		dataBuffer3->elementAt(bufferIndex,6) = baseJointTorque[EL1];	// measured spring torque
//		dataBuffer3->elementAt(bufferIndex,7) = baseOutputAngle[EL1];	// harmonic drive output angle
		dataBuffer3->elementAt(bufferIndex,8) = (float)(baseCommand[EL1]);		// elbow base motor command 

		dataBuffer4->elementAt(bufferIndex,0) = (*q)[SH1];	// shoulder joint position
		dataBuffer4->elementAt(bufferIndex,1) = (*jointTorqueDes)[SH1];
		dataBuffer4->elementAt(bufferIndex,2) = (*baseTorqueDes)[SH1];
		dataBuffer4->elementAt(bufferIndex,3) = baseTorqueCommand[SH1];	// mini torque command
		dataBuffer4->elementAt(bufferIndex,4) = baseSpringTorque[SH1];	// measured spring torque
		dataBuffer4->elementAt(bufferIndex,5) = LOHET[SH1];
		dataBuffer4->elementAt(bufferIndex,6) = baseJointTorque[SH1];	// measured spring torque
//		dataBuffer4->elementAt(bufferIndex,7) = baseOutputAngle[SH1];	// harmonic drive output angle
		dataBuffer4->elementAt(bufferIndex,8) = (float)(baseCommand[SH1]);		// shoulder base motor command 

		dataBuffer5->elementAt(bufferIndex,0) = miniTorqueDes[WA1];	// waist mini torque des
		dataBuffer5->elementAt(bufferIndex,1) = miniTorqueDes[SH1];	// shoulder mini torque des
		dataBuffer5->elementAt(bufferIndex,2) = miniTorqueDes[EL1];	// elbow mini torque des
		dataBuffer5->elementAt(bufferIndex,3) = miniCommand[WA1];	// waist mini torque dac command
		dataBuffer5->elementAt(bufferIndex,4) = miniCommand[SH1];	// shoulder mini torque dac command
		dataBuffer5->elementAt(bufferIndex,5) = miniCommand[EL1];	// elbow mini torque dac command

		dataBuffer6->elementAt(bufferIndex,0) = (*xRight)[0];
		dataBuffer6->elementAt(bufferIndex,1) = (*xRight)[1];
		dataBuffer6->elementAt(bufferIndex,2) = (*xRight)[2];
		dataBuffer6->elementAt(bufferIndex,3) = (*dxRight)[0];
		dataBuffer6->elementAt(bufferIndex,4) = (*dxRight)[1];
		dataBuffer6->elementAt(bufferIndex,5) = (*dxRight)[2];
		dataBuffer6->elementAt(bufferIndex,6) = (*dq)[0];
		dataBuffer6->elementAt(bufferIndex,7) = (*dq)[1];
		dataBuffer6->elementAt(bufferIndex,8) = (*dq)[2];

		dataBuffer7->elementAt(bufferIndex,0) = (*qdes)[WA1];
		dataBuffer7->elementAt(bufferIndex,1) = (*qdes)[SH1];
		dataBuffer7->elementAt(bufferIndex,2) = (*qdes)[EL1];
		dataBuffer7->elementAt(bufferIndex,3) = (*tjspace)[WA1];
		dataBuffer7->elementAt(bufferIndex,4) = (*tjspace)[SH1];
		dataBuffer7->elementAt(bufferIndex,5) = (*tjspace)[EL1];
		dataBuffer7->elementAt(bufferIndex,6) = ttraj;
//		dataBuffer7->elementAt(bufferIndex,6) = brakeMode;
		dataBuffer7->elementAt(bufferIndex,7) = (*dqdes)[WA1];
		dataBuffer7->elementAt(bufferIndex,8) = (*dqdes)[SH1];
		dataBuffer7->elementAt(bufferIndex,9) = (*dqdes)[EL1];

		bufferIndex++;
	}

}

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

	for(i=0;i<bufferIndex;i++)
	{
		fprintf(fdata,"%7.3f %7.3f %6.3f %6.2f %7.3f %6.4f %7.3f %8.4f %5.0f\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),dataBuffer1->elementAt(i,7),
						dataBuffer1->elementAt(i,8));

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

		fprintf(fdata3,"%7.3f %7.3f %6.3f %6.2f %7.3f %6.4f %7.3f %8.4f %5.0f\n",
						dataBuffer3->elementAt(i,0),dataBuffer3->elementAt(i,1),
						dataBuffer3->elementAt(i,2),dataBuffer3->elementAt(i,3),
						dataBuffer3->elementAt(i,4),dataBuffer3->elementAt(i,5),
						dataBuffer3->elementAt(i,6),dataBuffer3->elementAt(i,7),
						dataBuffer3->elementAt(i,8));

		fprintf(fdata4,"%7.3f %7.3f %6.3f %6.2f %7.3f %6.4f %7.3f %8.4f %5.0f\n",
						dataBuffer4->elementAt(i,0),dataBuffer4->elementAt(i,1),
						dataBuffer4->elementAt(i,2),dataBuffer4->elementAt(i,3),
						dataBuffer4->elementAt(i,4),dataBuffer4->elementAt(i,5),
						dataBuffer4->elementAt(i,6),dataBuffer4->elementAt(i,7),
						dataBuffer4->elementAt(i,8));

		fprintf(fdata5,"%7.2f %7.2f %7.2f %6.0f %6.0f %6.0f\n",
						dataBuffer5->elementAt(i,0),dataBuffer5->elementAt(i,1),
						dataBuffer5->elementAt(i,2),dataBuffer5->elementAt(i,3),
						dataBuffer5->elementAt(i,4),dataBuffer5->elementAt(i,5));

		fprintf(fdata6,"%6.4f %6.4f %6.4f %7.4f %7.4f %7.4f %7.3f %7.3f %7.3f\n",
						dataBuffer6->elementAt(i,0),dataBuffer6->elementAt(i,1),
						dataBuffer6->elementAt(i,2),dataBuffer6->elementAt(i,3),
						dataBuffer6->elementAt(i,4),dataBuffer6->elementAt(i,5),
						dataBuffer6->elementAt(i,6),dataBuffer6->elementAt(i,7),
						dataBuffer6->elementAt(i,8));

		fprintf(fdata7,"%7.3f %7.3f %7.3f %6.2f %6.2f %6.2f %7.2f %7.2f %7.2f %7.2f\n",
						dataBuffer7->elementAt(i,0),dataBuffer7->elementAt(i,1),
						dataBuffer7->elementAt(i,2),dataBuffer7->elementAt(i,3),
						dataBuffer7->elementAt(i,4),dataBuffer7->elementAt(i,5),
						dataBuffer7->elementAt(i,6),dataBuffer7->elementAt(i,7),
						dataBuffer7->elementAt(i,8),dataBuffer7->elementAt(i,9));
	}

	closeDataFile();
}	

// END OF DATA BUFFER AND DATA FILE RELATED FUNCTIONS
// ------------------------------------------------------------------------------- //


// ------------------------------------------------------------------------------- //
// SENSOR RELATED FUNCTIONS

void dm2Control::readHET(void)
{
	HETStatus = stgRight->RawDI(PORTD) & HET_MASK;

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

}

void dm2Control::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 dm2Control::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 < 10)
		{
			(*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]);
			(*dq)[i] = 1.0*0.9391*(*dqOld)[i] + 60.9181*((*q)[i] - (*qOld)[i]);
			(*dqOld)[i] = (*dq)[i];	// store old velocity information
		}

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

		// base actuator information
		// this is the "theoretical" harmonic drive output angle
		baseOutputAngle[WA1] = (float)(baseEncoder[WA1].Long)*360.0*(1.0/120.0)/8192.0;
		baseOutputAngle[SH1] = (float)(baseEncoder[SH1].Long)*360.0*(1.0/100.0)/8192.0;
		baseOutputAngle[EL1] = (float)(baseEncoder[EL1].Long)*360.0*(1.0/50.0)/8192.0;
	}

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

	if(counter < 10)
		counter++;
}

void dm2Control::readPosition(void)
{
	// read encoder counts from right arm STG card
	stgRight->EncoderLatch();
	stgRight->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;

	// base actuator encoders
	baseEncoder[WA1].Long = temp[BASEWA1].Long;
	baseEncoder[SH1].Long = temp[BASESH1].Long;
	baseEncoder[EL1].Long = temp[BASESH1].Long;

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


}

void dm2Control::resetBaseEncoder(void)
{
	stgRight->SetEncoderCounts(BASEWA1,0);
	stgRight->SetEncoderCounts(BASESH1,0);

}

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

	for(i=0;i<3;i++)
		position[i] = (*q)[i];
}
// 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 dm2Control::setJointAngle(int jointIndex)
{
	stgRight->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));
}

void dm2Control::readLOHET(void)
{
	// LOHET voltage
	LOHET[WA1] = (float)(stgRight->GetADC(WA1))*ADCFACTOR;
	LOHET[SH1] = (float)(stgRight->GetADC(SH1))*ADCFACTOR;
	LOHET[EL1] = (float)(stgRight->GetADC(EL1))*ADCFACTOR;

	lowPassFilter();

	const float wa1 = -0.0354, wa2 = 0.15, wa3 = -14.9, wa4 = -0.00336;	// big magnet
	const float sh1 = 0.101, sh2 = -13.6, sh3 = -0.0138;
	const float el1 = 0.0245, el2 = -0.16, el3 = 7.24, el4 = -0.300;

	//WA1 torque
	// using cubic fit
	baseSpringTorque[WA1] = wa1*LOHET[WA1]*LOHET[WA1]*LOHET[WA1] 
			+ wa2*LOHET[WA1]*LOHET[WA1] + wa3*LOHET[WA1] + wa4; 

	//SH1 torque
	baseSpringTorque[SH1] = sh1*LOHET[SH1]*LOHET[SH1] 
			+ sh2*LOHET[SH1] + sh3 + 0.0; 	//3.5
	//added +3.5 Nm offset, March 30 2005; gravity comp works with this
//	baseSpringTorque[SH1] = 0.714*baseSpringTorque[SH1] + 2.4;	// 3.5	// 4.5 // 1.6
//	baseSpringTorque[SH1] = 0.714*baseSpringTorque[SH1] + 1.5;	// 3.5	// 4.5 // 1.6
	baseSpringTorque[SH1] = 0.714*baseSpringTorque[SH1] - 1.3;	// 3.5	// 4.5 // 1.6
	//added this relationship to correct for slope and offset of the sensor reading
	//March 31, 2005; seems to work for SH G-comp


	//EL1 torque
	baseSpringTorque[EL1] = el1*LOHET[EL1]*LOHET[EL1]*LOHET[EL1]
			+ el2*LOHET[EL1]*LOHET[EL1] + el3*LOHET[EL1] + el4;

	baseSpringTorque[EL1] = 0.9*baseSpringTorque[EL1] - 1.3;

	// added -1.3 Nm offset, March 30 2005; gravity comp works with this

	baseLoadTorque[WA1] = baseSpringTorque[WA1]*1.54;	// 1.54 is the pulley reduction ratio
	baseLoadTorque[SH1] = baseSpringTorque[SH1]*1.54;
	baseLoadTorque[EL1] = baseSpringTorque[EL1]*1.54;

	//estimate ideal joint torque applied from linear combination
	//of individual base torque (due to kinematic coupling)
	baseJointTorque[WA1] = baseLoadTorque[WA1] + 0.4933*baseLoadTorque[SH1] + 0.8496*baseLoadTorque[EL1];
	//use ratio of 0.6 for the coupling torque, March 30 2005
	baseJointTorque[SH1] = baseLoadTorque[SH1] + 1.0*baseLoadTorque[EL1];
	baseJointTorque[EL1] = baseLoadTorque[EL1];
	
	baseSpringTorqueEnc[WA1] = ((baseOutputAngle[WA1]-initBaseOutputAngle[WA1])
					- 1.54*((*q)[WA1] - initOutputShaftAngle[WA1]))*(152.0/57.3)
					+ basePreloadTorque[WA1];

	// for elbow, the motor output angle is inverted
//	baseSpringTorqueEnc[EL1] = (-1.0*(baseOutputAngle[EL1]-initBaseOutputAngle[EL1])
//					- 1.54*((*q)[EL1] - initOutputShaftAngle[EL1]))*(57.0/57.3)
//					+ basePreloadTorque[EL1];
}


void dm2Control::lowPassFilter(void)
{
	int i;
	// 3RD ORDER BUTTERWORTH AT WC = 70Hz, Wsampling = 1000Hz
	int samplingRate = HZ;

#if (HZ == 1000)
	const float a1 = 0.006874, a2 = 0.02062, a3 = 0.02062, a4 = 0.006874;	//1000Hz
	const float b1 = 2.14, b2 = 1.613, b3 = 0.417;
#endif
#if(HZ == 500)
	const float a1 = 0.03618, a2 = 0.1085, a3 = 0.1085, a4 = 0.03618;	//500Hz
	const float b1 = 1.377, b2 = 0.8457, b3 = 0.1795;
#endif

	static float outOld[6] = {LOHET[WA1],LOHET[SH1],LOHET[EL1],0.0,0.0,0.0};
	static float outOld2[6] = {LOHET[WA1],LOHET[SH1],LOHET[EL1],0.0,0.0,0.0};
	static float outOld3[6] = {LOHET[WA1],LOHET[SH1],LOHET[EL1],0.0,0.0,0.0};
	float in[6] = {0.0,0.0,0.0,0.0,0.0,0.0};
	static float inOld[6] = {LOHET[WA1],LOHET[SH1],LOHET[EL1],0.0,0.0,0.0};
	static float inOld2[6] = {LOHET[WA1],LOHET[SH1],LOHET[EL1],0.0,0.0,0.0};
	static float inOld3[6] = {LOHET[WA1],LOHET[SH1],LOHET[EL1],0.0,0.0,0.0};

	static int counter = 0;

	for(i=0;i<3;i++)
	{
	//	i = 2;
		in[i] = LOHET[i];
		LOHET[i] = b1*outOld[i] - b2*outOld2[i] + b3*outOld3[i]
					+ a1*in[i] + a2*inOld[i] + a3*inOld2[i] + a4*inOld3[i];

		outOld3[i] = outOld2[i];
		outOld2[i] = outOld[i];
		outOld[i] = LOHET[i];
		inOld3[i] = inOld2[i];
		inOld2[i] = inOld[i];
	    inOld[i] = in[i];	
	}
}

//	END OF SENSOR RELATED FUNCTIONS
// ------------------------------------------------------------------------------- //



// ------------------------------------------------------------------------------- //
// DATA DISPLAYING FUNCTIONS
#define DISPLAY_WAIST	0
#define DISPLAY_SHOULDER	0
#define DISPLAY_ELBOW	0
#define DISPLAY_ALL		0
#define DISPLAY_ALL2	1
#define DISPLAY_RATE	(int)(HZ)

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

#if DISPLAY_WAIST
	if(counter == DISPLAY_RATE)
	{
		printf("WA q: %7.2f mq: %7.2f G: %6.2f LT: %6.3f LTC: %6.3f DAC: %d \n",
				(*q)[WA1],baseOutputAngle[WA1],
				(*torqueCommand)[WA1], 
				baseSpringTorque[WA1]*1.54, loadTorqueCommand[WA1], baseCommand[WA1]);
		counter = 0;
	}
	counter++;
#endif

#if DISPLAY_SHOULDER
	if(counter == DISPLAY_RATE)
	{
		printf("SH q: %7.2f mq: %7.2f G: %6.2f LT: %6.3f, %6.3f LTC: %6.3f DAC: %d \n",
				(*q)[SH1],baseOutputAngle[SH1],
				(*torqueCommand)[SH1], 
				baseSpringTorque[SH1]*1.54, baseSpringTorqueEnc[SH1]*1.54, 
				loadTorqueCommand[SH1], baseCommand[SH1]);
		counter = 0;
	}
	counter++;
#endif

#if DISPLAY_ELBOW
	if(counter == DISPLAY_RATE)
	{
		printf("EL q: %7.2f mq: %7.2f G: %6.2f LT: %6.3f, %6.3f LTC: %6.3f DAC: %d \n",
				(*q)[EL1],baseOutputAngle[EL1],
				(*torqueCommand)[EL1], 
				baseSpringTorque[EL1]*1.54, baseSpringTorqueEnc[EL1]*1.54, 
				loadTorqueCommand[EL1], baseCommand[EL1]);
		counter = 0;
	}
	counter++;
#endif

#if DISPLAY_ALL
	if(counter == DISPLAY_RATE)
	{
//		printf("q: %7.2f. %7.2f %7.2f G: %6.2f %6.2f %6.2f LT: %6.3f %6.3f %6.3f DAC: %d %d %d \n",
//				(*q)[EL1],(*q)[SH1],(*q)[WA1],
//				loadTorqueCommand[EL1], loadTorqueCommand[SH1], loadTorqueCommand[WA1],
//				baseSpringTorque[EL1]*1.54, baseSpringTorque[SH1]*1.54, baseSpringTorque[WA1]*1.54, 
//				baseCommand[EL1], baseCommand[SH1], baseCommand[WA1]);
		printf("q: %7.2f. %7.2f %7.2f G: %6.2f %6.2f %6.2f LT: %6.3f %6.3f %6.3f DAC: %d %d %d \n",
				(*q)[WA1],(*q)[SH1],(*q)[EL1],
				loadTorqueCommand[WA1], loadTorqueCommand[SH1], loadTorqueCommand[EL1],
				baseJointTorque[WA1], baseJointTorque[SH1], baseJointTorque[EL1], 
				baseCommand[WA1], baseCommand[SH1], baseCommand[EL1]);
		counter = 0;
	}
	counter++;
#endif

#if DISPLAY_ALL2
	if(counter == DISPLAY_RATE)
	{
		printf("q: %7.2f. %7.2f %7.2f G: %6.2f %6.2f %6.2f LT: %6.3f %6.3f %6.3f DAC: %d %d %d \n",
				(*q)[WA1],(*q)[SH1],(*q)[EL1],
			//	(*baseTorqueDes)[EL1], (*baseTorqueDes)[SH1], (*baseTorqueDes)[WA1],
				baseTorqueCommand[WA1], baseTorqueCommand[SH1], baseTorqueCommand[EL1],
				baseSpringTorque[WA1], baseSpringTorque[SH1], baseSpringTorque[EL1], 
				baseCommand[WA1], baseCommand[SH1], baseCommand[EL1]);
		counter = 0;
	}
	counter++;
#endif

}

void dm2Control::displayInfo2(void)
{
	static int counter = 0;
#if 1
	if(counter == DISPLAY_RATE)
	{
		printf("q: W%6.2f S%6.2f E%6.2f G: %5.2f %5.2f %6.2f ST: %5.2f %5.2f %5.2f \n",
				(*q)[WA1],(*q)[SH1],(*q)[EL1],
				gravity[WA1], gravity[SH1],gravity[EL1],
				baseSpringTorque[WA1], baseSpringTorque[SH1], baseSpringTorque[EL1]);
		counter = 0;
	}
	counter++;
#endif
}

void dm2Control::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++;
}
// END OF DATA DISPLAYING FUNCTIONS
// ------------------------------------------------------------------------------- //

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


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

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

// ------------------------------------------------------------------------------- //
// TORQUE AND TORQUE COMMAND RELATED FUNCTIONS

// Write DAC count to STG card by writing the corresponding u and v phase of the mini motor
void dm2Control::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)
	{
		stgRight->RawDAC(0+jointIndex*2,uCommand[jointIndex]);
		stgRight->RawDAC(1+jointIndex*2,vCommand[jointIndex]);
	}
	else
		printf("DAC write channel error \n");
	
}

void dm2Control::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 dm2Control::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);
		stgRight->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);
		stgRight->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 dm2Control::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);
	stgRight->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 dm2Control::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);
	stgRight->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 dm2Control::setDirection(int dir)
{
	direction = dir;

}

void dm2Control::readMotorCurrent(void)
{
	uCurrent = stgRight->GetADC(0);
	vCurrent = stgRight->GetADC(2);

}
void dm2Control::setSplineParameters(float a0_[], float a2_[], float a3_[], float tfinal_[], float qfinal[])
{
	int i = 0;

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

void dm2Control::calcSpline(PrVector qfinal)
{
	float dqmax = 5.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] = fabs(3*((*qgoal)[i] - (*q)[i])/(2*dqmax));
		// calculate cubic spline parameters

		if((*tfinal)[i] > 0.0)
		{
			(*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)
		}
		else
		{
			(*a0)[i] = 0.0;
			(*a2)[i] = 0.0;
			(*a3)[i] = 0.0;
		}
	}
	printf("EL joint: \n");
	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]);

	printf("SH joint: \n");
	printf("q: %f, qgoal: %f \n", (*q)[SH1], (*qgoal)[SH1]);
	printf("tfinal: %f \n", (*tfinal)[SH1]);
	printf("a0: %f, a2: %f, a3: %f \n", (*a0)[SH1],(*a2)[SH1],(*a3)[SH1]);

	printf("WA joint: \n");
	printf("q: %f, qgoal: %f \n", (*q)[WA1], (*qgoal)[WA1]);
	printf("tfinal: %f \n", (*tfinal)[WA1]);
	printf("a0: %f, a2: %f, a3: %f \n", (*a0)[WA1],(*a2)[WA1],(*a3)[WA1]);
}

// Create cubic spline trajectory based on parameters calculated in user.cpp
// and updated through the shared memory object
void dm2Control::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;
		}
	}

	ttraj = time;

#if 0
	if(counter == 100)
	{
		printf("t: %f, q: %f %f %f, qdes: %f %f %f \n",
					time, (*q)[EL1], (*q)[SH1], (*q)[WA1],
					(*qdes)[EL1], (*qdes)[SH1], (*qdes)[WA1]);
		counter = 0;
	}
	counter++;
#endif
}

// Used for demo to create velocity trajectory for the robot to track
void dm2Control::calcTrajVel(float time)
{
	int i;
  	static int counter = 0;
	static float dqMax[3], thalf[3], tstop[3];
	// at each servo tick calculate the desired position qdes(t)
	// and velocity dqdes(t) from the cubic spline equation

	for(i=0;i<NUM_JOINT;i++)
	{
		thalf[i] = 0.5*(*tfinal)[i];
		tstop[i] = 0.71 + thalf[i];	// 0.78

		if(time < thalf[i])
		{
				(*qdes)[i] = (*qgoal)[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;
				dqMax[i] = (*dqdes)[i];
				brakeMode = 0;
		}
		else if( (thalf[i] < time) && (time < tstop[i]))
		{
				(*qdes)[i] = (*qgoal)[i];
				(*dqdes)[i] = dqMax[i];
				brakeMode = 0;
		}
		else if(time > tstop[i])
		{
				(*qdes)[i] = (*q)[i];
				(*dqdes)[i] = 0.0;
				// THIS IS THE BUG HERE	
				// for the demo velocity control, only switch to brake mode
				// when the 2nd shoulder axis time passes the stop time
				if(i==1)
					brakeMode = 1;
		}
	}

//	(*qdes)[WA1] = (*qgoal)[WA1];
	(*qdes)[WA1] = (*q)[WA1];
//	(*qdes)[EL1] = (*q)[EL1];

	(*qdes)[SH1] = (*q)[SH1];
	(*dqdes)[WA1] = 0.0;
	(*dqdes)[EL1] = 0.0;

	ttraj = time;

}

void dm2Control::setJointControlMode(int mode)
{
	if(mode == POS_MODE)
		jMode = POS_MODE;
	else if (mode == DAMP_MODE)
		jMode = DAMP_MODE;
	else if (mode == VEL_MODE)
		jMode = VEL_MODE;
}

void dm2Control::jointControl(void)
{
//	const float Kp[6] = {80.0, 100.0, 0.0,0.0,0.0,0.0};
	const float Kp[6] = {120.0, 120.0, 40.0,0.0,0.0,0.0};
	// Kp[0] = 80.0; Kp[1] = 100.0;
	const float Kv[6] = {12.0, 6.0, 3.0,0.0,0.0,0.0};
	float KvServoMode[6] = {0.0, 110.0, 0.0,0.0,0.0,0.0};
	// Kv[0] = 9.0; Kv[1] = 9.0;
	float tlimit[3] = {30.0, 30.0, 10.0};
	int i;
	static int counter = 0;

	// standard joint position control
	if(jMode == POS_MODE)
	{
		for(i=0;i<NUM_JOINT;i++)
		{
			(*tjspace)[i] = Kp[i]*(((*qdes)[i] - (*q)[i])/57.3) 
					+ 1.0*Kv[i]*((0.4*(*dqdes)[i] - (*dq)[i])/57.3);
			if((*tjspace)[i] > tlimit[i])
				(*tjspace)[i] = tlimit[i];
			if((*tjspace)[i] < -tlimit[i])
				(*tjspace)[i] = -tlimit[i];
		}
	}
	// pure joint velocity damping mode
	else if(jMode == VEL_MODE)
	{
		// for the demo, joint 0 and 2 are at position hold mode
		for(i=0;i<NUM_JOINT;i++)
		{
			(*tjspace)[i] = Kp[i]*(((*qdes)[i] - (*q)[i])/57.3) 
					+ 1.0*Kv[i]*((0.4*(*dqdes)[i] - (*dq)[i])/57.3);
			if((*tjspace)[i] > tlimit[i])
				(*tjspace)[i] = tlimit[i];
			if((*tjspace)[i] < -tlimit[i])
				(*tjspace)[i] = -tlimit[i];
		}

		// and joint 1 is at velocity control mode
		if(brakeMode == 0)
			(*tjspace)[SH1] = 1.0*KvServoMode[SH1]*(((*dqdes)[SH1] - (*dq)[SH1])/57.3);
	}

#if 0
	if(counter == 100)
	{
//		printf("(*tjspace): %f, q: %f, dq: %f \n", (*tjspace)[EL1], (*q)[EL1], (*dq)[EL1]);
		printf("qdes: %6.2f %6.2f %6.2f, q: %6.2f %6.2f %6.2f, c: %6.2f %6.2f %6.2f \n",
					(*qdes)[EL1], (*qdes)[SH1], (*qdes)[WA1],
					(*q)[EL1], (*q)[SH1], (*q)[WA1],
					(*tjspace)[EL1], (*tjspace)[SH1], (*tjspace)[WA1]);
	
		counter = 0;
	}
	counter++;
#endif
}

// ----------------------------------------------------------------------- //
// DYNAMIC AND KINEMATIC RELATED FUNCTIONS

//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 with JR3 attached

// 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;	// no JR3
static const float l3x = 0.1216, l3y = -0.0225, l3z = 0.00002;	// with JR3 attached

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)
PrVector3 dm2Control::gravityCompensationRight (void)
{

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

	gravity[EL1] = (-sin(qt[WA1])*sin(qt23Right)*l3x 
					- sin(qt[WA1])*cos(qt23Right)*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(qt23Right)*l3x 
							- sin(qt[WA1])*cos(qt23Right)*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(qt23Right)*l3x
							- cos(qt[WA1])*sin(qt23Right)*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];
	}
*/
	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]);	
	// NOTE FOR MACRO TORQUE SIGN
	// WAIST JOINT: positive DAC = positive torque; vice versa
	// SHOULDER JOINT: negative DAC = positive torque; vice versa
	// ELBOW JOINT: negative DAC = positive torque; vice versa
	
//	(*torqueCommand)[WA1] = gravity2[WA1];
//	(*torqueCommand)[SH1] = gravity2[SH1];
//	(*torqueCommand)[EL1] = gravity2[EL1];
	return(gravity);

}

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

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


	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] = -1.05*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);

}



// 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;
static const float eex = 0.36328 + 0.03375, eey = -0.03889, eez = 0.0;	// with JR3 mounted on EE

// April 4, 2005
// function to calculate end-effector jacobian matrix manually
// will probably be replaced later with function from dynamic engine
void dm2Control::calcJacobian(void)
{
	// basic linear velocity jacobian for end-effector position
	//first column
	J->elementAt(0,0) = - sin(qt[WA1])*cos(qt23Right)*eex 
						+ sin(qt[WA1])*sin(qt23Right)*eey
						- cos(qt[WA1])*eez
						- sin(qt[WA1])*cos(qt[SH1])*d3x
						+ sin(qt[WA1])*sin(qt[SH1])*d3y;
	J->elementAt(1,0) = 0.0;
	J->elementAt(2,0) = cos(qt[WA1])*cos(qt23Right)*eex
						- cos(qt[WA1])*sin(qt23Right)*eey
						- sin(qt[WA1])*eez
						+ cos(qt[WA1])*cos(qt[SH1])*d3x
						- cos(qt[WA1])*sin(qt[SH1])*d3y;
	//second column
	J->elementAt(0,1) = - cos(qt[WA1])*sin(qt23Right)*eex
		   				- cos(qt[WA1])*cos(qt23Right)*eey
						- cos(qt[WA1])*sin(qt[SH1])*d3x
						- cos(qt[WA1])*cos(qt[SH1])*d3y;

	J->elementAt(1,1) = cos(qt[SH1]+qt[EL1])*eex
						- sin(qt[SH1]+qt[EL1])*eey
						+ cos(qt[SH1])*d3x
						- sin(qt[SH1])*d3y;

	J->elementAt(2,1) = - sin(qt[WA1])*sin(qt23Right)*eex
						- sin(qt[WA1])*cos(qt23Right)*eey
						- sin(qt[WA1])*sin(qt[SH1])*d3x
						- sin(qt[WA1])*cos(qt[SH1])*d3y;

	//third column
	J->elementAt(0,2) = - cos(qt[WA1])*sin(qt23Right)*eex
						- cos(qt[WA1])*cos(qt23Right)*eey;

	J->elementAt(1,2) = cos(qt23Right)*eex
						- sin(qt23Right)*eey;

	J->elementAt(2,2) = - sin(qt[WA1])*sin(qt23Right)*eex
						- sin(qt[WA1])*cos(qt23Right)*eey;	


	// basic angular velocity jacobian for end-effector orientation
}

// Function to calculate linear and angular velocity jacobians for used in
// inertia matrix calculation
void dm2Control::calcAllJacobians(void)
{
	// LINEAR VELOCITY JACOBIANS FOR CENTER OF MASS
	Jv1->elementAt(0,0) = - sin(qt[WA1])*l1x - cos(qt[WA1])*l1z;
	Jv1->elementAt(2,0) = cos(qt[WA1])*l1x - sin(qt[WA1])*l1z;

	Jv2->elementAt(0,0) = - sin(qt[WA1])*cos(qt[SH1])*l2x
							+ sin(qt[WA1])*sin(qt[SH1])*l2y
							- cos(qt[WA1])*l2z;
	Jv2->elementAt(2,0) = cos(qt[WA1])*cos(qt[SH1])*l2x
							- cos(qt[WA1])*sin(qt[SH1])*l2y
							- sin(qt[WA1])*l2z;
	Jv2->elementAt(0,1) = - cos(qt[WA1])*sin(qt[SH1])*l2x 
							- cos(qt[WA1])*cos(qt[SH1])*l2y;
	Jv2->elementAt(1,1) = cos(qt[SH1])*l2x - sin(qt[SH1])*l2y;
	Jv2->elementAt(2,1) = - sin(qt[WA1])*sin(qt[SH1])*l2x 
							- sin(qt[WA1])*cos(qt[SH1])*l2y;

	//first column of Jv3
	Jv3->elementAt(0,0) = - sin(qt[WA1])*cos(qt23Right)*l3x
							- sin(qt[WA1])*sin(qt23Right)*l3y
							- cos(qt[WA1])*l3z
							- sin(qt[WA1])*cos(qt[SH1])*d3x
							+ sin(qt[WA1])*sin(qt[SH1])*d3y;
//	Jv3->elementAt(1,0) = 0.0;
	Jv3->elementAt(2,0) = cos(qt[WA1])*cos(qt23Right)*l3x
							- cos(qt[WA1])*sin(qt23Right)*l3y
							- sin(qt[WA1])*l3z
							+ cos(qt[WA1])*cos(qt[SH1])*d3x
							- cos(qt[WA1])*sin(qt[SH1])*d3y;

	//2nd column of Jv3
	Jv3->elementAt(0,1) = - cos(qt[WA1])*sin(qt23Right)*l3x
							- cos(qt[WA1])*cos(qt23Right)*l3y
							- cos(qt[WA1])*sin(qt[SH1])*d3x
							- cos(qt[WA1])*cos(qt[SH1])*d3y;

	Jv3->elementAt(1,1) = cos(qt23Right)*l3x 
							- sin(qt23Right)*l3y 
							+ cos(qt[SH1])*d3x 
							- sin(qt[SH1])*d3y;
						
	Jv3->elementAt(2,1) = - sin(qt[WA1])*sin(qt23Right)*l3x
							- sin(qt[WA1])*cos(qt23Right)*l3y
							- sin(qt[WA1])*sin(qt[SH1])*d3x
							- sin(qt[WA1])*cos(qt[SH1])*d3y;

	//3rd column of Jv3
	Jv3->elementAt(0,2) = - cos(qt[WA1])*sin(qt23Right)*l3x
							- cos(qt[WA1])*cos(qt23Right)*l3y;

	Jv3->elementAt(1,2) = cos(qt23Right)*l3x - sin(qt23Right)*l3y;
			
	Jv3->elementAt(2,2) = - sin(qt[WA1])*sin(qt23Right)*l3x - sin(qt[WA1])*cos(qt23Right)*l3y;

	// ANGULAR VELOCITY JACOBIANS FOR CENTER OF MASS

	Jw1->elementAt(1,0) = -1.0;

	Jw2->elementAt(0,0) = - sin(qt[SH1]);
	Jw2->elementAt(1,0) = - cos(qt[SH1]);
	Jw2->elementAt(2,1) = 1.0;

	Jw3->elementAt(0,0) = - sin(qt23Right);
	Jw3->elementAt(1,0) = - cos(qt23Right);
	Jw3->elementAt(2,1) = 1.0;
	Jw3->elementAt(2,2) = 1.0;

}

#define DEBUG_DYNAMICS	1
void dm2Control::calcAMatrix(void)
{
	PrMatrix temp1(3,3), temp2(3,3), temp3(3,3);
//	PrMatrix temp4(3,3);
	static int counter = 0;
	
	temp1 = Jv1->transpose()*(*Jv1);
	temp1.multiply(m1,temp1);
	temp1 += Jw1->transpose()*(*I1)*(*Jw1);

	temp2 = Jv2->transpose()*(*Jv2);
	temp2.multiply(m2,temp2);
	temp2 += Jw2->transpose()*(*I2)*(*Jw2);

	temp3 = Jv3->transpose()*(*Jv3);
	temp3.multiply(m3,temp3);
	temp3 += Jw3->transpose()*(*I3)*(*Jw3);

	(*AMatrix) = temp1 + temp2 + temp3;

#if DEBUG_DYNAMICS
//	temp4 = Jw3->transpose()*(*I3)*(*Jw3);
//	temp4 = temp3 - temp4;
	if(counter == int(HZ))
	{
		AMatrix->display("A:");
		printf("\n");
/*		temp3.display("temp3:");
		printf("\n");
		I1->display("I1");
		printf("\n");
		Jw1->display("Jw1");
		printf("\n");
	//	printf("%f, %f \n", (l3x*l3x + l3y*l3y)*m3 + 0.0564, qt23Right*57.3);

	//	printf("%f \n", m3*(Jv3->elementAt(0,2)*Jv3->elementAt(0,2) 
	//					+ Jv3->elementAt(1,2)*Jv3->elementAt(1,2)
	//					+ Jv3->elementAt(2,2)*Jv3->elementAt(2,2)));
	//	printf("cos: %f, sin: %f \n", cos(qt23Right), sin(qt23Right));

		temp4.display("temp4 ");
		printf("\n");
*/		counter = 0;
	}
	counter++;
#endif
}

// Function to calculate operational space mass matrix with the end-effector as 
// the operational point
void dm2Control::calcLambdaMatrix(void)
{
	static int counter = 0;

	(*AMatrixInv) = AMatrix->operator~();	//get A inverse
	
	(*LambdaInv) = (*J)*(*AMatrixInv)*J->transpose();
	(*Lambda) = LambdaInv->operator~();

#if DEBUG_DYNAMICS
	if(counter == int(HZ))
	{
		J->display("J:");
		printf("\n");
		Lambda->display("Lambda:");
		printf("\n");
		counter = 0;
	}
	counter++;
#endif
}

// Function to calculate end-effector position of left and right arm
PrVector dm2Control::calcX(void)
{
	PrVector xPos(6);
	PrVector dqtemp(3);
	dqtemp[0] = (*dq)[WA1]/57.3;	// joint velocity converted to rad/s
	dqtemp[1] = (*dq)[SH1]/57.3;
	dqtemp[2] = (*dq)[EL1]/57.3;

	static int counter = 0;
	int i;
//	qt[WA1] = (*q)[WA1]/57.3;
//	qt[SH1] = (*q)[SH1]/57.3;
//	qt[EL1] = (*q)[EL1]/57.3;

#if 0
	// 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]);
#endif

	// RIGHT 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])*d3x + 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;
#if 0
	xPos[0] = (*xRight)[0];
	xPos[1] = (*xRight)[1];
	xPos[2] = (*xRight)[2];
	xPos[3] = (*xLeft)[0];
	xPos[4] = (*xLeft)[1];
	xPos[5] = (*xLeft)[2];
#endif

	// calculate end-point velocity using Jacobian and joint velocity
	*dxRight = (*J)*(dqtemp);	// endpoint cartesian velocity in m/s
	
	return(xPos);
}

// END OF DYNAMIC AND KINEMATIC RELATED FUNCTIONS
// ----------------------------------------------------------------------- //

// ------------------------------------------------------------------------- //
// FUNCTIONS RELATE TO CALCULATING AND SETTING TORQUE COMMANDS

// FUNCTION TO RAMP UP BASE TORQUE COMMAND FROM STARTING PRELOAD VALUE TO WHATEVER INITIAL
// VALUE WE WANT
// ONLY RUN THIS ONCE AT THE START OF EVERY PROGRAM
#define RAMP_TIME 1.0	// was 1.0
int dm2Control::modulateBaseTorque(float time, int dacCountDes)
{
	int dacCount;

	if(time < RAMP_TIME)
	{
		// basePreloadTorqueCount is the preload torque on the spring converted to
		// equivalent DAC count
		dacCount = basePreloadTorqueCount[WA1] + (int)(time * (dacCountDes-basePreloadTorqueCount[WA1]));
	}
	else
		dacCount = dacCountDes;

//	printf("t: %f, dac: %d \n", time, dacCount);
	return(dacCount);
}

// This function calculates desired joint torque based on
// the overall task.
void dm2Control::calcJointTorqueDes(int userMode)
{
	switch(userMode)
	{
		case(FLOAT):
		{
		//	(*jointTorqueDes)[WA1] = 0.95*gravity[WA1] - 2.3;
	//		(*jointTorqueDes)[WA1] = 0.95*gravity[WA1] - 3.1;
			(*jointTorqueDes)[WA1] = 0.95*gravity[WA1] - 1.5;
		//	(*jointTorqueDes)[SH1] = gravity[SH1] - 0.9;
			(*jointTorqueDes)[SH1] = gravity[SH1] - 0.0;
			(*jointTorqueDes)[EL1] = gravity[EL1] + 0.2;
			break;
		}
		case(JOINT_SERVO):
		{
			(*jointTorqueDes)[WA1] = (*tjspace)[WA1] + 0.95*gravity[WA1] - 1.7;
			(*jointTorqueDes)[SH1] = (*tjspace)[SH1] + gravity[SH1];
			(*jointTorqueDes)[EL1] = (*tjspace)[EL1] + gravity[EL1];
			break;
		}
		case(VEL_SERVO):
		{
			(*jointTorqueDes)[WA1] = (*tjspace)[WA1] + 0.95*gravity[WA1] - 1.4;
			(*jointTorqueDes)[SH1] = (*tjspace)[SH1] + gravity[SH1] - 0.0;
			(*jointTorqueDes)[EL1] = (*tjspace)[EL1] + gravity[EL1] + 0.2;
			break;
		}
		case(JOINT_DAMPING):
		{
			(*jointTorqueDes)[WA1] = (*tjspace)[WA1] + 0.95*gravity[WA1] - 2.3;
			(*jointTorqueDes)[SH1] = (*tjspace)[SH1] + gravity[SH1];
			(*jointTorqueDes)[EL1] = (*tjspace)[EL1] + gravity[EL1];
			break;
		}
	}
	
//	printf("%f \n", (*jointTorqueDes)[WA1]);
}

// This function takes as input a vector of desired joint torque
// and calculates the required desired base actuator torque based
// on kinematic coupling relationship using J1 matrix
void dm2Control::calcBaseTorqueDes(float time)
{
	PrVector *baseTorqueDes_ = new PrVector(3);
	static PrVector baseTorqueInit(3);
	
//	*baseTorqueDes_ = (*J1)*(*jointTorqueDes);
	(*baseTorqueDes_)[WA1] = (*jointTorqueDes)[WA1] - 0.3946*(*jointTorqueDes)[SH1] 
			- 0.7*0.3553*(*jointTorqueDes)[EL1];
//	(*baseTorqueDes_)[WA1] = (*jointTorqueDes)[WA1];
	(*baseTorqueDes_)[SH1] = (*jointTorqueDes)[SH1] - 0.3*1.0*(*jointTorqueDes)[EL1];
	(*baseTorqueDes_)[EL1] = (*jointTorqueDes)[EL1];
	
		int i;


	if (time < 0.002)
	{
		printf("t: %f \n",time);
		baseTorqueInit[WA1] = baseSpringTorque[WA1];
		baseTorqueInit[SH1] = baseSpringTorque[SH1];
		baseTorqueInit[EL1] = baseSpringTorque[EL1];
	}

	if(time < RAMP_TIME)
	{
		// the preload level is measured at the spring
		// Thus, it needs to be multipied by the pulley reduction ratio (1.54) since the torque command
		// is for the load in Nm
		for(i=0;i<3;i++)
		{
		//	(*baseTorqueDes)[i] = 1.54*basePreloadTorque[i] + time*((*baseTorqueDes_)[i] - 1.54*basePreloadTorque[i]);
			(*baseTorqueDes)[i] = 1.54*baseTorqueInit[i] + (time/RAMP_TIME)*((*baseTorqueDes_)[i] - 1.54*baseTorqueInit[i]);
		}
	}
	else
	{
		for(i=0;i<3;i++)
			(*baseTorqueDes)[i] = (*baseTorqueDes_)[i];
	}

//	printf("%f %f %f\n", (*jointTorqueDes)[WA1], (*baseTorqueDes)[WA1], (*baseTorqueDes_)[WA1]);

}

// overloaded function. This one has no input parameter.
// The load torque command is summation of all the desired tasks
void dm2Control::setLoadTorqueCommand(int mode)
{
	// the output of the joint space controller is added to the gravity torque
	// and becomes the load torque command
	// which then goes to the macro-mini loop
	gravityTorqueCommand[WA1] = 0.95*gravity[WA1] - 3.00;	// was 2.5
	gravityTorqueCommand[SH1] = gravity[SH1];
	gravityTorqueCommand[EL1] = gravity[EL1];

	if(mode == FLOAT)
	{
		loadTorqueCommandDes[WA1] = gravityTorqueCommand[WA1];
		loadTorqueCommandDes[SH1] = gravityTorqueCommand[SH1];
		loadTorqueCommandDes[EL1] = gravityTorqueCommand[EL1];
	}
	else if (mode == JOINT_SERVO)
	{
		loadTorqueCommandDes[WA1] = (*tjspace)[WA1] + gravityTorqueCommand[WA1];
		loadTorqueCommandDes[SH1] = (*tjspace)[SH1] + gravityTorqueCommand[SH1];
		loadTorqueCommandDes[EL1] = (*tjspace)[EL1] + gravityTorqueCommand[EL1];
	}
}

// overloaded function. This one takes as input parameter the desired joint torque command
void dm2Control::setLoadTorqueCommand(float loadTorqueCommand_[])
{
	gravityTorqueCommand[WA1] = 0.95*gravity[WA1] - 2.0;
	gravityTorqueCommand[SH1] = gravity[SH1];
	gravityTorqueCommand[EL1] = gravity[EL1];
	loadTorqueCommandDes[WA1] = gravityTorqueCommand[WA1];
	loadTorqueCommandDes[SH1] = gravityTorqueCommand[SH1];
	loadTorqueCommandDes[EL1] = gravityTorqueCommand[EL1];
//	loadTorqueCommandDes[WA1] = loadTorqueCommand_[WA1];
//	loadTorqueCommandDes[SH1] = loadTorqueCommand_[SH1];
//	loadTorqueCommandDes[EL1] = loadTorqueCommand_[EL1];
}

// Modulate (ramp up) initial closed-loop torque command so that it's not a step command
void dm2Control::modulateLoadTorqueCommand(float time)
{
	int i;
	if(time < RAMP_TIME)
	{
		// the preload level is measured at the spring
		// Thus, it needs to be multipied by the pulley reduction ratio (1.54) since the torque command
		// is for the load in Nm
		for(i=0;i<3;i++)
		{
		//	loadTorqueCommand[i] = basePreloadTorque[i]*1.54 + time*(loadTorqueCommandDes[i] - basePreloadTorque[i]*1.54);
			loadTorqueCommand[i] = baseJointTorque[i] + time*(loadTorqueCommandDes[i] - baseJointTorque[i]);
		}
	}
	else
	{
		for(i=0;i<3;i++)
			loadTorqueCommand[i] = loadTorqueCommandDes[i];
	}

//	printf("loadTorqueCommand[EL1]: %f \n", loadTorqueCommand[EL1]);
//	printf("t: %f, torque com: %5.2f \n", time, loadTorqueCommand[WA1]);
}

// END OF FUNCTIONS RELATE TO CALCULATING AND SETTING TORQUE COMMANDS
// ------------------------------------------------------------------------- //
void dm2Control::calcPreloadSpringTorque(void)
{
	readPosition();
	calcPosition();
	gravityCompensationRight();
	readLOHET();

	for(int i=0;i<3;i++)
	{
		basePreloadTorque[i] = baseSpringTorque[i];
		basePreloadTorqueCount[i] = (int)(basePreloadTorque[i]/(KTORQUE_BASE*KAMP_BASE[i]*KDAC*gearRatioBase[i]));
	}

	printf("Preload (at the spring) level: E %6.2f, S: %6.2f, W %6.2f \n", 
					basePreloadTorque[EL1], basePreloadTorque[SH1],
					basePreloadTorque[WA1]);
}

// ------------------------------------------------------------------------------- //
// MINI AND MACRO CONTROL RELATED FUNCTIONS


// macroControl with multiple axes support
// this is old version that closes the loop on joint torque
void dm2Control::macroControl(void)
{
	static float x[3] = {0.0,0.0,0.0}, xold[3] = {0.0,0.0,0.0},
   		y[3] = {0.0,0.0,0.0}, yold[3] = {0.0,0.0,0.0};
	int i;

	//USING P+PSEUDO-D+LAG
	static float yP[3], yD[3], yDOld[3] ={0.0,0.0,0.0}, yL[3], yLold[3] = {0.0,0.0,0.0};
	static float xOld[3] = {x[0],x[1],x[2]}, y2[3], y2old[3] = {0.0,0.0,0.0}, 
		x1[3] = {baseSpringTorqueEnc[WA1],baseSpringTorque[SH1],baseSpringTorque[EL1]},
		x3[3], y3[3];
	static float x1Old[3] = {x1[0],x1[1],x1[2]};	// x1old is measured spring torque at the prev. tick
	static float y3old[3] = {0.0,0.0,0.0}, x3old[3] = {0.0,0.0,0.0};

#if(HZ == 1000)
	// elbow Kp was 0.75
	float Kp[3] = {0.28125,0.18,0.55}, 
		a1[3] = {2.2042,1.5429,2.645}, 
		b1[3] = {0.5592,0.5592,0.5592};	
	// Kd = 0.0050, wc = 90Hz, T = 0.001 ms for WA
	// Kd = 0.0035, wc = 90Hz, T = 0.001 ms for SH

	const float Klag[3] = {0.03,0.0213,0.045}, 
	a2[3] = {1.0122,1.0594,1.0495}, 
	c2[3] = {0.9753,0.8868,0.9048}, 
	b2[3] = {0.9994,0.9988,0.999};	// T = 0.001 ms
#endif

#if(HZ == 500)
	float Kp[3] = {0.28125,0.0,0.3}, a1[3] = {2.2042,2.2042,2.2042}, 
		b1[3] = {0.5592,0.5592,0.5592};	
	// Kd = 0.0050, wc = 90Hz, T = 0.001 ms
	const float Klag[3] = {0.00,0.00,0.00}, a2[3] = {1.0122,1.0122,1.0122}, 
	c2[3] = {0.9753,0.9753,0.9753}, b2[3] = {0.9994,0.9994,0.9994};	// T = 0.001 ms
#endif

	x1[WA1] = baseSpringTorqueEnc[WA1];
	x1[SH1] = baseSpringTorque[SH1];
	x1[EL1] = baseSpringTorque[EL1];

	for(i=0;i<3;i++)
	{	
		// load torque command is 1.54*(base torque command) due to the pulley reduction ratio
		baseTorqueCommand[i] = loadTorqueCommand[i]/1.54;

		// feedback is in spring torque
		// x is the difference between desired and joint torque and scaled down by 
		// a factor of Nw (the gear reduction ratio)
		x[i] = baseTorqueCommand[i] - baseJointTorque[i]/1.54;	// trying estimate of effective joint torque due to kinematic coupling 
		baseJointTorqueError[i] = x[i]*1.54;	// scale this back up using Nw to get the real joint torque error
		// this is used in mini control

	// input to damping loop is filtered spring torque data
		//x1[i] is set just before the for loop above
	//	printf("yD: %f, x1: %f, %f \n", yD, x1, x1Old);
		yD[i] = b1[i]*yDOld[i] + a1[i]*(x1[i]- x1Old[i]);	//yD is the derivative term for the measured torque
		yDOld[i] = yD[i];	x1Old[i] = x1[i];
	
		// yP is proportional term
		yP[i] = Kp[i]*x[i];			// P term, Kp*(torque error)		
		
		// Lag term, input to this is torque error, x
		yL[i] = b2[i]*yLold[i] + a2[i]*(x[i] - c2[i]*xOld[i]);

		const float intLimit = 4.0;
		if(yL[i] > intLimit)
				yL[i] = intLimit;
		if(yL[i] < -intLimit)
				yL[i] = -intLimit;

		yLold[i] = yL[i];	xOld[i] = x[i];

		// Total compensator output
		y[i] = yP[i] + Klag[i]*yL[i] - yD[i];

		// calculate the corresponding DAC count to produce the desired MOTOR torque
		// HERE I AM TRYING FEEDFORWARD + COMPENSATOR OUTPUT MODEL
//		baseCommand[i] = 1*(short)(y[i]/(KDAC_BASE[i]*KAMP_BASE[i]*KTORQUE_BASE))
//							+ 1*(short)(baseTorqueCommand[i]/(KDAC_BASE*KAMP_BASE*KTORQUE_BASE*gearRatioBase[i]));

		// here only the gravity torque command is fedforward and then added to the
		// closed-loop control on the overall torque command
		baseCommand[i] = (short)(y[i]/(KDAC_BASE[i]*KAMP_BASE[i]*KTORQUE_BASE))
							+ (short)((gravityTorqueCommand[i]/1.54)/(KDAC_BASE[i]*KAMP_BASE[i]*KTORQUE_BASE*gearRatioBase[i]));
	}

//	dampingOutput = yD[SH1];

	// FOR ELBOW MACRO: positive torque command = negative DAC count; vice versa
	baseCommand[EL1] = -baseCommand[EL1];
	// FOR SHOULDER MACRO: positive torque command = negative DAC count; vice versa
	baseCommand[SH1] = -baseCommand[SH1];

//	printf("%f, %f, %f, %f, e %f \n", yD[EL1], Klag[EL1]*yL[EL1], yP[EL1], y[EL1], x[EL1]);

	for(i=0;i<3;i++)
	{
	// CLAMP VALUE TO MAX AND MIN VALUES
		if (baseCommand[i] > limitDAC2)
				baseCommand[i] = limitDAC2;
		else if (baseCommand[i] < -limitDAC2)
				baseCommand[i] = -limitDAC2;
	}
	
	// write to DAC
	stgRight->RawDAC(BASEDACWA1, baseCommand[WA1]);
	stgRight->RawDAC(BASEDACSH1, baseCommand[SH1]);

	// TEMPORARY CHANNEL FOR EL MACRO
	// CURRENTLY USING DAC 1 THAT ACTUALLY BELONGS TO WAIST MINI 
	stgRight->RawDAC(BASEDACEL1, baseCommand[EL1]);

#if 0
	static int counter = 0;
	if (counter == 200)
	{
//		printf("y: %f, y2: %f \n", y, y2);
//		printf("des: %5.3f, at: %5.3f, %5.3f, com: %d \n", torqueDes, baseSpringTorque[WA1],
//					LOHET[WA1],
//				   	baseCommand[WA1]);	
//		printf("des: %5.3f, at: %5.3f, %5.4f, com: %d \n", torqueDes, baseSpringTorque[EL1],
//					LOHET[EL1],
//				   	baseCommand[EL1]);	
//		printf("Des: %5.2f, at: %5.2f, com: %d \n", baseTorqueCommand[WA1]*1.54, baseSpringTorque[WA1]*1.54,
//					baseCommand[WA1]);
		printf("Des: %5.2f, at: %5.2f, com: %d \n", baseTorqueCommand[WA1]*1.54, baseSpringTorqueEnc[WA1]*1.54,
					baseCommand[WA1]);
		counter = 0;
	}
	counter++;
#endif
}

// macroControl with multiple axes support
// macroControlNew -> with closed control around SEA element
// new version... April 7, 2005
void dm2Control::macroControlNew(void)
{
	static float x[3] = {0.0,0.0,0.0}, xold[3] = {0.0,0.0,0.0},
   		y[3] = {0.0,0.0,0.0}, yold[3] = {0.0,0.0,0.0};
	int i;

	//USING P+PSEUDO-D+LAG
	static float yP[3], yD[3], yDOld[3] ={0.0,0.0,0.0}, yL[3], yLold[3] = {0.0,0.0,0.0};
	static float xOld[3] = {x[0],x[1],x[2]}, y2[3], y2old[3] = {0.0,0.0,0.0}, 
		x1[3] = {baseSpringTorqueEnc[WA1],baseSpringTorque[SH1],baseSpringTorque[EL1]},
		x3[3], y3[3];
	static float x1Old[3] = {x1[0],x1[1],x1[2]};	// x1old is measured spring torque at the prev. tick
	static float y3old[3] = {0.0,0.0,0.0}, x3old[3] = {0.0,0.0,0.0};

	float Kp[3] = {0.28125,0.18,0.55}, 
		a1[3] = {2.2042,1.5429,2.645}, 
		b1[3] = {0.5592,0.5592,0.5592};	
	// Kd = 0.0050, wc = 90Hz, T = 0.001 ms for WA
	// Kd = 0.0035, wc = 90Hz, T = 0.001 ms for SH

	const float Klag[3] = {0.03,0.0213,0.045}, 
	a2[3] = {1.0122,1.0594,1.0495}, 
	c2[3] = {0.9753,0.8868,0.9048}, 
	b2[3] = {0.9994,0.9988,0.999};	// T = 0.001 ms

	x1[WA1] = baseSpringTorqueEnc[WA1];
	x1[SH1] = baseSpringTorque[SH1];
	x1[EL1] = baseSpringTorque[EL1];

	for(i=0;i<3;i++)
	{	
		// load torque command is 1.54*(base torque command) due to the pulley reduction ratio
		baseTorqueCommand[i] = (*baseTorqueDes)[i]/1.54;

		// feedback is in spring torque
		// x is the difference between desired and measured spring torque 
		x[i] = baseTorqueCommand[i] - baseSpringTorque[i];
		baseActuatorTorqueError[i] = x[i]*1.54;	// scale this back up using Nw to get base actuator torque error
		// this is used in mini control

		// input to damping loop is filtered spring torque data
		//x1[i] is set just before the for loop above
	//	printf("yD: %f, x1: %f, %f \n", yD, x1, x1Old);
		yD[i] = b1[i]*yDOld[i] + a1[i]*(x1[i]- x1Old[i]);	//yD is the derivative term for the measured torque
		yDOld[i] = yD[i];	x1Old[i] = x1[i];
	
		// yP is proportional term
		yP[i] = Kp[i]*x[i];			// P term, Kp*(torque error)		
		
		// Lag term, input to this is torque error, x
		yL[i] = b2[i]*yLold[i] + a2[i]*(x[i] - c2[i]*xOld[i]);

		const float intLimit = 4.0;
		if(yL[i] > intLimit)
				yL[i] = intLimit;
		if(yL[i] < -intLimit)
				yL[i] = -intLimit;

		yLold[i] = yL[i];	xOld[i] = x[i];

		// Total compensator output
		y[i] = yP[i] + Klag[i]*yL[i] - 1.0*yD[i];

		// calculate the corresponding DAC count to produce the desired MOTOR torque
		baseCommand[i] = (short)(y[i]/(KDAC_BASE[i]*KAMP_BASE[i]*KTORQUE_BASE));
	}

	// FOR ELBOW MACRO: positive torque command = negative DAC count; vice versa
	baseCommand[EL1] = -baseCommand[EL1];
	// FOR SHOULDER MACRO: positive torque command = negative DAC count; vice versa
	baseCommand[SH1] = -baseCommand[SH1];

//	printf("%f, %f, %f, %f, e %f \n", yD[EL1], Klag[EL1]*yL[EL1], yP[EL1], y[EL1], x[EL1]);

	for(i=0;i<3;i++)
	{
	// CLAMP VALUE TO MAX AND MIN VALUES
		if (baseCommand[i] > limitDAC2)
				baseCommand[i] = limitDAC2;
		else if (baseCommand[i] < -limitDAC2)
				baseCommand[i] = -limitDAC2;
	}
	
	// write to DAC
	stgRight->RawDAC(BASEDACWA1, baseCommand[WA1]);
	stgRight->RawDAC(BASEDACSH1, baseCommand[SH1]);

	// TEMPORARY CHANNEL FOR EL MACRO
	// CURRENTLY USING DAC 1 THAT ACTUALLY BELONGS TO WAIST MINI 
	stgRight->RawDAC(BASEDACEL1, baseCommand[EL1]);

#if 0
	static int counter = 0;
	if (counter == 200)
	{
//		printf("y: %f, y2: %f \n", y, y2);
//		printf("des: %5.3f, at: %5.3f, %5.3f, com: %d \n", torqueDes, baseSpringTorque[WA1],
//					LOHET[WA1],
//				   	baseCommand[WA1]);	
//		printf("des: %5.3f, at: %5.3f, %5.4f, com: %d \n", torqueDes, baseSpringTorque[EL1],
//					LOHET[EL1],
//				   	baseCommand[EL1]);	
//		printf("Des: %5.2f, at: %5.2f, com: %d \n", baseTorqueCommand[WA1]*1.54, baseSpringTorque[WA1]*1.54,
//					baseCommand[WA1]);
		printf("Des: %5.2f, at: %5.2f, com: %d \n", baseTorqueCommand[WA1]*1.54, baseSpringTorqueEnc[WA1]*1.54,
					baseCommand[WA1]);
		counter = 0;
	}
	counter++;
#endif
}

void dm2Control::miniControl(void)
{

	static float rampCounter = 0.0, rampTime = 50.0;
	const float miniTorqueMax[3] = {24.0, 21.0, 05.0};
	// actually its 34, 31 and 11.6
	int i;

	// calculate joint torque error based on linear combination of base actuator error
	// using relationships from inverse of J transpose.
	baseJointTorqueError[WA1] = baseActuatorTorqueError[WA1] + 0.4933*baseActuatorTorqueError[SH1]
			+ 0.8486*baseActuatorTorqueError[EL1];
	baseJointTorqueError[SH1] = baseActuatorTorqueError[SH1] + baseActuatorTorqueError[EL1];
	baseJointTorqueError[EL1] = baseActuatorTorqueError[EL1];

	for(i=0;i<3;i++)
	{
	// CLAMP VALUE TO MAX AND MIN VALUES
		if (baseJointTorqueError[i] > miniTorqueMax[i])
				baseJointTorqueError[i] = miniTorqueMax[i];
		else if (baseJointTorqueError[i] < -miniTorqueMax[i])
				baseJointTorqueError[i] = -miniTorqueMax[i];
	}

	// the mini motor should apply the joint torque error measured in sea control.
	// baseJointTorqueError[] --> this is at the joint level
	// Initially the command is ramped up so it doesnt "step"
	if(rampCounter < rampTime)
	{
		miniTorqueDes[WA1] =  rampCounter*baseJointTorqueError[WA1]/rampTime;
		miniTorqueDes[SH1] =  rampCounter*baseJointTorqueError[SH1]/rampTime;
		miniTorqueDes[EL1] =  rampCounter*baseJointTorqueError[EL1]/rampTime;
		rampCounter = rampCounter + 1.0;
	}
	else
	{
		miniTorqueDes[WA1] = baseJointTorqueError[WA1];
		miniTorqueDes[SH1] = baseJointTorqueError[SH1];
		miniTorqueDes[EL1] = baseJointTorqueError[EL1];
	}

	// equivalent DAC command for mini motor
	// notice sign reversal for shoulder and elbow axes
	miniCommand[WA1] = miniTorqueDes[WA1]/(KDAC*KAMP*KT[WA1]*GEAR[WA1]);
	miniCommand[SH1] = -miniTorqueDes[SH1]/(KDAC*KAMP*KT[SH1]*GEAR[SH1]);
 	miniCommand[EL1] = -miniTorqueDes[EL1]/(KDAC*KAMP*KT[EL1]*GEAR[EL1]);


	// DO NOT UNCOMMENT THIS UNLES::
	// A. EL MACRO DAC HAS BEEN MOVED TO A NEW CHANNEL
	// B. WA MINI HAS BEEN MOVED A NEW CHANNEL
	// WARNING!!!!!
//	sinusoidCom(WA1, miniCommand[WA1]);

	sinusoidCom(SH1, miniCommand[SH1]);
	sinusoidCom(EL1, miniCommand[EL1]);	

}

int dm2Control::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 = 200.0;
	float torqueShutdown[3];

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

			miniTorqueDes[WA1] = torqueShutdown[WA1];
			miniTorqueDes[SH1] = torqueShutdown[SH1];
			miniTorqueDes[EL1] = torqueShutdown[EL1];

			// equivalent DAC command for mini motor
			// notice sign reversal for shoulder and elbow axes
			miniCommand[WA1] = miniTorqueDes[WA1]/(KDAC*KAMP*KT[WA1]*GEAR[WA1]);
			miniCommand[SH1] = -miniTorqueDes[SH1]/(KDAC*KAMP*KT[SH1]*GEAR[SH1]);
   			miniCommand[EL1] = -miniTorqueDes[EL1]/(KDAC*KAMP*KT[EL1]*GEAR[EL1]);
	
			// DO NOT UNCOMMENT THIS UNLES::
			// A. SH MACRO DAC HAS BEEN MOVED TO A NEW CHANNEL
			// B. EL MINI HAS BEEN MOVED A NEW CHANNEL
		//	sinusoidCom(WA1, miniCommand[WA1]);
			// WARNING!!!!!
	
			sinusoidCom(SH1, miniCommand[SH1]);
			sinusoidCom(EL1, miniCommand[EL1]);	

			//		printf("torque: %f, %f, %d \n", torqueShutdown[SH1], (*torqueCommand)[SH1], torque);
			return(0);
	}
	else
			return(1);


}
void dm2Control::recordChirp(float yChirp)
{
	chirpCommand = yChirp;
}

void dm2Control::chirpControl(void)
{
	static float x = 0.0, xold = 0.0, y = 0.0, yold = 0.0;
	int i;

	// load torque command is 1.54*(base torque command) due to the pulley reduction ratio
	baseTorqueCommand[WA1] = loadTorqueCommand[WA1]/1.54;

#if 1
	// WAIST JOINT DAMPING LOOP
	static float yP, yD, yDOld = 0.0, yL, yLold = 0.0;
	static float xOld = x, y2, y2old = 0.0, x1 = baseSpringTorqueEnc[WA1], x3, y3;
	static float x1Old = x1;	// x1old is measured spring torque at the prev. tick

#if(HZ == 1000)
//	float Kp, a1 = 13.0, b1 = -1.0;	// Kd = 0.0065, T = 0.001 ms
	float Kp, a1 = 2.2042, b1 = 0.5592;	// Kd = 0.0050, wc = 90Hz, T = 0.001 ms
#endif

	x1 = baseSpringTorqueEnc[WA1];
	yD = b1*yDOld + a1*(x1 - x1Old);	//yD is the derivative term for the measured torque
	yDOld = yD;	x1Old = x1;

	// damping loop output 
	y = -1.0*yD;

	dampingOutput = y;

	// calculate the corresponding DAC count to produce the desired MOTOR torque

	// HERE I AM TRYING GRAVITY FEEDFORWARD + DAMPING LOOP OUTPUT + CHIRP MODEL
	baseCommand[WA1] = 1*(short)(y/(KDAC_BASE[i]*KAMP_BASE[i]*KTORQUE_BASE))
						+ 1*(short)(baseTorqueCommand[WA1]/(KDAC_BASE[i]*KAMP_BASE[i]*KTORQUE_BASE*gearRatioBase[WA1]))
						+ 50*(short)(chirpCommand/(KDAC_BASE[i]*KAMP_BASE[i]*KTORQUE_BASE*gearRatioBase[WA1]));
#endif

	for(i=0;i<3;i++)
	{
	// CLAMP VALUE TO MAX AND MIN VALUES
		if (baseCommand[i] > limitDAC2)
				baseCommand[i] = limitDAC2;
		else if (baseCommand[i] < -limitDAC2)
				baseCommand[i] = -limitDAC2;
	}
	
	// write to DAC (chirp control)
	stgRight->RawDAC(6, baseCommand[WA1]);

#if 0
	static int counter = 0;
	if (counter == 200)
	{
//		printf("y: %f, y2: %f \n", y, y2);
//		printf("des: %5.3f, at: %5.3f, %5.3f, com: %d \n", torqueDes, baseSpringTorque[WA1],
//					LOHET[WA1],
//				   	baseCommand[WA1]);	
//		printf("des: %5.3f, at: %5.3f, %5.4f, com: %d \n", torqueDes, baseSpringTorque[EL1],
//					LOHET[EL1],
//				   	baseCommand[EL1]);	
//		printf("Des: %5.2f, at: %5.2f, com: %d \n", baseTorqueCommand[WA1]*1.54, baseSpringTorque[WA1]*1.54,
//					baseCommand[WA1]);
		printf("Des: %5.2f, at: %5.2f, com: %d \n", baseTorqueCommand[WA1]*1.54, baseSpringTorqueEnc[WA1]*1.54,
					baseCommand[WA1]);
		counter = 0;
	}
	counter++;
#endif

}


#define limitDAC	800
void dm2Control::baseMotorControl(float runTime)
{
	float driveFreq = 1.0;
	float driveAmplitude[3] = {10.0, 5.0, 25.0};
	float Kp[3] = {250.0, 20.0, 300.0};

#if 1	// WAIST CONTROL
	baseOutputAngleDes[WA1] = driveAmplitude[WA1]*sin(driveFreq*runTime);
//	baseOutputAngleDes[WA1] = 0.0;

	baseCommand[WA1] = (short)(Kp[WA1]*(baseOutputAngleDes[WA1] - baseOutputAngle[WA1]));

	if (baseCommand[WA1] > limitDAC)
			baseCommand[WA1] = limitDAC;
	else if (baseCommand[WA1] < -limitDAC)
			baseCommand[WA1] = -limitDAC;
	// TEMP HACK... SHOULD BE CHANNEL 6
	stgRight->RawDAC(6, baseCommand[WA1]);
#endif

#if 0	// ELBOW CONTROL
	baseOutputAngleDes[EL1] = driveAmplitude[EL1]*sin(driveFreq*runTime);

	baseCommand[EL1] = (short)(Kp[EL1]*(baseOutputAngleDes[EL1] - baseOutputAngle[EL1]));

	if (baseCommand[EL1] > limitDAC)
			baseCommand[EL1] = limitDAC;
	else if (baseCommand[EL1] < -limitDAC)
			baseCommand[EL1] = -limitDAC;
	stg->RawDAC(1, baseCommand[EL1]);

# endif
#if 0 
	printf("Des: %f, At: %f, control: %d \n", baseOutputAngleDes[WA1], baseOutputAngle[WA1],
				baseCommand[WA1]);
//	printf("Des: %f, At: %f, control: %d \n", baseOutputAngleDes[EL1], baseOutputAngle[EL1],
//				baseCommand[EL1]);
#endif

}
//#define ELDAC	300		// FOR ELBOW
#define ELDAC	130		// FOR WAIST
void dm2Control::moveBaseShaft(int direction)
{
	// +- 150 for WA motor
	// +- 400 for EL motor
#if 1	// WAIST
	if(direction == 0)
		stgRight->RawDAC(6,ELDAC);
	else if(direction == 1)
		stgRight->RawDAC(6,-ELDAC);
#endif

#if 0	// ELBOW
		// Testing EL1 base (temp. connection to channel 7)
	if(direction == 0)
		stg->RawDAC(1,ELDAC);
	else if(direction == 1)
		stg->RawDAC(1,-ELDAC);
#endif
}

// FEB 10 2005, PST NOTES
// Positive torque = negative DAC count
// Negative torque = positive DAC count
void dm2Control::applyBaseTorque(int dacCount)
{
	int i;

	baseCommand[WA1] = dacCount;
//	printf("dac: %d %d \n", dacCount, baseCommand[WA1]);
	i = 6;
	stgRight->RawDAC(i,dacCount);
}

// Overloaded function to apply torque to base actuator
// Takes input in form of load torque command and thus have to be divided by 1.54
// to get the desired spring torque
void dm2Control::applyBaseTorque(void)
{
	int i;

	// calculate equivalent DAC count to generate the desired spring torque (load torque / 1.54)
	for(i=0;i<3;i++)
	{
		baseCommand[i] = (int)((loadTorqueCommand[i])/(KTORQUE_BASE*KAMP_BASE[i]*KDAC*gearRatioBase[i]*1.54));
	//	printf("G: %f %d \n", loadTorqueCommand[WA1], baseCommand[WA1]);
	}

	baseCommand[SH1] = -baseCommand[SH1];
	baseCommand[EL1] = -baseCommand[EL1];

	stgRight->RawDAC(6,baseCommand[WA1]);
	stgRight->RawDAC(7,baseCommand[SH1]);
	stgRight->RawDAC(1,baseCommand[EL1]);
}

// END OF MINI AND MACRO CONTROL RELATED FUNCTIONS
// ------------------------------------------------------------------------------- //


// TEST MAIN LOOP

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

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

	timer.start();

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

	arm3dof->resetIndex();

	printf("dm2Control Test Program \n");
	printf("Enter command: \n");
/*	printf("(2) Sinusoid Commutation \n");
	printf("(3) Initialization with Index \n");
	printf("(4) Test HET \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("(5) Test Encoders \n");
	printf("(10) RIGHT ARM Gravity Compensation Test \n");
	printf("(13) LEFT 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("(14) Joint Encoder Init using Index Pulse \n");
	printf("(15) Display end-effector position \n");
	printf("(16) Waist base motor torque test \n");
	printf("(17) Experimental Waist Closed-loop SEA Control \n");				
	printf("(18) Waist or Elbow base open loop gravity compensation test \n");
	printf("(19) *** Waist or Elbow base closed-loop gravity compensation test \n");
	printf("(24) *** Joint Space Control using DMM for torque source \n");
	printf("(20) P-control to hold waist base motor angle to zero test \n");
	printf("(21) Display torque measurement data \n");
	printf("(22) Sys ID using chirp input \n");
	printf("(23) Elbow base open loop gravity compensation test \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 2:
			printf("Sinusoidal Commutation \n");
		
			// calibrate position with index signal (without HET signals)
//			arm3dof->initWithIndex2();
			arm3dof->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
				arm3dof->setDirection(CW);
				printf("up \n");
				while(!tcischars(1))
				{
					arm3dof->readHET();
					arm3dof->readPosition();
					arm3dof->calcPosition();
				//	arm3dof->sinusoidCom(jointIndex, -torque+000.0);
					arm3dof->sinusoidCom(jointIndex, 000.0);
					//		arm3dof->displayInfo();
					arm3dof->saveInfo();
					timer.wait();
				}
				cin.ignore();
#endif
				printf("down \n");
				arm3dof->setDirection(CCW);
				while(!tcischars(1))
				{
					arm3dof->readHET();
					arm3dof->readPosition();
					arm3dof->calcPosition();
					arm3dof->sinusoidCom(jointIndex, torque-000.0);
//					arm3dof->sinusoidCom(jointIndex, 0.00);
			//		arm3dof->displayInfo();
					arm3dof->saveInfo();
					timer.wait();
				}
				cin.ignore();
				printf("Loop: %2d \n", loop);	
				loop++;
			}
			break;

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

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

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


			break;

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

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

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


			break;

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

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

			break;


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

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

				arm3dof->readHET();
				arm3dof->displayInfo();

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

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

			break;

		case 6:
		{
//			arm3dof->resetEncoder();
			
			printf("Testing joint space trajectory control \n");
//			printf("Testing init with index \n");
//			arm3dof->initWithIndex2();
//			arm3dof->resetVelocity();

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

			int exit = 0;

//			while(!exit)
//			{
				arm3dof->getJointPosition(angle);

				printf("Current position: %6.2f, %6.2f %6.2f \n", angle[WAIST_JOINT],
							angle[SHOULDER_JOINT],angle[ELBOW_JOINT]);
				printf("Enter joint goal position in degrees: xx yy zz\n");
				printf("Enter 1000 to exit \n");
				printf("> ");
				cin >> elbowGoal;

				if(elbowGoal == 1000.0)
						exit = 1;
				else
				{
//					arm3dof->setGoal(elbowGoal);
					qgoal.zero();
					qgoal[WA1] = 0.0;
					qgoal[SH1] = -48.0;
					qgoal[EL1] = 110.0;

					// 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)
					arm3dof->readPosition();
		//			arm3dof->resetVelocity();
					arm3dof->calcPosition();	
		//			arm3dof->getJointPosition(angle);
					arm3dof->calcSpline(qgoal);

				}

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

				// reset time to 0
				time = 0.0;

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

					// at each servo tick calculate the desired position qdes(t)
					// and velocity dqdes(t) from the cubic spline equation
					//arm3dof->calcPosition();
					arm3dof->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)
					arm3dof->jointControl();
					
					// command torque using sinusoidal com
		//			arm3dof->readPosition();
		//			arm3dof->calcPosition();	
//					arm3dof->servoJoint();
		//			arm3dof->displayAngle();	
			//		arm3dof->displayInfo();	
					arm3dof->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))
			{
				arm3dof->readPosition();
				arm3dof->calcPosition();	
				arm3dof->gravityCompensationRight();	// for RIGHT arml
				arm3dof->saveInfo();
				timer.wait();
			}
			break;
		}	// END OF CASE 'A'

		// GRAVITY COMPENSATION FOR LEFT ARM
		case 13:
		{
			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))
			{
				arm3dof->readPosition();
				arm3dof->calcPosition();	
				arm3dof->gravityCompensationLeft();	// for LEFT arm
				arm3dof->saveInfo();
				misTick =timer.wait();
				if(misTick > 0)
						printf("%d\n", misTick);
			}
			break;
		}	// END OF CASE 'A'

		case 11:
		{
			int armSide = RIGHTARM;

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

//			arm3dof->resetVelocity();

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

			timer.start();

			while(!tcischars(1))
			{
					arm3dof->readPosition();
					arm3dof->calcPosition();
					arm3dof->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;

				arm3dof->setJointAngle(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(!arm3dof->hitIndex2(2*jointSelect+1));
					arm3dof->readPosition();
					arm3dof->calcPosition();
					arm3dof->displayInfo();
					printf("Next joint ... \n");
				}

				break;		// end of case 14
		}
		case 15:
		{
				printf("Displaying End-Effector Position \n");

				timer.start();

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

				break;		// end of case 14
		}

		case 16:
		{
			int dacCountDes, dacCount;
			printf("Enter DAC command: \n");
			printf("Positive torque = positive DAC \n");
			printf("Negative torque = negative DAC \n");
			printf(">  ");
			cin>> dacCountDes;
			printf("\n");

			arm3dof->calcPreloadSpringTorque();

			if(dacCountDes > 300)
					dacCountDes = 300;
			if(dacCountDes < -300)
					dacCountDes = -300;

			printf("Will apply DAC count of %d to waist motor\n", dacCountDes);
			printf("Press any key to contiune \n");
			cin.ignore();
			cin.ignore();

			int exit = 0;

			int misTick2;
			timer.start();
			timer.zero();
			while(!tcischars(1))
			{
				time = timer.time();
				dacCount = arm3dof->modulateBaseTorque(time, dacCountDes);
				arm3dof->applyBaseTorque(dacCount);
				arm3dof->readLOHET();
				arm3dof->readPosition();
				arm3dof->calcPosition();	
				arm3dof->gravityCompensationRight();	// for RIGHT arml
				arm3dof->saveInfo();
				arm3dof->displayInfo();
				misTick2 = timer.wait();
				if(misTick2 > 0)
						printf("missed %d ticks \n", misTick2);
			}
			break;
		}	// END OF CASE 16 
		case 17:
		{
			float loadTorqueDes[3] = {0.0,0.0,0.0};
			printf("Enter load torque command (Nm): \n");
			printf(">  ");
			cin>> loadTorqueDes[WA1];
			printf("\n");

			arm3dof->calcPreloadSpringTorque();

			printf("Will apply load torque of %5.2f Nm to waist joint\n", loadTorqueDes[WA1]);
			printf("Press any key to continue \n");
			cin.ignore();
			cin.ignore();

			int exit = 0;

			int misTick2;
			timer.start();
			timer.zero();
			while(!tcischars(1))
			{
				time = timer.time();
				arm3dof->readPosition();
				arm3dof->calcPosition();	
				arm3dof->gravityCompensationRight();	// for RIGHT arml
			
				arm3dof->readLOHET();	// read LOHET and calculate spring torque
				arm3dof->setLoadTorqueCommand(loadTorqueDes);	// set load torque command to desired
				arm3dof->modulateLoadTorqueCommand(time);	// modulate torque command so that initially it doesnt step
				arm3dof->macroControl();	// closed-loop torque control on SEA	
		
				arm3dof->saveInfo();
//				arm3dof->displayInfo();
				misTick2 = timer.wait();
				if(misTick2 > 0)
						printf("missed %d ticks \n", misTick2);
			}
			break;
		}	// END OF CASE 17 

		case 18:
		{
			float loadTorqueDes[3] = {0.0,0.0,0.0};
			PrVector3 gravityTorque;
			arm3dof->calcPreloadSpringTorque();

			printf("Will apply gravity compensation toque to waist joint\n");
			printf("Press any key to continue \n");
			cin.ignore();

			int exit = 0;

			int misTick2;
			timer.start();
			timer.zero();
			while(!tcischars(1))
			{
				time = timer.time();
				arm3dof->readPosition();
				arm3dof->calcPosition();	
			
				gravityTorque = arm3dof->gravityCompensationRight();	// for RIGHT arml

				loadTorqueDes[WA1] = gravityTorque[WA1];	
				loadTorqueDes[SH1] = gravityTorque[SH1];	
				loadTorqueDes[EL1] = gravityTorque[EL1];	

				arm3dof->readLOHET();	// read LOHET and calculate spring torque
				arm3dof->setLoadTorqueCommand(loadTorqueDes);	// set load torque command to desired
				arm3dof->modulateLoadTorqueCommand(time);	// modulate torque command so that initially it doesnt step
				arm3dof->applyBaseTorque();	
				arm3dof->saveInfo();
				arm3dof->displayInfo();
				misTick2 = timer.wait();
				if(misTick2 > 0)
						printf("missed %d ticks \n", misTick2);
			}
			break;
		}	// END OF CASE 18 
		case 19:
		{
			float loadTorqueDes[3] = {0.0,0.0,0.0};
			PrVector3 gravityTorque;
			PrVector jointPos(6);
			arm3dof->calcPreloadSpringTorque();

			printf("Will apply closed-loop gravity compensation toque\n");
			printf("SH1 and EL1 are macro-mini-controlled \n");
			printf("WA1 is macro only \n");
			printf("Press any key to continue \n");
			cin.ignore();

			int exit = 0;

			int misTick2;
			timer.start();
			timer.zero();
			while(!tcischars(1))
			{
				time = timer.time();
				arm3dof->readPosition();
				arm3dof->calcPosition();	
				arm3dof->calcJacobian();
				arm3dof->calcX();
				gravityTorque = arm3dof->gravityCompensationRight();	// for RIGHT arml

	//			jointPos = arm3dof->getQ();
			
				loadTorqueDes[WA1] = 0.95*gravityTorque[WA1] - 2.5;		
				loadTorqueDes[SH1] = 1.0*gravityTorque[SH1];
				loadTorqueDes[EL1] = gravityTorque[EL1];
				// this is fitted model using kinematic coupling eqns
				
				arm3dof->readLOHET();	// read LOHET and calculate spring torque
				arm3dof->setLoadTorqueCommand(loadTorqueDes);	// set load torque command to desired
				arm3dof->modulateLoadTorqueCommand(time);	// modulate torque command so that initially it doesnt step
				arm3dof->macroControl();	
//				arm3dof->miniControl();
				arm3dof->saveInfo();
				arm3dof->displayInfo();
				misTick2 = timer.wait();
				if(misTick2 > 0)
						printf("missed %d ticks \n", misTick2);
			}
			break;
		}	// END OF CASE 19 
		case 24:
		{
			float loadTorqueDes[3] = {0.0,0.0,0.0};
			PrVector3 gravityTorque;
			PrVector jointPos(6);
			arm3dof->calcPreloadSpringTorque();

			printf("Joint Space Control Test\n");
			printf("SH1 and EL1 are macro-mini-controlled \n");
			printf("WA1 is macro only \n");

			int exit = 0;
			int misTick2;
			timer.start();

			arm3dof->getJointPosition(angle);

			qgoal[WA1] = angle[WA1];
			qgoal[SH1] = angle[SH1] - 20.0;
			qgoal[EL1] = angle[EL1] + 00.0;

			arm3dof->calcSpline(qgoal);
			printf("Current position: %6.2f, %6.2f %6.2f \n", angle[WA1],
						angle[SH1],angle[EL1]);
			printf("Goal position: %7.2f, %7.2f ,%7.2f \n", qgoal[WA1], qgoal[SH1], qgoal[EL1]);
			printf("Make sure these are correct and press any key to continue to start motion\n");
			while(!tcischars(1));
			cin.ignore();

			timer.zero();
			while(!tcischars(1))
			{
				time = timer.time();
				arm3dof->readPosition();
				arm3dof->calcPosition();	
				arm3dof->calcJacobian();
				arm3dof->calcX();
				arm3dof->gravityCompensationRight();	// for RIGHT arml

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

				// calculate control torque based on qdes and dqdes
				// tau = Kp(qdes-q) + Kv(dqdes-dq)
				arm3dof->jointControl();
				
				arm3dof->readLOHET();						// read LOHET and calculate spring torque
				arm3dof->setLoadTorqueCommand();			// set load torque command to desired
				arm3dof->modulateLoadTorqueCommand(time);	// modulate torque command so that initially it doesnt step
				arm3dof->macroControl();	
				arm3dof->saveInfo();
				arm3dof->displayInfo();
				misTick2 = timer.wait();
				if(misTick2 > 0)
						printf("missed %d ticks \n", misTick2);
			}
			break;
		}	// END OF CASE 24 
		case 20:
		{
			float loadTorqueDes[3] = {0.0,0.0,0.0};
			PrVector3 gravityTorque;
			arm3dof->calcPreloadSpringTorque();
			arm3dof->resetBaseEncoder();
			printf("Will hold base motor angle to zero\n");
			printf("Press any key to continue \n");
			cin.ignore();

			int exit = 0;

			int misTick2;
			timer.start();
			timer.zero();
			while(!tcischars(1))
			{
				time = timer.time();
				arm3dof->readPosition();
				arm3dof->calcPosition();	
			
				gravityTorque = arm3dof->gravityCompensationRight();	// for RIGHT arml

				loadTorqueDes[WA1] = gravityTorque[WA1];	

				arm3dof->readLOHET();	// read LOHET and calculate spring torque
				arm3dof->baseMotorControl(time);	// P-control to lock base motor angle	
				arm3dof->saveInfo();
				arm3dof->displayInfo();
				misTick2 = timer.wait();
				if(misTick2 > 0)
						printf("missed %d ticks \n", misTick2);
			}
			break;
		}	// END OF CASE 20 
		case 21:
		{
			arm3dof->calcPreloadSpringTorque();

			int exit = 0;

			int misTick2;
			timer.start();
			timer.zero();
			while(!tcischars(1))
			{
				time = timer.time();
				arm3dof->readPosition();
				arm3dof->calcPosition();	
			
				arm3dof->gravityCompensationRight();	// for RIGHT arml

				arm3dof->readLOHET();	// read LOHET and calculate spring torque
				arm3dof->saveInfo();
				arm3dof->displayInfo2();
				misTick2 = timer.wait();
				if(misTick2 > 0)
						printf("missed %d ticks \n", misTick2);
			}
			break;
		}	// END OF CASE 21 
		case 22:
		{
			float loadTorqueDes[3] = {0.0,0.0,0.0};
			float chirpCommand;

			PrVector3 gravityTorque;
			arm3dof->calcPreloadSpringTorque();

			printf("Chirp input test function");
			printf("Press any key to continue \n");
			cin.ignore();

			int exit = 0;

			int misTick2;
			timer.start();
			timer.zero();
			while(!tcischars(1))
			{
				time = timer.time();
				arm3dof->readPosition();
				arm3dof->calcPosition();	
			
				gravityTorque = arm3dof->gravityCompensationRight();	// for RIGHT arml

				loadTorqueDes[WA1] = gravityTorque[WA1];	

				arm3dof->setLoadTorqueCommand(loadTorqueDes);	// set load torque command to desired
				arm3dof->modulateLoadTorqueCommand(time);	// modulate torque command so that initially it doesnt step
	
				chirpCommand = generateChirp(time);

				arm3dof->recordChirp(chirpCommand);
				arm3dof->readLOHET();	// read LOHET and calculate spring torque
				arm3dof->chirpControl();
				arm3dof->saveInfo();
				arm3dof->displayInfo();
				misTick2 = timer.wait();
				if(misTick2 > 0)
						printf("missed %d ticks \n", misTick2);
			}
			break;
		}	// END OF CASE 22 
	}

#if 0
	printf("shutting down mini \n");
	while(!arm3dof->shutdownMotors())
	{
		arm3dof->saveInfo();
		timer.wait();
	}
#endif

	arm3dof->saveBuffer();
//	arm3dof->closeDataFile();
	delete arm3dof;
}
#endif	// ENDIF TO REMOVE MAIN
	
