/**************************************************************************/
//升降馬達狀態管理函數,在10ms定時中斷中調用
//函數名稱：UdStatusContrel (void)
//創建時間：080603
//說明：該程序為調試所用，若為實際運用時需要重新判斷限位和過流信號
/**************************************************************************/
#define UD_DRIVER
#include <includes.h>
#define  UPDOWN_DEATH_MAX_CNT    0 //馬達死區時間為30ms
UPDOWN_STATUS  UpDownStatus = UPDOWN_IDLE;
extern INT8U UDAimPst;//升降馬達目標位置，全局變量
extern INT8U UDAimSpd;//升降馬達目標速度，全局變量
extern INT8U IC4Cnt;//升降計數
extern INT8U Limit_D,Limit_U;//限位標志
extern INT8U UDNowPst;//升降馬達當前位置，局部變量

INT8U  UDDeathCnt;   //死區時間計時

void UdStatusContrel (void)
{
    switch (UpDownStatus)
    {
        case  UPDOWN_IDLE:
            {
                //if(UDNowPst>UDAimPst+CNT_UD_ERROR)
                if(UDNowPst>UDAimPst)
                {
                    UpDownStatus = UPDOWN_UP;  //上升 
                    IC4Cnt = 0;                               //計數器清零
                    IC4CONbits.ICM=2;       //下降沿觸發          
                }
                //else if (UDNowPst+CNT_UD_ERROR<UDAimPst)
                else if (UDNowPst<UDAimPst)
                {
                    UpDownStatus = UPDOWN_DOWN;  //下降 
                    IC4Cnt = 0;                               //計數器清零                    
                    IC4CONbits.ICM=3;       //上升沿觸發          
                }
                else 
                {
                   UpDownStatus = UPDOWN_IDLE;
                }
            }

            break;
        case  UPDOWN_DEATHCNT:
            if(UDDeathCnt>=UPDOWN_DEATH_MAX_CNT )   //死區等待時間到
            {
                UpDownStatus =  UPDOWN_IDLE;
                UDDeathCnt = 0;              
            }
            else if(UDDeathCnt<UPDOWN_DEATH_MAX_CNT )
            {
                UDDeathCnt ++;
            }
            break;
        case  UPDOWN_DOWN:
             if((Limit_D==1) || (MS13Sensor ==0)) //馬達到達下限位且未過流
            {
                UDNowPst = PB(0);//當前位置設為最大值
                UDAimPst = PB(0);
                IC4Cnt = 0xFF; //關閉升降計數器
                UDDeathCnt = 0;// 啟動停止計時
                //UpDownStatus =  UPDOWN_DEATHCNT; //設為死區等待階段
                UpDownStatus =  UPDOWN_IDLE; //設為空閑階段
                DownPwmSet = 0;//下降PWM清除,
                //發布下限位信息  
                OSMboxPost(MboxUD_Sur,YZ_MBOX_PST_ARRV);           
            }
            else//未過流且前后限位都未到達
            {
                if(IC4Cnt != 0xff)//及時更新位置信息
                {
                	if(UDNowPst<=0xFF-IC4Cnt)
                    	UDNowPst = UDNowPst+IC4Cnt;
                    else
                    	UDNowPst=0xFF;
                     IC4Cnt = 0;
                }
                if (UDNowPst >= UDAimPst)//當前位置〉=目標位置
                {
                	//if(UDNowPst <= UDAimPst+CNT_UD_ERROR)
                	if(UDNowPst == UDAimPst)
                    	//發布到位信息
            			OSMboxPost(MboxUD_Sur,YZ_MBOX_PST_ARRV);
                    IC4Cnt = 0xFF; //關閉升降計數器
                    UDDeathCnt = 0;// 啟動停止計時
                	//UpDownStatus =  UPDOWN_DEATHCNT; //設為死區等待階段
                	UpDownStatus =  UPDOWN_IDLE; //設為空閑階段
                    DownPwmSet = 0;//下降PWM清除,                    
                }
                else
                {
                    SETUP_DOWN(UDAimSpd);
                    UpDownStatus = UPDOWN_DOWN;  //下降
                }
             
            }

            break;
        case  UPDOWN_UP:
           if((Limit_U==1) || (MS10Sensor ==0)) //馬達到達上限位且未過流
            {
                UDNowPst = 0x01;//當前位置設為最大值
                UDAimPst = 0x01;
                IC4Cnt = 0xFF; //關閉升降計數器
                UDDeathCnt = 0;// 啟動停止計時
                //UpDownStatus =  UPDOWN_DEATHCNT; //設為死區等待階段
                UpDownStatus =  UPDOWN_IDLE; //設為空閑階段
                UpPwmSet = 0;//上升PWM清除,
                //發布上限位信息 
                OSMboxPost(MboxUD_Sur,YZ_MBOX_PST_ARRV);   
            }
            else//未過流且上下限位都未到達
            {
                if(IC4Cnt != 0xff) //及時更新位置信息
                {
                	if(UDNowPst>=IC4Cnt)
                    	UDNowPst = UDNowPst-IC4Cnt;
                    else
                    	UDNowPst=0x00;
                     IC4Cnt = 0;
                }
                if  ((UDNowPst!=0x00)&&(UDNowPst <= UDAimPst))//當前位置<=目標位置
                {
                	//if(UDNowPst+CNT_UD_ERROR >= UDAimPst)
                	if(UDNowPst== UDAimPst)
                    	//發布到位信息
            			OSMboxPost(MboxUD_Sur,YZ_MBOX_PST_ARRV);
                    IC4Cnt = 0xFF; //關閉升降計數器
                    UDDeathCnt = 0;// 啟動停止計時
                	//UpDownStatus =  UPDOWN_DEATHCNT; //設為死區等待階段
                	UpDownStatus =  UPDOWN_IDLE; //設為空閑階段
                    UpPwmSet = 0;//上升PWM清除,                    
                }
                else
                {
                    SETUP_UP(UDAimSpd);
                    UpDownStatus = UPDOWN_UP;  //上升
                }
             
            }
            break;
        default:
            break;
    }
}