/*
	control.h
	******

	Header file for low-level motor (base and mini) control module
	Used by control.cpp
	Modeled after bldc.cpp and baseControl.cpp which are now unified in control.cpp
*/

// ENCODER CHANNELS DEFINITION
// SAME FOR LEFT AND RIGHT ARM ON THEIR RESPECTIVE STG BOARD
#define WAIST_MINI	0		// brushless encoder
#define WAIST_JOINT	1		// CANNON encoder
#define SHOULDER_MINI	2	// brushless encoder
#define SHOULDER_JOINT	3	// CANNON encoder
#define ELBOW_MINI	4		// brushless encoder 
#define ELBOW_JOINT	5		// CANNON encoder

// JOINT INDEX DEFINITION
// ##1 means RIGHT ARM, ##2 means LEFT ARM
#define WA1			0
#define SH1			1
#define EL1			2

// BASE MOTOR DAC CHANNEL ASSIGNMENT
#define BASEWA1		6
#define BASESH1		7
#define BASEEL1		1	// extra DAC channel using MAX547

#define BASEDACWA1		6
#define BASEDACSH1		7
#define BASEDACEL1		1	// extra DAC channel using MAX547

// LEFT OR RIGHT ARM INDEX
#define RIGHTARM		0
#define LEFTARM			1

// BIT MASKS FOR HET SIGNAL
#define HET_MASK	0x07
#define HAB_MASK	0x04
#define HBC_MASK	0x02
#define HCA_MASK	0x01

// BIT DEFINITIONS FOR BRUSHLESS HET INITIALIZATION STEP
#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

// DIRECTION DEFINITION FOR BRUSHLESS COMMUTATION
#define CW		1
#define CCW		2

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

// GENERAL CONVERTION FACTORS
#define DEG_TO_RAD	57.3
#define ENCODER_RES	8192
#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

// STG CARD DIGITAL IO PORT DEFINITIONS
#define PORTA		0
#define PORTB		1
#define PORTC		2
#define PORTD		3

// BOOLEAN 
#define TRUE		1
#define FALSE		0

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

class dm2Control 
{
	private:
		ServoToGo *stgRight;
		ServoToGo *stgLeft;

		// u and v phase commands for brushless motor
		short uCommand[3];
		short vCommand[3];

		// sensed motor current (unused)
		short uCurrent;
		short vCurrent;

		// brushless commutation direction
		int direction;

		//	brushless HET information
		char HAB;
		char HBC;
		char HCA;
		unsigned char HETStatus;

		// brushless motor angles information
		float mAngle[8];	// mechanical angle
		float eAngle[8];	// electrical angle
		float mAngleInit[8];

		// base motor angle and torque information	
		STG_LONGBYTE baseEncoder[6];
		STG_LONGBYTE baseEncoderInit[6];
		float baseOutputAngle[6];
		float baseOutputAngleDes[6];
		float baseSpringTorque[6];
		float baseSpringTorqueEnc[6];
		float baseLoadTorque[6];
		float baseJointTorque[6];		// effective torque acting at the joint axis
		float baseJointTorqueError[6];
		float baseActuatorTorqueError[6];
		float miniTorqueDes[6];
		float loadTorqueCommand[6];
		float loadTorqueCommandDes[6];
		float baseTorqueCommand[6];
		short baseCommand[6];
		float miniCommand[6];
		float basePreloadTorque[6];
		int basePreloadTorqueCount[6];	// equivalent preload torque on spring in DAC count
		float initBaseOutputAngle[6];
		float initOutputShaftAngle[6];
		float dampingOutput;

		PrVector3 gravity;
		PrVector3 gravityTorqueCommand;
		PrVector3 gravity2;

		float chirpCommand;

		// SEA LOHET sensor
		float LOHET[6];
		short initLOHETValue[6];
		float initLOHET[6];

		// Digital IO port variable
		STG_IO32 portD;

		PrVector *torqueCommand;
		
		//trajectory stuff
		PrVector *a0;
		PrVector *a2;
		PrVector *a3;
		PrVector *tfinal;
		float ttraj;

		// Joint angles and velocities information
		STG_LONGBYTE temp[8];
		STG_LONGBYTE encoder[8];
		STG_LONGBYTE encoderInit[8];
		PrVector *qgoal;
		PrVector *qdes;
		PrVector *dqdes;
		PrVector *q;	// joint angle in degrees
		PrVector3 qt;	// joint angle in radian for sin and cos calculations
		PrVector *qOld;
		PrVector *dq;
		PrVector *dqOld;
		float jointLimitMax[6];
		float jointLimitMin[6];
		float t;

		// Jacobians
		PrMatrix *J;		// linear velocity jacobian of end-effector
		PrMatrix *Jv1;		// Jv1, Jv2 and Jv3 are linear velocity of
		PrMatrix *Jv2;		// each link's center of mass
		PrMatrix *Jv3;
		PrMatrix *Jw1;		// angular velocity jacobian of each link
		PrMatrix *Jw2;
		PrMatrix *Jw3;
		PrMatrix *I1;		// inertia matrix of each link
		PrMatrix *I2;
		PrMatrix *I3;
		
		PrVector *jointTorqueDes;
		PrVector *baseTorqueDes;
		PrMatrix *J1;	// this is actuator jacobian transpose, (Jact)'
		PrMatrix *J2;	// this is inverse of actuator jacobian transpose ((Jact)')^-1
		PrVector *tjspace;

		// End Effector position information
		PrVector *xLeft;
		PrVector *xRight;
		PrVector *dxLeft;
		PrVector *dxRight;

		int jMode;
		int brakeMode;

		float goal[6];

		// Data buffer for saved information
		FILE *fdata;
		FILE *fdata2;
		FILE *fdata3;
		FILE *fdata4;
		FILE *fdata5;
		FILE *fdata6;
		FILE *fdata7;
		long int bufferIndex;
		PrMatrix *dataBuffer1;
		PrMatrix *dataBuffer2;
		PrMatrix *dataBuffer3;
		PrMatrix *dataBuffer4;
		PrMatrix *dataBuffer5;
		PrMatrix *dataBuffer6;
		PrMatrix *dataBuffer7;

	public:
		short torque;

		dm2Control(ServoToGo *stGPtr);
		~dm2Control();

		void initSystem(void);

		// Joint angle, velocity and encoder index related functions
		void resetIndex(void);
		void initIndex(void);
		int hitIndex(void);
		int hitIndex2(int channel);
		void initPosition(void);
		void calcPosition(void);
		void readPosition(void);
		void setJointAngle(int jointIndex);
		void displayInfo(void);
		void displayInfo2(void);
		void displayAngle(void);
		void resetEncoder(void);
		void resetVelocity(void);
		void initJointLimit(void);
		void initVariables(void);

		// mini brushless motor related functions
		void readHET(void);
		void writeTorque(int jointIndex);	
		void sinusoidCom(int jointIndex, float command);
		void setDirection(int dir);
		void initWithIndex(void);
		void initWithIndex2(void);
		void initMiniEncoder(int jointIndex, int armSide);
		int shutdownMotors(void);
		void readMotorCurrent(void);

		// Control related functions
		void setGoal(float goalPosition);
		void getJointPosition(float position[]);
		PrVector3 gravityCompensation(void);
		PrVector3 gravityCompensationRight(void);
		PrVector3 gravityCompensationLeft(void);
		PrVector calcX(void);
		void calcJacobian(void);
		PrVector getQ(void) {return *q;};
		//trajectory stuff
		void calcSpline(PrVector qgoal);
		void calcTraj(float time);
		void calcTrajVel(float time);
		void jointControl(void);
		void setJointControlMode(int mode);
		void setSplineParameters(float a0_[], float a2_[], float a3_[], float tfinal_[], float qfinal[]);

		// Data file related functions
		void closeDataFile(void);
		void openDataFile(void);
		void saveInfo(void);
		void saveBuffer(void);
		void initDataBuffer(void);

		// Base control
		void readLOHET(void);
		void lowPassFilter(void);
		void notchFilter(void);
		void calcTorque(void);
		void resetBaseEncoder(void);
		void baseMotorControl(float runTime);
		void moveBaseShaft(int direction);
		int	modulateBaseTorque(float time, int dacCountDes);
		void applyBaseTorque(int dacCount);
		void applyBaseTorque(void);
		void macroControl(void);
		void miniControl(void);
		void torqueTrajectory(float torqueDes, float runTime);
		void calcPreloadSpringTorque(void);
		void setLoadTorqueCommand(float loadTorqueCommand[]);
		void setLoadTorqueCommand(int mode);
		void modulateLoadTorqueCommand(float time);
		void initTorqueMeasFromEncoder(void);
		void calcBaseTorqueDes(float time);
		void calcJointTorqueDes(int userMode);
		void initActuatorJacobian(void);
		void macroControlNew(void);

		// Dynamic-related functions
		void initInertiaMatrices(void);
		
		void chirpControl(void);
		void recordChirp(float yChirp);
};






