

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

#include "driver/PrTimer.h"
#include "driver/PrSTGDefn.h"
#include "matrix/PrVector.h"
#include "matrix/PrMatrix.h"
#include "baseControl.h"

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

#define HZ  1000	// WHEN CHANGING THIS REMEMBER THAT THE DIGITAL FILTER
					// COEFFS NEED TO BE CHANGED ALSO!!!!
					// lowPassFilter();
					// ALSO COEFFS FOR DIGITAL COMPENSATOR

#define BUFFER_SIZE		HZ*60	
#define BUFFER_COL		10	

#define NUM_JOINT	3
#define NUM_JOINT1	6

//#define PRINT
baseControl::baseControl()
{
	int i;
	stg = new ServoToGo(STG_ARM_IRQ, STG_ARM_ADDRESS);
//	stg2 = new ServoToGo(STG2_ARM_IRQ, STG2_ARM_ADDRESS);

	for(int i=0;i<6;i++)
	{
		baseCommand[i] = 0;
	}

	initSystem();

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

	baseOutputAngleOld[EL1] = 0.0;

	dataBuffer3 = new PrMatrix(BUFFER_SIZE,BUFFER_COL);
	dataBuffer4 = new PrMatrix(BUFFER_SIZE,BUFFER_COL);
	dataBuffer3->zero();
	dataBuffer4->zero();
	printf("Constructor finished \n");


}

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

void baseControl::initSystem(void)
{
	stg->EncoderInit();
	stg->EncoderLatch();
//	stg->SetEncoderCounts(baseWA1,0);
//	stg->SetEncoderCounts(baseSH1,0);
	initIndex();
//	initLOHETValue[WA1] = stg->GetADC(WA1);
//	initLOHETValue[SH1] = stg->GetADC(EL1);
//	initLOHETValue[EL1] = stg->GetADC(EL1);
	initLOHET[WA1] = (float)(stg->GetADC(WA1))*2.0*5.0/4092.0;
	initLOHET[WA1] = (float)(stg->GetADC(WA1))*2.0*5.0/4092.0;
	initLOHET[WA1] = (float)(stg->GetADC(WA1))*2.0*5.0/4092.0;
	printf("Initial LOHET value: %f \n", initLOHET[WA1]);
}

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

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

int baseControl::hitIndex2(int channel)
{
	int check = 1<<channel;
	int result = stg->GetIndexLatches();
	if(stg->GetIndexLatches() & (1<<channel))
	{
//			printf("index: %d, %x, %d, %d \n", result, 
//							result, check, result & check  );
			return(1);
	}	
	else
		return(0);
}

void baseControl::openDataFile(void)
{
	fdata = fopen("dataBase.txt","w");
//	fdata2 = fopen("datajoint.txt","w");
}

void baseControl::closeDataFile(void)
{
	fclose(fdata);
//	fclose(fdata2);
}

void baseControl::readPosition(void)
{
	stg->EncoderLatch();
	stg->EncReadAll(temp);

	baseEncoder[WA1].Long = temp[baseWA1].Long;
	// temporary HACK HERE!!!
	// ELBOW CURRENTLY USING baseSH1 CHANNEL. SHOULD REVERT BACK TO baseEL1 LATER!!!
	baseEncoder[EL1].Long = temp[baseSH1].Long;
	baseEncoder[SH1].Long = temp[baseSH1].Long;
//	printf("%d \n", baseEncoder[WA1].Long);
}

void baseControl::calcPosition(void)
{
		//all angles are in DEGREES!!!
	// USE THIS FOR WAIST JOINT
	baseOutputAngle[WA1] = (float)(baseEncoder[WA1].Long) * 360.0*(1.0/120.0)/8192.0;
	// USE THIS FOR ELBOW JOINT
	baseOutputAngle[EL1] = (float)(baseEncoder[EL1].Long) * 360.0*(1.0/50.0)/8192.0;

	// TEMPORARY HACK FOR SHOULDER 2 AXIS
	baseOutputAngle[SH1] = (float)(baseEncoder[SH1].Long) * 360.0*(1.0/100.0)/8192.0;


	baseOutputVel[EL1] = baseOutputAngle[EL1] - baseOutputAngleOld[EL1];
	baseOutputAngleOld[EL1] = baseOutputAngle[EL1];

//	printf("Angle: %f, %d  \n", baseOutputAngle[WA1], baseEncoder[WA1].Long);
#ifdef PRINT
	printf("Angle: %f  \n", baseOutputAngle[WA1]);
#endif
}

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

	if(counter == 100)
	{
//		printf("Angle: %6.2f, LOHET: %5.3f, torque: %5.2f \n", baseOutputAngle[WA1], LOHET[WA1],
//						baseSpringTorque[WA1]);
		printf("[WA1]: %5.3f, %6.3f Nm, [SH1]: %5.3f, %6.3f Nm [EL1]: %5.3f, %6.3f Nm \n", 
						LOHET[WA1], baseSpringTorque[WA1], LOHET[SH1],baseSpringTorque[SH1], 
						LOHET[EL1], baseSpringTorque[EL1]);
		counter = 0;
	}
	counter++;
}

void baseControl::resetBaseEncoder(void)
{
	stg->SetEncoderCounts(baseWA1,0);	
	stg->SetEncoderCounts(baseSH1,0);	
	printf("Base encoder reset \n");
}

#define ADCFACTOR	10.0/4096.0
#define EL1BIAS		0
void baseControl::readLOHET(void)
{
	// LOHET voltage
	LOHET[WA1] = (float)(stg->GetADC(WA1))*ADCFACTOR - 0.012; 
	LOHET[SH1] = (float)(stg->GetADC(SH1))*ADCFACTOR;
	LOHET[EL1] = (float)(stg->GetADC(EL1))*ADCFACTOR - 0.000;	// the reading is biased by 37 counts

#if 1
//	notchFilter();
	lowPassFilter();
#endif

//	printf("LOHET: %f, %f, %f \n", LOHET[WA1], LOHET[SH1], LOHET[EL1]);

	const float m[3] = {27.3, 0.0, -12.0};
	const float b[3] = {0.00, 0.0, 0.0};

	//wa4 = -0.847
	const float wa1 = -0.0354, wa2 = 0.15, wa3 = -14.9, wa4 = -0.0;
//	const float el1 = 0.0115, el2 = 0.0181, el3 = 7.36;
	const float sh1 = 0.101, sh2 = -13.6, sh3 = -0.0138;
	const float el1 = 0.0242, el2 = -0.16, el3 = 7.36;

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

	//SH1 torque
	// using square fit since the cubic doesnt improve accuracy that much for SH
	baseSpringTorque[SH1] = sh1*LOHET[SH1]*LOHET[SH1] 
			+ sh2*LOHET[SH1] + sh3; 

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

}

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

}

// 46Hz Notch filter to suppress noise from flat panel monitor
void baseControl::notchFilter(void)
{
	const float a1 = 0.9183, a2 = 1.553, a3 = 0.9183;
	const float b1 = 1.553, b2 = 0.8367;
//	static float inOld[6] = {0.0,0.0,0.0,0.0,0.0,0.0};
//	static float inOld2[6] = {0.0,0.0,0.0,0.0,0.0,0.0};
	float in[6] = {0.0,0.0,0.0,0.0,0.0,0.0};
	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 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};

	for(int i=0;i<3;i++)
	{
		in[i] = LOHET[i];
		LOHET[i] = b1*outOld[i] - b2*outOld2[i] + a1*in[i] - a2*inOld[i] + a3*inOld2[i];
		outOld2[i] = outOld[i];
		outOld[i] = LOHET[i];
		inOld2[i] = inOld[i];
	    inOld[i] = in[i];	
	}

}

void baseControl::calcTorque(void)
{
//	float m[3] = {27.3, 0.0, 5.12};
	float m[3] = {27.3, 0.0, -12.0};
	float b[3] = {0.00, 0.0, 0.0};
	//WA1 torque
	baseSpringTorque[WA1] = m[WA1]*LOHET[WA1] + b[WA1];
	//EL1 torque
	baseSpringTorque[EL1] = m[EL1]*LOHET[EL1] + b[EL1];

}

void baseControl::saveInfo(void)
{
	static long int index = 0;

	if(index < BUFFER_SIZE)
	{
		dataBuffer3->elementAt(index,0) = LOHET[WA1];
		dataBuffer3->elementAt(index,1) = LOHET[SH1];
		dataBuffer3->elementAt(index,2) = LOHET[EL1];
		dataBuffer3->elementAt(index,3) = baseOutputAngle[WA1];
		dataBuffer3->elementAt(index,4) = baseOutputAngle[SH1];
		dataBuffer3->elementAt(index,5) = baseOutputAngle[EL1];
//		dataBuffer3->elementAt(index,6) = (float)(baseCommand[WA1]);
		dataBuffer3->elementAt(index,6) = (float)(baseCommand[EL1]);
//		dataBuffer3->elementAt(index,7) = (float)(baseTorqueCommand[WA1]);
		dataBuffer3->elementAt(index,7) = (float)(baseTorqueCommand[EL1]);
		index++;
		bufferIndex = index;
	}

}

void baseControl::saveBuffer(void)
{
	int i;
	openDataFile();
	PrTimer bufferTimer(2000.0);

	printf("Saving data buffer \n");
	bufferTimer.start();
	for(i=0;i<bufferIndex;i++)
	{
		fprintf(fdata,"%6.3f %6.3f %6.3f %8.3f %8.3f %8.3f %5.0f %8.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),
						dataBuffer3->elementAt(i,6), dataBuffer3->elementAt(i,7));
		bufferTimer.wait();
	}

	closeDataFile();

	printf("Saving completed \n");
}

#define limitDAC	800
void baseControl::baseMotorControl(float runTime)
{
	float driveFreq = 0.5;
	float driveAmplitude[3] = {20.0, 6.0, 25.0};
	float Kp[3] = {300.0, 320.0, 350.0};
//	float Kp[3] = {125.0, 20.0, 300.0};

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

	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
	stg->RawDAC(6, baseCommand[WA1]);
#endif

#if 1	// SHOULDER CONTROL
	baseOutputAngleDes[SH1] = driveAmplitude[SH1]*sin(driveFreq*runTime);

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

	if (baseCommand[SH1] > limitDAC)
			baseCommand[SH1] = limitDAC;
	else if (baseCommand[SH1] < -limitDAC)
			baseCommand[SH1] = -limitDAC;
	// TEMP HACK... CURRENTLY STEALING DAC CHANNEL FROM ELBOW MINI 
	stg->RawDAC(7, baseCommand[SH1]);
#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]);
	printf("Des: %f, At: %f, control: %d \n", baseOutputAngleDes[SH1], baseOutputAngle[SH1],
				baseCommand[SH1]);
#endif

}

#define ELDAC	340		// FOR ELBOW
#define SHDAC	250		// FOR SHOULDER 2
#define WADAC	135		// FOR WAIST
void baseControl::moveBaseShaft(int direction)
{
	// +- 150 for WA motor
	// +- 400 for EL motor

#if 0	// WAIST / SHOULDER 1
	if(direction == 0)
		stg->RawDAC(6,WADAC);
	else if(direction == 1)
		stg->RawDAC(6,-WADAC);
#endif

#if 1	// SHOULDER 2
	// temporarily borrowing DAC channel from elbow mini?
	if(direction == 0)
		stg->RawDAC(7,SHDAC);
	else if(direction == 1)
		stg->RawDAC(7,-SHDAC);
#endif

#if 0	// ELBOW
		// Testing EL1 base (temp. connection to channel 7)
	if(direction == 0)
	{
		stg->RawDAC(7,ELDAC);
//		printf("writing %d to 1 \n", ELDAC);
	}
	else if(direction == 1)
	{
		stg->RawDAC(1,-ELDAC);
//		printf("writing %d to 1 \n", -ELDAC);

	}
#endif
}

#define KDAC	10.0/4096.0	// DAC gain, V/count
#define KAMP	2.0		// amplifier gain, A/V
#define KTORQUE	0.0727	// motor torque constant, N.m./A
#define limitDAC2	3500
void baseControl::SEAcontrol(float torqueDes)
{
	static float x = 0.0, xold = 0.0, y = 0.0, yold = 0.0;
	int i;

	baseTorqueCommand[WA1] = torqueDes;
	baseTorqueCommand[EL1] = torqueDes;
	x = baseTorqueCommand[EL1] - baseSpringTorque[EL1]; 
	// x is the difference between desired and spring torque


#if 0
	// ELBOW CONTROLLER
	//USING P+PSEUDO-D+LAG (LAST WORKING VERSION 02/05/05)
//	float Kp = 0.2, a1 = 0.54081, b1 = 0.6395;	// wc = 35Hz, T = 0.002 ms
	float Kp = 0.2, a1 = 0.5944, b1 = 0.8019;	// Kd = 0.003, wc = 35Hz, T = 0.001 ms
//	float Kp = 0.2, a1 = 0.3963, b1 = 0.8019;	// Kd = 0.002, wc = 35Hz, T = 0.001 ms

	static float y2, y2old = 0.0, x1, x3, y3;
//	x1 = baseSpringTorque[WA1];	// x1 is the measured spring torque
	x1 = baseSpringTorque[EL1];	// x1 is the measured spring torque

	Kp = 0.15;	// 0.16 for EL
	x3 = Kp*x;			// P term, Kp*(torque error)		
	// x3 is torque error * Kp
	static float x1old = x1;	// x1old is measured spring torque at the prev. tick
	static float y3old = 0.0, x3old = 0.0;
	
//	float a2 = 1.006, b2 = 0.9987, c2 = 0.9869;	// T = 0.002 ms
	float a2 = 1.003, b2 = 0.9993, c2 = 0.9934;	// T = 0.001 ms
	// P + lag term
	// input to this is (torque error * Kp), x3
	y3 = b2*y3old + a2*(x3 - c2*x3old);	// lag term
	y3old = y3;	x3old = x3;

	y2 = b1*y2old + a1*(x1 - x1old);	//y2 is the derivate term for the measured torque

	y = y3 - y2;	// (P+lag) - (derivative) term 

	y2old = y2;
	x1old = x1;

#endif

#if 1
	// ELBOW CONTROLLER
	//USING P+PSEUDO-D+LAG
	static float yP, yD, yDOld = 0.0, yL, yLold = 0.0;
	static float xOld = x, y2, y2old = 0.0, x1, x3, y3;
	static float x1Old = x1;	// x1old is measured spring torque at the prev. tick
	static float y3old = 0.0, x3old = 0.0;

	x1 = baseSpringTorque[EL1];	// x1 is the measured spring torque

#if(HZ == 1000)
//	float Kp, a1 = 1.2218, b1 = 0.7285;	// Kd = 0.0045, wc = 50Hz, T = 0.001 ms
//	float Kp, a1 = 1.0047, b1 = 0.7767;	// Kd = 0.0045, wc = 40Hz, T = 0.001 ms
	float Kp, a1 = 1.3*1.8076, b1 = 0.6395;	// Kd = 0.0045, wc = 70Hz, T = 0.001 ms
//	float Kp, a1 = 1.5864, b1 = 0.6395;	// Kd = 0.0044, wc = 70Hz, T = 0.001 ms
//	float Kp, a1 = 3.0646, b1 = 0.6395;	// Kd = 0.0085, wc = 70Hz, T = 0.001 ms
//	float Kp, a1 = 2.1902, b1 = 0.4524;	// Kd = 0.004, wc = 120Hz, T = 0.001 ms
//	float Kp, a1 = 2.5238, b1 = 0.6395;	// Kd = 0.007, wc = 80Hz, T = 0.001 ms
//	float Kp, a1 = 0.66981, b1 = 0.7767;	// Kd = 0.0045, wc = 40Hz, T = 0.001 ms
//	const float a2 = 1.0097, b2 = 0.9995, c2 = 0.9802;	// T = 0.001 ms
	const float Klag = 0.12, a2 = 1.0122, b2 = 0.9994, c2 = 0.9753;	// T = 0.001 ms
#endif
#if(HZ == 500)
	const float a2 = 1.0195, b2 = 0.999, c2 = 0.9608;	// T = 0.002 ms
	float Kp, a1 = 0.80339, b1 = 0.5983;	// Kd = 0.0045, wc = 40Hz, T = 0.002 ms
#endif
	yD = b1*yDOld + a1*(x1 - x1Old);	//yD is the derivative term for the measured torque
	yDOld = yD;	x1Old = x1;

	Kp = 0.25;	// 0.16 for EL
	yP = Kp*x;			// P term, Kp*(torque error)		
	// yP is proportional term
	
	// Lag term, input to this is torque error, x
	yL = b2*yLold + a2*(x - c2*xOld);

	const float intLimit = 2.0;
	if(yL > intLimit)
			yL = intLimit;
	if(yL < -intLimit)
			yL = -intLimit;


	yLold = yL;	xOld = x;

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

//	printf("yP: %f, yL: %f\n", yP, yL);

#endif

#if 0
	// waist conTROLLER
	//USING P+PSEUDO-D CONTROLLER+LAG
//	float Kp = 0.2, a1 = 0.59489, b1 = 0.6395;	// wc = 35Hz
	float Kp = 0.07, a1 = 0.3122, b1 = 0.7285;	// wc = 35Hz
	static float y2, y2old = 0.0, x1, x3, y3;
	x1 = baseSpringTorque[WA1];	// x1 is the measured spring torque
//	printf("x1 1: %f ", x1);
	Kp = 0.07;	// 0.16 FOR el
	x3 = Kp*x;			// P term, Kp*(torque error)		
	// x3 is torque error * Kp
	static float x1old = x1;	// x1old is measured spring torque at the prev. tick
	static float y3old = 0.0, x3old = 0.0;
	
	float a2 = 1.0054, b2 = 0.9988, c2 = 0.9881;
	// P + lag term
	// input to this is (torque error * Kp), x3
	y3 = b2*y3old + a2*(x3 - c2*x3old);	// lag term
	y3old = y3;	x3old = x3;

	// TEMPORARY TESTING USING ENCODER VELOCITY FOR THE DERIVATIVE TERM
	// COMMENT THIS OUT IF USING LOHET SENSOR FOR DERIVATIVE (NORMAL SITUATION)
//	x1 = baseOutputAngle[WA1]*152.0/57.3;
//	printf("x1 2: %f \n", x1);
	y2 = b1*y2old + a1*(x1 - x1old);	//y2 is the derivate term for the measured torque

	y = y3 - y2;	// (P+lag) - (derivative) term 

	y2old = y2;
	x1old = x1;

	// trying 60-Hz low-pass filter here on the output command
	// to knock-off high frequency content (>100Hz) contents from torque command
//	float count, d1 = 0.4524, d2 = 0.2738, d3 = 0.2738;	// coeff. for 60Hz low pass filter
//	static float countOld = 0;

	// count is DAC count command to motor
//	count = d1*countOld + d2*y + d3*yold;

//	countOld = count;

	
//	yold = y;

//	baseCommand[WA1] = (short)(count/(KDAC*KAMP*KTORQUE));
#endif


	// calculate the corresponding DAC count to produce the desired MOTOR torque
//	baseCommand[WA1] = (short)(y/(KDAC*KAMP*KTORQUE));
	baseCommand[EL1] = (short)(y/(KDAC*KAMP*KTORQUE));

	
//	if(baseCommand[EL1] > 0)
//					baseCommand[EL1] += 200;

	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
//	stg->RawDAC(6, baseCommand[WA1]);
	stg->RawDAC(1, baseCommand[EL1]);

#if 1
	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]);	
			counter = 0;
	}
	counter++;
#endif

}

#define slopeTime	1.0
void baseControl::torqueTrajectory(float torqueDes, float runTime)
{
	if(runTime < slopeTime)
			baseTorqueCommand[WA1] = runTime * (torqueDes/slopeTime);
	else
			baseTorqueCommand[WA1] = torqueDes;

//	if((runTime > 1.0) && (runTime < 3.00)
//		   torqueDesTraj = 4.0;
//	else if ((runTime => 3.00) && (runTime < 5.00))
//		torqueDesTraj = 0.0;

//	printf("time: %f, com: %f \n", runTime, baseTorqueCommand[WA1]);
}
#if 1

void main(void)
{
	baseControl *base = new baseControl();
	int choice;
	PrTimer timer(HZ);
	static float time = 0.0;
	float torqueCommand;

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

	base->resetIndex();

	printf("Base Control Test Program \n");

	char answer;
	printf("Reset base encoder (y/n)? \n");
	cin >> answer;

	if (answer == 'y')
			base->resetBaseEncoder();

//	printf("(1) Move base cw \n");
//	printf("(2) Move base ccw \n");
	printf("(1) Apply negative DAC count \n");
	printf("(2) Apply positive DAC count \n");
	printf("(3) Test LOHET sensor \n");
	printf("(4) Test Encoders \n");
	printf("(5) Test Position Loop \n");
	printf("(6) Closed-loop SEA test \n");

	cin >> choice;

	int temp;
	static int counter = 0;
	timer.start();
	if(choice == 6)
	{
			printf("Enter torque command: \n");
			cin >> torqueCommand;
			printf("Torque command: %5.3f \n", torqueCommand);
//			printf("Press any key to continue \n");
//			cin >> temp;

			while(counter < int(HZ))
			{
//					printf("%d \n", counter);
					base->readPosition();
					base->calcPosition();
					base->readLOHET();
//					base->calcTorque();
					base->saveInfo();
					counter++;
					timer.wait();
			}
			printf("Starting... \n");
	}

	timer.start();
	timer.zero();

	while(!tcischars(1))
	{

		switch(choice)
		{
			case 1:
					base->moveBaseShaft(1);
					base->readPosition();
					base->calcPosition();
					base->readLOHET();
//					base->calcTorque();
					base->displayInfo();
					break;
			case 2:
					base->moveBaseShaft(0);
					base->readPosition();
					base->calcPosition();
					base->readLOHET();
//					base->calcTorque();
					base->displayInfo();
					break;
			case 3:
					base->readLOHET();
					base->displayInfo();
					break;
			case 4:
					base->readPosition();
					base->calcPosition();
					break;
			case 5:
					time = timer.time();
					base->readPosition();
					base->calcPosition();
					base->readLOHET();
					base->baseMotorControl(time);
					break;

			case 6:
					time = timer.time();
					base->readPosition();
					base->calcPosition();
					base->readLOHET();
	//				base->calcTorque();
		//			base->torqueTrajectory(torqueCommand, time);
					base->SEAcontrol(torqueCommand);
					break;
		}

		base->saveInfo();
		timer.wait();
	}

	base->saveBuffer();
	

}


#endif
