STM32
直播中

张勇

7年用户 1463经验值
私信 关注
[问答]

如何对基于STM32的寻迹小车进行PID调速呢

怎样去设计一种基于STM32的寻迹小车呢?
如何对基于STM32的寻迹小车进行PID调速呢?

回帖(1)

马杰

2021-11-18 14:58:45
  #include “adc.h”
  #include “common.h”
  #include “usart.h”
  #include “L298N.h”
  #include “senser.h”
  #include “NVIC.h”
  #include “ir1838.h”
  #include “DMA.h”
  #define kP 11
  #define kdI 200 //nI除数 kI=1/kdI
  #define kD 120
  #define kdPID 28//PID除数
  #define D_INTERVAL 1200
  #define I_INTERVAL 15
  #define ONLINE_MINVAL 25
  #define POSVAL_MAX SENSER_MAXVAL*2//位置量值域(-POSVAL_MAX,POSVAL_MAX)
  #define SPEED_MIN 1700
  #define SPEED_NORMAL 1950
  #define SPEED_MAX 2000
  #define CHECK_WAIT_TIMEOUT 100000
  extern u16 ADC_DataBuffer[ADC_DMA_BUFFER_LENGTH];
  extern u32 senVals[3];
  u16 mod=0;
  int main()
  {
  s32 PID;
  s32 nP=0;
  s32 nI=0;
  s32 nD=0;
  s32 nLastP=0;
  u32 nDC=D_INTERVAL;
  u32 nIC=I_INTERVAL;
  s32 carSpeed=SPEED_NORMAL;
  u32 checkWait=0;
  u8 adjustMod=100;
  common_init();
  ADC_init();
  #ifdef USART_ON
  usart_init();
  #endif
  L298N_init();
  #ifdef IR_CTRL_
  IR1838_init();
  #else
  //mod=1;
  #endif
  DMA_init();
  NVIC_init();
  delayms(120);//等待DMA将 adc_buffer 填满,避免senser校准值出错(如果有0会导致minvs不正确)
  while (1)
  {
  #ifdef IR_CTRL_
  Ircordpro();
  #endif
  switch (mod)
  {
  case 0:
  //等待车被放上跑道
  senVals[0]=ADC_DMA_getVal(0);
  senVals[1]=ADC_DMA_getVal(1);
  senVals[2]=ADC_DMA_getVal(2);
  #ifdef USART_ON
  #define LSHIFTBIT 9
  #define SENVALGATE 9
  //printDebug(“senVals[0]=”,senVals[0]);
  //printDebug(“senVals[1]=”,senVals[1]);
  //printDebug(“senVals[2]=”,senVals[2]);
  //printDebug(“-------------n”,0);
  if(senVals[2]》SENVALGATE)
  {
  senVals[2]-=SENVALGATE;
  }
  else
  {
  senVals[2]=0;
  }
  senVals[2]《《=LSHIFTBIT;
  L298N_setLS(senVals[2]);
  L298N_setRS(senVals[2]);
  //delayms(3000);
  continue;
  #endif
  if (senVals[0]》SENSER_ORIGIN_MIN&&senVals[0]》SENSER_ORIGIN_MIN&&senVals[0]》SENSER_ORIGIN_MIN)//车被提起
  {
  checkWait=CHECK_WAIT_TIMEOUT;
  adjustMod=100;
  L298N_setLS(0);
  L298N_setRS(0);
  }
  else
  {
  if(adjustMod==100)
  {
  adjustMod=0;
  Senser_reset();
  }
  }
  switch (adjustMod)
  {
  case 0:
  if (checkWait)
  {
  checkWait--;
  }
  else
  {
  adjustMod=1;
  beep(900,300);
  }
  break;
  case 1:
  //将左传感器移动到线上
  L298N_setLS(1400);
  L298N_setRS(0);
  if(senVals[0]》SENSER_ORIGIN_MIN)
  {
  adjustMod=2;
  }
  break;
  case 2:
  //将左传感器移动到线右边
  if(senVals[0]
  {
  L298N_setLS(0);
  L298N_setRS(0);
  adjustMod=3;
  beep(900,300);
  }
  break;
  case 3:
  //左转开始扫描,直到右传感在线上
  L298N_setLS(0);
  L298N_setRS(1400);
  if(senVals[2]》SENSER_ORIGIN_MIN)
  {
  adjustMod=4;
  }
  break;
  case 4:
  //直到右传感器移动到线左边
  if(senVals[2]
  {
  adjustMod=5;
  beep(900,300);
  }
  break;
  case 5:
  //右转直到中间传感器在线上
  L298N_setLS(1400);
  L298N_setLS(0);
  if(senVals[1]》SENSER_ORIGIN_MIN)
  {
  beep(800,300);
  delayms(10);
  beep(600,300);
  delayms(10);
  beep(600,300);
  delayms(10);
  beep(600,300);
  mod=1;
  }
  }
  if(adjustMod==3||adjustMod==4)
  {
  //校正
  Senser_getsv(0);
  Senser_getsv(1);
  Senser_getsv(2);
  }
  case 1:
  Senser_getsv(0);
  Senser_getsv(1);
  Senser_getsv(2);
  /* ------------P------------ */
  /* 计算位置 */
  if (senVals[1]》ONLINE_MINVAL)
  {
  //线在中间传感器下
  if (carSpeed
  {
  //加速
  carSpeed++;
  }
  if (senVals[0]》ONLINE_MINVAL)
  {
  //中间偏左
  nP=POSVAL_MAX/2-senVals[1];
  }
  else
  {
  //中间偏右
  nP=senVals[1]-POSVAL_MAX/2;
  }
  }
  else if (senVals[0]》ONLINE_MINVAL)
  {
  //线在左边传感器偏左
  nP=POSVAL_MAX-senVals[0];
  }
  else if (senVals[2]》ONLINE_MINVAL)
  {
  //线在右边传感器偏右
  nP=senVals[2]-POSVAL_MAX;
  }
  else
  {
  //线离开传感器检测范围
  if (carSpeed》SPEED_MIN)
  {
  //减速
  carSpeed--;
  }
  if (nP《0)
  {
  nP=-POSVAL_MAX;
  }
  else
  {
  nP=POSVAL_MAX;
  }
  }
  //nP/=4;
  /* ------------I------------ */
  if(nIC)
  {
  nIC--;
  }
  else
  {
  nIC=I_INTERVAL;
  if(nP》ONLINE_MINVAL||nP
  {
  nI+=nP/20;//nI=9*nI/10;
  }
  else
  {
  nI=0;
  }
  }
  /* ------------D------------ */
  if(nDC)
  {
  nDC--;
  }
  else
  {
  nDC=D_INTERVAL;
  nD=nP-nLastP;
  nLastP=nP;
  }
  /* PID */
  PID=nP*kP + nI/kdI + nD*kD;
  PID/=kdPID;
  L298N_setSpeed(carSpeed,PID);
  /*
  printDebug(“nP=”,nP*kP);
  printDebug(“nI=”,nI/kdI);
  printDebug(“nD=”,nD*kD);
  printDebug(“PID=”,PID);
  printChar(‘n’,USART2);
  printDebug(“LS=”,TIM4-》CCR3);
  printDebug(“RS=”,TIM4-》CCR4);
举报

更多回帖

发帖
×
20
完善资料,
赚取积分