xushenghao
/
motor_f103
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

222

This commit is contained in:
许晟昊 2024-12-09 19:21:37 +08:00
parent 1116b68ead
commit de68a0b1a9
7 changed files with 1142 additions and 921 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
MDK-ARM/motor.uvoptx
View File

@ -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>

1840
MDK-ARM/motor/motor.hex
View File

File diff suppressed because it is too large Load Diff

2
User/application/app.c
View File

@ -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:

16
User/application/work.c
View File

@ -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;
}

2
User/application/work.h
View File

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

30
User/lib/control/inc/s_curve.h Normal file
View File

@ -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

171
User/lib/control/src/s_curve.c Normal file
View File

@ -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;
}
}