/*****************************************************************************
 *   main.c:  main C entry file for Philips LPC29xx Family Microprocessors
 *
 *   Copyright(C) 2009, NXP Semiconductor
 *   All rights reserved.
 *
 *   History
 *   2009.12.71  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.
 *****************************************************************************/

#include "LPC29xx.h"                        /* LPC29xx definitions */
#include "type.h"
#include "irq.h"
#include "stdlib.h"
#include "main.h"	   
#include "timer.h"
#include "uart.h"
#include "extint.h"
#include "FOC.h"
#include "pwm.h"
#include "qei.h"
#include "mtimer.h"
#include "adc.h" 
#include "dma.h"
#include "SE95.h"
#include "i2c.h"
#include "cordic.h"
#include "extint.h"
#include "eeprom.h"

volatile FOC_STRUCT FOC;				// Definition of structure that holds all FOC parameters for calculation
volatile FOC_STRUCT FOC_tx;				// Definition of structure that holds all FOC parameters to be transmitted
volatile CALCVAR_STRUCT CALCVAR;
PARAM_STRUCT EEParam;

extern volatile DWORD timer_counter;			// TIMER extern variable declarations
extern volatile BYTE displayval;				// 7Seg display display value, value to put on 7Seg 
extern volatile BYTE UART0Buffer[BUFSIZE];
extern volatile DWORD UART0Count;
extern volatile BYTE UART0Timeout;
extern volatile BYTE I2C0Buffer[I2CBUFSIZE];	// I2C extern variable declarations
extern volatile BYTE I2C0state;
extern volatile BYTE I2C0txBytes;
extern volatile double SE95_temp;				// The SE95 temperature 
extern volatile BYTE btn_pressed;

/*****************************************************************************
** Function name:		main
**
** Description:			main routine for FOC motor control
**
** parameters:			None
** Returned value:		int
**
*****************************************************************************/		
int main (void)
{
	DWORD cnt_100ms=0, cnt_1s=0;
	
	GetDeviceID(FOC.deviceID);
   
	GPIO0_DR |= (0xFF<<18);				// P018 - P025 LED output
	GPIO0_OR |= (0xFF<<18);				// Set leds off

	EXTINT_Init();						// Button on interrupt
	Timer_Init(1000);					// Initialize timer	: 1 msec
  	Timer_Enable();
	SE95_init();

	UART0Count=0;
	UART0Buffer[0]=0;

  	EEPROM_Init();						// Initialize the EEPROM 
	EEPROM_ReadEEparam(&EEParam);		// Read parameters from EEPROM

	if(EEParam.saved != EEPROM_SAVED)	// If EEPROM does not contain saved data 
	{
		FOC_DefaultEEparam(&EEParam);	// Default parameters 
		EEParam.saved = EEPROM_SAVED;	// Mark saved state
		EEPROM_WriteEEparam(&EEParam);	// Save default parameters in EEPROM
	}
	
	CopyEEparamToFOC();
		
	FOC_Init();
	FOC_CalcVar();
	FOC.size = sizeof(FOC_tx);
	UART0Init(115200);			// Initialize UART0 @ 115200 baud
	DMA_Init(&FOC_tx);
	INIT_QEI();					// Initialize QEI 
	ADC_1_2_Init();				// Initialize the ADC 
	PWM0_Init();	   			// Initialize PWM0
	init_mtimer0(PWM_CYCLE);	// Initialize and enable the MTimer0, as ADC convert trigger 
	PWM0_Start();				// PWM0 must be started before synchronisation with MTIMER
	enable_mtimer0();  			// Enable MTIMER for synchronisation of ADC and PWM

	if(FOC.mode == MODE_QEI || FOC.mode == MODE_QEI_RAMP)GPIO0_OR &= ~(1<<19);	  	// LED P0.19 on if QEI

	// Loop forever 
	while(1) 
  	{	
		if(!FOC.busy)CopyFOCStruct(&FOC_tx,&FOC,sizeof(FOC)-1);	// Copy FOC	to FOC_tx when FOC loop is not running
		if(UART0Count>=5)ProcessUart();						// one cmd byte + 4 data bytes
		if(timer_counter>=cnt_100ms){cnt_100ms = timer_counter+100;Call_100ms();}
		if(timer_counter>=cnt_1s)	{cnt_1s = timer_counter+1000;Call_1s();}
		if(btn_pressed)				   		// Button switches between QEI and sensorless
		{
		  	btn_pressed = FALSE;
		  	if(FOC.mode == MODE_QEI)
			{
				FOC.mode = MODE_SL;
				GPIO0_OR |= (1<<19); 		// LED P0.19 off if sensorless
			}
			else
			{
				FOC.mode = MODE_QEI;
				GPIO0_OR &= ~(1<<19);	  	// LED P0.19 on if QEI
			}
		}
	}
}

/*****************************************************************************
** Function name:		Call_100ms
**
** Description:			100 ms Call function
**
** parameters:			None
** Returned value:		None
**
*****************************************************************************/
void Call_100ms(void)
{
	static BYTE leds=0;

	GPIO0_OR |= (0xFC<<18);				// 6 leds off
	GPIO0_OR &= ~((1<<leds++)<<20);		// one led on
	if(leds==6)leds=0;
}

/*****************************************************************************
** Function name:		Call_1s
**
** Description:			1 sec Call function for temperature reading
**
** parameters:			None
** Returned value:		None
**
*****************************************************************************/                                                          
void Call_1s(void)
{
	FOC.temp = SE95_read_temp();
}

/*****************************************************************************
** Function name:		ProcessUart
**
** Description:			Processes received UART data
**
** parameters:			None
** Returned value:		None
**
*****************************************************************************/
void ProcessUart(void)
{
	BYTE resetUARTcnt = TRUE;
	BYTE saveEEParam = TRUE;

	switch(UART0Buffer[0])
	{
 		case(0):	// FOC enabled = 0
			FOC.enabled=UART0Buffer[1];
			if(!FOC.enabled)FOC.busy = FALSE;	// Always copy FOC buffer to txFOC buffer
			break;
 		case(1):	// SpeedControlEnabled = 1
			FOC.speedEnabled = UART0Buffer[1];
			break;
 		case(2):	// Mode = 2
			FOC.mode = UART0Buffer[1];
			if(FOC.mode==MODE_QEI)GPIO0_OR &= ~(1<<19);	  	// LED P0.19 on if QEI
			else GPIO0_OR |= (1<<19); 						// LED P0.19 off if sensorless
			break;
 		case(3):	// QSP = 3
			FOC.Q_SP.full = UartDataToUS(); 
			saveEEParam = FALSE;
			break;
 		case(4):	// DSP = 4
			FOC.D_SP.full = UartDataToUS(); 
			break;
 		case(5):	// DKp = 5
			FOC.D_Kp.full = UartDataToUS(); 
			break;
 		case(6):	// DKi = 6
			FOC.D_Ki.full = UartDataToUS(); 
			break;
 		case(7):	// QKp = 7
			FOC.Q_Kp.full = UartDataToUS(); 
			break;
 		case(8):	// QKi = 7
			FOC.Q_Ki.full = UartDataToUS();
			break;
 		case(9):	// Speed SP = 8
			FOC.speed_SP.full = UartDataToUS(); 
			saveEEParam = FALSE;
			break;
 		case(10):	// SpeedKp = 10
			FOC.speed_Kp.full = UartDataToUS(); 
			break;
 		case(11):	// SpeedKi = 11
			FOC.speed_Ki.full = UartDataToUS(); 
			break;
 		case(12):	// SpeedKd = 12
			FOC.speed_Kd.full = UartDataToUS(); 
			break;
 		case(13):	// Fsmo = 13
			FOC.F.full = UartDataToSL(); 
			break;
 		case(14):	// Gsmo = 14
			FOC.G.full = UartDataToSL();
			break;
		case(15):	// K_smc = 15
			FOC.K_smc.full = UartDataToSL();
			break;
		case(16):	// maxError = 16
			FOC.smc_max_err.full = UartDataToSL(); 
			break;
		case(17):	// omegaFltrCoef = 17
			FOC.Cflt_omega.full = UartDataToSL(); 
			break;
		case(18):	// Test1 = 18
			FOC.test1.full = UartDataToSL();
			saveEEParam = FALSE;
			break;
 		case(19):	// Test2 = 19
			FOC.test2.full = UartDataToSL();
			saveEEParam = FALSE;
			break;
 		case(20):	// Test3 = 20
			FOC.test3.full = UartDataToSL();
			saveEEParam = FALSE;
			break;
 		case(21):	// Test4 = 21
			FOC.test4.full = UartDataToSL();
			saveEEParam = FALSE;
		case('M'):	// Receive match register value's	
			if(UART0Count==25) 		
			{	
				FOC.mACT[0] = (UART0Buffer[2]<<8) | UART0Buffer[1];
				FOC.mACT[1] = (UART0Buffer[4]<<8) | UART0Buffer[3];
				FOC.mACT[2] = (UART0Buffer[6]<<8) | UART0Buffer[5];
				FOC.mACT[3] = (UART0Buffer[8]<<8) | UART0Buffer[7];
				FOC.mACT[4] = (UART0Buffer[10]<<8) | UART0Buffer[9];
				FOC.mACT[5] = (UART0Buffer[12]<<8) | UART0Buffer[11];
				FOC.mDACT[0] = (UART0Buffer[14]<<8) | UART0Buffer[13];
				FOC.mDACT[1] = (UART0Buffer[16]<<8) | UART0Buffer[15];
				FOC.mDACT[2] = (UART0Buffer[18]<<8) | UART0Buffer[17];
				FOC.mDACT[3] = (UART0Buffer[20]<<8) | UART0Buffer[19];
				FOC.mDACT[4] = (UART0Buffer[22]<<8) | UART0Buffer[21];
				FOC.mDACT[5] = (UART0Buffer[24]<<8) | UART0Buffer[23];
				PWM0_Set();
			}
			else resetUARTcnt=FALSE;
			saveEEParam = FALSE;
			break;
	}
	
	// Save new parameters in EEPROM
	if(saveEEParam)
	{
		CopyFOCToEEparam();
		EEParam.saved = EEPROM_SAVED;
		EEPROM_WriteEEparam(&EEParam);
	}
	
	FOC_CalcVar(); 
	if(resetUARTcnt)
	{
		UART0Count=0;
		FOC.uartBusy = FALSE;
	}
}

/*****************************************************************************
** Function name:		EEPROM_ReadEEparam
**
** Description:			Read FOC parameters from EEPROM
**
** parameters:			pointer to parameter structure
** Returned value:		None
**
*****************************************************************************/
void EEPROM_ReadEEparam(PARAM_STRUCT *param)
{
	EEPROM_ReadPage(EEPROM_PAGE_INDEX, (DWORD *) param, EEPROM_PAGE_SIZE, READ32 );
}

/*****************************************************************************
** Function name:		EEPROM_WriteEEparam
**
** Description:			Write FOC parameters to EEPROM
**
** parameters:			pointer to parameter structure
** Returned value:		None
**
*****************************************************************************/
void EEPROM_WriteEEparam(PARAM_STRUCT *param)
{
	EEPROM_WritePage(EEPROM_PAGE_INDEX, (DWORD *) param, EEPROM_PAGE_SIZE, WRITE32 );
}

/*****************************************************************************
** Function name:		CopyFOCToEEparam
**
** Description:			Copy FOC structure parameters to EEparam structure  
**
** parameters:			None
** Returned value:		None
**
*****************************************************************************/
void CopyFOCToEEparam(void)
{
	EEParam.currentOffset = FOC.currentOffset;
	EEParam.Q_SP.full = FOC.Q_SP.full;		
	EEParam.D_SP.full = FOC.D_SP.full; 		
	EEParam.Q_Kp.full = FOC.Q_Kp.full;		
	EEParam.Q_Ki.full = FOC.Q_Ki.full;		
	EEParam.D_Kp.full = FOC.D_Kp.full;		
	EEParam.D_Ki.full = FOC.D_Ki.full;			 	
	EEParam.speed_Kp.full = FOC.speed_Kp.full;		
	EEParam.speed_Ki.full = FOC.speed_Ki.full;		
	EEParam.speed_Kd.full = FOC.speed_Kd.full;		
	EEParam.speed_SP.full = FOC.speed_SP.full;		
	
	EEParam.F.full = FOC.F.full;
	EEParam.G.full = FOC.G.full;
	EEParam.K_smc.full = FOC.K_smc.full;
	EEParam.smc_max_err.full = FOC.smc_max_err.full;
	EEParam.Cflt_omega.full = FOC.Cflt_omega.full; 
}

/*****************************************************************************
** Function name:		CopyEEparamToFOC
**
** Description:			Copy EEparam structure parameters to FOC structure 
**
** parameters:			None
** Returned value:		None
**
*****************************************************************************/					   
void CopyEEparamToFOC(void)
{
 	FOC.currentOffset = EEParam.currentOffset;
	FOC.Q_SP.full = EEParam.Q_SP.full;		
	FOC.D_SP.full = EEParam.D_SP.full; 		
	FOC.Q_Kp.full = EEParam.Q_Kp.full;		
	FOC.Q_Ki.full = EEParam.Q_Ki.full;		
	FOC.D_Kp.full = EEParam.D_Kp.full;		
	FOC.D_Ki.full = EEParam.D_Ki.full;			 	
	FOC.speed_Kp.full = EEParam.speed_Kp.full;		
	FOC.speed_Ki.full = EEParam.speed_Ki.full;		
	FOC.speed_Kd.full = EEParam.speed_Kd.full;		
	FOC.speed_SP.full = EEParam.speed_SP.full;		
	
	FOC.F.full = EEParam.F.full;
	FOC.G.full = EEParam.G.full;
	FOC.K_smc.full = EEParam.K_smc.full;
	FOC.smc_max_err.full = EEParam.smc_max_err.full;
	FOC.Cflt_omega.full = EEParam.Cflt_omega.full; 
}

/*****************************************************************************
** Function name:		UartDataToSL
**
** Description:			Convert 4 UART bytes to signed long
**
** parameters:			None
** Returned value:		converted value
**
*****************************************************************************/
signed long UartDataToSL(void)
{
	return (UART0Buffer[4]<<24) | (UART0Buffer[3]<<16) | (UART0Buffer[2]<<8) | UART0Buffer[1];
}

/*****************************************************************************
** Function name:		UartDataToUS
**
** Description:			Convert 2 UART bytes to unsigned short
**
** parameters:			None
** Returned value:		converted value
**
*****************************************************************************/
unsigned short UartDataToUS(void)
{
	return (UART0Buffer[2]<<8) | UART0Buffer[1];
}

/*****************************************************************************
** Function name:		GetDeviceID
**
** Description:			Reads die specific ID
**
** parameters:			pointer to devicedID array to write to
** Returned value:		nothing
**
*****************************************************************************/
void GetDeviceID(volatile DWORD *deviceID)
{
    FCTR = (FS_ISS| FS_LOADREQ | FS_WPB | FS_WEB | FS_WRE | FS_CS);
 	deviceID[0] = (*(volatile unsigned long *)(0x20000400));
    deviceID[1] = (*(volatile unsigned long *)(0x20000408));
  	FCTR = FS_DCR | FS_CS;
    FBWST = SPECALWAYS | 0x04; 
}

/*****************************************************************************
** Function name:		CopyFOCStruct
**
** Description:			Copy FOC structure 
**
** parameters:			pointer to FOC structure destiny and source 
** Returned value:		nothing
**
*****************************************************************************/
void CopyFOCStruct(FOC_STRUCT *dest, FOC_STRUCT *src, size_t count) 
{
	char *dst8 = (char *)dest;
    char *src8 = (char *)src;

    while(count--)*dst8++ = *src8++;
}

/******************************************************************************
**                            End Of File
******************************************************************************/

