/* ###################################################################
**     Filename    : main.c
**     Project     : MAPS-MC-2IN1_DualServo
**     Processor   : MC56F84789VLL
**     Version     : Driver 01.16
**     Compiler    : CodeWarrior DSP C Compiler
**     Date/Time   : 2018-06-12, 13:55, # CodeGen: 0
**     Abstract    :
**         Main module.
**         This module contains user's application code.
**     Settings    :
**     Contents    :
**         No public methods
**
** ###################################################################*/
/*!
** @file main.c
** @version 01.16
** @brief
**         Main module.
**         This module contains user's application code.
*/         
/*!
**  @addtogroup main_module main module documentation
**  @{
*/         
/* MODULE main */


/* Including needed modules to compile this module/procedure */
#include "Cpu.h"
#include "Events.h"
#include "FMSTR1.h"
#include "FMSTR_SCI1.h"
#include "PWMA.h"
#include "PWMB.h"
#include "ADC1.h"
#include "AOI1.h"
#include "M1_ENC_A0.h"
#include "M1_MTCap_A1.h"
#include "M1_ZCnt_A2.h"
#include "M1_WatchDog_A3.h"
#include "M2_ENC_B0.h"
#include "M2_MTCap_B1.h"
#include "M2_ZCnt_B2.h"
#include "M2_WatchDog_B3.h"
#include "M1_HU_C4.h"
#include "M1_HV_G8.h"
#include "M1_HW_G9.h"
#include "M2_HU_F8.h"
#include "M2_HV_F6.h"
#include "M2_HW_G11.h"
#include "GF13.h"
#include "GF12.h"
#include "GPIOD5.h"
#include "GPIOD6.h"
/* Including shared modules, which are used for whole project */
#include "PE_Types.h"
#include "PE_Error.h"
#include "PE_Const.h"
#include "IO_Map.h"

#include "motor_def.h"
#include "M1_statemachine.h"
#include "M2_statemachine.h"

/************************* Function prototypes ****************************/
void M1_Html_Input(void);
void M2_Html_Input(void);
void User_Control(void);
void Botton_Key(void);

// Variables for FreeMASTER 
static 			Word16							FmIBase;			// mA
static 			Word16							FmIObsrvBase;		// mA
static 			Word16							FmUBase;			// Volts
static 			Word16							FmBemfBase;			// Volts
static 			Word16							FmSpdElRadBase; 	// electrical:rad.s-1
static 			Word16							FmSpdElHzBase;		// electrical:Hz
static 			Word16   						FmSpdMechRpmBase;	// mechanical:rpm
static 			Word16							FmPosElBase;		// electrical:deg		
static			Word16							FmPosMechBase;		// mechanical: rounds
static			Word16							FmSpdCtrlKpBase;	// Speed controller Parameter: Kp
static			Word32							FmBBase;			// inertia identification: B
static			Word16							FmTorqueBase;		// torque : N *M^2
static			Word16							FmOmegaBase;		// angular speed: Rad/s
static			Word16							FmFlagRecorder;		// flag for recorder
static			Word16							FmSineWaveFreqBase;	// sine wave reference frequency: hz
static			Word16							FmPosCtrlKpBase;	// position controller Parameter: Kp

// X-Y Axis
static			Word16							w16AxisFlag = 0;
static			Frac32							f32AxisAngle;
static			Frac32							f32AxisSpeed;
static			Frac32							f32AxisR;

// Html
static			Word16							w16HtmlSwitch = 0;
static			Word16							w16HtmlM1Flag = 99;
static			Word16							w16HtmlM2Flag = 99;
static			Frac32							f32HtmlPosMax;
static			Frac32							f32HtmlPosMin;	
#define			HTML_CONTROL	0

static			Word16							w16UserMode = 0;
static			Word16							w16Key1State = 0;
static			Word16							w16Key2State = 0;

static			Word16							w16Key1Value;
static			Word16							w16Key1ValuePre;
static			Word16							w16Key1TimeCounter;
static			Word16							w16Key1Dir;

static			Word16							w16Key2Value;
static			Word16							w16Key2ValuePre;
static			Word16							w16Key2TimeCounter;
static			Word16							w16Key2Dir;

void main(void)
{
  /* Write your local variable definition here */

  /*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/
  PE_low_level_init();
  /*** End of Processor Expert internal initialization.                    ***/

  /* Write your code here */
  /* 50% duty is set through PE, important!! */
  
  /* disable the output trigger of PWMA_SM1_VAL4 */
  clrReg16Bits(PWMA_SM1TCTRL, PWMA_SM1TCTRL_OUT_TRIG_EN4_MASK); 
  
  /* disable the compare interrupt of PWMA_SM1_VAL5 */
  clrReg16Bits(PWMA_SM1INTEN, PWMA_SM1INTEN_CMPIE5_MASK); 

  /* disable the output trigger of PWMB_SM0_VAL4 */
  clrReg16Bits(PWMB_SM0TCTRL, PWMB_SM0TCTRL_OUT_TRIG_EN4_MASK);
  
  /* ADC Mapping for the first time */
  	  /* S0  S1  S2  	S3  S4  S5  	S6  S7	*/
  	  /* A0  A4  A0 	A4  A0  A4  	A0  A0	*/
  	  /* 1a	 2a	 xx 	2a	1a	xx			*/
  setReg16(ADC12_CLIST1, 0x4040);
  setReg16(ADC12_CLIST2, 0x0040);
  	  /* S8  S9  S10 	S11 S12 S13 	S14 S15 */
  	  /* B0  B4  B1  	B4  B0  B5  	B7  B7  */
	  /* 1c	 2c	 1Vdcb	2c	1c	2Vdcb			*/
  setReg16(ADC12_CLIST3, 0xC9C8);
  setReg16(ADC12_CLIST4, 0xFFD8);
    
  /* configure encoder timer */
  setReg16(TMRA_0_COMP1, M1_ENC_MODULO);
  setReg16(TMRA_0_COMP2, -M1_ENC_MODULO);
  setReg16(TMRB_0_COMP1, M2_ENC_MODULO);
  setReg16(TMRB_0_COMP2, -M2_ENC_MODULO);
  
  /* Delay before power up of system */
  Cpu_Delay100US(5000); // 50ms
      
  /* FreeMsater Variables Initial */
  FM_Variables_Init();
  
  /* initial speed loop counter */
  gsM1_Drive.uw16DividerSlowLoop	= M1_SPEED_LOOP_CNTR;
  gsM1_Drive.uw16CounterSlowLoop	= gsM1_Drive.uw16DividerSlowLoop;
  gsM1_Drive.uw16FlagSlowLoop		= 0;

  gsM2_Drive.uw16DividerSlowLoop	= M2_SPEED_LOOP_CNTR;
  gsM2_Drive.uw16CounterSlowLoop	= gsM2_Drive.uw16DividerSlowLoop;
  gsM2_Drive.uw16FlagSlowLoop		= 0;
  
  /* X-Y Axis */
  w16AxisFlag 		= 0;
  f32AxisAngle		= 0;
  f32AxisSpeed		= FRAC32(0.1L /M1_MC_SLOW_CONTROL_LOOP_FREQ); // 0.1 Round/S
  f32AxisR			= FRAC32(90.0L /M1_POS_BASE); 
  
  /* Html */
  w16HtmlSwitch		= 0;
  w16HtmlM1Flag		= 99;
  w16HtmlM2Flag 	= 99;
  f32HtmlPosMax		= FRAC32(90.0L /M1_POS_BASE);
  f32HtmlPosMin		= FRAC32(-90.0L /M1_POS_BASE);
  
  /* User Control */
  w16UserMode		= 0;
  w16Key1State		= 0;
  w16Key2State		= 0;
  w16Key1ValuePre   = 1;
  w16Key2ValuePre   = 1;
  w16Key1TimeCounter = 0;
  w16Key2TimeCounter = 0;
  w16Key1Dir		= 1;
  w16Key2Dir		= 1;

   // Start PWM counter
  PWMA_RUN();
  PWMB_RUN();
  
  for(;;)
  {
	  FMSTR1_Poll();
  }
}

/***************************************************************************//*!
*
* @brief   First, enable ADC trigger, ADC interrupt and PWM interrupt.
* 			Then, disable itself(Enable_Trig_isr).
*
* @param   void
*
* @return  none
*
******************************************************************************/
#pragma interrupt saveall
void Enable_Trig_isr(void)
{	
	/* disable the compare interrupt of PWMA_SM0_VAL5 */
	clrReg16Bits(PWMA_SM0INTEN, PWMA_SM0INTEN_CMPIE5_MASK);
	
	/* clear compare flag of PMWA_SM1: Clear these bits by writing a 1 to a bit position! */
	PeriphSafeClearFlags16(PWMA_SM1STS, PWMA_SM1STS_CMPF_MASK, PWMA_SM1STS_CMPF5_MASK);
	
	/* enable the output trigger of PWMA_SM1_VAL4 */
	setReg16Bits(PWMA_SM1TCTRL, PWMA_SM1TCTRL_OUT_TRIG_EN4_MASK);
	
	/* enable the compare interrupt of PWMA_SM1_VAL5 for Motor 1 calculation*/
	setReg16Bits(PWMA_SM1INTEN, PWMA_SM1INTEN_CMPIE5_MASK);
	
	/* enable the output trigger of PWMB_SM0_VAL4 */
	setReg16Bits(PWMB_SM0TCTRL, PWMB_SM0TCTRL_OUT_TRIG_EN4_MASK);
}
#pragma interrupt off

/* Interrupt service routines */
/***************************************************************************//*!
*
* @brief   PWMA fault interrupt service routine
*
* @param   void
*
* @return  none
*
******************************************************************************/
#pragma interrupt saveall
void PWMA_Fault_Isr(void)
{	
    /* check overcurrent fault */
	if (M1_OVERCURRENT_FAULT())
    {
		/* M1 fault routine */
		M1_Fault();

		/* Overcurrent fault clear */
		M1_CLEAR_OVERCURRENT_FAULT();
    }
}
#pragma interrupt off

/***************************************************************************//*!
*
* @brief   PWM B fault interrupt service routine
*
* @param   void
*
* @return  none
*
*
******************************************************************************/
#pragma interrupt saveall
void PWMB_Fault_Isr(void)
{
    /* check overcurrent fault */
	if (M2_OVERCURRENT_FAULT())
    {
		/* M1 fault routine */
		M2_Fault();

		/* Overcurrent fault clear */
		M2_CLEAR_OVERCURRENT_FAULT();
    }
}
#pragma interrupt off

/***************************************************************************//*!
*
* @brief   PWMA CMP1 on VAL5 ISR for motor 1 loop control
*
* @param   void
*
* @return  none
*
*
******************************************************************************/
#pragma interrupt saveall
void M1_isr(void)
{	
	GF12_NegVal();
	
	/* Debug, get M1 rotor position */
	gsM1_Drive.sSpeedPos.uw16PositionMechRaw = TMRA_0_CNTR;
	MCSTRUC_AngleFromEncoder(&gsM1_Drive.sSpeedPos);
	
	/* M1 Current */
	gsM1_I.f16A = getReg16(ADC12_RSLT0);
	gsM1_I.f16B = getReg16(ADC12_RSLT8);
	
	/*DC bus voltage */
    gsM1_Drive.sFocPMSM.f16UDcBus = getReg16(ADC12_RSLT10);
    
    /* Hall */
    gsM1_Drive.sHall.uw16U 		= (GPIOC_DR & GPIOC_DR_D4_MASK) >> 4;
    gsM1_Drive.sHall.uw16V		= (GPIOG_DR & GPIOG_DR_D8_MASK) >> 8;
    gsM1_Drive.sHall.uw16W 		= (GPIOG_DR & GPIOG_DR_D9_MASK) >> 9;
    gsM1_Drive.sHall.uw16Num	= add(add((gsM1_Drive.sHall.uw16U << 2), (gsM1_Drive.sHall.uw16V << 1)), gsM1_Drive.sHall.uw16W); 

	/****======================= Start - Motor 2 slow loop calculation ============================*****/	
    /* M2 Current offset */
    gsM2_IOffset.f16A = getReg16(ADC12_RSLT1);
    gsM2_IOffset.f16B = getReg16(ADC12_RSLT9);
    
	/*==============  M2 speed calculation Begin ===================*/
	if(--gsM2_Drive.uw16CounterSlowLoop == 0)
	{
		gsM2_Drive.uw16CounterSlowLoop = gsM2_Drive.uw16DividerSlowLoop;
		gsM2_Drive.uw16FlagSlowLoop = 1;
		
		/* Enable capture to get speed */
//		gsM2_Drive.uw16CounterSpdCal = 0;
		TMRB_0_SCTRL &= ~0x0800; // clear capture flag of qep counter
		TMRB_1_SCTRL &= ~0x0800; // clear capture flag of tmrMTSpd counter
		TMRB_1_SCTRL |= 0x0400;  // enable capture interrupt
						
		if(TMRB_3_SCTRL & 0x2000)// roll over flag
		{
			TMRB_3_SCTRL &= ~0x2000; //clear flag
			gsM2_Drive.sSpeedCalc.w16TimeOutFlag = 1;
			gsM2_Drive.sSpeedPos.f32Speed = 0;
		}
		
		/* Get improved speed of resolution */
	 	gsM2_Drive.sSpeedCalc.uw16TCntDeltaNow = TMRB_3_CNTR;
	 	gsM2_Drive.sSpeedPos.f32SpeedImproved = MCSTRUC_SpeedClacImproved(&gsM2_Drive.sSpeedCalc, gsM2_Drive.sSpeedPos.f32Speed);
				
		/* X-Y Axis */
		if(1 == w16AxisFlag)
		{
			f32AxisAngle = L_add(f32AxisAngle, f32AxisSpeed);
			gsM1_Drive.SPosCtrl.f32PosDesired = L_mult_ls(f32AxisR, GFLIB_Sin_F16(extract_h(f32AxisAngle)));
			gsM2_Drive.SPosCtrl.f32PosDesired = L_mult_ls(f32AxisR, GFLIB_Cos_F16(extract_h(f32AxisAngle)));
		}
		
#if	HTML_CONTROL == 1
		M2_Html_Input();
#endif
	}
	/*==============  M2 speed calculation End ==============*/
	
    /* M2 Slow loop calculation */
    gsM2_StateRunLoop = SLOW;
		
    /* StateMachine call */
    SM_StateMachine(&gsM2_Ctrl);
     
     /****======================== End- Motor 2 slow loop calculation ============================****/
	
    /* Fast loop calculation */
    gsM1_StateRunLoop = FAST;    
    
    /* StateMachine call */
    SM_StateMachine(&gsM1_Ctrl);

    /* Channel update */
	/* S0  S1  S2  		S3  S4  S5 */
	/* XX  --  --  		--  XX  -- */
    setReg16BitGroupVal(ADC12_CLIST1, SAMPLE0, gsM1_IChannels.uw16Ph0);
    setReg16BitGroupVal(ADC12_CLIST2, SAMPLE4, gsM1_IChannels.uw16Ph0); // M1_offset
	/* S8  S9  S10 		S11 S12 S13 */
	/* XX  --  1Vdcb  	--  XX  2Vdcb  */ 
    setReg16BitGroupVal(ADC12_CLIST3, SAMPLE8, gsM1_IChannels.uw16Ph1);
    setReg16BitGroupVal(ADC12_CLIST4, SAMPLE12, gsM1_IChannels.uw16Ph1);// M1_offset
    
	/* html */
#if	HTML_CONTROL == 1
    User_Control();
#endif
    
	/* clear compare flag of PMWA_SM1: Clear these bits by writing a 1 to a bit position! */
	PeriphSafeClearFlags16(PWMA_SM1STS, PWMA_SM1STS_CMPF_MASK, PWMA_SM1STS_CMPF5_MASK);

}
#pragma interrupt off

/***************************************************************************//*!
*
* @brief   ADC12_CC0 ISR for motor 2 loop control
*
* @param   void
*
* @return  none
*
*
******************************************************************************/
#pragma interrupt saveall
void M2_isr(void)
{	
	GF13_NegVal();
	
	/* Debug, get M1 rotor position */
	gsM2_Drive.sSpeedPos.uw16PositionMechRaw = TMRB_0_CNTR;	
	MCSTRUC_AngleFromEncoder(&gsM2_Drive.sSpeedPos);
	
	/* M2 Current */
	gsM2_I.f16A = getReg16(ADC12_RSLT3);
	gsM2_I.f16B = getReg16(ADC12_RSLT11);
	
	/*DC bus voltage */
    gsM2_Drive.sFocPMSM.f16UDcBus = getReg16(ADC12_RSLT13);
    
    /* Hall */
    gsM2_Drive.sHall.uw16U 		= (GPIOF_DR & GPIOF_DR_D8_MASK) >> 8;
    gsM2_Drive.sHall.uw16V		= (GPIOF_DR & GPIOF_DR_D6_MASK) >> 6;
    gsM2_Drive.sHall.uw16W 		= (GPIOG_DR & GPIOG_DR_D11_MASK) >> 11;
    gsM2_Drive.sHall.uw16Num	= add(add((gsM2_Drive.sHall.uw16U << 2), (gsM2_Drive.sHall.uw16V << 1)), gsM2_Drive.sHall.uw16W); 
    
    /****====================== Start - Motor 1 slow loop calculation ==============*****/
    /* M1 Current offset */
    gsM1_IOffset.f16A = getReg16(ADC12_RSLT4);
    gsM1_IOffset.f16B = getReg16(ADC12_RSLT12);
    
	/*===============  M1 speed calculation Begin =================*/
	if(--gsM1_Drive.uw16CounterSlowLoop == 0)
	{
		gsM1_Drive.uw16CounterSlowLoop = gsM1_Drive.uw16DividerSlowLoop;
		gsM1_Drive.uw16FlagSlowLoop = 1;
		
		/* Enable capture to get speed */
//		gsM1_Drive.uw16CounterSpdCal = 0;
		TMRA_0_SCTRL &= ~0x0800; // clear capture flag of qep counter
		TMRA_1_SCTRL &= ~0x0800; // clear capture flag of tmrMTSpd counter
		TMRA_1_SCTRL |= 0x0400;  // enable capture interrupt
						
		if(TMRA_3_SCTRL & 0x2000)// roll over flag
		{
			TMRA_3_SCTRL &= ~0x2000; //clear flag
			gsM1_Drive.sSpeedCalc.w16TimeOutFlag = 1;
			gsM1_Drive.sSpeedPos.f32Speed = 0;
		}
		
		/* Get improved speed of resolution */
	 	gsM1_Drive.sSpeedCalc.uw16TCntDeltaNow = TMRA_3_CNTR;
	 	gsM1_Drive.sSpeedPos.f32SpeedImproved = MCSTRUC_SpeedClacImproved(&gsM1_Drive.sSpeedCalc, gsM1_Drive.sSpeedPos.f32Speed);
		
#if	HTML_CONTROL == 1
	 	M1_Html_Input();
#endif
	}
	/*===============  M1 speed calculation End =============*/
	
    /* Slow loop calculation */
    gsM1_StateRunLoop = SLOW;
	
    /* StateMachine call */
    SM_StateMachine(&gsM1_Ctrl);
  
    /****===================== End - Motor 1 slow loop calculation ============================*****/

    /* Fast loop calculation */
    gsM2_StateRunLoop = FAST;

    /* StateMachine call */
    SM_StateMachine(&gsM2_Ctrl);
        
    /* Channel update */
	/* S0  S1  S2  		S3  S4  S5 */
	/* --  XX  --  		XX  --  -- */
    setReg16BitGroupVal(ADC12_CLIST1, SAMPLE1, gsM2_IChannels.uw16Ph0);
    setReg16BitGroupVal(ADC12_CLIST1, SAMPLE3, gsM2_IChannels.uw16Ph0);
	/* S8  S9  S10 		S11 S12 S13 */
	/* --  XX  1Vdcb  	XX  --  2Vdcb  */ 
    setReg16BitGroupVal(ADC12_CLIST3, SAMPLE9, gsM2_IChannels.uw16Ph1);
    setReg16BitGroupVal(ADC12_CLIST3, SAMPLE11, gsM2_IChannels.uw16Ph1);
	
	//  recorder
	FMSTR1_Recorder();
	
	// clear EOSI0 flag
	setReg16Bits(ADC12_STAT, ADC12_STAT_EOSI0_MASK);
}
#pragma interrupt off

#pragma interrupt on
void M1_tmr_MTCap_isr(void)
{	
	gsM1_Drive.sSpeedCalc.w16EncCntRaw = TMRA_0_CAPT;
	gsM1_Drive.sSpeedCalc.w16TCnt = TMRA_1_CAPT;	
	MCSTRUC_SpeedClacMTMethod(&gsM1_Drive.sSpeedCalc);
	gsM1_Drive.sSpeedPos.f32Speed = gsM1_Drive.sSpeedCalc.f32Speed;
	// disable capture interrupt
	TMRA_1_SCTRL &= ~0x0400;
}
#pragma interrupt off

#pragma interrupt on
void M2_tmr_MTCap_isr(void)
{	
	gsM2_Drive.sSpeedCalc.w16EncCntRaw = TMRB_0_CAPT;
	gsM2_Drive.sSpeedCalc.w16TCnt = TMRB_1_CAPT;	
	MCSTRUC_SpeedClacMTMethod(&gsM2_Drive.sSpeedCalc);
	gsM2_Drive.sSpeedPos.f32Speed = gsM2_Drive.sSpeedCalc.f32Speed;
	// disable capture interrupt
	TMRB_1_SCTRL &= ~0x0400;
}
#pragma interrupt off

void FM_Variables_Init(void)
{
	/* Variables for FreeMASTER */
	FmFlagRecorder		= 0;
	FmIBase 			= M1_FM_I_SCALE;			// mA
//	FmSpdElHzBase 		= FREQ_MAX;				// Hz
//	FmSpdElRadBase		= FM_SPEED_EL;			// rad.s-1	
	FmSpdMechRpmBase 	= M1_FM_NMAX;				// rpm-mechanical	
	FmPosElBase			= M1_FM_POS_SCALE;			// deg
	FmPosMechBase		= M1_POS_BASE;
	FmSpdCtrlKpBase		= M1_SPD_CTRL_AW_KP_BASE;
	FmSineWaveFreqBase	= M1_SINE_WAVE_FREQ_BASE;
	FmPosCtrlKpBase		= M1_POS_CTRL_PROP_GAIN_BASE;
}

void User_Control(void)
{
	if(w16UserMode != 2)
	{
		if(0 == w16HtmlSwitch)
		{
			w16HtmlSwitch = 2;
			mbM1_SwitchAppOnOff = 0;
			mbM2_SwitchAppOnOff = 0;
		}
		else if(1 == w16HtmlSwitch)
		{
			w16HtmlSwitch = 3;
			mbM1_SwitchAppOnOff = 1;
			mbM2_SwitchAppOnOff = 1;
			
			w16UserMode = 1;
		}
		
		if(mbM1_SwitchAppOnOff && mbM2_SwitchAppOnOff)
		{
		}
		else
		{
			mbM1_SwitchAppOnOff = 0;
			mbM2_SwitchAppOnOff = 0;
			w16AxisFlag = 0;
		}
	}
	
	Botton_Key();
}

void Botton_Key(void)
{
	Frac32 f32Key1Temp, f32Key2Temp;
	
	if(w16UserMode != 1)
	{
		f32Key1Temp = gsM1_Drive.SPosCtrl.f32PosDesired;
		f32Key2Temp = gsM2_Drive.SPosCtrl.f32PosDesired;
		
		w16Key1Value	= (GPIOD_DR & GPIOF_DR_D6_MASK) >> 6; //GPIOD6
		w16Key2Value	= (GPIOD_DR & GPIOF_DR_D5_MASK) >> 5; //GPIOD5
		
		/* key1 */
		if(0 == w16Key1State)
		{
			if((w16Key1Value == 0) & (w16Key1ValuePre == 0))
			{
				if(w16Key1TimeCounter++ > 20000)
				{
					mbM1_SwitchAppOnOff = 1;
					w16Key1State = 1;
					w16UserMode = 2;
					w16Key1TimeCounter = 0;
				}
			}
			else
			{
				w16Key1TimeCounter = 0;
			}
		}
		else if(1 == w16Key1State)
		{
			if((w16Key1Value == 0) & (w16Key1ValuePre == 1))
			{
				if(1 == w16Key1Dir)
				{
					f32Key1Temp += FRAC32(2.0L/M1_POS_BASE);
				}
				else
				{
					f32Key1Temp += FRAC32(-10.0L/M1_POS_BASE);
				}
				
				if(f32Key1Temp > f32HtmlPosMax)
				{
					f32Key1Temp = f32HtmlPosMax;
					w16Key1Dir = 0;
				}
				else if(f32Key1Temp < f32HtmlPosMin)
				{
					f32Key1Temp = f32HtmlPosMin;
					w16Key1Dir = 1;
				}				
			}
		}
		
		/* key2 */
		if(0 == w16Key2State)
		{
			if((w16Key2Value == 0) & (w16Key2ValuePre == 0))
			{
				if(w16Key2TimeCounter++ > 20000)
				{
					mbM2_SwitchAppOnOff = 1;
					w16Key2State = 1;
					w16UserMode = 2;
				}
			}
			else
			{
				w16Key2TimeCounter = 0;
			}
		}
		else if(1 == w16Key2State)
		{
			if((w16Key2Value == 0) & (w16Key2ValuePre == 1))
			{
				if(1 == w16Key2Dir)
				{
					f32Key2Temp += FRAC32(2.0L/M1_POS_BASE);
				}
				else
				{
					f32Key2Temp += FRAC32(-10.0L/M1_POS_BASE);
				}
				
				if(f32Key2Temp > f32HtmlPosMax)
				{
					f32Key2Temp = f32HtmlPosMax;
					w16Key2Dir = 0;
				}
				else if(f32Key2Temp < f32HtmlPosMin)
				{
					f32Key2Temp = f32HtmlPosMin;
					w16Key2Dir = 1;
				}				
			}
		}
		
		if((1 == w16Key1State) & (1 == w16Key2State))
		{
			if((w16Key1Value == 0) & (w16Key1ValuePre == 0) & (w16Key2Value == 0) & (w16Key2ValuePre == 0))
			{
				if(w16Key1TimeCounter++ > 20000)
				{
					w16Key1State = 2;
					w16Key2State = 2;
					w16Key1TimeCounter = 0;
					w16AxisFlag = 1;
				}
			}
			else
			{
				w16Key1TimeCounter = 0;
			}
		}
		else if((2 == w16Key1State) & (2 == w16Key2State))
		{
			if((w16Key1Value == 0) & (w16Key1ValuePre == 0) & (w16Key2Value == 0) & (w16Key2ValuePre == 0))
			{
				if(w16Key1TimeCounter++ > 20000)
				{
					w16Key1State = 1;
					w16Key2State = 1;
					w16Key1TimeCounter = 0;
					w16AxisFlag = 0;
				}
			}
			else
			{
				w16Key1TimeCounter = 0;
			}
		}
		
		w16Key1ValuePre = w16Key1Value;
		w16Key2ValuePre = w16Key2Value;
		
		gsM1_Drive.SPosCtrl.f32PosDesired = f32Key1Temp;
		gsM2_Drive.SPosCtrl.f32PosDesired = f32Key2Temp;
	}
}

void M1_Html_Input(void)
{
	Frac32 f32Temp;
	
	f32Temp = gsM1_Drive.SPosCtrl.f32PosDesired;
	
	if(99 == w16HtmlM1Flag)
	{

	}
	else
	{
		switch(w16HtmlM1Flag)
		{
		case 0:
		default:
			f32Temp = 0;
			break;
			
		case 1:
			f32Temp += FRAC32(0.1L/M1_POS_BASE);
			break;
			
		case -1:
			f32Temp += FRAC32(-0.1L/M1_POS_BASE);
			break;
			
		case 2:
			f32Temp += FRAC32(1.0L/M1_POS_BASE);
			break;
			
		case -2:
			f32Temp += FRAC32(-1.0L/M1_POS_BASE);
			break;
			
		case 3:
			f32Temp += FRAC32(10.0L/M1_POS_BASE);
			break;
			
		case -3:
			f32Temp += FRAC32(-10.0L/M1_POS_BASE);
			break;
			
		case 4:
			f32Temp = f32HtmlPosMax;
			break;
			
		case -4:
			f32Temp = f32HtmlPosMin;
			break;
		}
		
		if(f32Temp > f32HtmlPosMax)
		{
			f32Temp = f32HtmlPosMax;
		}
		else if(f32Temp < f32HtmlPosMin)
		{
			f32Temp = f32HtmlPosMin;
		}
		gsM1_Drive.SPosCtrl.f32PosDesired = f32Temp;
		
		w16HtmlM1Flag = 99;
	}
}

void M2_Html_Input(void)
{
	Frac32 f32Temp;
	
	f32Temp = gsM2_Drive.SPosCtrl.f32PosDesired;
	
	if(99 == w16HtmlM2Flag)
	{

	}
	else
	{
		switch(w16HtmlM2Flag)
		{
		case 10:
		default:
			f32Temp = 0;
			break;
			
		case 11:
			f32Temp += FRAC32(0.1L/M2_POS_BASE);
			break;
			
		case -11:
			f32Temp += FRAC32(-0.1L/M2_POS_BASE);
			break;
			
		case 12:
			f32Temp += FRAC32(1.0L/M2_POS_BASE);
			break;
			
		case -12:
			f32Temp += FRAC32(-1.0L/M2_POS_BASE);
			break;
			
		case 13:
			f32Temp += FRAC32(10.0L/M2_POS_BASE);
			break;
			
		case -13:
			f32Temp += FRAC32(-10.0L/M2_POS_BASE);
			break;
			
		case 14:
			f32Temp = f32HtmlPosMax;
			break;
			
		case -14:
			f32Temp = f32HtmlPosMin;
			break;
		}
		
		if(f32Temp > f32HtmlPosMax)
		{
			f32Temp = f32HtmlPosMax;
		}
		else if(f32Temp < f32HtmlPosMin)
		{
			f32Temp = f32HtmlPosMin;
		}
		gsM2_Drive.SPosCtrl.f32PosDesired = f32Temp;
		
		w16HtmlM2Flag = 99;
	}
}

/* END main */
/*!
** @}
*/
/*
** ###################################################################
**
**     This file was created by Processor Expert 10.3 [05.09]
**     for the Freescale 56800 series of microcontrollers.
**
** ###################################################################
*/
