int add_speed[200];
int aim_speed=Max_Speed;
int count_curve=0;//彎曲次數(shù)



//--------------------------------------------------------------------------------------
// 函數(shù)名稱： int DianJi_PID(int AD10)
// 函數(shù)功能： PID電機控制
//--------------------------------------------------------------------------------------
long dian_P=20  ,dian_I  ,dian_D=0  , speed_error0  ,speed_error1  ,speed_error2  ;
long DianJi_PID(long present_speed) 
{
  
  speed_error2=speed_error1;
  speed_error1=speed_error0;
  speed_error0=aim_speed-present_speed;
  return dian_P*(speed_error0-speed_error1)+dian_I*(speed_error0)+dian_D*(speed_error0-2*speed_error1+speed_error2);
}

long speed_PID=0;
long low_speed,count_low_speed;


//--------------------------------------------------------------------------------------
// 函數(shù)名稱： int DianJi_PID(int AD10)
// 函數(shù)功能： PID電機控制
//--------------------------------------------------------------------------------------
void Speed_Control() 
{
  static int pinl;
  static int last_pinl;
  static int count_break;
  
  if((PWME==0x0a)&&speed_break==1&&pinl>500)    //剎車  
  { 
    Speed_Brake(pinl);
    speed_break=0;
    count_break++;
      speed_PID=0; 
      speed_error2=0;
      speed_error1=0;
      speed_error0=0; 
      count_break=0;//put_str("-------------------------");
  }   
  
  
  if(time_up==1)     //測速
  {
    
    pinl=calculate_speed();
    
    //put_numb(pinl);//put_str("-");put_numb(aim_speed*10);put_str("-");put_numb(speed_PID);
   // put_str("\n\r");             
   //  put_numb(count_break);
  //  if(count_break!=0)
   // {
    /*  //PWMDTY23=0;
      speed_PID=0; 
      speed_error2=0;
      speed_error1=0;
      speed_error0=0;  count_break=0;put_str("-------------------------");*/
   // }
  Speed_Stead(pinl);  
   
    
    //if(pinl<=0&&last_pinl>2)PWME_PWME3=0;//停車
    
    
    last_pinl=pinl;
  }  
  
   
}


//--------------------------------------------------------------------------------------
// 函數(shù)名稱： void Speed_Brake(void)
// 函數(shù)功能： 
//--------------------------------------------------------------------------------------
void Speed_Brake(int pinl)
{
 /*if(pinl>1500)
  {    
    PWME=0x22;
    DELAY(45);//put_char('s');
    PWME=0x0a;
  
  }

  else   */
  {   
    PWME=0x22;
    DELAY(7);//put_char('s');
    PWME=0x0a;
    DELAY(6);
    PWME=0x22;
    DELAY(7);
    
    DELAY(7);
    PWME=0x0a;
    DELAY(6);
    PWME=0x22;
    DELAY(7);
    PWME=0x0a; 
  }

}   


//--------------------------------------------------------------------------------------
// 函數(shù)名稱： void Speed_Stead(voidlong speed_now)
// 函數(shù)功能： 
//--------------------------------------------------------------------------------------
void Speed_Stead(long speed_now) 
{
    long add;
    
    add=DianJi_PID(speed_now/10);
    speed_PID+=add; 
    if(speed_error0>2000)PWMDTY23=10000;
    else if(speed_error0>1500)PWMDTY23=9000;
    else if(speed_error0>1200)PWMDTY23=8000;
    else if(speed_error0>1000)PWMDTY23=5000;
    else if(speed_error0>800)PWMDTY23=4000;
    
    
    else if(speed_error0<-1000)PWMDTY23=0;
    else if(speed_error0<-800)PWMDTY23=300;
    else 
    {       
      if(speed_PID>10000)speed_PID=10000;
      else if(speed_PID<0)speed_PID=0;
      PWMDTY23=speed_PID;
    }