/*  INCLUDE  */
#include <hidef.h> /* for EnableInterrupts macro */
#include <MC68HC908GZ60.h> /* include peripheral declarations */
#include "COMMON.h"
#include "FAULT.h"

/* Global Variables */
UINT8 motor_fault_flag;

/*****************************************************************************
 * Function:        Motor_Fault()
 *
 * Description:     none
 *
 * Returns:         none
 *
 * Notes:           This handler is the interrupt function of SWI
 *					        interrupt.
 *
 *****************************************************************************/
 void Motor_Fault(void){
    
    UINT8 MOTOR_MIX_m,MOTOR_CYCLE_m,MOTOR_MODE_m,MOTOR_CENTER_m;
    UINT8 i=0xFF,j=0xFF;
    
    DisableInterrupts;
    
    MOTOR_MIX_EN      =OFF;			   //Turn motors off
    MOTOR_CYCLE_EN    =OFF;
    MOTOR_MODE_EN     =OFF;
    MOTOR_CENTER_EN   =OFF;
    
    MOTOR_MIX_m       =MOTOR_MIX;       //Memory motor select state
    MOTOR_CYCLE_m			=MOTOR_CYCLE;
    MOTOR_MODE_m      =MOTOR_MODE;
    MOTOR_CENTER_m    =MOTOR_CENTER;
    
    MOTOR_CYCLE       =OFF;	       //Turn LED OFF
    MOTOR_MODE        =OFF;
    MOTOR_CENTER      =OFF;
    MOTOR_MIX         =OFF;
    
    __RESET_WATCHDOG();
    
        
    while(FAULT_MIX==0 || FAULT_CYCLE==0 || FAULT_MODE==0 || FAULT_CENTER==0){
       
       for(;i>0;i--){
          __RESET_WATCHDOG();
          for(;j>0;j--);
       }
       if(FAULT_MIX==0){MOTOR_MIX=~MOTOR_MIX;}
       if(FAULT_CYCLE==0){MOTOR_CYCLE=~MOTOR_CYCLE;} 
       if(FAULT_MODE==0){MOTOR_MODE=~MOTOR_MODE;} 
       if(FAULT_CENTER==0){MOTOR_CENTER=~MOTOR_CENTER;}
       __RESET_WATCHDOG();
    }
    
    MOTOR_MIX         =MOTOR_MIX_m;      //Motor select state recovery
    MOTOR_CYCLE       =MOTOR_CYCLE_m;
    MOTOR_MODE        =MOTOR_MODE_m;
    MOTOR_CENTER      =MOTOR_CENTER_m;
    
 		EnableInterrupts;
 }