C++ (Cpp) AngleCalculate Examples

Example #1

File: Events.c Project: jackyheng/galaxy2012

                         /* is set to 'yes' (#pragma interrupt saveall is generated before the ISR)      */
void TI1_OnInterrupt(void)
{
  oneMsCounter++;
  
  //------------------------------------------- 
    g_nspeedControPeriod ++;
  	speedControlOutput();
  //___________________________________________
  
  //-------------------------------------------
  
    g_nDirectionControlPeriod ++;
  	DirectionControlOutput();
  //___________________________________________
  
  if(oneMsCounter == 1)            
  {
     if(AD_Flag)    updataSensor();  		       //读取ADC的值
  }
//--------------------------------------------------  
  else if(oneMsCounter == 2)       
  {
    AngleCalculate();              //计算角度
  	AngleControl();                //角度的PD值控制  	 
  	motorControl();                //电机输出控制		
  }
//-----------------------------------------------  
  else if(oneMsCounter == 3)       
  {  
    g_nSpeedControlCount++;
    oneHundredMsCounter++;
    
    if(g_nSpeedControlCount >= SPEED_CONTROL_COUNT)
    {
      speedControl();               //速度控制
      g_nSpeedControlCount=0;
      g_nspeedControPeriod=0;	
    }
    
    if(standFlag){
        if(oneHundredMsCounter == 100)         
        {
          if(CAR_SPEED_SET < speedNeed3)
              CAR_SPEED_SET += 10;
          else
          {
              CAR_SPEED_SET = speedNeed3;
              speedStarEndFlag = 1;
          }   
          oneHundredMsCounter = 0;         
        }
    }
    else
    {
        CAR_SPEED_SET = 0;
        oneHundredMsCounter = 0;
        speedStarEndFlag = 0;
    }
  }
//--------------------------------------------------  
  else if(oneMsCounter == 4)       
  {
      g_nDirectionControlCount ++;
      DianciCalculate();
      DirectionVoltageSigma();
      if(g_nDirectionControlCount >= DIRECTION_CONTROL_COUNT) {
          DirectionControl();
          g_nDirectionControlCount = 0;
          g_nDirectionControlPeriod = 0;
      }  	  
  }
//-------------------------------------------------  
  else if(oneMsCounter >= 5)
  {
  	oneMsCounter = 0; 
  	GetMotorPulse();
  	UartFlag = 1;
  }
}

Example #2

File: PIT_ISR.c Project: Chasingwhat/Self-Balancing-Car

void PIT_Signal(void)
{   
  DisableInterrupts;
  
  //--------------------------------------------------------------------------
  g_nSpeedControlPeriod++;
  SpeedControlOutput();  
  g_nDirectionControlPeriod ++;
  DirectionControlOutput();
  //---------------------------------------------------------------------------  
  g_nCarCount++;//中断服务片选
  switch(g_nCarCount)
  {
    //---------------------------计数：0   
  case 1:
   {   
      asm("nop"); 
      
   }; break;
    
     //---------------------------计数：1   AD采集                   第1个1ms  
  case 2:          
   {
      asm("nop");
      AngleCalculate(); 
      g_nDirectionGyro = (int16)LPLD_ADC_Get(ADC0, AD11); //方向陀螺仪采集
   }; break;
    
    //----------------------------计数：2   直立控制 电机PWM输出     第2个1ms
  case 3:
    { 
       asm("nop");
       AngleControl();
       MotorOutput();
    };  break;
    
    //----------------------------计数：3    车模速度控制            第3个1ms
  case 4:
    {  
       asm("nop");
       g_nSpeedControlCount++;  
      if(g_nSpeedControlCount>=20)        //控制是否进入速度更新环节
      { 
        SpeedControl();
        g_nSpeedControlCount = 0;
        g_nSpeedControlPeriod = 0; 
      }
    };break;
    
    //----------------------------计数：4    车模方向控制           第4个1ms
  case 5:   
    {  
      asm("nop");
      g_nCarCount=0; 
      
      g_nDirectionControlCount ++;
      DirectionVoltageSigma();           //电感采集、滤波                                             
      
      if( g_nDirectionControlCount >= 2)//控制是否进入方向更新环节
       {
          DirectionControl();
          g_nDirectionControlCount = 0;
          g_nDirectionControlPeriod = 0;
       }
    };break;
  }
  
  
  EnableInterrupts;
}