/*****************************************************************************
 *   foc.h:  Header file for NXP LPC29xx Family Microprocessors
 *
 *   Copyright(C) 2009, NXP Semiconductor
 *   All rights reserved.
 *
 *   History
 *   2009.12.17  ver 1.00    Preliminary version, first Release
 *
 ****************************************************************************
 * Software that is described herein is for illustrative purposes only
 * which provides customers with programming information regarding the
 * products. This software is supplied "AS IS" without any warranties.
 * NXP Semiconductors assumes no responsibility or liability for the
 * use of the software, conveys no license or title under any patent,
 * copyright, or mask work right to the product. NXP Semiconductors
 * reserves the right to make changes in the software without
 * notification. NXP Semiconductors also make no representation or
 * warranty that such application will be suitable for the specified
 * use without further testing or modification.
 *****************************************************************************/

#ifndef __FOC_H
#define __FOC_H

#define PI				3.14159		// PI
#define SQRT3			1.73205		// sqrt(3) = 1.73205
#define INV_SQRT3		0.57735  	// 1/sqrt(3) = 0.57735
#define SIGNEDSHORTMAX	32767

#define MODE_QEI_RAMP	0		// Ramp for QEI operation
#define MODE_SL_RAMP	1		// Ramp for sensorless operation
#define MODE_QEI		2		// QEI operation
#define MODE_HALL		3		// HALL operation
#define MODE_SL			4		// Sensorless operation

#define STARTUP_TORQUE	F6_10_CONST(25.0)
#define A_OMEGA	0.5
#define I_MAX 2.0									// Max power = 2.0 Amp
#define I_TOP I_MAX * 6.9							// 32.0/17.5)		
#define I_FLOOR I_TOP * 0.75 						// Scaled Amp rating from where controlling Vq is started
#define FILTERCOEFFICIENT_ISUM 0.3
#define FILTERCOEFFICIENT_VQMAX 0.01
#define K_SMC			0.85						// Slide Mode Controller gain 
#define SMC_MAX_ERR    	0.005						// Maximum error of SMC 
												
// MAXON 118889
#define PHASE_RESISTANCE	((float)0.915)			// Phase resistance in Ohms
#define PHASE_INDUCTANCE	((float)0.0001425) 		// Phase inductance in Henrys.
// MAXON 136293
//#define PHASE_RESISTANCE		((float)0.4985)			// Phase resistance in Ohms. 
//#define PHASE_INDUCTANCE		((float)0.0000735) 		// Phase inductance in Henrys.

#define PWMFREQUENCY	20000						// PWM frequency [Hz]
#define PWMLOOPTIME		(1.0/PWMFREQUENCY) 			// PWM Period = 1.0 / PWMFREQUENCY
#define SPEEDLOOPFREQ	1000						// Speed loop frequency [Hz]
#define SPEEDLOOPTIME 	(1.0/SPEEDLOOPFREQ) 		// Speed Control Period
#define PWMSPEEDRATE (unsigned int)(SPEEDLOOPTIME/PWMLOOPTIME)	// PWM loops per velocity calculation

#define RPM0 500
#define RPM1 1000
#define RPM2 1500
#define RPM3 2000
#define RPM4 2500
#define RPM5 3000
#define RPM6 3500
#define RPM7 4000
#define RPM8 4500
#define RPM9 5000
 
#define OMEGA0	(float) 2.0*PI*RPM0*SPEEDLOOPTIME/60.0 
#define OMEGA1	(float) 2.0*PI*RPM1*SPEEDLOOPTIME/60.0
#define OMEGA2	(float) 2.0*PI*RPM2*SPEEDLOOPTIME/60.0
#define OMEGA3	(float) 2.0*PI*RPM3*SPEEDLOOPTIME/60.0
#define OMEGA4	(float) 2.0*PI*RPM4*SPEEDLOOPTIME/60.0
#define OMEGA5	(float) 2.0*PI*RPM5*SPEEDLOOPTIME/60.0
#define OMEGA6	(float) 2.0*PI*RPM6*SPEEDLOOPTIME/60.0
#define OMEGA7	(float) 2.0*PI*RPM7*SPEEDLOOPTIME/60.0
#define OMEGA8	(float) 2.0*PI*RPM8*SPEEDLOOPTIME/60.0
#define OMEGA9	(float) 2.0*PI*RPM9*SPEEDLOOPTIME/60.0

#define OFFSET0	1.47	
#define OFFSET1	1.57	
#define OFFSET2	1.61	
#define OFFSET3	1.63
#define OFFSET4	1.64	
#define OFFSET5	1.65	
#define OFFSET6	1.66	
#define OFFSET7	1.67	
#define OFFSET8	1.67	
#define OFFSET9	1.67	

#define SLOPE0 (float)((OFFSET1 - OFFSET0) / (OMEGA1 -OMEGA0))
#define SLOPE1 (float)((OFFSET2 - OFFSET1) / (OMEGA2 -OMEGA1))
#define SLOPE2 (float)((OFFSET3 - OFFSET2) / (OMEGA3 -OMEGA2))
#define SLOPE3 (float)((OFFSET4 - OFFSET3) / (OMEGA4 -OMEGA3))
#define SLOPE4 (float)((OFFSET5 - OFFSET4) / (OMEGA5 -OMEGA4))
#define SLOPE5 (float)((OFFSET6 - OFFSET5) / (OMEGA6 -OMEGA5))
#define SLOPE6 (float)((OFFSET7 - OFFSET6) / (OMEGA7 -OMEGA6))
#define SLOPE7 (float)((OFFSET8 - OFFSET7) / (OMEGA8 -OMEGA7))
#define SLOPE8 (float)((OFFSET9 - OFFSET8) / (OMEGA9 -OMEGA8))

typedef volatile struct _CALCVAR_STRUCT {
	volatile signed long Q_int_max;	
	volatile signed long D_int_max;
}CALCVAR_STRUCT;

signed short F6_10_sin(signed short a);
signed short F6_10_cos(signed short a);

void FOC_Init(void);
void FOC_DefaultEEparam(PARAM_STRUCT *param);
void FOC_loop(void);
void SpeedPID(void);
signed short EstimateAngle(void);
void FOC_CalcVar(void);
signed short F6_10_sin(signed short a);
signed short F6_10_cos(signed short a);
signed long sqrt_F22_10(signed long value);
signed short Abs_F6_10(F6_10 value);
signed long Abs_F16_16(F16_16 value);
signed short ss_clamp(signed long sl, signed short clamp);
signed long sl_clamp(signed long sl, signed long clamp);

#endif
