標(biāo)題: 基于STM32F4的S曲線加減速的實(shí)現(xiàn)程序 [打印本頁(yè)]
作者: 洋樣楊    時(shí)間: 2020-4-8 15:58
標(biāo)題: 基于STM32F4的S曲線加減速的實(shí)現(xiàn)程序
  1. /**
  2.   * 函數(shù)功能: 主函數(shù).
  3.   * 輸入?yún)?shù): 無(wú)
  4.   * 返 回 值: 無(wú)
  5.   * 說    明: 無(wú)
  6.   */
  7. int main(void)
  8. {
  9.   /* 復(fù)位所有外設(shè)，初始化Flash接口和系統(tǒng)滴答定時(shí)器 */
  10.   HAL_Init();
  11.   /* 配置系統(tǒng)時(shí)鐘 */
  12.   SystemClock_Config();
  13.   /* 配置GPIO作為按鍵使用 */
  14.   KEY_GPIO_Init();
  15.   /* 配置串口輸出 */
  16.   USARTx_Init();
  17.   /* 無(wú)限循環(huán) */
  18.   while (1)
  19.   {
  20.     if(KEY1_StateRead() == KEY_DOWN)
  21.     {
  22.       /* 計(jì)算S型加減速曲線的加速段速度值*/
  23.       CalcSpeed(0,10,4);    // 從0加速到10r/min,加速時(shí)間是4s
  24.     }
  25.   }
  26. }

  28. /**
  29.   * 函數(shù)功能：速度表計(jì)算函數(shù)
  30.   * 輸入?yún)?shù)：V0      初速度，單位：轉(zhuǎn)/min
  31.   *          Vt      末速度，單位：轉(zhuǎn)/min
  32.   *          Time    加速時(shí)間 單位: s
  33.   * 返 回 值：無(wú)
  34.   * 說    明: 根據(jù)速度曲線和加速時(shí)間,將數(shù)據(jù)密集化,即計(jì)算每一步的速度值
  35. */
  36. void CalcSpeed(int Vo, int Vt, float Time)
  37. {
  38.   int32_t i = 0;
  39.   
  40.   int32_t Vm =0;      // 中間點(diǎn)速度
  41.   float Jerk = 0;   // 加加速度
  42.   float Tn = 0;     // 時(shí)間間隔
  43.   float DeltaV = 0; // 速度的增量dv  
  44.   float TimeDel = 0;

  46.   /* 這里采用的數(shù)學(xué)模型是勻變速直線運(yùn)動(dòng)
  47.    * 加速段的曲線有兩部分組成,第一是加速度遞增的加加速段,
  48.    * 第二是加速度遞減的減加速段,兩段曲線可以視為關(guān)于中心對(duì)稱(中心是中點(diǎn)速度).這兩段曲線所用時(shí)間相等
  49.    * 所以中點(diǎn)速度 Vm = (Vt + Vo)/2;
  50.    * 加加速段:
  51.    *     加加速段的加速度曲線是一條過原點(diǎn)的遞增的直線,對(duì)加速度積分得到的就是速度的增加量
  52.    *     所以有:Vm - Vo = 1/2 * Jerk * t^2,得到加加速度Jerk.
  53.    *     最后得到位移方程 S = 1/6 Jerk * t^3
  54.    *  :分析過程請(qǐng)結(jié)合工程文件夾下得曲線圖理解.
  55.    *     由于S型曲線是V-t曲線,所以這里對(duì)時(shí)間進(jìn)行等分,然后根據(jù)每一份的時(shí)間計(jì)算出速度表
  56.    */
  57.   Speed.Vo = ROUNDPS_2_STEPPS(Vo);    // 起速:Step/s
  58.   Speed.Vt = ROUNDPS_2_STEPPS(Vt);    // 末速:Step/s
  59.   Time = ACCEL_TIME(Time);                                // 得到加加速段的時(shí)間
  60.   Vm =  MIDDLEVELOCITY( Speed.Vo , Speed.Vt );            // 計(jì)算中點(diǎn)速度
  61.   Jerk = ACCELL_INCREASE( Speed.Vo, Vm, Time );           // 根據(jù)中點(diǎn)速度計(jì)算加加速度
  62.   Speed.AcceleratingStep  = (int32_t)ACCEL_DISPLACEMENT(Jerk,Time);// 加加速需要的步數(shù)
  63.   
  64.   /* 申請(qǐng)內(nèi)存空間存放速度表 */
  65.   Speed.AcceleratingStep  = abs(Speed.AcceleratingStep ); // 減速計(jì)算的時(shí)候防止出現(xiàn)負(fù)數(shù)
  66.   if( Speed.AcceleratingStep  % 2 != 0)     // 由于浮點(diǎn)型數(shù)據(jù)轉(zhuǎn)換成整形數(shù)據(jù)帶來了誤差,所以這里加1
  67.     Speed.AcceleratingStep  += 1;
  68.   Speed.AccelStep = Speed.AcceleratingStep * 2;           // 加速段的步數(shù)
  69.   Speed.VelocityTab = (float*)(malloc( Speed.AccelStep * sizeof(float) + 1 ));//申請(qǐng)內(nèi)存
  70.   if(Speed.VelocityTab == NULL)
  71.   {
  72.     printf("內(nèi)存不足!請(qǐng)修改曲線參數(shù)!\n");
  73.     return ;
  74.   }
  75.   
  76.   /* 目標(biāo)的速度曲線是對(duì)時(shí)間的方程,所以這里對(duì)時(shí)間等分成Speed.S份,根據(jù)時(shí)間,計(jì)算出對(duì)應(yīng)的速度 */
  77.   TimeDel = Time / Speed.AcceleratingStep;
  78.   for(i = 0; i <= Speed.AcceleratingStep; i++)
  79.   {
  80.     Tn = i * TimeDel;                       // 計(jì)算第n個(gè)時(shí)刻t
  81.     DeltaV = 0.5 * Jerk * pow(Tn,2);        // dv = 1/2 * Jerk * t^2;
  82.     Speed.VelocityTab[i] = Speed.Vo + DeltaV;  // 得到每一時(shí)刻對(duì)應(yīng)的速度 // 加加速過程與減加速是中心對(duì)稱,可以直接求出后半段速度
  83.     Speed.VelocityTab [ Speed.AccelStep - i] = Speed.Vt - DeltaV ;        // 減加速過程對(duì)稱點(diǎn)的速度
  84.   }
  85.   
  86.   /* 串口打印輸出速度表 */
  87.   for(i = 0; i <= Speed.AccelStep ; i++)
  88.   {
  89.     printf("VelocityTab[%d] = %.3f\n",i,Speed.VelocityTab[i]);
  90.   }
  91.   free((void*)Speed.VelocityTab);
  92.   Speed.VelocityTab = NULL;
  93. }

  95. /******************* (C) COPYRIGHT 2015-2020 硬石嵌入式開發(fā)團(tuán)隊(duì) *****END OF FILE****/
復(fù)制代碼


YSF4_HAL_MOTOR-134. 57&amp;42步進(jìn)電機(jī)S型加減速速度計(jì)算.7z

2.88 MB, 下載次數(shù): 98, 下載積分: 黑幣 -5

作者: huaishang    時(shí)間: 2020-6-17 14:30
謝謝樓主分享�。�！
作者: hzd530    時(shí)間: 2020-6-18 08:22
正在找步進(jìn)電機(jī)控制資料，謝謝分享！



歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1