/*
	bldc.h
	******

	Header file for brushless motor control module

*/

#define ELBOW_MINI	4	// encoder for brushless motor
#define ELBOW_JOINT	5
#define SHOULDER_MINI	2
#define SHOULDER_JOINT	3
#define WAIST_MINI	0	// encoder for brushless motor
#define WAIST_JOINT	1

#define U_CHANNEL	4	// u-phase DAC output
#define V_CHANNEL	5	// v-phase DAC output

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

#define RIGHTARM		0
#define LEFTARM			1

#define HET_MASK	0x07
#define HAB_MASK	0x04
#define HBC_MASK	0x02
#define HCA_MASK	0x01

#define CW_STEP_ONE	0x04	// xxxx 0100
#define CW_STEP_TWO	0x06	// xxxx 0100
#define CW_STEP_THREE	0x02	// xxxx 0010
#define CW_STEP_FOUR	0x03	// xxxx 0011
#define CW_STEP_FIVE	0x01	// xxxx 0001
#define CW_STEP_SIX	0x05	// xxxx 0101

#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 CANNON_ENCODER_RES	4*81000
#define CANNON_COUNT_PER_DEG	900


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

#define	NONE		0
#define ONE		1
#define TWO		2
#define THREE		3
#define FOUR		4
#define FIVE		5
#define SIX		6
#define START		1

#define TRUE		1
#define FALSE		0

#define CW		1
#define CCW		2

#define POLES		12	// for RBE1506 test motor
				// RBE1511 has 12 poles (real motor)
//#define MECH_TO_ELEC	POLES/2
#define MECH_TO_ELEC	6	

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

class BLDC
{
	private:
		ServoToGo *stg;

		short uCommand[3];
		short vCommand[3];

		short uCurrent;
		short vCurrent;

		int direction;

		STG_IO32 portD;

		STG_LONGBYTE temp[8];
		STG_LONGBYTE encoder[8];
		STG_LONGBYTE encoderInit[8];

		float mAngle[8];	// mechanical angle
		float eAngle[8];	// electrical angle
		float mAngleInit[8];

		PrVector *torqueCommand;
		float jointLimitMax[6];
		float jointIndexAngle[6];
		float jointLimitMin[6];
		PrVector *xLeft;
		PrVector *xRight;
		PrVector *dxLeft;
		PrVector *dxRight;
		
		float goal[8];

		char HAB;
		char HBC;
		char HCA;

		unsigned char HETStatus;
		unsigned char commStatus;

		FILE *fdata;
		FILE *fdata2;
		long int bufferIndex;

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

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

		PrMatrix *dataBuffer1;
		PrMatrix *dataBuffer2;

	public:
		short torque;

		BLDC(ServoToGo *stGPtr);
		~BLDC();
		void initSystem(void);
		void resetIndex(void);
		void initIndex(void);
		int hitIndex(void);
		int hitIndex2(int channel);
		void initPosition(void);
		void calcPosition(void);
		void readPosition(void);
		void readHET(void);
		void writeTorque(int jointIndex);	
		void sinusoidCom(int jointIndex, float command);
		void calcTorque(void);
		unsigned char hitIndex(unsigned char channel);
		void closeDataFile(void);
		void openDataFile(void);
		void initWithIndex(void);
		void initWithIndex2(void);
		void initMiniEncoder(int jointIndex, int armSide);
//		void initWithIndex2(int motorChannel);
		void setJointAngleMax(int jointIndex);
		void setJointAngleAtIndex(int jointIndex);
		void setJointAngleMin(int jointIndex);
		void setDirection(int dir);
		void readMotorCurrent(void);
		void displayInfo(void);
		void displayAngle(void);
		void resetEncoder(void);
		void saveInfo(void);
		void saveBuffer(void);
		void setGoal(float goalPosition);
		void getJointPosition(float position[]);
		void resetVelocity(void);
		PrVector gravityCompensation(void);
		PrVector gravityCompensationRight(void);
		PrVector gravityCompensationLeft(void);
		PrVector calcX(void);

		int shutdownMotors(void);
		
		//trajectory stuff
		void calcSpline(PrVector qgoal);
		void calcTraj(float time);
		void jointControl(void);
};






