222

MDK-ARM/motor.uvoptx

@@ -177,7 +177,7 @@
       <DebugFlag>
         <trace>0</trace>
         <periodic>1</periodic>
-        <aLwin>0</aLwin>
+        <aLwin>1</aLwin>
         <aCover>0</aCover>
         <aSer1>0</aSer1>
         <aSer2>0</aSer2>

User/application/app.c

@@ -21,6 +21,8 @@ static uint8_t business_inspection(struct flow *fl)
             ssd1306_clear_buffer();
             ssd1306_f8x16_string(0, 0, " PERCENT:");
             ssd1306_f8x16_number(88, 0, work.pwm_percent, 0);
+            ssd1306_f8x16_string(0, 2, " CUR:");
+            ssd1306_f8x16_number(48, 2, work.encoder_cnt, 0);
             ssd1306_update_screen();
             break;
         case WORK_MOTOR_POS:

User/application/work.c

@@ -46,6 +46,10 @@ static void pwm_map(void)
     set_motor_pwm(work.pwm_percent);
 }
 
+static void pwm_s_curve(void)
+{
+}
+
 static void motor_pos(void)
 {
     int32_t code = work.encoder_cnt;
@@ -82,7 +86,8 @@ static void pwm_map_key_handle(button_id_e id)
         }
         break;
     case KEY_S:
-        work.pwm_percent = 0;
+        work.pwm_percent = 10;
+        work.encoder_cnt = 0;
         break;
     default:
         break;
@@ -181,6 +186,9 @@ void work_process(void)
     case WORK_MOTOR_SPEED:
         motor_speed();
         break;
+    case WORK_S_CURVE:
+        pwm_s_curve();
+        break;
     default:
         break;
     }
@@ -210,7 +218,7 @@ void work_init(void)
     PWM_SET_DUTY(PWM_TIM, PWM_CHANNEL, 0);
     work.pwm_feq = PWM_GET_FREQ(PWM_TIM);
     work.pwm_arr = PWM_GET_ARR(PWM_TIM);
-    work.type = WORK_MOTOR_SPEED;
+    work.type = WORK_PWM_MAP;
 
     // PID初始化
 
@@ -264,6 +272,10 @@ void work_init(void)
     work.target_pos = TARGET_POS;
     work.target_speed = TARGET_SPEED;
     work.timer_cycle = TIM_CYCLE(WORK_TIM);
+    if (work.type == WORK_PWM_MAP)
+    {
+        work.pwm_percent = 10;
+    }
 
     return;
 }

User/application/work.h

@@ -13,6 +13,8 @@ typedef enum
     WORK_MOTOR_POS = 1,
     // 电机速度控制
     WORK_MOTOR_SPEED = 2,
+    // S曲线
+    WORK_S_CURVE = 3,
 } work_e;
 
 typedef struct

User/lib/control/inc/s_curve.h

@@ -0,0 +1,30 @@
+#ifndef __S_CURVE_H
+#define __S_CURVE_H
+#include "lib.h"
+
+typedef struct
+{
+    // 起始频率
+    float32 f0;
+    // 加加速段斜率
+    float32 faa;
+    // 减减速段斜率
+    float32 frr;
+    // 加加速段时间
+    float32 taa;
+    // 匀加速段时间
+    float32 tua;
+    // 减加速段时间
+    float32 tra;
+    // 匀速段时间
+    float32 tuu;
+    // 加减速段时间
+    float32 tar;
+    // 匀减速段时间
+    float32 tur;
+    // 减减速段时间
+    float32 trr;
+} s_curve_t;
+
+void s_curve_table_gen(uint16_t tmin, uint16_t tmax);
+#endif // __S_CURVE_H

User/lib/control/src/s_curve.c

@@ -0,0 +1,171 @@
+#include "s_curve.h"
+
+// s曲线加速度各段参数定义
+// 起始速度
+#define F0 0.0f
+
+// 加加速度与减减速度
+#define FAA 0.0f
+#define FRR 0.0f
+
+// 加速段三个时间
+#define TAA 0.1f
+#define TUA 0.2f
+#define TRA 0.1f
+
+// 匀速段
+#define TUU 5.0f
+
+// 减速段
+#define TAR 0.1f
+#define TUR 0.2f
+#define TRR 0.1f
+
+// 表格长度
+#define S_CURVE_TABLE_LEN 400
+int16_t s_curve_table[S_CURVE_TABLE_LEN] = {0};
+
+static int16_t s_curve_func(s_curve_t *s, float32 t, float32 *freq, float32 *acc)
+{
+    // 辅助常数项
+    float32 A, B, C, D, E, F;
+    // 表达式中间值
+    float32 f3, f4, f5;
+    // 加速段，匀速段，减速段时间
+    float32 Ta, Tu, Tr;
+    // 减加速与加减速段斜率
+    float32 fra, far;
+    // 起始频率与加加速频率，减减速频率
+    float32 f0, faa, frr;
+    float32 taa, tua, tra, tuu, tar, tur, trr;
+
+    // 获取参数
+    faa = s->faa;
+    frr = s->frr;
+
+    taa = s->taa;
+    tua = s->tua;
+    tra = s->tra;
+    tuu = s->tuu;
+    tar = s->tar;
+    tur = s->tur;
+    trr = s->trr;
+
+    f0 = s->f0;
+
+    fra = faa * taa / tra;
+    far = frr * trr / tar;
+
+    Ta = taa + tua + tra;
+    Tu = tuu;
+    Tr = tar + tur + trr;
+
+    A = f0;
+    B = f0 - 0.5 * faa * taa * taa;
+    C = f0 + 0.5 * faa * taa * taa + faa * taa * tua + 0.5 * fra * (taa + tua) * (taa + tua) - fra * Ta * (taa + tua);
+
+    // f1 = f0 + 0.5 * faa * taa * taa;
+    // f2 = f0 + 0.5 * faa * taa * taa + faa * taa * tua;
+    f3 = 0.5 * fra * Ta * Ta + C;
+
+    D = f3 - 0.5 * far * (Ta + Tu) * (Ta + Tu);
+    f4 = -far * 0.5 * (Ta + Tu + tar) * (Ta + Tu + tar) + far * (Ta + Tu) * (Ta + Tu + tar) + D;
+
+    E = f4 + far * tar * (Ta + Tu + tar);
+    f5 = -far * tar * (Ta + Tu + Tr - trr) + E;
+
+    F = f5 + frr * (Ta + Tu + Tr) * (Ta + Tu + Tr - trr) - 0.5 * frr * (Ta + Tu + Tr - trr) * (Ta + Tu + Tr - trr);
+
+    // 如果时间点在全行程规定的时间段内
+    if ((t >= 0) && (t <= Ta + Tu + Tr))
+    {
+        // 加加速段
+        if ((t >= 0) && (t <= taa))
+        {
+            *freq = 0.5 * faa * t * t + A;
+            *acc = faa * t;
+        }
+        // 匀加速段
+        else if ((t >= taa) && (t <= taa + tua))
+        {
+            *freq = faa * taa * t + B;
+            *acc = faa * taa;
+        }
+        // 加减速段
+        else if ((t >= taa + tua) && (t <= taa + tua + tra))
+        {
+            *freq = -0.5 * fra * t * t + fra * Ta * t + C;
+            *acc = -fra * t + fra * Ta;
+        }
+        // 匀速段
+        else if ((t >= Ta) && (t <= Ta + tuu))
+        {
+            *freq = f3;
+            *acc = 0;
+        }
+        // 加减速段
+        else if ((t >= Ta + Tu) && (t <= Ta + Tu + tar))
+        {
+            *freq = -0.5 * far * t * t + far * (Ta + Tu) * t + D;
+            *acc = -far * t + far * (Ta + Tu);
+        }
+        // 匀减速
+        else if ((t >= Ta + Tu + tar) && (t <= Ta + Tu + tar + tur))
+        {
+            *freq = -far * tar * t + E;
+            *acc = -far * tar;
+        }
+        // 减减速
+        else if ((t >= Ta + Tu + Tr - trr) && (t <= Ta + Tu + Tr))
+        {
+            *freq = 0.5 * frr * t * t - frr * (Ta + Tu + Tr) * t + F;
+            *acc = frr * t - frr * (Ta + Tu + Tr);
+        }
+    }
+    else
+    {
+        return -1;
+    }
+
+    return 0;
+}
+
+// S型曲线初始化
+void s_curve_table_gen(uint16_t tmin, uint16_t tmax)
+{
+    uint16_t i, tint;
+    float32 ti;
+    float32 freq;
+    float32 acc;
+    float32 fi;
+    s_curve_t s;
+    osel_memset((uint8_t *)&s, 0, sizeof(s_curve_t));
+
+    s.f0 = F0;
+    s.taa = TAA;
+    s.tua = TUA;
+    s.tra = TRA;
+    s.tuu = TUU;
+    s.tar = TAR;
+    s.tur = TUR;
+    s.trr = TRR;
+
+    // 根据约束条件求出加加速段与减减速段斜率
+    s.faa = 2.0 / (s.taa * (s.taa + s.tra + 2 * s.tua));
+    s.frr = 2.0 / (s.trr * (s.tar + s.trr + 2 * s.tur));
+
+    for (i = 0; i < S_CURVE_TABLE_LEN; i++)
+    {
+        // 求出每个时间点对应的频率以及加速度
+        fi = i * (TAA + TUA + TRA + TUU + TAR + TUR + TRR) / S_CURVE_TABLE_LEN;
+        s_curve_func(&s, fi, &freq, &acc);
+
+        // 根据最大与最小装载值确定定时器实际值
+        ti = tmax - (tmax - tmin) * freq;
+        // 转换为整数值
+        tint = (uint16_t)ti;
+
+        // 存入s曲线表
+        s_curve_table[i] = tint;
+    }
+}