#include "CTRL.H"
#include "string.h"
#include "test.h"
#include "modbus.h"
#include "task.h"
#include "EEPROM.H"
#include "DAC.H"
#include "math.h"
IED_DATABASE IED;
///////////////////////////////////////////////////////

                                //PF0
#define PORT(CLASS,PORTNO,DIR)          _PORT(CLASS,PORTNO,DIR)
#define _PORT(CLASS,PORTNO,DIR)         P##CLASS##PORTNO##_##DIR

//#define PIN_POS_RIGHT                   PORT(F,POS_RIGHT,O)//
//#define PIN_POS_LEFT                    PORT(F,POS_LEFT,O) //

#define PIN_MANUAL_CTRL                 PORT(F,PIN_MANUAL,I) 


#define MOTOR_SAMPLE_DIR_BIT            PD0_O 
#define MOTOR_SAMPLE_RUN_BIT            PD1_O   

#define PIN_DJ                          PD2_O
#define PIN_SPEAKER                   PD3_O 

#define MOTOR_SAMPLENO_DIR_BIT        PD4_O
#define MOTOR_SAMPLENO_RUN_BIT        PD5_O 

#define PIN_JB                         PD6_O
#define PIN_PUMB                    PD7_O

#define  POS_LEFT_PIN       POS_LEFT__PIN()   //PORT(F,POS_LEFT,I)
#define  POS_RIGHT_PIN      POS_RIGHT__PIN() //PORT(F,POS_RIGHT,I)  

#define  SPK                PD3_O       
#define  SAMPLE_POS_1_PIN   PORT(F,SAMPLE_POS_1,I)  
#define  SAMPLE_POS_N_PIN   PORT(F,SAMPLE_POS_N,I) 
/////////////////////////////////////////////////////////////
#define MOTOR_CHECK_PRIOD     50L
#define POS_CHECK_PRIOD       2L
void GetCurretPos(void);   
void StopMotor(void);        
///////////////////////////////////////////////////////        
void InitPort(void)
{   
    PD3_O = 1;  //MUTE    
    PA0_O = 1;
    PA1_O = 1;  
    
    POUT = 1;//加热口
   // TODO:初始化 输出口
}

/////////////////////////////////////////////////////////////       
uint32_t POS_LEFT__PIN(void) 
{
    if(IED.DownloadWorkParam.bAutoCly)
        return PORT(F,POS_LEFT_AUTO,I); 
    else                               
        return PORT(F,POS_LEFT_MANUAL,I);  
}

uint32_t POS_RIGHT__PIN(void) 
{
	uint32_t pin = 0;
    if(IED.DownloadWorkParam.bAutoCly) 
        pin =  PORT(F,POS_RIGHT_AUTO,I); 
    else                               
        pin =  PORT(F,POS_RIGHT_MANUAL,I); 
	
	//低电平到位
	return pin;
}
/////////////////////////////////////////////////////////////

unsigned char BUF[2038];
void InitIED(void)
{       
	int i;
	USHORT *pEncPos = NULL;
	int MaxSample = 24;
	int EncodeCnt = 2400*2/MaxSample;
    uint8  in;
    memset(&IED,0,sizeof(IED)/sizeof(char));
    IED.cID = 11;                       //测硫仪的ID为11 开始   
    //样车运行时间          
    IED.DownloadWorkParam.cRightToMiddle1     = 400;       //放样位到预分解1
    IED.DownloadWorkParam.cRightToMiddle2     = 600;       //放样位到预分解2
    IED.DownloadWorkParam.cRightToLeft        = 800;      //放样位到高温位
     

//    IED.DownloadWorkParam.cMoveTimeReserve    = 0;        //保留
    //时间    
    IED.DownloadWorkParam.cTimeDecompose1     = 100;      //分解时间1            
    IED.DownloadWorkParam.cTimeDecompose2     = 100;      //分解时间2
    IED.DownloadWorkParam.cTimeDelay          = 120;      //结束延时

    IED.DownloadWorkParam.cTimeoutSamplePosN  = 5000;        //5s检测一次
    IED.DownloadWorkParam.cTimeoutSamplePos1  = 5000;    
    
 //   IED.DownloadWorkParam.cDecompTimeReserve  = 0;        //保留   
    ///

    IED.DownloadWorkParam.sMaxTestTime        = MAXTESTTIME;
    IED.DownloadWorkParam.cCoolTime           = DEFAULTCOOLTIME;  //冷却10S
    IED.DownloadWorkParam.MaxSampleCnt        = DEFAULT_MAX_SAMPLE_COUNT;   //默认最大值
    IED.DownloadWorkParam.m_ResetPos            = 1;
    
    IED.DAC_Param.Ka = DEFAULT_K;  //斜率
    IED.DAC_Param.C  = DEFAULT_C;   //节距
    //#define DEFAULT_K  1.0f
    //#define DEFAULT_C  0. 
    IED.MotorCtrl.MotorSampleRunTime     = IED.DownloadWorkParam.cRightToLeft*TICKCOUNT/10L/MOTOR_CHECK_PRIOD;
    IED.Freq.DJSwitch                    = 1;
    strcpy(IED.InstrumentName,"TF_CLY-2011");
    //strcpy(IED.Endian.c,"12345678");
    memset(&IED.AdValue,0,sizeof(IED.AdValue));      
    getCoilVal(0,&in);
    //for(i=0;i<sizeof(BUF);i++)
    //    BUF[i] = i%0xFF;
    //EEWriteStruct(BUF,sizeof(BUF),2048+10);
    //while(1);
	//初始化编码器位置
	pEncPos = IED.POSEncode.SamplePos;
	
	for(i=0;i<24;i++)
	{
		pEncPos[i]=EncodeCnt*i+110;
	}
	
	IED.PosCtrl.EncodePos = 0;
	
    ReadIdFromEE(); 
    if(ReadDACCoffFromEE()==0)
    {
        WrtieADCCoffToEE();
    }
}
//////////////////////////////////////////////////////////////
void RunMotor(void* param)
{ 

    MOTOR_SAMPLE_RUN_BIT            = 1;  
    IED.MotorCtrl.cMotorDlyTimer    = -1;
    IED.MotorCtrl.cSampleMoving     = 1;
    ActiveTask(TASK_STOPMOTOR,MOTOR_CHECK_PRIOD);         //每10ms检测一次电机是否到位
    
}

//////////////////////////////////////////////////////////////                                                                            
SCHAR MovMotor(UCHAR cPos)
{ 
    GetCurretPos();    
    if(POS_UNKNOWN==cPos)
    {
        #ifdef _DEBUG
        printf("紧急停车\n");
        #endif           
        StopMotor();
        return -1;
    }
    if(IED.MotorCtrl.cSampleMoving==1)
    {
        #ifdef _DEBUG
        printf("正在运动!\n");
        #endif   
        return -1;
    }
    if(cPos==IED.MotorCtrl.cPos)
    {
        #ifdef _DEBUG
        printf("当前位置与目标位置相同\n");
        #endif   
        return -2;
    }
    if((POS_RIGHT_PIN==0)&&(POS_LEFT_PIN==0))//过热保护
    {
        #ifdef _DEBUG
        printf("高温位%d,放样位%d",POS_LEFT_PIN,POS_RIGHT_PIN);
        printf("霍尔元件损坏\n");
        #endif                       
         return -3;
    }  
    if(IED.PosCtrl.cSamplePosMoving==1)
    {
        #ifdef _DEBUG
        printf("正在运动!\n");
        #endif   
        return -4;
    }                            
    MOTOR_SAMPLE_RUN_BIT = 0; 
    switch(cPos)
    {
         case POS_PUT_SAMPLE:                                      //到放样位,方向为向右
            IED.MotorCtrl.cSampleMotorDir         = MOTOR_MOVE_RIGHT;
            IED.MotorCtrl.MotorSampleTimeOutCnt   = 0;                     //到最右部,超时时间为0 
            IED.MotorCtrl.MotorSampleRunTime     += 0;       
            break;
        case POS_HIGH_TEMP:
            IED.MotorCtrl.cSampleMotorDir         = MOTOR_MOVE_LEFT;         //到底,方向为向下
            IED.MotorCtrl.MotorSampleTimeOutCnt   = IED.DownloadWorkParam.cRightToLeft*TICKCOUNT/10L/MOTOR_CHECK_PRIOD;        //到底部,超时时间为TimeTopToBot     
            break;
        case POS_MIDDLE_1:    
            IED.MotorCtrl.MotorSampleTimeOutCnt = IED.DownloadWorkParam.cRightToMiddle1*TICKCOUNT/10L/MOTOR_CHECK_PRIOD;     
            if(IED.MotorCtrl.MotorSampleRunTime<IED.DownloadWorkParam.cRightToMiddle1*TICKCOUNT/10L/MOTOR_CHECK_PRIOD)            
                 {IED.MotorCtrl.cSampleMotorDir =  MOTOR_MOVE_LEFT;}
             else                                                     
                 {IED.MotorCtrl.cSampleMotorDir = MOTOR_MOVE_RIGHT ;}    
            break;
        case POS_MIDDLE_2:                        
            IED.MotorCtrl.MotorSampleTimeOutCnt = IED.DownloadWorkParam.cRightToMiddle2*TICKCOUNT/10L/MOTOR_CHECK_PRIOD; 
            if(IED.MotorCtrl.MotorSampleRunTime<IED.DownloadWorkParam.cRightToMiddle2*TICKCOUNT/10L/MOTOR_CHECK_PRIOD)
                 {IED.MotorCtrl.cSampleMotorDir = MOTOR_MOVE_LEFT;}
             else                                     
                 {IED.MotorCtrl.cSampleMotorDir = MOTOR_MOVE_RIGHT ;}                      
            break;  
        case POS_UNKNOWN:     
        default:
            #ifdef _DEBUG
            printf("目的地指示出错\n");
            #endif          
            break;
    }                                         
    MOTOR_SAMPLE_DIR_BIT = IED.MotorCtrl.cSampleMotorDir;
    __NOP();
    
    //启动电机到位检测任务            
    IED.MotorCtrl.cMotorDlyTimer = StartTimer(TIMER_MODE_ONCEROUTINE,10,RunMotor,0);                       //延时启动定时器

    #ifdef _DEBUG                                                 
    printf("启动电机!\n");
    #endif           

    return 0;
}
/////////////////////////////////////////////////////////////////////

void GetCurretPos()
{       
    {     
        if(POS_RIGHT_PIN==0)
        {
            IED.MotorCtrl.cPos                      = POS_PUT_SAMPLE;
            IED.MotorCtrl.MotorSampleRunTime        = 0;
        }
        else if(POS_LEFT_PIN==0)
        {
            IED.MotorCtrl.cPos                      =   POS_HIGH_TEMP;        
        }
        else
        {
            IED.MotorCtrl.cPos=POS_UNKNOWN;
        }
    }
    #ifdef _DEBUG
    printf("Current POS is %d\n",(int)IED.MotorCtrl.cPos);
    #endif
     
}
////////////////////////////////////////////////////////////////////
//检测是否到位,到位后停止电机
#define TIME_OVERROLL   10000L //升降误差10s 
void TaskStopMotor()                                //TASK_MOTOR
{                        
    if(((IED.MotorCtrl.cSampleMotorDir==MOTOR_MOVE_LEFT)&&(POS_LEFT_PIN==0))        //运行方向向右,检测到右到位 停止
        ||((IED.MotorCtrl.cSampleMotorDir==MOTOR_MOVE_RIGHT)&&(POS_RIGHT_PIN==0))    //运行方向向左,检测到左到位 停止
        ||((POS_LEFT_PIN==0)&&(POS_RIGHT_PIN==0)))                            //两个同时接通    损坏
    {
                                  
        #ifdef _DEBUG        
        printf("电机到位\n左到位=%d,右到位=%d,继电器释放!\n",(int)POS_LEFT_PIN,(int)POS_RIGHT_PIN);
        #endif
        StopMotor();

    }
    //位置检测            
    GetCurretPos();
    //
    if(IED.MotorCtrl.cSampleMotorDir==MOTOR_MOVE_RIGHT)              //向下,计时递减
    {
        IED.MotorCtrl.MotorSampleRunTime--;                      //  
        if(IED.MotorCtrl.MotorSampleRunTime<=0L
                                            -TIME_OVERROLL/MOTOR_CHECK_PRIOD)                        //到顶
        {
            //IED.MotorCtrl.cPos = POS_PUT_SAMPLE ; 
            StopMotor();  
            Beep(10,100);   //超时警告   
			
			
			strcpy(IED.ErrorInfo,"退样未到位");
			IED.cTestState = TEST_ERROR;
        }
    }
    else                                  //向上,计时递增
    {
        IED.MotorCtrl.MotorSampleRunTime++;  
        if(IED.MotorCtrl.MotorSampleRunTime>=IED.MotorCtrl.MotorSampleTimeOutCnt)
                                           // + TIME_OVERROLL/MOTOR_CHECK_PRIOD)
        {
            IED.MotorCtrl.cPos = POS_HIGH_TEMP ;  
            StopMotor();  
            Beep(1,100);   //超时警告   
        }        
    } 
    //////////////////////////////////////////////////////////
    //超时停止    
    if(IED.MotorCtrl.MotorSampleRunTime==IED.MotorCtrl.MotorSampleTimeOutCnt)       //超时时间到.停止
    {            
        //根据时间判断电机所在位置           
        if(IED.MotorCtrl.MotorSampleTimeOutCnt==IED.DownloadWorkParam.cRightToMiddle1*TICKCOUNT/10L/MOTOR_CHECK_PRIOD)      //计时时间为中位时间
        {
            IED.MotorCtrl.cPos = POS_MIDDLE_1 ;                //中位反馈;   
            __NOP();                                                    
            StopMotor();      
        }            
        else if(IED.MotorCtrl.MotorSampleTimeOutCnt==IED.DownloadWorkParam.cRightToMiddle2*TICKCOUNT/10L/MOTOR_CHECK_PRIOD)
        {        
                //计时时间为中位时间
            IED.MotorCtrl.cPos = POS_MIDDLE_2 ;                //中位反馈;                                 
            __NOP(); 
            StopMotor();      
        } 
    } 
                                                                                             
}
                          
void StopMotor(void)
{ 
    MOTOR_SAMPLE_RUN_BIT = 0;    
    StopTimer(&IED.MotorCtrl.cMotorDlyTimer);
    SuspendTask(TASK_STOPMOTOR);
    IED.MotorCtrl.cSampleMoving = 0;
    #ifdef _DEBUG
    printf("电机停止 !\n");      
    #endif    
      
}



void CommandCmd()
{
	//IED.Cmd
	char cmd 	= IED.Cmd.CmdParam1;
	int* Param = (int*)IED.Cmd.CmdReserve;
	switch(cmd)
	{
		case CMD_MOVEMOTORSAMPLE_ENCODEPOS: //走1/20样位置	 // 1/20*200       // 1/4样位置     *200
			MoveSampleEncodePos(Param[0]);
			break;
		case CMD_MOVEMOTORSAMPLE_ENCODEPOS_STOP://停车
			StopSampleMotor();
			break; 
	}
	
}
void sinvol(void* param)
{
    static int i = 0;
    //DACOut(i++); 
    IED.Freq.DJSwitch = 1;    
    DACOut(i==1?0:20);
    //i=i%0x7FF;
    i=++i%2;
}

void TaskParsaCmd(void)
{                           
    uint8_t nID;
    double mA[2];
    static double premA[2];
    
    switch(IED.Cmd.CmdType)
    {
            case CMD_NAK:
                __NOP();
                break;              //无命令
            case CMD_SETID:                                          
                nID = IED.Cmd.CmdParam1;          
                if(nID!=IED.cID) 
                {
                    IED.cID = nID;
                    SaveIdToEE(IED.cID);  //char 按long写入FLASH,虽然会写过界,但是不要紧
                }
                __NOP();    
                break;              //设置ID
            case CMD_SETPW:
                __NOP();
                break;              //设置密码
            case CMD_LOGON:
                __NOP();
                break;              //登录
            case CMD_SETDATETIME:
                __NOP();
                break;              //设置时间
            case CMD_READDATETIME:
                __NOP();
                break;              //读取时间
            case CMD_RESET_INST:         
                IED.cResetState = RESET_INIT    ;
                ActiveTask(TASK_RESET,1000)  ;
                break;              //复位设备
            case CMD_MOVEMOTORSAMPLE:  //移动样车           param1 放样位0 中间位1 中间位2 高温位3
                MovMotor(IED.Cmd.CmdParam1);
                __NOP();
                break;              
            case CMD_MOVEMOTORSAMPLENO: //移动样品转盘       PARAM  目标样位
                __NOP();
				if(IED.Cmd.CmdParam1>0)
				{					
					MoveSamplePos(IED.Cmd.CmdParam1);  
				}			
				else
				{
					StopSamplePosMotor( );  
				}
                __NOP();
                break;                  
            case CMD_START_MEASURE:    //启动测试
                IED.cTestState=TEST_INIT2;
                if(IED.DownloadWorkParam.bAutoCly)
                    MoveSamplePos(IED.Cmd.CmdParam1);
                ActiveTask(TASK_TEST,TEST_SCAN_PERIOD)  ;
                __NOP();
                break;              
            case CMD_STOP_MEASURE:   //停止测试  
                StopMotor();
                MovMotor(POS_PUT_SAMPLE);
                SuspendTask(TASK_TEST)  ;                     
                SuspendTask(TASK_DJ)  ;
                StopDJ();
                MovMotor(POS_PUT_SAMPLE);
                StopTimer(&DecompseDelayTimer);
                IED.cTestState=0;
                break;              
            case CMD_ELE:
                __NOP();
                if(IED.Cmd.CmdParam1)
                {
                    int Current = (((int)IED.Cmd.CmdParam2)<<8)+IED.Cmd.CmdParam3;
                    //if(Current==200)
                    //    Current = 100;
                    //StartTimer(TIMER_MODE_CYCROUTINE,10,sinvol,0);                    
                    StartDigitalDJ(Current);
                    //DACOut(4);                    
                }
                else
                    StopDJ();
                break;              //启动300mA电解       param1 1 0
            case CMD_WARMUP:
                __NOP();
                CtrlStove(IED.Cmd.CmdParam1, 
                        &IED.DownloadWorkParam.lDestTempADValue,
                        &IED.DownloadWorkParam.m_WarmSpeed);   
                __NOP();
                break;              //温度升温            param1 1 0            
            case CMD_ADJ_CUR:
                memcpy(mA,IED.Cmd.CmdReserve,sizeof(mA)); 
                if(memcmp(premA,mA,sizeof(mA))!=0)
                {
                    Cal_K_And_C(mA[0],mA[1]);        
                    //保存校准系数
                    WrtieADCCoffToEE(); 
                    memcpy(premA,mA,sizeof(mA));
                }
                break;
			case COMMAND_CMD:
				CommandCmd();
				break;
            default:
                __NOP();
                if(IED.Cmd.CmdType&0x80)
                {
                    __NOP();
                    #ifdef _DEBUG
                        //printf("命令已经执行\n");
                    #endif   
                }
                else
                {
                    __NOP();
                    #ifdef _DEBUG
                        printf("命令下载错误,不予执行");
                    #endif
                }
                break;
    }
    IED.Cmd.CmdType |=0x80;

    
}


void JB(char bEnable)
{    
    IED.OutputCoil.iCoil8s.DC_JB    = bEnable ;  
    PIN_JB                          = bEnable;  
}


void Pumb(char  bEnable)
{
    IED.OutputCoil.iCoil8s.DC_PUMB  = bEnable;
    PIN_PUMB                                = bEnable;
}


/****************************************************************************
    任务：停止样盘移动
*****************************************************************************/    

unsigned char   Pre_Sample_Pos_N_Pin  = 0; 
static int32_t MotorTimeout = 0;
///根据光电编码器停止旋转
void StopSampleMotor(void)
{ 
	MOTOR_SAMPLENO_RUN_BIT           = 0;     
	IED.PosCtrl.cSamplePosMoving     = 0;  
	SuspendTask(TASK_STOP_SAMPLEPOS2);      //停止“停止样盘移动”任务  
	SuspendTask(TASK_STOP_ENCODE_POS);      //停止“停止样盘移动”任务  
}


///根据光耦判位置停止旋转
void StopSamplePosMotor(void)
{
	MOTOR_SAMPLENO_RUN_BIT           = 0;     
	IED.PosCtrl.cSamplePosMoving     = 0; 
	SuspendTask(TASK_STOP_SAMPLEPOS);      //停止“停止样盘移动”任务 
	SuspendTask(TASK_STOP_ENCODE_POS);      //停止“停止样盘移动”任务  
	//if(MotorTimeout==0)  
	//    IED.PosCtrl.cSamplePos=IED.PosCtrl.cDestPos;
}

 
static int32_t MAX_ENCODE_ERROR = 20;
static int32_t destEncodePos = 0;

int GetCurrentSamplePos()
{
	int Error1 =0; 
	int maxcount = MAX_SAMPLE_COUNT;
	int cSamplePos = 0;
	int i = 0;
	uint32_t EncodeCnt  = 0;
	if(!EncodeInitOK())
		return cSamplePos;
	
	EncodeCnt = GetEncodeCnt();
	for(i =0;i<maxcount;i++)  
	{
		Error1 = EncodeCnt-IED.POSEncode.SamplePos[i];
		if( fabs(Error1)<MAX_ENCODE_ERROR)
		{
			cSamplePos = i+1;//
			break;
		} 
	}
	
	return cSamplePos;
}

void TaskStopSamplePos2(void)
{  
	int destPos = IED.PosCtrl.cDestPos;  
	int maxcount = MAX_SAMPLE_COUNT; 
	MotorTimeout--;
 
	if(EncodeInitOK()) //尚未初始化,一直转
	{  	//样品位置从1 开始
		int pos = 0;
		int i = 0;
		//已经到位 
		pos =  GetCurrentSamplePos();
		if(pos!=0)
		{
			IED.PosCtrl.cSamplePos   = pos;
			if(pos==destPos)
			{
				StopSampleMotor(); 
				IED.PosCtrl.cSamplePos = destPos;//
			} 
		}
	}
	
	if(MotorTimeout<0)  //时间到0,重新初始化
	{
		IED.PosCtrl.cSamplePos = 0;
		Beep(20,1000); //长鸣
		EncodeReInit();
		StopSampleMotor();
		IED.cTestState = TEST_ERROR;
		strcpy(IED.ErrorInfo,"转盘被卡住");
	} 
}


void TaskStopSamplePos(void)
{ 
    //IED.PosCtrl.iTimeout--; 
    MotorTimeout    --;
    ///////////////////////////////////////////////////////////////////////////////////////////////////    
    if((IED.PosCtrl.cDestPos!=1)&&(IED.PosCtrl.cSamplePos!=0))   //复位样盘到第一样位
    {             
        if((Pre_Sample_Pos_N_Pin==1) &&(!SAMPLE_POS_N_PIN))                     //检测到一个下降沿
        { 
            IED.PosCtrl.cSamplePos = IED.PosCtrl.cSamplePos%MAX_SAMPLE_COUNT+1; 
            #ifdef _DEBUG
            printf("Pulse Detected!\n")    ;
            printf("cSamplePos=%d,cDestPos=%d!\n",IED.PosCtrl.cSamplePos,IED.PosCtrl.cDestPos)    ;
            #endif
        }
    } 
    //if((!SAMPLE_POS_N_PIN)&&(!SAMPLE_POS_1_PIN))     //样位1 //样位N亮 1不亮
    if(!SAMPLE_POS_1_PIN)
    {     
        IED.PosCtrl.cSamplePos = 1;    
    }
    /////
    Pre_Sample_Pos_N_Pin =      SAMPLE_POS_N_PIN; 
    //////////////////////////////////////////////
    if((IED.PosCtrl.cSamplePos==IED.PosCtrl.cDestPos )
        ||(MotorTimeout<=0))
    { 
		StopSamplePosMotor();
    }   
}


//***************************************************************************
//    移动样盘到目标位,自动计算超时,cPos-移动目标
//****************************************************************************

int MoveSamplePos(int _cPos)
{ 
	int delayTime = 100;
    int n;
    int pin = POS_RIGHT_PIN;
	
	int cPos = _cPos;
	int CurrentPos = GetCurrentSamplePos();
    
    //IED.PosCtrl.cDestPos = cPos;
    if(!SAMPLE_POS_1_PIN)   //防止开机1号位走动
    {     
        //IED.PosCtrl.cSamplePos = 1;    
    } 
	
    if(IED.PosCtrl.cSamplePosMoving)           //
        return -1;    
    if(IED.MotorCtrl.cSampleMoving)           //
        return -2;               
    if(cPos==IED.PosCtrl.cSamplePos)           //位置和目标位置相同,不移动
        return -3;
	
	
    if(POS_RIGHT_PIN)                             //样杆不在放样区不能移动样盘
	{
		Beep(1,1000);
        return -4;
	} 
    if(!IED.DownloadWorkParam.bAutoCly)     //不是自动硫
        return -5;
    IED.PosCtrl.cDestPos = cPos;
    //cPos = cPos>0?(cPos-1)%MAX_SAMPLE_COUNT+1:MAX_SAMPLE_COUNT; 
    cPos = (cPos+MAX_SAMPLE_COUNT-1)%MAX_SAMPLE_COUNT+1;
    if(cPos==MAX_SAMPLE_COUNT)
	{
		_nop_();
	}
	
	if(CurrentPos==cPos)
	{
		return -6;
	}
	
	
    IED.PosCtrl.cSamplePosMoving = 1;

    if(cPos>IED.PosCtrl.cSamplePos)
    {
        n=cPos-IED.PosCtrl.cSamplePos;
    }
    else
    {
        n=cPos+MAX_SAMPLE_COUNT-IED.PosCtrl.cSamplePos; 
    }
    MOTOR_SAMPLENO_DIR_BIT = 0;                        //正转 
    IED.PosCtrl.cDestPos = cPos;
    if((cPos==1)||(IED.PosCtrl.cSamplePos==0))
    {
        MotorTimeout = ((int32_t)IED.DownloadWorkParam.cTimeoutSamplePos1)                        //有多少个0.1S
                                *(TICKCOUNT/10/POS_CHECK_PRIOD);     //多少个样位  
    }
    else
    {
        MotorTimeout     = ((int32_t)IED.DownloadWorkParam.cTimeoutSamplePosN)* n*
                                (TICKCOUNT/10/POS_CHECK_PRIOD); 
    } 
    IED.PosCtrl.iTimeout = MotorTimeout>0xFFFF?0xFFFF:MotorTimeout; 
    MOTOR_SAMPLENO_RUN_BIT             = 1;  //能使样盘移动
    if(IED.PosCtrl.cMotorDlyTimer!=-1)
         StopTimer(&IED.PosCtrl.cMotorDlyTimer);
	//编码器启动不得<0.4s不然会错过
	if(cPos!=1)
	{
		delayTime = 2;   
	}
	else
	{ 
		delayTime = 100;   
	} 
	IED.PosCtrl.cMotorDlyTimer     = StartTimer(TIMER_MODE_ONCEROUTINE,delayTime,DelayCheckPos,0); 
    return 0;
    
}

 
void DelayCheckEncodePos(void* param)
{ 
    IED.PosCtrl.cMotorDlyTimer = -1; 
	ActiveTask(TASK_STOP_ENCODE_POS,1);      //启动“停止样盘移动”任务
}

void  MoveSampleEncodePos(int EncodePos)
{
    MOTOR_SAMPLENO_DIR_BIT	= 0;                        //正转 
    MOTOR_SAMPLENO_RUN_BIT	= 1;  //能使样盘移动
	IED.PosCtrl.cSamplePosMoving	= 1; 
	destEncodePos = EncodePos; 
	
	MotorTimeout = ((int32_t)IED.DownloadWorkParam.cTimeoutSamplePos1)                        //有多少个0.1S
                                *(TICKCOUNT/10/POS_CHECK_PRIOD);     //多少个样位  
	ActiveTask(TASK_STOP_ENCODE_POS,1);      //启动“停止样盘移动”任务
	
    if(IED.PosCtrl.cMotorDlyTimer!=-1)
         StopTimer(&IED.PosCtrl.cMotorDlyTimer); 
	//20ms后检测
    IED.PosCtrl.cMotorDlyTimer     = StartTimer(TIMER_MODE_ONCEROUTINE,2,DelayCheckEncodePos,0);    
} 

void TaskStopSampleEncodePos(void)
{
	int Error2; 
	int EncodeCnt = GetEncodeCnt(); //获取编码器位置
	MotorTimeout--;
 
	if(EncodeInitOK()) //尚未初始化,一直转
	{  
		Error2 = EncodeCnt-destEncodePos;
		if(fabs(Error2)<MAX_ENCODE_ERROR)
		{ 
			StopSampleMotor();
			__NOP();
		}   
	}
	
	if(MotorTimeout<0)  //时间到0,重新初始化
	{
		IED.PosCtrl.cSamplePos = 0;
		Beep(1,1000); //长鸣
		EncodeReInit();
		StopSampleMotor();
	} 
}
  
void DelayCheckPos(void* param)
{ 
    IED.PosCtrl.cMotorDlyTimer = -1;
    //ActiveTask(TASK_STOP_SAMPLEPOS,POS_CHECK_PRIOD);      //启动“停止样盘移动”任务
	ActiveTask(TASK_STOP_SAMPLEPOS2,POS_CHECK_PRIOD);      //启动“停止样盘移动”任务
}

//************************************************************************
//    任务：手动进位，按进位按钮一次，进1个样位
//*************************************************************************
void TaskManualMovePos(void)
{    
    int pin =     PIN_MANUAL_CTRL;         
    UCHAR b;
    static char preManual  =1;  
    if(!IED.DownloadWorkParam.bAutoCly)
        return;
    if((preManual==0)&&(PIN_MANUAL_CTRL==1)&&(!IED.PosCtrl.cSamplePosMoving))
    {            
        if(IED.PosCtrl.cSamplePos!=0)
        {    
            b=IED.PosCtrl.cSamplePos+4;
            MoveSamplePos(b);             
        }
        else
        {
            MoveSamplePos(1);
        }
    }
    preManual = PIN_MANUAL_CTRL; 
    
}



void TaskReset(void)
{
 
    __NOP();
    switch(IED.cResetState)
    {
        case RESET_INIT:
            MovMotor(POS_PUT_SAMPLE);
            CtrlStove(0,&IED.DownloadWorkParam.lDestTempADValue,
                        &IED.DownloadWorkParam.m_WarmSpeed); 
            Beep(1,500);
            JB(0);
            Pumb(0); 
            DACOut(0);
            IED.cResetState = RESET_POS;
            break;
        case RESET_POS:                                       
            if((IED.MotorCtrl.cPos == POS_PUT_SAMPLE)
                &&(IED.MotorCtrl.cSampleMoving==0))
            {            
                    if(!IED.DownloadWorkParam.bAutoCly)
                        IED.cResetState = RESET_END;
                    else
                    {                                                   //移动到放样位
                        IED.cResetState = RESET_SAMPLE_POS;
                    } 
            }
            break;
        case RESET_SAMPLE_POS:                                //复位到转盘1号位
            if(IED.MotorCtrl.cPos == POS_PUT_SAMPLE)
            {
                MoveSamplePos(IED.DownloadWorkParam.m_ResetPos);
                IED.cResetState = RESET_SAMPLE_POS_END;
            }                                  
            break;
        case RESET_SAMPLE_POS_END:   
            if((IED.PosCtrl.cSamplePos == IED.DownloadWorkParam.m_ResetPos)
                &&(IED.PosCtrl.cSamplePosMoving == 0))
            {  
				IED.cResetState = RESET_END ;
				IED.cTestState = TEST_NONE;
            }
            break;
        case RESET_END:
        default:                              
            SuspendTask(TASK_RESET)  ;
            break;

    }
    
}

unsigned char gcBeepTrig = 0;
unsigned int gcNumBeep   = 0;
void TaskBeep(void)    
{
    if(gcBeepTrig)
    {
        SPK = 0;     
    }
    else
    {
        SPK = 1;                            
        //PD3_O = 1;
        
        gcNumBeep--;
        if(gcNumBeep==0)
        {
            SuspendTask(TASK_BEEP);
        }
    }
    gcBeepTrig=!gcBeepTrig;
}      
void Beep(UCHAR    cNumBeep,UINT beepTime)
{
    int    a;
    gcNumBeep=cNumBeep;
    gcBeepTrig=1;

    if(beepTime<100)
        beepTime = 100;
    a=beepTime;    //mS                  
    ActiveTask(TASK_BEEP,a);
}

//空闲进程不停读转盘位置
void IdleRoutine(void)
{ 
	IED.PosCtrl.EncodePos  = GetEncodeCnt(); //获取编码器位置
}