基于步进电机自平衡小车

2019-08-14 17:08发布

void pid(float angle, float angle_dot)
{
  u32 temp;
  u16 sl, sr;

  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
  TIM_OCInitTypeDef  TIM_OCInitStructure;

  now_error = set_point - angle;

  speed_filter();

  speed *= 0.7;
  speed += speed_out * 0.3;
  position += speed;
  position -= speed_need;
  
  if(position<-60000) position = -60000;
  if(position> 60000) position =  60000;

  rout = proportion * now_error + derivative * angle_dot - position * integral2 - derivative2 * speed;//什么意思
  
  speed_l = -rout + turn_need_l;
  speed_r = -rout + turn_need_r;

  if(speed_l > MAX_SPEED) 
  {
    speed_l = MAX_SPEED; 
  }
  else if(speed_l < -MAX_SPEED) 
  {
speed_l = -MAX_SPEED;
  }

  if(speed_r > MAX_SPEED) 
  {
    speed_r = MAX_SPEED; 
  }
  else if(speed_r < -MAX_SPEED) 
  {
speed_r = -MAX_SPEED;
  }
  
  if(speed_l > 0)
  {
    GPIO_ResetBits(GPIOB, GPIO_Pin_8);  //left fr
sl = speed_l;
  }
  else 
  {
    GPIO_SetBits(GPIOB, GPIO_Pin_8);
sl = speed_l * (-1);
  }

  if(speed_r > 0)
  {
    GPIO_SetBits(GPIOA, GPIO_Pin_3);  //right fr
sr = speed_r;
  }
  else 
  {
    GPIO_ResetBits(GPIOA, GPIO_Pin_3);
sr = speed_r * (-1);
  }
  
  temp = 1000000 / sl;
  if(temp > 65535)
  {
    sl = 65535;
  }
  else 
  {
    sl = (u16)temp;
  }
  
  temp = 1000000 / sr;
  if(temp > 65535)
  {
    sr = 65535;
  }
  else 
  {
    sr = (u16)temp;
  } 

  /* Time base configuration */
  TIM_TimeBaseStructure.TIM_Period = sl - 1;  // (Period + 1) * (Prescaler + 1) / 72M = 1ms  
  TIM_TimeBaseStructure.TIM_Prescaler = 30 - 1;  
  TIM_TimeBaseStructure.TIM_ClockDivision = 0;
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  

  TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);

  TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
  TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
  TIM_OCInitStructure.TIM_Pulse = sl >> 1;

  TIM_OC4Init(TIM4, &TIM_OCInitStructure);  //PB9  left

  /* Time base configuration */
  TIM_TimeBaseStructure.TIM_Period = sr - 1;    
  TIM_TimeBaseStructure.TIM_Prescaler = 30  - 1;  
  TIM_TimeBaseStructure.TIM_ClockDivision = 0;
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  

  TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);

  TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
  TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
  TIM_OCInitStructure.TIM_Pulse = sr >> 1;

  TIM_OC3Init(TIM2, &TIM_OCInitStructure);  //PA2 right  
}
这里的pid是什么意思,怎么跟正常的不一样。
我看的是kp*e+ki*e+kd*e
友情提示: 此问题已得到解决,问题已经关闭,关闭后问题禁止继续编辑,回答。