//******************************************************************************
//    FILE:       FUZZY.C
//    POPURSE:    仿人温度控制
//    WRITER:     xukaiming 
//    DATE:       2020.09.01
//******************************************************************************
  
#include <includes.h>  
#include "fuzzy.h"   
//-----------------------------
#define RTT_PRINT
#ifdef RTT_PRINT
#include "SEGGER_RTT.h"  
#endif
//-----------------------------
#define KC 1
#define Ki 2
#define KI (30*1)
#define Kp 1
 
#define MAXNEGINTERG -500//-1000
//----------------------------

//打印重定向 zhang comment


static char buff[64];  
int  __printf(const char* format,...)
{
   va_list args; 
   int ret =0;
   va_start(args, format); 
   ret =  vsnprintf(buff, sizeof(buff)-1,format, args);
   SEGGER_RTT_WriteString(0, buff);  
   va_end(aptr);
   return ret;
}




/************************************************************************
仿人智能控制器//Humanoid Intelligent Controller
************************************************************************/

 AI_CONTROL  Ai_CTRL;
static int i = 0;
void TaskFuzzy(void)
{    
  AI_CONTROL *pAi_CTRL = &Ai_CTRL;      
  i++;
  if(pAi_CTRL->WarmFlag)
  {
    pAi_CTRL->Error[1] = pAi_CTRL->Error[0];
    long temp =*pAi_CTRL->gTemp;
    pAi_CTRL->Error[0]  = ((pAi_CTRL->iDestTemp-temp )/KC);    //计算偏差 0.1度
    pAi_CTRL->dErr      = pAi_CTRL->Error[0]-pAi_CTRL->Error[1];           //计算偏差变化率												//计算偏差变化率    
    if(pAi_CTRL->Error[0]<pAi_CTRL->CTEMP)                                 
    {  //偏差进入了控温范围
      if(((pAi_CTRL->Error[0]*pAi_CTRL->dErr)>0) //偏差变大的情况
         || ((pAi_CTRL->Error[0]==0) 
             &&( pAi_CTRL->dErr!=0))
        )   
      { 
        pAi_CTRL->SumErrLimit +=pAi_CTRL->Error[0];   //误差变大的时候,求积分			
        if(pAi_CTRL->SumErrLimit<MAXNEGINTERG)	    //限幅,如果超过负的最大值, 保持为负的最大值
          pAi_CTRL->SumErrLimit=MAXNEGINTERG;
        //////////////////////////////////////////////////////////// 
        //Kp 变小的时候 ki要变大 
        pAi_CTRL->CurOut = pAi_CTRL->Error[0]*Kp+pAi_CTRL->SumErrLimit*Ki*Kp/KI;	//增益抑制模式	                         //				
      }
      else              								//开环保持模式
      {
        if(((pAi_CTRL->Error[0]*pAi_CTRL->dErr)<0)||(pAi_CTRL->dErr==0))	        //误差变小的情况
        {		   
          pAi_CTRL->CurOut =  pAi_CTRL->SumErrLimit*Ki*Kp/KI;                  
        }
        else
        {
          pAi_CTRL->CurOut = pAi_CTRL->PreOut;	
        }
      } 
    }
    else	
      pAi_CTRL->CurOut=PWM_PERIOD;      //全速升温
  
    //歸一化
    if(pAi_CTRL->CurOut<0) pAi_CTRL->CurOut=0; 
    if(pAi_CTRL->CurOut>PWM_PERIOD) pAi_CTRL->CurOut=PWM_PERIOD;  
    //--RTT DUBUG---------------------
 #ifdef RTT_PRINT 	 
    __printf("%ld,",pAi_CTRL->CTEMP);
    __printf("%ld,%ld,",pAi_CTRL->Error[0],pAi_CTRL->dErr);
    __printf("%ld,%ld\n" ,pAi_CTRL->SumErrLimit, pAi_CTRL->CurOut); 
 #endif  
    //---------------------------------
    //输出加热值
    SetPOut(pAi_CTRL->CurOut); 
    //保存上次输出
    pAi_CTRL->PreOut=pAi_CTRL->CurOut;	   
  }
  else
  {
    SetPOut(PWM_PITCED);
  } 
}     

void SetPOut(int Power)
{
    
    PIDOutStart(Power);
    //SEGGER_RTT_printf(0,"Tempra.%d\r\n",Power);
}
/*******************************************************************
    初始化控制数据变量
********************************************************************/
void InitFuzzy(void)
{      
    AI_CONTROL *pAi_CTRL = &Ai_CTRL;              
    memset(pAi_CTRL,0,sizeof(AI_CONTROL));
    StopHeat(); 
}

void StopHeat(void)
{ 
    AI_CONTROL *pAi_CTRL = &Ai_CTRL;    
    pAi_CTRL->WarmFlag = 0;
    SetPOut(PWM_PITCED);
}
     

/*********************************************************************
    启动加热
**********************************************************************/
void CtrlStove(u16 cOpen,u16 *CurTemp, u16 lDestTemp,u16  speed)
{    
    AI_CONTROL *pAi_CTRL = &Ai_CTRL;              
    //SuspendTask(TASK_FUZZY);
    InitFuzzy(); 
    pAi_CTRL->WarmFlag = cOpen;
    if(cOpen)       //启动
    {                       
        pAi_CTRL->gTemp           = CurTemp;                //当前温度
        pAi_CTRL->iDestTemp       = lDestTemp;              //  目标 温度值  
        pAi_CTRL->speed           = speed;                  //升溫速度，暫時未啓用
        pAi_CTRL->CTEMP           = 4000;                   //预控温400度 差值大於400度滿功率輸出
        pAi_CTRL->SumErrLimit     = 0;//1000*KI/Kp/Ki;          //預製2000的輸出
        //pAi_CTRL->SumErrLimit*Ki/KI*Kp;   
        //ActiveTask(TASK_FUZZY, PWM_SCAN);     
    #ifdef RTT_PRINT
     
        __printf("Ki = %ld,",Ki);  
        __printf("KI = %ld,",KI);
        __printf("Kp = %ld,",Kp);  
    #endif  
    }  
    else
    {
      StopHeat();
    }

}