/*
	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 "matrix/PrMatrix3.h"
#include "control.h"
#include "max547Driver.h"	// custom MAX547 interface driver
#include "Definitions.h"
#include "sysID.h"
//#include "driver/PrForceSensorJr3ISA.h"	// JR3 ISA driver
#include "driver/PrForceSensorJr3PCI.h"	// JR3 ISA driver
#include "driver/PrForceSensorJr3.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);
//	forceRight = new PrForceSensorJr3ISA();		// force sensor sitting at 0x240
	forceRight = new PrForceSensorJr3PCI();		// force sensor sitting at PCI bus
}


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

	stgRight->RawDAC(0,0);
	stgRight->RawDAC(1,0);
	stgRight->RawDAC(2,0);
	stgRight->RawDAC(3,0);
	stgRight->RawDAC(4,0);
	stgRight->RawDAC(5,0);
	stgRight->RawDAC(6,0);
	stgRight->RawDAC(7,0);
}

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(3);
	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);
	xRightDes = new PrVector(3); dxRightDes = new PrVector(3);
	xRightGoal = new PrVector(3);

	F = new PrVector(3); 
	fstar = new PrVector(3);
	tau = new PrVector(3);

	FContactDesired[0] = 0.0;
	FContactDesired[1] = 0.0;
	FContactDesired[2] = 0.0;
	FContactError[0] = 0.0;
	FContactError[1] = 0.0;
	FContactError[2] = 0.0;

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

	RF3.zero();
	RF3.elementAt(0,1) = 1.0;
	RF3.elementAt(1,2) = -1.0;
	RF3.elementAt(2,0) = 1.0;
	
	R3F = RF3.transpose();
}

void dm2Control::setTime(float time_)
{
	time = time_;

}
// 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);
	dataBuffer8 = new PrMatrix(BUFFER_SIZE,BUFFER_COL);
	dataBuffer9 = new PrMatrix(BUFFER_SIZE,BUFFER_COL);
	dataBuffer1->zero();dataBuffer2->zero();dataBuffer3->zero();
	dataBuffer4->zero();dataBuffer5->zero();dataBuffer6->zero();
	dataBuffer7->zero();dataBuffer8->zero();dataBuffer9->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)
{
#if 0
	fdata = fopen("datamotor.txt","w");
	fdata2 = fopen("datajoint.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");
	fdata8 = fopen("dataoscontrol.txt","w");
#endif
	fdata = fopen("databtorque.txt","w");
	fdata2 = fopen("datajtorque.txt","w");
	fdata3 = fopen("datajoint.txt","w");
	fdata4 = fopen("datafcontrol.txt","w");
	fdata5 = fopen("datamini.txt","w");
	fdata6 = fopen("dataee.txt","w");
	fdata7 = fopen("datajcontrol.txt","w");
	fdata8 = fopen("dataoscontrol.txt","w");
	fdata9 = fopen("dataminienc.txt","w");
}

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

void dm2Control::saveInfo(void)
{
	if(bufferIndex < BUFFER_SIZE)
	{
		// databtorque.txt
		dataBuffer1->elementAt(bufferIndex,0) = baseTorqueCommand[WA1]; 
		dataBuffer1->elementAt(bufferIndex,1) = baseSpringTorque[WA1];
		dataBuffer1->elementAt(bufferIndex,2) = baseTorqueCommand[SH1]; 
		dataBuffer1->elementAt(bufferIndex,3) = baseSpringTorque[SH1];
		dataBuffer1->elementAt(bufferIndex,4) = baseTorqueCommand[EL1]; 
		dataBuffer1->elementAt(bufferIndex,5) = baseSpringTorque[EL1];
		dataBuffer1->elementAt(bufferIndex,6) = (float)(baseCommand[WA1]);		// waist base motor command 
		dataBuffer1->elementAt(bufferIndex,7) = (float)(baseCommand[SH1]);		// waist base motor command 
		dataBuffer1->elementAt(bufferIndex,8) = (float)(baseCommand[EL1]);		// waist base motor command 
		dataBuffer1->elementAt(bufferIndex,9) = LOHET[WA1];		// waist base motor command 
	
		// datajtorque.txt	
		dataBuffer2->elementAt(bufferIndex,0) = (*jointTorqueDes)[WA1];
		dataBuffer2->elementAt(bufferIndex,1) = baseJointTorque[WA1];	
		dataBuffer2->elementAt(bufferIndex,2) = (*jointTorqueDes)[SH1];
		dataBuffer2->elementAt(bufferIndex,3) = baseJointTorque[SH1];
		dataBuffer2->elementAt(bufferIndex,4) = (*jointTorqueDes)[EL1];
		dataBuffer2->elementAt(bufferIndex,5) = baseJointTorque[EL1];

		// datajoint.txt
		dataBuffer3->elementAt(bufferIndex,0) = (*q)[WA1];	// waist joint position
		dataBuffer3->elementAt(bufferIndex,1) = (*q)[SH1];	// shoulder joint position
		dataBuffer3->elementAt(bufferIndex,2) = (*q)[EL1];	// elbow joint position
		dataBuffer3->elementAt(bufferIndex,3) = (*dq)[0];
		dataBuffer3->elementAt(bufferIndex,4) = (*dq)[1];
		dataBuffer3->elementAt(bufferIndex,5) = (*dq)[2];

		// datamini.txt
		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

		// dataee.txt
		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];

		// datajcontrol.txt
		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,7) = (*dqdes)[WA1];
		dataBuffer7->elementAt(bufferIndex,8) = (*dqdes)[SH1];
		dataBuffer7->elementAt(bufferIndex,9) = (*dqdes)[EL1];

		// dataoscontrol.txt
		dataBuffer8->elementAt(bufferIndex,0) = (*xRightDes)[0];
		dataBuffer8->elementAt(bufferIndex,1) = (*xRightDes)[1];
		dataBuffer8->elementAt(bufferIndex,2) = (*xRightDes)[2];
		dataBuffer8->elementAt(bufferIndex,3) = (*F)[0];
		dataBuffer8->elementAt(bufferIndex,4) = (*F)[1];
		dataBuffer8->elementAt(bufferIndex,5) = (*F)[2];
		dataBuffer8->elementAt(bufferIndex,6) = ttraj;
		dataBuffer8->elementAt(bufferIndex,7) = (*dxRightDes)[0];
		dataBuffer8->elementAt(bufferIndex,8) = (*dxRightDes)[1];
		dataBuffer8->elementAt(bufferIndex,9) = (*dxRightDes)[2];

		// datafcontrol.txt
	//	dataBuffer4->elementAt(bufferIndex,0) = Fdesired[0];
	//	dataBuffer4->elementAt(bufferIndex,1) = Fdesired[1];
	//	dataBuffer4->elementAt(bufferIndex,2) = Fdesired[2];
		dataBuffer4->elementAt(bufferIndex,0) = Fmeas[0];
		dataBuffer4->elementAt(bufferIndex,1) = Fmeas[1];
		dataBuffer4->elementAt(bufferIndex,2) = Fmeas[2];
		dataBuffer4->elementAt(bufferIndex,3) = (*F)[0];
		dataBuffer4->elementAt(bufferIndex,4) = (*F)[1];
		dataBuffer4->elementAt(bufferIndex,5) = (*F)[2];
		dataBuffer4->elementAt(bufferIndex,6) = Fdesired[0];
		dataBuffer4->elementAt(bufferIndex,7) = Fdesired[1];
//		dataBuffer4->elementAt(bufferIndex,7) = Fdesired[1];
		dataBuffer4->elementAt(bufferIndex,7) = FContactError[2];
		dataBuffer4->elementAt(bufferIndex,8) = Fdesired[2];
//		dataBuffer4->elementAt(bufferIndex,6) = (*tjspace)[WA1];
//		dataBuffer4->elementAt(bufferIndex,7) = (*tjspace)[SH1];
//		dataBuffer4->elementAt(bufferIndex,8) = (*tjspace)[EL1];

		// dataminienc.txt
		dataBuffer9->elementAt(bufferIndex,0) = eAngle[0];	// waist joint position
		dataBuffer9->elementAt(bufferIndex,1) = eAngle[2];	// shoulder joint position
		dataBuffer9->elementAt(bufferIndex,2) = eAngle[4];	// elbow joint position
		dataBuffer9->elementAt(bufferIndex,3) = (float)(encoder[0].Long);	// elbow joint position

		bufferIndex++;
	}

}

void dm2Control::saveBuffer(void)
{
	printf("Saving data buffer... \n");
	int i;
	openDataFile();

	for(i=0;i<bufferIndex;i++)
	{
		// databtorque.txt
		fprintf(fdata,"%7.2f %7.2f %7.2f %7.2f %7.2f %7.2f %8.0f %8.0f %8.0f %6.4f\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),dataBuffer1->elementAt(i,9));
	
		// datajtorque.txt
		fprintf(fdata2,"%7.2f %7.2f %7.2f %7.2f %7.2f %7.2f\n",
						dataBuffer2->elementAt(i,0),dataBuffer2->elementAt(i,1),
						dataBuffer2->elementAt(i,2),dataBuffer2->elementAt(i,3),
						dataBuffer2->elementAt(i,4),dataBuffer2->elementAt(i,5));

		// datajoint.txt
		fprintf(fdata3,"%7.3f %7.3f %7.3f %7.3f %7.3f %7.3f\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));

		// datafcontrol.txt
		fprintf(fdata4,"%6.3f %6.3f %6.3f %6.3f %6.3f %6.3f %6.2f %6.2f %6.2f\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));

		// datamini.txt
		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));

		// dataee.txt
		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));
	
		// data jcontrol.txt
		fprintf(fdata7,"%7.3f %7.3f %7.3f %6.2f %6.2f %6.2f %7.3f %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));

		// dataoscontrol.txt
		fprintf(fdata8,"%6.4f %6.4f %6.4f %6.2f %6.2f %6.2f %7.3f %7.3f %7.3f %7.3f\n",
						dataBuffer8->elementAt(i,0),dataBuffer8->elementAt(i,1),
						dataBuffer8->elementAt(i,2),dataBuffer8->elementAt(i,3),
						dataBuffer8->elementAt(i,4),dataBuffer8->elementAt(i,5),
						dataBuffer8->elementAt(i,6),dataBuffer8->elementAt(i,7),
						dataBuffer8->elementAt(i,8),dataBuffer8->elementAt(i,9));

		// dataminienc.txt
		fprintf(fdata9,"%8.2f %8.2f %8.2f %8.0f\n",
						dataBuffer9->elementAt(i,0),dataBuffer9->elementAt(i,1),
						dataBuffer9->elementAt(i,2),dataBuffer9->elementAt(i,3));
	}

	closeDataFile();

	printf("Saving buffer finished \n");
}	


// OLD VERSION
#if 0
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,7) = (*dqdes)[WA1];
		dataBuffer7->elementAt(bufferIndex,8) = (*dqdes)[SH1];
		dataBuffer7->elementAt(bufferIndex,9) = (*dqdes)[EL1];

		dataBuffer8->elementAt(bufferIndex,0) = (*xRightDes)[0];
		dataBuffer8->elementAt(bufferIndex,1) = (*xRightDes)[1];
		dataBuffer8->elementAt(bufferIndex,2) = (*xRightDes)[2];
		dataBuffer8->elementAt(bufferIndex,3) = (*F)[0];
		dataBuffer8->elementAt(bufferIndex,4) = (*F)[1];
		dataBuffer8->elementAt(bufferIndex,5) = (*F)[2];
		dataBuffer8->elementAt(bufferIndex,6) = ttraj;
		dataBuffer8->elementAt(bufferIndex,7) = (*dxRightDes)[0];
		dataBuffer8->elementAt(bufferIndex,8) = (*dxRightDes)[1];
		dataBuffer8->elementAt(bufferIndex,9) = (*dxRightDes)[2];

		dataBuffer2->elementAt(bufferIndex,0) = (*q)[WA1];	// shoulder joint position
		dataBuffer2->elementAt(bufferIndex,1) = (*q)[SH1];	// shoulder joint position
		dataBuffer2->elementAt(bufferIndex,2) = (*q)[EL1];	// shoulder joint position

		bufferIndex++;
	}

}

void dm2Control::saveBuffer(void)
{
	printf("Saving data buffer... \n");
	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,"%7.3f %7.3f %7.3f %\n",
						dataBuffer2->elementAt(i,0),dataBuffer2->elementAt(i,1),
						dataBuffer2->elementAt(i,2));

		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.3f %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));
	
		fprintf(fdata8,"%6.4f %6.4f %6.4f %6.2f %6.2f %6.2f %7.3f %7.3f %7.3f %7.3f\n",
						dataBuffer8->elementAt(i,0),dataBuffer8->elementAt(i,1),
						dataBuffer8->elementAt(i,2),dataBuffer8->elementAt(i,3),
						dataBuffer8->elementAt(i,4),dataBuffer8->elementAt(i,5),
						dataBuffer8->elementAt(i,6),dataBuffer8->elementAt(i,7),
						dataBuffer8->elementAt(i,8),dataBuffer8->elementAt(i,9));
	}

	closeDataFile();

	printf("Saving buffer finished \n");
}	

#endif

// 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]);	//1Hz
			(*dq)[i] = 1.0*0.9752*(*dqOld)[i] + 24.8208*((*q)[i] - (*qOld)[i]);	//4Hz
	//		(*dq)[i] = 1.0*0.9391*(*dqOld)[i] + 60.9181*((*q)[i] - (*qOld)[i]);	//10 Hz
			(*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;
	// PST HACK 06/01/2005
	// I am having problem with ENC1 not updating count at all so I moved 
	// Waist CANNON to ENC7
	encoder[WAIST_JOINT].Long = temp[7].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::readForceSensor(void)
{
	PrForceSensorJr3::ForceArray fdata;
	fdata = forceRight->getForces(3);

	for(int j=0;j<3;j++)
	{
		FmeasRaw[j] = fdata.axis[j];
//		Fmeas[j] = FmeasRaw[j];
	}

	compensateForceSensorReading();
	transformForceSensorReading();
}

void dm2Control::initForceSensorOffset(void)
{
	printf("Zeroing force sensor... \n");
	forceRight->zero();
	PrVector3 initialG;

	initialG[0] = 0.0; 
	initialG[1] = 0.0;
	initialG[2] = 0.165*9.81;

	initOffset = RF0*initialG;
	// this is the initial offset that the JR3 code subtracts from the force reading
	// it is in force sensor local frame

	printf("Initial force sensor offset: %5.3f %5.3f %5.3f \n",
				initOffset[0], initOffset[1], initOffset[2]);

}

void dm2Control::transformForceSensorReading(void)
{
	Fmeas = R0F*FmeasLocal;
}

void dm2Control::compensateForceSensorReading(void)
{	
	PrVector3 g0, g5;
	g0[0] = 0.0; g0[1] = 0.0; g0[2] = -0.165*9.81;
	g5 = RF0*g0;

	FmeasLocal[0] = FmeasRaw[0] - g5[0] - initOffset[0];
	FmeasLocal[1] = FmeasRaw[1] - g5[1] - initOffset[1];
	FmeasLocal[2] = FmeasRaw[2] - g5[2] - initOffset[2];

}

void dm2Control::readLOHET(void)
{
	// LOHET voltage
	LOHET[WA1] = (float)(stgRight->GetADC(WA1))*ADCFACTOR;
//	LOHET[SH1] = (float)(stgRight->GetADC(SH1))*ADCFACTOR;
	// TEMP HACK BY PST 06/10/05
	// I moved the SH1 LOHET from [1] to [4] as I'm having weird problem with
	// AD[1] not working correctly
	LOHET[SH1] = (float)(stgRight->GetADC(4))*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] - 4.3;	// 3.5	// 4.5 // 1.6
//	baseSpringTorque[SH1] = 0.714*baseSpringTorque[SH1] - 1.30;	// 3.5	// 4.5 // 1.6
	baseSpringTorque[SH1] = 0.714*baseSpringTorque[SH1] - 2.00;	// 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] - 0.4;
	baseSpringTorque[EL1] = 0.9*baseSpringTorque[EL1] - 0.2;

	// 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("%t: %6.2f, 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",
				time,(*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 == 2*DISPLAY_RATE)
	{
		printf("t: %6.2f, q: %6.2f %6.2f %6.2f G: %5.2f %5.2f %5.2f LT: %5.3f %5.3f %5.3f DAC: %4d %4d %4d \n",
				time,(*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]);

//		printf("x,y,z: %6.4f %6.4f %6.4f, Fxyz: %5.3f %5.3f %5.3f \n", 
//					(*xRight)[0],(*xRight)[1],(*xRight)[2], Fmeas[0], Fmeas[1], Fmeas[2]);
		printf("x,y,z: %6.4f %6.4f %6.4f \n", 
					(*xRight)[0],(*xRight)[1],(*xRight)[2]);
		printf("Fxyz meas: %5.3f %5.3f %5.3f, Fxyz des: %5.3f %5.3f %5.3f \n\n", 
				//	Fmeas[0],Fmeas[1],Fmeas[2], (*F)[0], (*F)[1], (*F)[2]);
					Fmeas[0],Fmeas[1],Fmeas[2], Fdesired[0], Fdesired[1], Fdesired[2]);
//		printf("WA-mini: %f, enc: %d \n\n", eAngle[0], encoder[0].Long);

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

	for(i=0;i<3;i++)
	{
		(*xRightGoal)[i] = xfinal[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.0*((*qgoal)[i] - (*q)[i])/(2*dqmax));
		// calculate cubic spline parameters

		if((*tfinal)[i] > 0.0)
		{
			(*a0)[i] = (*q)[i];	// a0 = qinitial 
			(*a2)[i] = (3.0/((*tfinal)[i]*(*tfinal)[i]))*((*qgoal)[i] - (*q)[i]);
			// a2 = (3/tfinal^2)*(qfinal - qinitial)
			(*a3)[i] = -(2.0/((*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.0*(*a2)[i]*time + 3.0*(*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
}

void dm2Control::calcXTraj(float time)
{
	int i;
  	static int counter = 0;
	for(i=0;i<NUM_JOINT;i++)
	{
		if(time < (*tfinal)[i])
		{
				(*xRightDes)[i] = (*a0)[i] + (*a2)[i]*time*time + (*a3)[i]*time*time*time;
				(*dxRightDes)[i] = 2.0*(*a2)[i]*time + 3.0*(*a3)[i]*time*time;
		}
		else
		{
				(*xRightDes)[i] = (*xRightGoal)[i];
				(*dxRightDes)[i] = 0.0;
		}
	}

	ttraj = time;
}

void dm2Control::calcOscTraj(float time, int jNumber, float qdes_, float dqdes_, float qinit[])
{
	int i=0;
	for(i=0;i<3;i++)
	{
		(*qdes)[i] = qinit[i];
		(*dqdes)[i] = 0.0;
	}
	(*qdes)[jNumber] = qdes_;
	(*dqdes)[jNumber] = dqdes_;

	ttraj = time;
}

void dm2Control::calcXOscTraj(float time, int axisNumber, float xdes_[], float dxdes_[], float xinit[])
{
	int i=0;
	for(i=0;i<3;i++)
	{
		(*xRightDes)[i] = xdes_[i];
		(*dxRightDes)[i] = dxdes_[i];
	}
	(*xRightDes)[axisNumber] = xinit[axisNumber];
	(*dxRightDes)[axisNumber] = 0.0;

	ttraj = time;
}
// 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::checkJointLimit(void)
{
	int i;

	//jointLimitMax and jointLimitMin are in deg
	//qdes is in radian
	for(i=0;i<3;i++)
	{
		if((*qdes)[i]>jointLimitMax[i])
			(*qdes)[i] = jointLimitMax[i];
		else if((*qdes)[i] < jointLimitMin[i])
			(*qdes)[i] = jointLimitMin[i];
	}
}

void dm2Control::setUserMode(int mode)
{
	userMode = mode;	
}

// non-dynamic joint space control
void dm2Control::jointControlNonDynamic(int miniMode)
{
//	const float Kp[6] = {80.0, 100.0, 0.0,0.0,0.0,0.0};
	float Kp[3], Kv[3];
	if (miniMode)
	{
	//	Kp[0] = 300.0;	Kv[0] = 12.0;	// wn = 2.0 Hz, zeta = 0.3, I ~ 1.00
		Kp[0] = 600.0;	Kv[0] = 20.0;	// wn = 4.78 Hz, zeta = 0.3, I ~ 1.00
//		Kp[0] = 900.0;	Kv[0] = 18.0;	// wn = 4.78 Hz, zeta = 0.3, I ~ 1.00
//		Kp[1] = 800.0;	Kv[1] = 17.0;	// wn = 4.5 Hz, zeta = 0.3, I ~ 1.00
		Kp[1] = 600.0;	Kv[1] = 20.0;	// wn = 4.5 Hz, zeta = 0.3, I ~ 1.00
//		Kp[2] = 53.99;	Kv[2] = 1.29;	// wn = 4.0 Hz, zeta = 0.3, I = 0.08548
		Kp[2] = 30.00;	Kv[2] = 1.0;	// wn = 4.0 Hz, zeta = 0.3, I = 0.08548
//		Kp[2] = 76.932;	Kv[2] = 1.5386;	// wn = 4.78 Hz, zeta = 0.3, I = 0.08548
//		Kp[2] = 84.3654;	Kv[2] = 1.6113;	// wn = 5.00 Hz, zeta = 0.3, I = 0.08548
//		Kp[2] = 102.516;	Kv[2] = 1.7724;	// wn = 5.00 Hz, zeta = 0.3, I = 0.08548
//		Kp[2] = 121.4861;	Kv[2] = 1.9335;	// wn = 6.00 Hz, zeta = 0.3, I = 0.08548
//		Kp[2] = 121.4861;	Kv[2] = 1.28;	// wn = 6.00 Hz, zeta = 0.2, I = 0.08548
		// 400, 15
		// 500, 20
		// 600, 25; 650, 25
		// 700, 30; 800, 35
		// 800, 35
		// 900, 35
	}
	else if (!miniMode)
	{
		Kp[0] = 160.0;	Kv[0] = 10.0;	// wn = 2.0 Hz, zeta = 0.3, I ~ 1.00
		Kp[1] = 160.0;	Kv[1] = 8.0;	// wn = 2.0 Hz, zeta = 0.3, I ~ 1.00
	//	Kp[2] = 13.6768;	Kv[2] = 0.6487;	// wn = 2.0 Hz, zeta = 0.3, I = 0.085480
		Kp[2] = 30.35;	Kv[2] = 1.0;	// wn = 2.0 Hz, zeta = 0.2, I = 0.085480
//		Kp[2] = 53.96;	Kv[2] = 4.400;	// wn = 4.0 Hz, zeta = 0.2, I = 0.085480
	//	Kp[2] = 40.0;	Kv[2] = 3.0;	// wn = 4.0 Hz, zeta = 0.2, I = 0.085480
//		Kp[2] = 40.0;	Kv[2] = 4.0;	// wn = 4.0 Hz, zeta = 0.2, I = 0.085480
		// 200, 15
	}

//	float Kff[6] = {0.3, 0.3, 0.3, 0.0, 0.0, 0.0}; // velocity feedforward gain
	float Kff[6] = {0.2, 0.2, 0.0, 0.0, 0.0, 0.0}; // velocity feedforward gain
	
	float KvServoMode[6] = {0.0, 110.0, 0.0,0.0,0.0,0.0};
	float tlimit[3] = {30.0, 30.0, 10.0};
	int i;
	static int counter = 0;

	checkJointLimit();

	// standard joint position control
	if(jMode == POS_MODE)
	{
		for(i=0;i<NUM_JOINT;i++)
		{
			// was 0.4 
			(*tjspace)[i] = Kp[i]*(((*qdes)[i] - (*q)[i])/57.3) 
					+ Kv[i]*((Kff[i]*(*dqdes)[i] - (*dq)[i])/57.3);
			if((*tjspace)[i] > tlimit[i])
				(*tjspace)[i] = tlimit[i];
			if((*tjspace)[i] < -tlimit[i])
				(*tjspace)[i] = -tlimit[i];
		}
	}

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

#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 joint-space control
void dm2Control::jointControlDynamic(int miniMode)
{
	float Kp[3], Kv[3];
	if (miniMode)
	{
		// Kp = 900 is stable for waist and shoulder only motion
//		Kp[0] = 900.0;	Kv[0] = 35.0;	// wn = 4.78 Hz, zeta = 0.3, I ~ 1.00
//		Kp[0] = 600.0;	Kv[0] = 20.0;	// wn = 3.78 Hz, zeta = 0.3, I ~ 1.00
//		Kp[1] = 600.0;	Kv[1] = 20.0;	// wn = 4.5 Hz, zeta = 0.3, I ~ 1.00
//		Kp[0] = 900.0;	Kv[0] = 35.0;	// wn = 4.78 Hz, zeta = 0.3, I ~ 1.00
//		Kp[1] = 900.0;	Kv[1] = 35.0;	// wn = 4.78 Hz, zeta = 0.3, I ~ 1.00
		Kp[0] = 800.0;	Kv[0] = 16.97;	// wn = 4.78 Hz, zeta = 0.3, I ~ 1.00
		Kp[1] = 800.0;	Kv[1] = 16.97;	// wn = 4.78 Hz, zeta = 0.3, I ~ 1.00
		Kp[2] = 450.00;	Kv[2] = 25.00;		// wn = 4.0 Hz, zeta = 0.3, I = 0.08548
		// 400, 15
		// 500, 20
		// 600, 25; 650, 25
		// 700, 30; 800, 35
		// 800, 35
		// 900, 35
	}
	else if (!miniMode)
	{
		// 160, 8
		Kp[0] = 160.0;	Kv[0] = 8.0;	// wn = 2.0 Hz, zeta = 0.3, I ~ 1.00
		Kp[1] = 160.0;	Kv[1] = 8.0;	// wn = 2.0 Hz, zeta = 0.3, I ~ 1.00
	//	Kp[2] = 300.0;	Kv[2] = 15.0;	// wn = 2.0 Hz, zeta = 0.2, I = 0.085480
		Kp[2] = 200.0;	Kv[2] = 10.0;	// wn = 2.0 Hz, zeta = 0.2, I = 0.085480
	}

	float Kff[6] = {0.2, 0.2, 0.35, 0.0, 0.0, 0.0}; // velocity feedforward gain
//	float Kff[6] = {0.0, 0.0, 0.35, 0.0, 0.0, 0.0}; // velocity feedforward gain
	
	int i;
	static int counter = 0;
	float tlimit[3] = {30.0, 30.0, 10.0};

	checkJointLimit();

	// standard joint position control
	if(jMode == POS_MODE)
	{
		for(i=0;i<NUM_JOINT;i++)
		{
			// was 0.4 
			(*tjspace)[i] = Kp[i]*(((*qdes)[i] - (*q)[i])/57.3) 
					+ Kv[i]*((Kff[i]*(*dqdes)[i] - (*dq)[i])/57.3);

//			(*tjspace)[i] = AMatrix->elementAt(i,i)*(*tjspace)[i];
		
		}
	}

//	printf("%f, ", (*tjspace)[1]);
//	printf("%f, %f; ", AMatrix->elementAt(1,1)*(*tjspace)[1], 
//					AMatrix->elementAt(1,2)*(*tjspace)[2]);
	// multiply by A matrix for dynamic control
	(*tjspace) = (*AMatrix)*(*tjspace);
/*
	for(i=0;i<NUM_JOINT;i++)
	{
		if((*tjspace)[i] > tlimit[i])
			(*tjspace)[i] = tlimit[i];
		if((*tjspace)[i] < -tlimit[i])
			(*tjspace)[i] = -tlimit[i];
	}
*/
//	printf("%f %f \n", (*tjspace)[1], (*tjspace)[2]);
	
//	printf("q2: %f, q2d: %f, %f, %f, A:%f, %f \n", 
//			(*q)[2],(*qdes)[2], Kp[2]*((*qdes)[2]-(*q)[2])/57.3,
//			(*tjspace)[2],AMatrix->elementAt(2,1), AMatrix->elementAt(2,2));

//	AMatrix->display();

}

void dm2Control::virtualWallControl(int miniMode)
{
	static int counter = 0;
	const float Fmax[3] = {40.0, 40.0, 40.0};
	const float yConstraint = 0.1366;
	const float KsurfaceDM2 = 10000.0;
	const float KsurfaceM = 1000.0;
	int i;

	if(miniMode)
	{
		fstar->zero();
		if((*xRight)[1] >= yConstraint)
			(*fstar)[1] = KsurfaceDM2*(yConstraint - (*xRight)[1]);

	}
	else if (!miniMode)
	{
		fstar->zero();
		if((*xRight)[1] >= yConstraint)
			(*fstar)[1] = KsurfaceM*(yConstraint - (*xRight)[1]);
	}

//	(*F) = (*Lambda)*(*fstar);
	(*F)[0] = 0.0;
	(*F)[1] = (*fstar)[1]*Lambda->elementAt(1,1);
	(*F)[2] = 0.0;

	for(i=0;i<3;i++)
	{
		if((*F)[i] > Fmax[i])
			(*F)[i] = Fmax[i];
		else if ((*F)[i] < -Fmax[i])
			(*F)[i] = -Fmax[i];
	}

	if(counter == int(HZ))
	{
		printf("dy %f, fstar: %f, F: %f \n", yConstraint - (*xRight)[1], 
					(*fstar)[1], (*F)[1]);
		counter = 0;
	}
	counter++;

}


void dm2Control::opSpaceControl(int miniMode, int osMode)
{
	static int counter = 0;
	float wnx[6] = {0.0,0.0,0.0,0.0,0.0,0.0};
	float zetax[6] = {0.3,0.3,0.3,1.0,1.0,1.0};
	float Kpx[6] = {0.0,0.0,0.0,0.0,0.0,0.0};
	float Kvx[6] = {0.0,0.0,0.0,0.0,0.0,0.0};
	const float Kff[6] = {0.2,0.2,0.2,0.0,0.0,0.0};
	int i;
//	float Fmax[3] = {30.0, 30.0, 30.0};
	// 06/07/2005 PST increased Fmax to 40
	const float Fmax[3] = {40.0, 40.0, 40.0};
	const float Kpf[3] = {0.40, 0.4, 0.6};
	const float KpfCompliance[3] = {0.48, 0.0, 0.40};

	switch(osMode)
	{	
		// op-space position servo loop
		case(OS_SERVO):
		{
			if(miniMode)
			{
				wnx[0] = 5.0*2*PI;	// x-direction wn
				wnx[1] = 5.0*2*PI;	// y-direction wn
				wnx[2] = 5.0*2*PI;	// z-direction wn
			}
			else if(!miniMode)
			{
				wnx[0] = 2.0*2*PI;	// x-direction wn
				wnx[1] = 2.0*2*PI;	// y-direction wn
				wnx[2] = 2.0*2*PI;	// z-direction wn
			}
			for(i=0;i<3;i++)
			{
				Kpx[i] = wnx[i]*wnx[i];
				Kvx[i] = 2.0*wnx[i]*zetax[i];

				(*fstar)[i] = Kpx[i]*((*xRightDes)[i] - (*xRight)[i]) 
						+ Kvx[i]*(Kff[i]*(*dxRightDes)[i] - (*dxRight)[i]);
			}
			// dynamic compensation with Lambda matrix
			(*F) = (*Lambda)*(*fstar);
			break;
		}
		// op-space compliance control
		case(OS_HOLD):
		{
			if(miniMode)
			{
				wnx[0] = 7.0*2*PI;	// x-direction wn
				wnx[1] = 6.5*2*PI;	// y-direction wn
				wnx[2] = 6.5*2*PI;	// z-direction wn
				zetax[0] = 1.0;
				zetax[1] = 1.0;
				zetax[2] = 1.0;
			}
			else if(!miniMode)
			{
				wnx[0] = 2.0*2*PI;	// x-direction wn
				wnx[1] = 2.0*2*PI;	// y-direction wn
				wnx[2] = 2.0*2*PI;	// z-direction wn
				zetax[0] = 1.0;
				zetax[1] = 1.0;
				zetax[2] = 1.0;
			}
			for(i=0;i<3;i++)
			{
				Kpx[i] = wnx[i]*wnx[i];
				Kvx[i] = 2.0*wnx[i]*zetax[i];

				if(i==complianceAxis)
				{
					Kpx[i] = 0.0;
				//	Kvx[i] = -2.0*wnx[i]*0.006;
					Kvx[i] = 0.0;
					// positive damping in the compliance direction to help with motion
				}

				(*fstar)[i] = Kpx[i]*((*xRightDes)[i] - (*xRight)[i]) 
						+ Kvx[i]*(Kff[i]*(*dxRightDes)[i] - (*dxRight)[i]);

				//		(*F)[i] = (*fstar)[i]*Lambda->elementAt(i,i);
			}

			(*fstar)[complianceAxis] = KpfCompliance[complianceAxis]*Fdesired[complianceAxis];

			// dynamic compensation with Lambda matrix
			(*F) = (*Lambda)*(*fstar);
			break;
		}
		// closed-loop float + force control
		case(FLOAT_FORCE):
		{
			for(i=0;i<3;i++)
				(*fstar)[i] = Kpf[i]*(Fdesired[i]);

			(*F) = (*Lambda)*(*fstar);
		
		//	(*F)[0] = (*fstar)[0]*Lambda->elementAt(0,0);
		//	(*F)[1] = (*fstar)[1]*Lambda->elementAt(1,1);
		//	(*F)[2] = (*fstar)[2]*Lambda->elementAt(2,2);
			break;
		}

		//contact force control
		case(OS_CONTACT_CONTROL):
		{
	//		const float KpfContactDM2[3] = {0.0, 0.0, 0.700};
	//		const float KvfContactDM2[3] = {80.0, 80.0, 59.0};
	//		const float KifContactDM2 = 0.0066;
			const float KpfContactDM2[3] = {0.0, 0.0, 0.515};
			const float KvfContactDM2[3] = {80.0, 80.0, 50.5};
			const float KifContactDM2 = 0.0066;
	
		//	const float KpfContactM[3] = {0.0, 0.0, 0.64};
		//	const float KvfContactM[3] = {80.0, 80.0, 40.0};
			const float KpfContactM[3] = {0.0, 0.0, 0.64};
			const float KvfContactM[3] = {80.0, 80.0, 40.0};
			const float KifContactM = 0.0066;
			if(miniMode)
			{
				for(i=0;i<3;i++)
					(*fstar)[i] = -KpfContactDM2[i]*(FContactDesired[i] - Fmeas[i]) 
							- KvfContactDM2[i]*(*dxRight)[i];
				(*fstar)[2] -= KifContactDM2*FContactError[2];

				// highest 4.3
				if(fabs(FContactDesired[2] - Fmeas[2]) < 3.0)
					FContactError[2] += (FContactDesired[2] - Fmeas[2]);
				else
					FContactError[2] = 0.0;	// reset error integral
			}
			else if(!miniMode)
			{
				for(i=0;i<3;i++)
					(*fstar)[i] = -KpfContactM[i]*(FContactDesired[i] - Fmeas[i]) 
							- KvfContactM[i]*(*dxRight)[i];
				(*fstar)[2] -= KifContactM*FContactError[2];

				if(fabs(FContactDesired[2] - Fmeas[2]) < 3.3)
					FContactError[2] += (FContactDesired[2] - Fmeas[2]);
				else
					FContactError[2] = 0.0;	// reset error integral
			}
			// the "minus" term in front of the KpfContact is because 
			// i'm using the force sensor convention for the sign of 
			// the measured force (Jaeheung is using the inverse)	



			(*F) = (*Lambda)*(*fstar);
//			(*F)[0] = (*fstar)[0]*Lambda->elementAt(0,0);
//			(*F)[1] = (*fstar)[1]*Lambda->elementAt(1,1);
//			(*F)[2] = (*fstar)[2]*Lambda->elementAt(2,2);

		// OPEN LOOP HACK JUST FOR TESTING
			F->zero();
			(*F)[2] -= FContactDesired[2];
			
		//	(*F)[2] -= FContactDesired[2];
		//	(*F)[2] -= Fmeas[2];
			break;
		}
	}


	for(i=0;i<3;i++)
	{
		if((*F)[i] > Fmax[i])
			(*F)[i] = Fmax[i];
		else if ((*F)[i] < -Fmax[i])
			(*F)[i] = -Fmax[i];
	}

//	(*F) = (*fstar);
//	F->display();

	if(counter == (int)(2.0*HZ))
	{
		printf("at: %5.3f %5.3f %5.3f, des: %5.3f %5.3f %5.3f, F: %6.3f %6.3f %6.3f, tau: %6.2f %6.2f %6.2f \n",
				(*xRight)[0], (*xRight)[1], (*xRight)[2], 
				(*xRightDes)[0], (*xRightDes)[1], (*xRightDes)[2],
				(*F)[0], (*F)[1], (*F)[2],
				(*tjspace)[WA1], (*tjspace)[SH1], (*tjspace)[EL1]);
		counter = 0;
	}
	counter++;
}
#if 0
void dm2Control::opSpaceControl(int miniMode, int osMode)
{
	static int counter = 0;
	float wnx[6] = {0.0,0.0,0.0,0.0,0.0,0.0};
	float zetax[6] = {0.3,0.3,0.3,1.0,1.0,1.0};
	float Kpx[6] = {0.0,0.0,0.0,0.0,0.0,0.0};
	float Kvx[6] = {0.0,0.0,0.0,0.0,0.0,0.0};
	float Kff[6] = {0.2,0.2,0.2,0.0,0.0,0.0};
	int i;
	float Fmax[3] = {30.0, 30.0, 30.0};
	float Kpf[3] = {0.6, 0.4, 0.6};

	if(miniMode)	// if dm2
	{
		if(osMode == OS_HOLD)
		{
			wnx[0] = 6.0*2*PI;	// x-direction wn
			wnx[1] = 6.0*2*PI;	// y-direction wn
			wnx[2] = 6.0*2*PI;	// z-direction wn
			zetax[0] = 1.0;
			zetax[1] = 1.0;
			zetax[2] = 1.0;

		}
		else
		{
			wnx[0] = 5.0*2*PI;	// x-direction wn
			wnx[1] = 5.0*2*PI;	// y-direction wn
			wnx[2] = 5.0*2*PI;	// z-direction wn
		}
	}
	else if(!miniMode)	// if macro only
	{
		if(osMode == OS_HOLD)
		{
			wnx[0] = 3.0*2*PI;	// x-direction wn
			wnx[1] = 3.0*2*PI;	// y-direction wn
			wnx[2] = 3.0*2*PI;	// z-direction wn
			zetax[0] = 1.0;
			zetax[1] = 1.0;
			zetax[2] = 1.0;

		}
		wnx[0] = 2.0*2*PI;	// x-direction wn
		wnx[1] = 2.0*2*PI;	// y-direction wn
		wnx[2] = 2.0*2*PI;	// z-direction wn
	}

	if(osMode == FLOAT_FORCE)
	{
		for(i=0;i<3;i++)
			(*fstar)[i] = Kpf[i]*(Fdesired[i]);
	}
	else
	{
		for(i=0;i<3;i++)
		{
			Kpx[i] = wnx[i]*wnx[i];
			Kvx[i] = 2.0*wnx[i]*zetax[i];

			// check if we are in OS HOLD compliance mode
			// then set the gain for the compliance direction to zero
			if(osMode == OS_HOLD)
			{
				if(i==complianceAxis)
				{
					Kpx[i] = 0.0;
				//	Kvx[i] = 0.0;
					Kvx[i] = -2.0*wnx[i]*0.008;
					// positive damping in the compliance direction to help with motion
				}
			}
	
			(*fstar)[i] = Kpx[i]*((*xRightDes)[i] - (*xRight)[i]) 
					+ Kvx[i]*(Kff[i]*(*dxRightDes)[i] - (*dxRight)[i]);
		}
	}

	(*F) = (*Lambda)*(*fstar);

#if 0
	// in closed-loop float + force control we add the measured force to F
	if(osMode == FLOAT_FORCE)
	{
		for(i=0;i<3;i++)
			(*F)[i] += Fmeas[i];
#endif

	for(i=0;i<3;i++)
	{
		if((*F)[i] > Fmax[i])
			(*F)[i] = Fmax[i];
		else if ((*F)[i] < -Fmax[i])
			(*F)[i] = -Fmax[i];
	}

//	(*F) = (*fstar);
//	F->display();

	if(counter == (int)(HZ))
	{
		printf("at: %5.3f %5.3f %5.3f, des: %5.3f %5.3f %5.3f, F: %6.3f %6.3f %6.3f, tau: %6.2f %6.2f %6.2f \n",
				(*xRight)[0], (*xRight)[1], (*xRight)[2], 
				(*xRightDes)[0], (*xRightDes)[1], (*xRightDes)[2],
				(*F)[0], (*F)[1], (*F)[2],
				(*tjspace)[WA1], (*tjspace)[SH1], (*tjspace)[EL1]);
		counter = 0;
	}
	counter++;
}
#endif

void dm2Control::opSpaceHoldParameters(float xhold[], int axis)
{
	(*xRightDes)[0] = xhold[0];
	(*xRightDes)[1] = xhold[1];
	(*xRightDes)[2] = xhold[2];

	(*xRightDes)[axis] = (*xRight)[axis];
	complianceAxis = axis;
}

void dm2Control::setDesiredForce(void)
{
	float threshold = 0.3;
	int i;
	
	for(i=0;i<3;i++)
	{
		if(fabs(Fmeas[i]) < threshold)
			Fdesired[i] = 0.0;
		else if (Fmeas[i] > threshold)
			Fdesired[i] = (Fmeas[i] - threshold);
		else if (Fmeas[i] < -threshold)
			Fdesired[i] = (Fmeas[i] + threshold);
	}

}

void dm2Control::setDesiredContactForce(void)
{
	static float runTime = 0.0;
	const float T = 0.001;
	const float forceInit = 6.0, forceFinal = 11.0;
	// forceInit = initial contact force desired
	// forceFinal = final contact force desired 
	int i;
	static int counter = 0;
	const float duration = 1.5;

	if(counter < 3)
	{
		if(runTime <= duration) 
		{
			runTime += T;
			FContactDesired[2] = forceInit;
		}
		else if((runTime > duration) & (runTime <= 2.0*duration))
		{
			runTime += T;
			FContactDesired[2] = forceFinal;
		}
		else if(runTime > 2.0*duration)
		{
			runTime = 0.0;
			counter++;
		}
	}
	else
	{
		FContactDesired[2] = forceInit;
	}
	
	for(i=0;i<3;i++)
		Fdesired[i] = FContactDesired[i];

//	printf("Fd: %f, runTime: %f, counter %d\n", FContactDesired[2], runTime, counter);
}

// ----------------------------------------------------------------------- //
// 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
static float m1 = 4.3861, m2 = 3.4693, m3 = 1.76214;	// 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;

}

void dm2Control::calcRotationMatrix(void)
{
	//R03 is the rotation matrix that transform vector in [3] into [global] 
	R03.elementAt(0,0) = cos(qt[WA1])*cos(qt23Right);
	R03.elementAt(0,1) = -cos(qt[WA1])*sin(qt23Right);
	R03.elementAt(0,2) = -sin(qt[WA1]);
	R03.elementAt(1,0) = sin(qt23Right);
	R03.elementAt(1,1) = cos(qt23Right);
	R03.elementAt(1,2) = 0.0;
	R03.elementAt(2,0) = sin(qt[WA1])*cos(qt23Right);
	R03.elementAt(2,1) = -sin(qt[WA1])*sin(qt23Right);
	R03.elementAt(2,2) = cos(qt[WA1]);

	R0F = R03*R3F;
	
	RF0 = R0F.transpose();
}

#define DEBUG_DYNAMICS	0
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
#if 0
	(*xRight)[0] = cos(qt[WA1])*cos(qt[SH1])*d3x - cos(qt[WA1])*sin(qt[SH1])*d3y
					+ cos(qt[WA1])*cos(qt[SH1]+qt[EL1])*eex
					- cos(qt[WA1])*sin(qt[SH1]+qt[EL1])*eey
					- sin(qt[WA1])*eez;

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

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

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

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

	(*xRight)[2] = sin(qt[WA1])*cos(qt[SH1])*d3x - sin(qt[WA1])*sin(qt[SH1])*d3y
					+ sin(qt[WA1])*cos(qt23Right)*eex
					- sin(qt[WA1])*sin(qt23Right)*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 0.8	// 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)
{
	float waOffset = -1.5;
	float waSlope = 1.0;
	float elOffset = 0.2;

	switch(userMode)
	{
		case(FLOAT):
		{
			(*jointTorqueDes)[WA1] = waSlope*gravity[WA1] + waOffset;
			(*jointTorqueDes)[SH1] = gravity[SH1];
			(*jointTorqueDes)[EL1] = gravity[EL1] + elOffset;
			break;
		}
		case(JOINT_SERVO):
		{
			(*jointTorqueDes)[WA1] = (*tjspace)[WA1] + waSlope*gravity[WA1] + waOffset;
			(*jointTorqueDes)[SH1] = (*tjspace)[SH1] + gravity[SH1];
			(*jointTorqueDes)[EL1] = (*tjspace)[EL1] + gravity[EL1] + elOffset;
			break;
		}
		case(JOINT_OSC):
		{
			(*jointTorqueDes)[WA1] = (*tjspace)[WA1] + waSlope*gravity[WA1] + waOffset;
			(*jointTorqueDes)[SH1] = (*tjspace)[SH1] + gravity[SH1];
			(*jointTorqueDes)[EL1] = (*tjspace)[EL1] + gravity[EL1] + elOffset;
			break;
		}
		case(OS_SERVO):
		{
			(*tjspace) = (J->transpose())*(*F);

			(*jointTorqueDes)[WA1] = (*tjspace)[WA1] + waSlope*gravity[WA1] + waOffset;
			(*jointTorqueDes)[SH1] = (*tjspace)[SH1] + gravity[SH1];
			(*jointTorqueDes)[EL1] = (*tjspace)[EL1] + gravity[EL1] + elOffset;
			break;
		}
		case(OS_HOLD):
		{
			(*tjspace) = (J->transpose())*(*F);

			(*jointTorqueDes)[WA1] = (*tjspace)[WA1] + waSlope*gravity[WA1] + waOffset;
			(*jointTorqueDes)[SH1] = (*tjspace)[SH1] + gravity[SH1];
			(*jointTorqueDes)[EL1] = (*tjspace)[EL1] + gravity[EL1] + elOffset;
			break;
		}
		case(OS_CONTACT_CONTROL):
		{
			(*tjspace) = (J->transpose())*(*F);

			(*jointTorqueDes)[WA1] = (*tjspace)[WA1] + waSlope*gravity[WA1] + waOffset;
			(*jointTorqueDes)[SH1] = (*tjspace)[SH1] + gravity[SH1];
			(*jointTorqueDes)[EL1] = (*tjspace)[EL1] + gravity[EL1] + elOffset;
			break;
		}
		case(OS_VWALL):
		{
			(*tjspace) = (J->transpose())*(*F);
//			tjspace->zero();

			(*jointTorqueDes)[WA1] = (*tjspace)[WA1] + waSlope*gravity[WA1] + waOffset;
			(*jointTorqueDes)[SH1] = (*tjspace)[SH1] + gravity[SH1];
			(*jointTorqueDes)[EL1] = (*tjspace)[EL1] + gravity[EL1] + elOffset;
			break;
		}
		case(FLOAT_FORCE):
		{
			(*tjspace) = (J->transpose())*(*F);
		//	tjspace->zero();
			(*jointTorqueDes)[WA1] = (*tjspace)[WA1] + waSlope*gravity[WA1] + waOffset;
			(*jointTorqueDes)[SH1] = (*tjspace)[SH1] + gravity[SH1];
			(*jointTorqueDes)[EL1] = (*tjspace)[EL1] + gravity[EL1] + elOffset;
			break;
		}
		case(OS_OSC):
		{
			(*tjspace) = (J->transpose())*(*F);

			(*jointTorqueDes)[WA1] = (*tjspace)[WA1] + waSlope*gravity[WA1] + waOffset;
			(*jointTorqueDes)[SH1] = (*tjspace)[SH1] + gravity[SH1];
			(*jointTorqueDes)[EL1] = (*tjspace)[EL1] + gravity[EL1] + elOffset;
			break;
		}
		case(VEL_SERVO):
		{
			(*jointTorqueDes)[WA1] = (*tjspace)[WA1] + waSlope*gravity[WA1] + waOffset;
			(*jointTorqueDes)[SH1] = (*tjspace)[SH1] + gravity[SH1];
			(*jointTorqueDes)[EL1] = (*tjspace)[EL1] + gravity[EL1] + elOffset;
			break;
		}
		case(JOINT_DAMPING):
		{
			(*jointTorqueDes)[WA1] = (*tjspace)[WA1] + waSlope*gravity[WA1] + waOffset;
			(*jointTorqueDes)[SH1] = (*tjspace)[SH1] + gravity[SH1];
			(*jointTorqueDes)[EL1] = (*tjspace)[EL1] + gravity[EL1] + elOffset;
			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] - 0.3946*(*jointTorqueDes)[SH1] 
			- 0.0*0.3553*(gravity)[EL1];
//	(*baseTorqueDes_)[WA1] = (*jointTorqueDes)[WA1];
//	(*baseTorqueDes_)[SH1] = (*jointTorqueDes)[SH1] - 0.3*1.0*(*jointTorqueDes)[EL1];
//	(*baseTorqueDes_)[SH1] = (*jointTorqueDes)[SH1] - 0.00*1.0*(*jointTorqueDes)[EL1];
	(*baseTorqueDes_)[SH1] = (*jointTorqueDes)[SH1] - 1.0*gravity[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(userMode == FLOAT)
	{
		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];
		}
	}
	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]);
	writeElbowDAC(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};

	// Kp = {0.28125, 0.18, 0.55}
	// Klag = {0.03,0.0213,0.045}

	float Kp[3] = {0.25,0.18,0.42}, 
//		a1[3] = {2.2042,1.5429,2.645}, 
		a1[3] = {2.2042,1.5429,2.2042}, 
		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
	// Kd = 0.0060, wc = 90Hz, T = 0.001 ms for EL 

	// May 31 2005, PST removed all lag gains from SEA controller
//	const float Klag[3] = {0.00,0.00,0.00}, 
//	const float Klag[3] = {0.00,0.0213,0.045}, 
	const float Klag[3] = {0.02,0.0213,0.02}, 
	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]);
	writeElbowDAC(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
}

// function to write DAC to Elbow DAC channel which is connected to the
// extra MAX547 chip.
void dm2Control::writeElbowDAC(float elbowDAC)
{
	dacRight->writeData(elbowDAC);
}

void dm2Control::miniControl(int miniMode)
{
	static float rampCounter = 0.0, rampTime = 50.0;
//	const float miniTorqueMax[3] = {24.0, 21.0, 06.0};
	// 06/07/2005 PST increased miniTorqueMax to max spec
	const float miniTorqueMax[3] = {34.0, 31.0, 11.6};
	// actually its 34, 31 and 11.6
	int i;

	if(miniMode)
	{
		// 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]);	
	}
	else if(!miniMode)
	{
		zeroMiniOutput();
		miniTorqueDes[WA1] = 0.0;
		miniTorqueDes[SH1] = 0.0;
		miniTorqueDes[EL1] = 0.0;
	}
			
}

void dm2Control::zeroMiniOutput(void)
{
	stgRight->RawDAC(0,0);
	stgRight->RawDAC(1,0);
	stgRight->RawDAC(2,0);
	stgRight->RawDAC(3,0);
	stgRight->RawDAC(4,0);
	stgRight->RawDAC(5,0);
}

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