main.c

ucos2 应用

/********************************************************************  
// Name         :  main.c
// Organization :  Hunan University
// Author       :  Huangzhi
// Date         :  2007.09.1
// Description  :  main routine for HCU
// Version      :  V1.0
// History      :
// date         author      version discription
// 2007.09.17   huangzhi    V1.1    add fault diagnose routines
********************************************************************/




#include <hidef.h>          /* common defines and macros */
#include "hcs12db128.h"     /* derivative information */
#include "GlobalDef.h"
#pragma  LINK_INFO DERIVATIVE "mc9s12db128b"  

// extern subroutines
extern  void    InitS12(void); 
extern  void    InitSCI(uchar Port, uint BaudRate) ;
extern  void    PrintString(uchar Port, char * pBuf);
extern  uchar   GetChar(uchar Port,uchar BufLen, char *pBuf) ;
extern  void    GetAD0Result(int * pADResult) ;
extern  void    GetAD1Result(int * pADResult) ;
extern  void    GetHSCounter(uint * pHSCounter) ;
extern  void    GetLSCounter(uint * pLSCounter) ;

// global subroutines 
void  ResetPumpDisp(void);
void  Brake(void);
void  Reverse(void);
void  Run4WD(void);               
void  Run2WD(void);
void  Disp2Zero(void) ;
void  CalEngineTrq(void);
void  SamplePressure(void);
void  SamplePumpDisp(void);
void  CalPumpDisp(void);
void  FaultDiag(void);

// global variables
int   Pressure[PUMP_NUM];
uint  MotorSpd[PUMP_NUM];
uint  EngineSpd;
int   EngineTrqAvail;
int   PumpTrqAvail;

char  CurWorkMode  = 0;  // 0: reset, 1: 4WD, 2: 2WD, 4:Braking, 8:Reverse
char  fMDC10ms  = 0;
char  fRTI2ms   = 0;
struct PUMP_CTL PumpCtl[PUMP_NUM];

/********************************** Main routine *****************************/
void main(void) {
  
  InitS12();                // Initialize S12 hardware
  DBG_MSG("Restarting...\r\n");
  
  DISABLE_START;         // disable engine start 
  
  EnableInterrupts;
  ResetPumpDisp();          // set pump's displacement to zero
  

  CurWorkMode = ST_4WD;
  SET_4WD;
  DBG_MSG("Entering 4WD mode!\r\n"); 
  
  for(;;) {
  
    if (fRTI2ms != 0) {
    
      SamplePumpDisp();
      fRTI2ms   = 0;      
    }
      
    if (fMDC10ms == TRUE){
    
      GetLSCounter(&EngineSpd);
      
      if (EngineSpd < ENGINE_IDLE_SPD)   // only enable starter when engine's speed 
        ENABLE_START;                // lower than idle speed
      else
        DISABLE_START;
     
      if (SW_BRAKE != RELEASE)
        Brake();
      
      else if (SW_BACK != RELEASE)
        Reverse();
      
      else if (CurWorkMode == ST_4WD)
        Run4WD();
      else
        Run2WD();
      
      fMDC10ms  = 0;
    }
  }
}

/****************************** Brake routine ******************************/
void Brake() {

  if (CurWorkMode != ST_BRAKE )
    DBG_MSG("Entering BRAKE mode!\r\n");
  
  CurWorkMode = ST_BRAKE; 
  Disp2Zero();
  return;
}

/****************************** Forward routine *****************************/
void ForwardRoutine(){
  
  if (EngineSpd < ENGINE_WORK_SPD)
    Disp2Zero();
  else{
    CalEngineTrq();
    SamplePressure();
    CalPumpDisp();    
  }
  return;
}

/****************************** Driven by 4 wheels *************************/
void  Run4WD() {

  if (SW_4WD == 0) {      // if 2WD mode is selected mannually
                          // change to 2WD mode after displacement has been set to 0.
    Disp2Zero();
    if ((PumpCtl[0].Cur == 0) &&
        (PumpCtl[1].Cur == 0) &&
        (PumpCtl[2].Cur == 0) &&
        (PumpCtl[3].Cur == 0)) {
        
        CurWorkMode = ST_2WD;
        SET_2WD;
        DBG_MSG("Entering 2WD mode!\r\n");
        } 
        return;
  }
  SET_4WD;
  ForwardRoutine();
}


/****************************** Driven by 2 wheels ********************************/

void Run2WD() {

  if (SW_4WD != 0){     // if 4WD mode is selected mannually
                        // change to 4WD mode after displacement has been set to 0  
    Disp2Zero();
    if ((PumpCtl[0].Cur == 0) &&
        (PumpCtl[1].Cur == 0) &&
        (PumpCtl[2].Cur == 0) &&
        (PumpCtl[3].Cur == 0)) {
        
        CurWorkMode = ST_4WD;
        SET_4WD;
        DBG_MSG("Entering 4WD mode!\r\n");
        } 
        return;
  }
  SET_2WD;
  ForwardRoutine();
}

/****************************** Run backwards ***********************************/

void Reverse() {

  uchar i;
  int   x,y;
  
  if (CurWorkMode != ST_REVERSE)
    DBG_MSG("Entering Back mode!\r\n");
  
  CurWorkMode = ST_REVERSE;
  SET_4WD;
  
  if (EngineSpd < ENGINE_WORK_SPD)
    Disp2Zero();
  else{
    for (i=0; i<PUMP_NUM; i++){
      
      x = PumpCtl[i].Dest;
      y = ENGINE_WORK_SPD - EngineSpd ; // engine speed: 1000 ~ 2500rpm
      if (y <  DISP_LO_LIMITS)
        y =  DISP_LO_LIMITS;
      x = (x + y -1)/2;                 // displacement: 0    ~ -100
      PumpCtl[i].Dest = x;   
    }
  }
}


/************************* reset pump's displacement *****************************/

void ResetPumpDisp(){
  
  uchar i;
  CurWorkMode = ST_RESET;
  for (i=0; i<PUMP_NUM; i++){
    
    PumpCtl[i].Cur  = 0;   
    PumpCtl[i].Dest = 0;                                      // displacement regulator will run forward until 
                                                              // reaches Up limits ,then return to zero
    PumpCtl[i].Spd  = MAX_SERVO_SPD; // full speed            
    PumpCtl[i].Count= 0x0;
    
  }
  
  while (fRTI2ms == 0) {
  ;
  }
  SamplePumpDisp(); 
  fRTI2ms   = 0; 

  DBG_MSG("Waiting for displacement reset to zero.\r\n") ;
  while ( (PumpCtl[0].Cur != 0) ||
          (PumpCtl[1].Cur != 0) ||
          (PumpCtl[2].Cur != 0) ||
          (PumpCtl[3].Cur != 0) ) 
  {
    if (fRTI2ms != 0) {
      fRTI2ms   = 0;      
      SamplePumpDisp();  
    }
  }            
}

/********************* Set destination displacement to 0 *************************/
void Disp2Zero() {
 
  uchar i;
  for (i=0; i< PUMP_NUM; i++){
    
    PumpCtl[i].Dest = 0;
    PumpCtl[i].Spd  = MAX_SERVO_SPD;
    PumpCtl[i].Count= 0;
  }
}


/********************* Calculate output torque of engine *************************/

void CalEngineTrq() {
  
  // output torque of engine depends on engine speed
  // engine speed scale : 0.1, 100/1000rpm
  if (EngineSpd < ENGINE_WORK_SPD) { 
    
     EngineTrqAvail = 0; 
     return;
  }
  else if (EngineSpd < ENGINE_SPD_P1) {
    
    EngineTrqAvail  = (EngineSpd - ENGINE_WORK_SPD)* 7;
    return;
  } else {
    EngineTrqAvail  = 785 - EngineSpd;
    if (EngineTrqAvail <0 )
      EngineTrqAvail  = 0;
    return;
  }           
}

/********************* Sample  oil pressure *************************/

void SamplePressure() {

  // pressure scope : 0~35Mpa, output signal: 1V~ 3.8V
  // A/D result is 12Bit resolution, A/D value of 1V is 819
  uchar i;
  GetAD0Result(Pressure) ;
  
  for (i=0; i< PUMP_NUM; i++){
    
    Pressure[i] -= 819;   //  adjust Pressure value to 0~2240 (3.8*819-819) corresponding to 0~35Mpa
                          //  Pressure scale = 65.5/Mpa
    if (Pressure[i] < 32) //  avoid overflow when used as divisor
      Pressure[i] = 32;
  }                       
}



/********************** Sample pump's displacement **************************/
void SamplePumpDisp() {

  // regulator range: -135 ~ 135 deg;  (30*4.5)
  // sensor range   : 45~ 340 deg
  // corresponding A/D result :(43 ~ 981)*4   12Bits 
  // subtract by offset : -1876~1876  
  // normalization coefficient is 18.7(56/3)
  #define   PUMP0_OFFSET  512*4
  #define   PUMP1_OFFSET  512*4
  #define   PUMP2_OFFSET  512*4
  #define   PUMP3_OFFSET  512*4
  
  
  int  PumpDisp[PUMP_NUM];
  char i;  

  GetAD1Result(PumpDisp) ;
  
   
  PumpDisp[0] -=  PUMP0_OFFSET;
  PumpDisp[1] -=  PUMP1_OFFSET;
  PumpDisp[2] -=  PUMP2_OFFSET;
  PumpDisp[3] -=  PUMP3_OFFSET;
  
  for (i=0; i<PUMP_NUM; i++)  {
    PumpDisp[i]       = (PumpDisp[i] * 3 )/56; 
    PumpCtl[i].Cur    = PumpDisp[i]; 
  }
  
}

/******************* calculate destination displacement *********************/

void CalPumpDisp(){

  PRIVATE int DispCmdFilter[PUMP_NUM]={0,0,0,0};
  uchar  i;
  int    x[PUMP_NUM];
  long   lx;                                        
  
  PumpTrqAvail  = (EngineTrqAvail+2)>>2;
  lx            = PumpTrqAvail;
  for (i=0; i< PUMP_NUM; i++) {         
  
    x[i] = (int)(lx * 735  * 4 / Pressure[i]); // multiply by 4 to reduce computation error 
    
    if (x[i] > DISP_UP_LIMITS *4)     
      x[i]  = DISP_UP_LIMITS *4;
    
    if (x[i] < 0)
      DBG_MSG("ERROR!\r\n") ;
  }
  
  
  
  
  for (i=0; i< PUMP_NUM; i++) {
    
    DispCmdFilter[i] = (DispCmdFilter[i] *7 +4 ) /8 ;
    
    DispCmdFilter[i] += x[i]; 
    x[i]          = (DispCmdFilter[i]+8) / 32;
    
    
    PumpCtl[i].Dest = x[i];
  }
}

/******************* fault diagnose rountin *********************/

void FaultDiag(){
  
  // failure case
  // 1.  tube break down
  // 2.  wheel skid
  // 3.  actuator failure
  // 4.      


}