/*
	baseControl.h
	******

	Header file for base motor control module

*/

#define WA1			0
#define SH1			1
#define EL1			2

//#define baseWA1		6
#define baseWA1		6
#define baseSH1		7

#define DEG_TO_RAD	57.3
#define ENCODER_RES	8192	// 8000 for test motor; 8192 for real motor
#define COUNT_TO_DEG	360.0/ENCODER_RES
#define COUNT_PER_DEG	ENCODER_RES	

#define PORTA		0
#define PORTB		1
#define PORTC		2
#define PORTD		3

#define	NONE		0
#define TRUE		1
#define FALSE		0

#define CW		1
#define CCW		2

#include "driver/ServoToGo.h"
class ServoToGo;

class baseControl
{
	private:
		ServoToGo *stg;
		ServoToGo *stg2;

		int direction;

		STG_IO32 portD;

		STG_LONGBYTE temp[8];
		STG_LONGBYTE baseEncoder[6];
		STG_LONGBYTE baseEncoderInit[6];

		int bufferIndex;
		float baseOutputAngle[6];
		float baseOutputAngleOld[6];
		float baseOutputAngleDes[6];
		float baseOutputVel[6];
		float angleOld[6];
		float velocity[6];
		float velocityOld[6];
		float baseSpringTorque[6];
		float baseTorqueCommand[6];

		short baseCommand[6];
		short initLOHETValue[6];
		float initLOHET[6];

		float goal[6];

		FILE *fdata;
		FILE *fdata2;

		//trajectory stuff
		PrVector *a0;
		PrVector *a2;
		PrVector *a3;
		PrVector *tfinal;

		PrVector *qgoal;
		PrVector *qdes;
		PrVector *dqdes;
		PrVector *q;
		PrVector *dq;
		float t;

		PrMatrix *dataBuffer3;
		PrMatrix *dataBuffer4;

		float LOHET[6];

	public:
		short torque;

		baseControl();
		~baseControl();
		void initSystem(void);
		void resetIndex(void);
		void initIndex(void);
		int hitIndex2(int channel);
		void initPosition(void);
		void calcPosition(void);
		void readPosition(void);
		void readLOHET(void);
		void notchFilter(void);
		void displayInfo(void);
		void writeTorque(int jointIndex);	
		void calcTorque(void);
		void closeDataFile(void);
		void openDataFile(void);
		void initWithIndex(void);
		void initWithIndex2(void);
		void resetBaseEncoder(void);
		void saveInfo(void);
		void saveBuffer(void);
	
		void lowPassFilter(void);

		int shutdownMotors(void);
		
		//trajectory stuff
		void baseMotorControl(float runTime);
		void moveBaseShaft(int direction);
		void SEAcontrol(float torqueDes);
		void torqueTrajectory(float torqueDes, float runTime);

};






