/******************************************************************************
*
* Freescale Semiconductor Inc.
* (c) Copyright 2004-2009 Freescale Semiconductor, Inc.
* (c) Copyright 2001-2004 Motorola, Inc.
* ALL RIGHTS RESERVED.
*
***************************************************************************//*!
*
* @file      main.c
*
* @author    rg002c
*
* @version   1.0.1.0
*
* @date      Oct-26-2011
*
* @brief     Sample application for TWR-56F8257 and TWR-MC-LV3PH boards
* 		     Close loop BLDC motor control using Hall Sensors
*
*			 Motor LINIX 45ZWN24-40 Connection for TWR-MC-LV3PH J5 connector:
*								- phase A J5.1 - green		 			 
*								- phase B J5.2 - blue
*								- phase C J5.3 - white
*
*			 Hall Sensors Connection for TWR-MC-LV3PH J8 connector:
*								- +5V J8.1  - red		 			 
*								- GND J8.2  - black
*								- HS_A J8.3 - white
*								- HS_B J8.4 - blue
*								- HS_C J8.5 - green
*
******************************************************************************/
/* required DSP56F800E_Quick_Start header */
#include "qs.h"

/* low-level driver headers for each module used */
#include "occs.h"
#include "sys.h"
#include "adc.h"
#include "cop.h"
#include "crc.h"
#include "dac.h"
#include "gpio.h"
#include "hscmp.h"
#include "iic.h"
#include "intc.h"
#include "pwm.h"
#include "qtimer.h"
#include "vref.h"
#include "sci.h"
#include "spi.h"
#include "xbar.h"
#include "freemaster.h"

/* MC33927/37 driver configuration */
#include "SPI_TWR56F8257.h"
#include "MC33927.h"

/* application constants */
#include "main.h"

/* general function library */
#include "gflib.h"

/******************************************************************************
* Type definition
******************************************************************************/

/* pointer to function */
typedef void (*tPointerFcn)(void); 

/* user interface - BUTTONS, SWITCHES */
typedef union uInterfaceStatus
  {
  UWord8 W8;
  struct
    {
    unsigned int Up  			   :1;
    unsigned int Down       	   :1;
    unsigned int Both              :1;
    unsigned int bit3              :1;
    unsigned int bit4		       :1;
    unsigned int bit5              :1;
    unsigned int bit6              :1;
    unsigned int bit7              :1;
    }bit;
  }tInterfaceStatus;
  
  /* application faults */
typedef union uAppFaults
  {
  UWord8 W8;
  struct
    {
    unsigned int OverCurrent	   :1;
    unsigned int OverVolt     	   :1;
    unsigned int UnderVolt         :1;
    unsigned int bit3              :1;
    unsigned int bit4		       :1;
    unsigned int bit5              :1;
    unsigned int bit6              :1;
    unsigned int bit7              :1;
    }bit;
  }tApplicationFaults;
  
/******************************************************************************
* Local function prototypes
******************************************************************************/
static void PeripheralCoreInit(void);
static void CheckManualInterface (tInterfaceStatus *Status);

/* state machine main and transient functions */
static void AppInit(void);
static void AppRun(void);
static void AppStop(void);
static void AppError(void);
static void AppInitToStop(void);
static void AppStopToRun(void);
static void AppRunToStop(void);
static void AppErrorToStop(void);
static tPointerFcn AppStateMachine[] = {                                     
                                        AppInit,
                                        AppStop,
                                        AppRun,
                                        AppError,
                                       };


/******************************************************************************
* Local Constants
******************************************************************************/
/* motor pole-pair number */
#define POLE_PAIRS						  2
/* Hz */
#define IPBCLK							  60000000 
#define QT_PRESCLR						  128
/* mechanical RPM */
#define N_MAX							  5000	   
/* minimal speed */
#define N_MIN							  FRAC16(250.0/N_MAX)
/* push button pressing speed increment */	
#define N_INC							  N_MIN*2  
/* constant to calculate speed using HS period1 */
#define PERIOD_TO_SPEED					  (IPBCLK/N_MAX/QT_PRESCLR*60/(2*POLE_PAIRS)) 
/* speed ramp increment */
#define RAMP_INCR						  200

/* set to 20 Volts */
#define UNDERVOLT_LIMIT					  FRAC16(20.0/36.3)		
/* set to 28 Volts */
#define OVERVOLT_LIMIT					  FRAC16(28.0/36.3)		


/* push buttons pins */
#define BUTTON_UP						  0x40
#define BUTTON_DOWN						  0x80	

/* pressing debounce number x QT0 ISR (10ms) */
#define DEBOUNCE_VALUE                    10

/* basic application states */
#define APP_INIT                          0
#define APP_STOP                          1
#define APP_RUN                           2
#define APP_ERROR                         3

/* PWM modulo constant */
#define PWM_MODULO                        (PWM_0_VAL1_INIT-PWM_0_INIT_INIT)
#define MIN_DUTY_CYCLE                    FRAC16(1.0/10.0)


/* PWM Mask */
/* none of phase is masked */
#define NO_MASK   0x0000
/* PhA is masked (Both transistors of PhA are switched off) */
#define MASK_PHA  0x0110
/* PhB is masked (Both transistors of PhB are switched off) */
#define MASK_PHB  0x0220
/* PhC is masked (Both transistors of PhC are switched off) */
#define MASK_PHC  0x0440
/* all three phases are masked (All six transistors of power module are switched off) */
#define MASK_ALL  0x0770

/* PWM Mask table */
const unsigned int commutationTableMask[8] = {
  											  NO_MASK,\
  											  MASK_PHB,\
  											  MASK_PHA,\
  											  MASK_PHC,\
  											  MASK_PHC,\
  											  MASK_PHA,\
  											  MASK_PHB,\
  											  MASK_ALL
											 };

/* PWM Software Control */
/* none of phase is set to SW control */
#define NO_SWC   0x0000
/* PhA set to SW control (Bottom transistor of PhA is switched ON) */
#define SWC_PHA  0x0008
/* PhB set to SW control (Bottom transistor of PhB is switched ON) */
#define SWC_PHB  0x0080
/* PhC set to SW control (Bottom transistor of PhC is switched ON) */
#define SWC_PHC  0x0800

/* PWM Software Control table */
const unsigned int commutationTableSWC[8] = {
  											  NO_SWC,\
  											  SWC_PHC,\
  											  SWC_PHB,\
  											  SWC_PHB,\
  											  SWC_PHA,\
  											  SWC_PHC,\
  											  SWC_PHA,\
  											  NO_SWC
											};

/******************************************************************************
* Local variables
******************************************************************************/
/* MC33937 driver variables */
static 	MC33927_LATCH_T      mudtStatusRegister0;
static 	MC33927_MODE_T       mudtStatusRegister1;
static 	MC33927_LATCH_T      mudtStatusRegister2;
static 	byte			     mudtStatusRegister3;
static 	MC33927_LATCH_T      mudtClearFlags;
static  MC33927_MODE_COMMAND_T	mudtModeCommands;
/* actual Application state */
static 	UWord16              uw16AppState;
static  bool				 bMainSwitch;
/* duty cycle */
static 	Frac16               f16DutyCycle;
/* motor rotation direction */
static 	UWord16              f16DirectionOfRotation;
/* sensed quantities */
static 	UWord16              f16DcbVoltage;
/* Hall Sensors state */
static UWord16 uw16HallStateRaw;
static UWord16 uw16HallState;
/* user's interface (push buttons) */
static 	tInterfaceStatus     udtInterfaceStatus;
/* arch IO structure */
volatile arch_sIO           *pIO;

/* application faults */
static tApplicationFaults	udtAppFaults;
static UWord16				uw16FaultClear;

/* speed variables */
static 	UWord16				uw16HSedgeK_1;
static 	UWord16				uw16HSedge;
static 	UWord16				uw16HSperiod;
static	Frac16				f16SpeedActual;
static  Frac16				f16SpeedCommand;
static  Frac16				f16SpeedRequired;
static  Frac16				f16SpeedRamp;
static  Frac16				f16SpeedError;
static  GFLIB_RAMP16_T 	    gfRampSpeed = {RAMP_INCR,RAMP_INCR};
static  GFLIB_CONTROLLER_PI_P_PARAMS_T  gfPICtrlParamsSpeed;
static  Int16 				i16PiSatFlagSpeed;

/******************************************************************************
* Local functions
******************************************************************************/
/*****************************************************************************
*
* Function: 		void PeripheralCoreInit(void)
*
* Description: 		Call initalization functions of peripherals set in GCT
*
* Returns: 			None
*
* Global Data: 		None
*
* Arguments: 		None
*
* Range Issues: 	None
*
* Special Issues: 	None
*
*****************************************************************************/
static void PeripheralCoreInit(void)
{

    pIO = &ArchIO;
    /* initialize SYS module */
    ioctl(SYS, SYS_INIT, NULL);

    /* configure COP module */
    ioctl(COP, COP_INIT, NULL);
    
    /* configure all GPIO modules */
    ioctl(GPIO, GPIO_INIT_ALL, NULL);

    /* configure QTimer A0 */
    ioctl(QTIMER_A0, QT_INIT, NULL);

    /* configure QTimer A1 */
    ioctl(QTIMER_A1, QT_INIT, NULL);
    
    /* configure QTimer A2 */
    ioctl(QTIMER_A2, QT_INIT, NULL);
    
    /* configure QTimer A3 */
    ioctl(QTIMER_A3, QT_INIT, NULL);
    
    /* Clear Intput Capture Interrupt flags */
    ioctl(QTIMER_A0, QT_CLEAR_FLAG, QT_INPUT_EDGE_FLAG);
    ioctl(QTIMER_A1, QT_CLEAR_FLAG, QT_INPUT_EDGE_FLAG);
    ioctl(QTIMER_A2, QT_CLEAR_FLAG, QT_INPUT_EDGE_FLAG);

    /* configure ADC module */
    ioctl(ADC, ADC_INIT, NULL);

    /* configure XBAR module */
    ioctl(XBAR, XBAR_INIT, NULL);

    /* configure PWM module */
    ioctl(EFPWM, EFPWM_INIT, NULL);
        
    /* configure SPI module */
    ioctl(SPI, SPI_INIT, NULL);
	
	/* configure SCI 1 module */
    ioctl(SCI_1, SCI_INIT, NULL);
      
    /* initialize interrupt controller and enable interrupts */
    ioctl(INTC, INTC_INIT, NULL);
}


/*****************************************************************************
*
* Function: 		void inline void commutationHS(unsigned int direction)
*
* Description:
*   	The function decodes Hall Sensors signals and provide commutation
*     
* Returns: 			None
*   
* Global Data: 		hallSensor   - decoded stated of hall sensors
*
* Arguments:
*   				unsigned int direction  - required direction of rotation
*
* Range Issues: 	None
* Special Issues: 	None
*****************************************************************************/
static inline void commutationHS(unsigned int uiDirection)
{
	/* read all GPIO_C pins  */
	uw16HallStateRaw = 	ioctl(GPIO_C,GPIO_READ_RAW_DATA,NULL);
  	
  	/* scale to range <0-7> */
  	uw16HallState = 	((uw16HallStateRaw&0x0040)>>4)  | // HS_C
  					  	((uw16HallStateRaw&0x0010)>>3)  | // HS_B
  					  	((uw16HallStateRaw&0x0008)>>3);   // HS_A
  		
  	/* for Counterclockwise direction negate HS state */
  	if(uiDirection)
  		uw16HallState = (~uw16HallState)&0x7;					  
  
  	/* mask required PWM pairs according to Hall sensors state */
  	ioctl(EFPWM, EFPWM_WRITE_MASK_REG, commutationTableMask[uw16HallState]);
  	/* switch to Sofware control one of bottom transitor according to Hall sensors state */
  	ioctl(EFPWM, EFPWM_WRITE_DEADTIME_SOURCE_REG, commutationTableSWC[uw16HallState]);
  	/* apply FORCE event to change current PWM channel configuration */
  	ioctl(EFPWM_SUB0, EFPWMS_ACTIVE_FORCE_INIT, NULL);
}

/*****************************************************************************
*
* Function: 		void IsrHallSensorA(void)
*
* Description:      Interrupt routine called every Hall Sensor A edge
* Interrupt source: QTimer_A1
* Priority Level:   0
*
* Returns: None
*
* Global Data: 		f16DirectionOfRotation      - direction of rotation
*              
* Arguments: 		None
*
* Range Issues: 	None
*
* Special Issues: 	None
*
*****************************************************************************/
#pragma interrupt on
void IsrHallSensorA(void)
{
  	/* call commutation routine */
  	commutationHS(f16DirectionOfRotation);
  	
  	/* period calculation between two HS_A edges */
  	uw16HSedge 		= ioctl(QTIMER_A1, QT_READ_CAPTURE_REG, NULL);
  	uw16HSperiod 	= (UWord16)sub((Word16)uw16HSedge,(Word16)uw16HSedgeK_1);
  	uw16HSedgeK_1 	= uw16HSedge;
  	/* define maximum period */
  	if(uw16HSperiod>32767)
  		uw16HSperiod = 32767;
  	
  	/* clear input capture flag */
  	ioctl(QTIMER_A0, QT_CLEAR_FLAG, QT_INPUT_EDGE_FLAG);
}
#pragma interrupt off

/*****************************************************************************
*
* Function: 		void IsrHallSensorB(void)
*
* Description:      Interrupt routine called every Hall Sensor B edge
* Interrupt source: QTimer_A1
* Priority Level:   1
*
* Returns: 			None
*
* Global Data: 		f16DirectionOfRotation      - direction of rotation
*              
* Arguments: 		None
*
* Range Issues: 	None
*
* Special Issues: 	None
*
*****************************************************************************/
#pragma interrupt on
void IsrHallSensorB(void)
{
  	/* call commutation routine */
  	commutationHS(f16DirectionOfRotation);
  	/* clear input capture flag */
  	ioctl(QTIMER_A1, QT_CLEAR_FLAG, QT_INPUT_EDGE_FLAG);
}
#pragma interrupt off

/*****************************************************************************
*
* Function: 		void IsrHallSensorC(void)
*
* Description:      Interrupt routine called every Hall Sensor C edge
* Interrupt source: QTimer_A2
* Priority Level:   2
*
* Returns: 			None
*
* Global Data: 		f16DirectionOfRotation      - direction of rotation
*              
* Arguments: 		None
*
* Range Issues: 	None
*
* Special Issues: 	None
*
*****************************************************************************/
#pragma interrupt on
void IsrHallSensorC(void)
{
  	/* call commutation routine */
  	commutationHS(f16DirectionOfRotation);
  	/* clear input capture flag */
  	ioctl(QTIMER_A2, QT_CLEAR_FLAG, QT_INPUT_EDGE_FLAG);
}
#pragma interrupt off

/*****************************************************************************
*
* Function: 		void IsrQTA3Compare(void)
*
* Description:      Interrupt routine called every 10 ms
* Interrupt source: QTimer_A3
* Priority Level:   3
*
* Returns: 			None
*
* Global Data: 		udtInterfaceStatus          - Status of user interface
*              
* Arguments: 		None
*
* Range Issues: 	None
*
* Special Issues: 	None
*
*****************************************************************************/
#pragma interrupt saveall
void IsrQTA3Compare(void)
{
  	/* call push buttons interface */
  	CheckManualInterface(&udtInterfaceStatus);
  
  	/* actual speed calucation for both directions */
  	if(f16DirectionOfRotation)
  		f16SpeedActual = div_s(PERIOD_TO_SPEED,(Word16)uw16HSperiod);
  	else
  		f16SpeedActual = negate(div_s(PERIOD_TO_SPEED,(Word16)uw16HSperiod));
  	
  	/* for 0 period set 0 actual speed */
  	if(!uw16HSperiod)
  		f16SpeedActual = FRAC16(0.0);
  	
  	/* speed ramp */
  	f16SpeedRamp = GFLIB_Ramp16(f16SpeedRequired, f16SpeedRamp, &gfRampSpeed);
  	
  	/* speed controller */
	f16SpeedError	= sub(abs_s(f16SpeedRamp),abs_s(f16SpeedActual));
	f16DutyCycle	= GFLIB_ControllerPIp(f16SpeedError, &gfPICtrlParamsSpeed, &i16PiSatFlagSpeed);		
  	
  	/* clear compare flag */
  	ioctl(QTIMER_A3, QT_CLEAR_FLAG, QT_COMPARE_FLAG);
  	
  	/* Read SR0 register */
    MC33927_GetSR0(&mudtStatusRegister0);
    MC33927_GetSR1(&mudtStatusRegister1);
    MC33927_GetSR2(&mudtStatusRegister2);
    mudtStatusRegister3	=	MC33927_GetSR3();
    
    /* DC-Bus voltage sensing */
  	f16DcbVoltage = ioctl(ADC, ADC_READ_SAMPLE, 0);
    
}
#pragma interrupt off

/*****************************************************************************
*
* Function: 		void CheckManualInterface (tInterfaceStatus *Status)
*
* Description:
*   The function checks status of user interface Up/Down Buttons.
*	Any event on user interface is stored in *Status variable. 
*   The function considers following I/O ports for
*   user interface:
*   Down Button:    port F - bit 4
*   Up button:      port F - bit 5
*     
* Returns: 			None
*   
* Global Data:		None
*
* Arguments:
*   				tInterfaceStatus *Status - Status of user interface
*
* Range Issues:
*   DEBOUNCE_VALUE - define debounce period
*                    debounce period = DEBOUNCE_VALUE * execution period of
*                    this function
* Special Issues:
*   The function should be regularly called to solidly detect even on user
*   interface
*****************************************************************************/
#pragma interrupt called
static void CheckManualInterface (tInterfaceStatus *Status)
{
	static UWord8  uw8UpCounter, uw8DownCounter;
  	static UWord16 uw16RawPortData;
  
  	/* read all F port status */
  	uw16RawPortData = ioctl(GPIO_F,GPIO_READ_RAW_DATA,NULL);;
 
 	
 	/* both buttons pressing detection */
 	if ((!(uw16RawPortData & BUTTON_DOWN))&&(!(uw16RawPortData & BUTTON_UP)))
 		Status->bit.Both = 1;
 	
 	
 	/* check Down button pressing */
 	if (!(uw16RawPortData & BUTTON_DOWN) && (!Status->bit.Down))
  	{
    	if (uw8DownCounter < DEBOUNCE_VALUE)
		    uw8DownCounter++;
    
  	}
  	else
  	{
     	if (uw8DownCounter < DEBOUNCE_VALUE)
    		uw8DownCounter = 0;
    
  	}
  
  	/* after debounce counter set Down bit */
  	if ((uw8DownCounter == DEBOUNCE_VALUE) && !(uw16RawPortData & BUTTON_DOWN))
  	{
    	Status->bit.Down = 1;
    	uw8DownCounter = 0;
  	}
 
  
  	/* check Up button pressing */
  	if (!(uw16RawPortData & BUTTON_UP) && (!Status->bit.Up))
  	{
    	if (uw8UpCounter < DEBOUNCE_VALUE)
      		uw8UpCounter++;
  	}
  	else
  	{
    	if (uw8UpCounter < DEBOUNCE_VALUE)
    		uw8UpCounter = 0;
  	}
  
    /* after debounce counter set Up bit */
  	if ((uw8UpCounter == DEBOUNCE_VALUE) && !(uw16RawPortData & BUTTON_UP))
  	{
    	Status->bit.Up = 1;
    	uw8UpCounter = 0;
  	}
}

/*****************************************************************************
*
* Function: 		static void FaultDetection(void)
*
* Description: 		Application fault detection
*
* Returns: 			None
*
* Global Data: 		
*
* Arguments: 		None
*
* Range Issues: 	None
*
* Special Issues: 	None
*
*****************************************************************************/
static void FaultDetection(void)
{
	static UWord16 uw16FaultCounter;
	
	
	/* dc-bus over current fault detection */
    if(ioctl(EFPWM,EFPWM_READ_FAULT_STATUS_REG,NULL) & EFPWM_FAULT_0)
    {
       	udtAppFaults.bit.OverCurrent  = 1;
       	
    }
    
    /* dc-bus undervoltage fault detection */
    if(f16DcbVoltage < UNDERVOLT_LIMIT)
    {
        if (uw16FaultCounter++>100)
        {
        	udtAppFaults.bit.UnderVolt  = 1;
        	uw16FaultCounter = 0;	
        }
    }

	/* dc-bus overvoltage fault detection */
    if(f16DcbVoltage > OVERVOLT_LIMIT)
    {
        
        if (uw16FaultCounter++>10)
        {
			udtAppFaults.bit.OverVolt  = 1;
        	uw16FaultCounter = 0;
        }
    }
	
    
    /* no fault detected, fault status bit is clear */
    if (udtAppFaults.W8)
    {
    	/* turn on fault LED */
    	ioctl(GPIO_B,GPIO_SET_PIN,BIT_4);
    	/* go to ERROR state */
    	AppError();
    	uw16AppState = APP_ERROR;	
    }
}

/*****************************************************************************
*
* Function: 		static void AppInit(void)
*
* Description: 		State function of application state machine 
*              		State can be derived from the function name
*
* Returns: 			None
*
* Global Data: 		
*
* Arguments: 		None
*
* Range Issues: 	None
*
* Special Issues: 	None
*
*****************************************************************************/
static void AppInit(void)
{
    /* Set to clear all error flags */
    mudtClearFlags.W8 = 0xFF;
    /* Clear all flags in SR0 register*/
    MC33927_ClearFlags(&mudtClearFlags);
    /***** Mode setup *****/
    /* 0: when desaturation or phase error on any MOSFET turns off all MOSFETs; 1: desat faults ignored */
    mudtModeCommands.B.DesatFaultMode = 1; 	
    MC33927_ModeCommand(&mudtModeCommands);
    
    /* Set zero deadtime */
    MC33927_ZeroDeadtime();
    /* Read SR0 register */
    MC33927_GetSR0(&mudtStatusRegister0);
    /* set DRV_EN for MC33937*/
    periphBitSet(0x0001,&ArchIO.PortF.DataReg);
    
    /* speed controler  */
	gfPICtrlParamsSpeed.f16PropGain 		= FRAC16(0.5);
	gfPICtrlParamsSpeed.f16IntegGain 		= FRAC16(0.5);
	gfPICtrlParamsSpeed.i16PropGainShift 	= 0;
	gfPICtrlParamsSpeed.i16IntegGainShift 	= -3;
	gfPICtrlParamsSpeed.f32IntegPartK_1 	= FRAC32(0.0);
	gfPICtrlParamsSpeed.f16UpperLimit 		= FRAC16(0.99);
	gfPICtrlParamsSpeed.f16LowerLimit 		= FRAC16(0.05);	
	i16PiSatFlagSpeed						= 0;
    
    /* DC-Bus voltage sensing */
  	f16DcbVoltage = ioctl(ADC, ADC_READ_SAMPLE, 0);
    
    /* go to next transition state */
    AppInitToStop();
}

/*****************************************************************************
*
* Function: 		void AppInitToStop(void)
*
* Description: 		Transition function of application state machine 
*              		Transition can be derived from the function name
*
* Returns: 			None
*
* Global Data: 		uw16AppState          - State of the application state machine
*
* Arguments: 		None
*
* Range Issues: 	None
*
* Special Issues: 	None
*
*****************************************************************************/
static void AppInitToStop(void)
{
  	/* go to app state */
  	uw16AppState = APP_STOP;
}

/*****************************************************************************
*
* Function: 		static void AppStop(void)
*
* Description: 		State function of application state machine 
*              		State can be derived from the function name
*
* Returns: 			None
*
* Global Data: 		udtInterfaceStatus          - Status of user interface
*
* Arguments: 		None
*
* Range Issues: 	None
*
* Special Issues: 	None
*
*****************************************************************************/
static void AppStop(void)
{
    
  	/* clear variables */
  	uw16HSperiod				= 0;
  	f16DutyCycle 				= 0;
	gfPICtrlParamsSpeed.f32IntegPartK_1 	= FRAC32(0.0);
    
    
    /* if a push button pressed set non-zero speed command */
    if (udtInterfaceStatus.bit.Up)
   		f16SpeedCommand += N_INC;
  
	if (udtInterfaceStatus.bit.Down)
    	f16SpeedCommand -= N_INC;
    
    
    /* for non-zero speed command go to run state */
  	if(f16SpeedCommand)
  	{
    	AppStopToRun();
    	/* set main application switch */
    	bMainSwitch = true;
  	}
}


/*****************************************************************************
*
* Function: 		static void AppStopToRun(void)
*
* Description: 		State function of application state machine 
*              		State can be derived from the function name
*
* Returns: 			None
*
* Global Data: 		udtInterfaceStatus          - Status of user interface
*              		f16DirectionOfRotation      - direction of rotation
*              		f16DutyCycle                - actual duty cycle
*              		uw16AppState                 - State of the application state machine
*
* Arguments: 		None
*
* Range Issues: 	None
*
* Special Issues: 	None
*
*****************************************************************************/
static void AppStopToRun(void)
{
	
	/* set motor direction of rotation */
	/* clockwise */
	if((f16SpeedCommand>=0)&&(bMainSwitch))
		f16DirectionOfRotation = 1;
	/* counterclockwise */
	if((f16SpeedCommand<0)&&(bMainSwitch))
		f16DirectionOfRotation = 0;
	
  	/* clear status bits */
  	udtInterfaceStatus.bit.Up = 0;
  	udtInterfaceStatus.bit.Down = 0;

	/* set initial pwm outputs */
	commutationHS(f16DirectionOfRotation);
	
   	/* update PWM channels */
  	ioctl(EFPWM_SUB0,EFPWMS_WRITE_VALUE_REG_3,(Word16) mult_r(PWM_MODULO, f16DutyCycle));
  	ioctl(EFPWM_SUB1,EFPWMS_WRITE_VALUE_REG_3,(Word16) mult_r(PWM_MODULO, f16DutyCycle));
  	ioctl(EFPWM_SUB2,EFPWMS_WRITE_VALUE_REG_3,(Word16) mult_r(PWM_MODULO, f16DutyCycle));
  	ioctl(EFPWM,EFPWM_SET_LOAD_OK,EFPWM_SUBMODUL_2 | EFPWM_SUBMODUL_1 | EFPWM_SUBMODUL_0);
	
  	/* Enable PWM outputs */
  	ioctl(EFPWM, EFPWM_WRITE_OUTPUT_ENABLE_REG, 0x0770);
  	
  	/* go to run state */
  	uw16AppState = APP_RUN;
}


/*****************************************************************************
*
* Function: 		static void AppRun(void)
*
* Description: 		State function of application state machine 
*              		State can be derived from the function name
*
* Returns: 			None
*
* Global Data: 		udtInterfaceStatus - Status of user interface
*              		f16DutyCycle       - actual duty cycle
*              		uw16AppState       - State of the application state machine
*
* Arguments: 		None
*
* Range Issues: 	None
*
* Special Issues: 	None
*
*****************************************************************************/
static void AppRun(void)
{
  	/* FreeMASTER control page */
  	if((!bMainSwitch)||(abs_s(f16SpeedCommand)<N_MIN))
  	{
  		AppRunToStop();
	    f16SpeedCommand			= FRAC16(0.0);
  	}
  	
  	/* for zero Duty cycle or both pushed buttons pressed at a time go to Stop state */
  	if(udtInterfaceStatus.bit.Both)
  	{
	    AppRunToStop();
	    f16SpeedCommand			= FRAC16(0.0);
	    bMainSwitch				= false;
  	}
  	
  	/* increase speed command based on up/down button pressing */	
  	if (udtInterfaceStatus.bit.Up)
   		f16SpeedCommand += N_INC;
  
	if (udtInterfaceStatus.bit.Down)
    	f16SpeedCommand -= N_INC;
	
  	/* clockwise direction */
  	if(f16DirectionOfRotation)
  	{
  		/* speed limitation to 4500 rpm */
  		if(f16SpeedCommand>FRAC16(4500.0/N_MAX))
  			f16SpeedCommand = FRAC16(4500.0/N_MAX);
  		
  		/* set Speed commnad to required speed  */
  		f16SpeedRequired = f16SpeedCommand;
  		
  		/* revers to negative speed */
  		if(f16SpeedCommand<0)
  		{
			f16SpeedRequired = (N_MIN/2);  			
			if(f16SpeedRamp < N_MIN)
				AppRunToStop();		
  		}
  	}
  	else
  	{   /* counterclockwise direction */ 
	  	/* speed limitation to -4500 rpm */
	  	if(f16SpeedCommand<FRAC16(-4500.0/N_MAX))
  			f16SpeedCommand = FRAC16(-4500.0/N_MAX);
  		
  		/* set Speed commnad to required speed  */
  		f16SpeedRequired = f16SpeedCommand;
  		
  		/* revers to positive speed */ 
  		if(f16SpeedCommand>0)
  		{
			f16SpeedRequired = (-N_MIN/2);  			
			if(f16SpeedRamp > -N_MIN)
				AppRunToStop();		
  		}
  	}
  	
  	/* clear interface status bits */
  	udtInterfaceStatus.bit.Up = 0;
  	udtInterfaceStatus.bit.Down = 0;

	/* update PWM channels */
	ioctl(EFPWM_SUB0,EFPWMS_WRITE_VALUE_REG_3,(Word16) mult_r(PWM_MODULO, f16DutyCycle));
	ioctl(EFPWM_SUB1,EFPWMS_WRITE_VALUE_REG_3,(Word16) mult_r(PWM_MODULO, f16DutyCycle));
	ioctl(EFPWM_SUB2,EFPWMS_WRITE_VALUE_REG_3,(Word16) mult_r(PWM_MODULO, f16DutyCycle));
	ioctl(EFPWM,EFPWM_SET_LOAD_OK,EFPWM_SUBMODUL_2 | EFPWM_SUBMODUL_1 | EFPWM_SUBMODUL_0);
}


/*****************************************************************************
*
* Function: 		void AppRunToStop(void)
*
* Description: 		Transition function of application state machine 
*              		Transition can be derived from the function name
*
* Returns: 			None
*
* Global Data: 		f16DutyCycle        - actual duty cycle
*              		dutyCycelDesired    - Desired duty cycle
*              		uw16AppState        - State of the application state machine
*
* Arguments: 		None
*
* Range Issues: 	None
*
* Special Issues: 	Not implemented in this version
*
*****************************************************************************/
static void AppRunToStop(void)
{
	/* clear variables */
	f16DutyCycle 				= FRAC16(0.0);
  	udtInterfaceStatus.bit.Both = 0;
  	f16SpeedActual 				= FRAC16(0.0);
  	f16DutyCycle 				= FRAC16(0.0);
  	uw16HSperiod				= 0;
  	f16SpeedRequired			= FRAC16(0.0);
  	f16SpeedRamp				= FRAC16(0.0);
  	
  	/* Disable PWM outputs */
  	ioctl(EFPWM, EFPWM_WRITE_OUTPUT_ENABLE_REG, 0x0000);
  	
  	/* go to STOP state */
  	uw16AppState = APP_STOP;
}



/*****************************************************************************
*
* Function: 		static void AppError(void)
*
* Description: 		State function of application state machine 
*              		State can be derived from the function name
*
* Returns: 			None
*
* Global Data: 		None
*
* Arguments: 		None
*
* Range Issues: 	None
*
* Special Issues: 	None
*
*****************************************************************************/
static void AppError(void)
{
	/* Disable PWM outputs */
  	ioctl(EFPWM, EFPWM_WRITE_OUTPUT_ENABLE_REG, 0x0000);
  	
  	/* go to STOP state */
  	AppErrorToStop();
}

/*****************************************************************************
*
* Function: 		static void AppErrorToStop(void)
*
* Description: 		State function of application state machine 
*              		State can be derived from the function name
*
* Returns: 			None
*
* Global Data: 		uw16AppState     - State of the application state machine 
*
* Arguments: 		None
*
* Range Issues: 	None
*
* Special Issues: 	None
*
*****************************************************************************/
static void AppErrorToStop(void)
{
	/* clear variables */
	f16DutyCycle 				= FRAC16(0.0);
  	udtInterfaceStatus.bit.Both = 0;
  	f16SpeedActual 				= FRAC16(0.0);
  	f16DutyCycle 				= FRAC16(0.0);
  	uw16HSperiod				= 0;
  	f16SpeedRequired			= FRAC16(0.0);
  	f16SpeedRamp				= FRAC16(0.0);
  	bMainSwitch 				= false;
	
	
	/* clearing fault state bits */
	if(uw16FaultClear)
  	{
  		udtAppFaults.W8 	= 0;
  		uw16FaultClear 		= 0;
  		/* go to STOP state */
		uw16AppState = APP_STOP;
		/* turn off fault LED */
    	ioctl(GPIO_B,GPIO_CLEAR_PIN,BIT_4);
  	}
	
}

/*****************************************************************************
*
* Function: 		void main(void)
*
* Description: 		Main fuction
*
* Returns: 			None
*
* Global Data: 		None
*
* Arguments: 		None
*
* Range Issues: 	None
*
* Special Issues: 	None
*
*****************************************************************************/
void main (void) 
{

	/* device peripheral init functions */	  
	PeripheralCoreInit();
  	
  	/* FreeMASTER initialization */
  	FMSTR_Init();
  	
  	/* clear PWM fault flag */
	ioctl(EFPWM,EFPWM_CLEAR_FAULT_FLAGS,EFPWM_FAULT_0);
  	
  	/* enable interrupts */
  	archEnableInt();
  
  	/* background loop */
  	while (1)
	{

		/* main application state machine */
		AppStateMachine[uw16AppState]();
		
		/* check application faults */
		FaultDetection();
		
		/* FreeMASTER polling function */
		FMSTR_Poll();
	}
}


