-
S曲线控制
关于运动控制中S型速度曲线的简单演示（C++实现）_速度曲线算位置曲线-CSDN博客
步进电机有很多种算法，主要集中在速度的控制，其中S比较常见。上边是其他作者的作品， 这里截取一段代码 作为记录。
- #include
- #include
- #define FP64 double
- int main(void)
- {
- /**************************** 定义相关变量(需要映射到PLC寄存器才能进行监控) ***********************/
- FP64 v_start; //初速度
- FP64 v_end; //末速度
- FP64 v_max = 0; //最大速度(有匀速段时,v_max = v_target)
- /*根据需要修改*/
- v_start = 100;
- v_end = 150;
- FP64 s_target; //目标位移量
- FP64 v_target; //目标速度
- FP64 a_target; //目标加速度
- FP64 d_target; //目标减速度
- FP64 j_max; //目标加加速度
- FP64 t_sum;
- FP64 L; //算出的位移量
- FP64 v_real; //实际的最大速度
- FP64 s_max; //L>s_target时候规划的位移
- /*可以根据需要修改*/
- j_max = 2000; //目标加加速度
- a_target = 2000; //目标加速度
- d_target = 3000; //目标减速度
- v_target = 5000; //目标速度
- s_target = 3000; //目标位移
- int t; //运行时间
- FP64 t_aver; //匀速区时间
- FP64 t1; //加加速时间
- FP64 t2; //匀加速时间
- FP64 t3; //减加速时间
- FP64 t4; //匀速运行时间
- FP64 t5;
- FP64 t6;
- FP64 t7;
- FP64 v1;
- FP64 v2;
- FP64 v3;
- FP64 v4;
- FP64 v5;
- FP64 v6;
- FP64 v7;
- FP64 a_max; //加速度最大值(拥有匀加速段时 a_max = a_target)
- FP64 d_max;
- FP64 s1; //加加速位移 s1
- FP64 s2; //匀加速位移 s2
- FP64 s3; //减加速位移 s3
- FP64 s4; //匀速位移 s4
- FP64 s5; //加减速位移 s5
- FP64 s6; //匀减速位移 s6
- FP64 s7; //减减速位移 s7
- FP64 s_acc; //加速区位移
- FP64 s_dec; //减速区位移
- FP64 j_cur; //当前加加速度
- FP64 a_cur; //当前加速度
- FP64 v_cur; //当前速度
- FP64 s_cur; //当前位移量
- FP64 v_down;
- FP64 v_low;
- FP64 v_high;
- short int end_count=0;
- short int flag = 0;
- short int flag1 = 0;
- short int count=0;
- for (t = 0; t < 10000000000; t++)
- {
- if (v_max == 0) //只有第一遍
- {
- /***********************************根据目标速度,加速度,加加速度等规划出加速区和减速区******************/
- if ((v_target - v_start) > a_target * a_target / j_max) //匀加速段
- {
- a_max = a_target;
- t1 = a_max / j_max;
- v1 = v_start + 0.5 * j_max * t1 * t1;
- s1 = v_start * t1 + (j_max * t1 * t1 * t1) / 6;
- t2 = (v_target - v_start) / a_max - t1;
- v2 = v1 + a_target * t2;
- s2 = v1 * t2 + 0.5 * a_max * t2 * t2;
- t3 = t1;
- v3 = v2 + a_max * t3 - 0.5 * j_max * t3 * t3;
- s3 = v2 * t3 + j_max * t3 * t3 * t3 / 3;
- }
- else
- {
- a_max = sqrt((v_target - v_start) * j_max);
- t1 = sqrt((v_target - v_start) / j_max);
- s1 = v_start * t1 + (j_max * t1 * t1 * t1) / 6;
- v1 = v_start + 0.5 * j_max * t1 * t1;
- t2 = 0;
- v2 = v1;
- s2 = 0;
- t3 = t1;
- s3 = v2 * t3 + j_max * t3 * t3 * t3 / 3;
- v3 = v2 + a_max * t3 - 0.5 * j_max * t3 * t3;
- }
- if (v_target - v_end > (d_target * d_target) / j_max)
- {
- d_max = d_target;
- t7 = d_max / j_max;
- s7 = v_end * t7 + (j_max * t7 * t7 * t7) / 6;
- v6 = v_end + 0.5 * j_max * t7 * t7;
- t6 = (v_target - v_end) / d_max - t7;
- s6 = v6 * t6 + 0.5 * d_max * t6 * t6;
- v5 = v6 + d_target * t6;
- t5 = t7;
- s5 = v5 * t5 + j_max * t5 * t5 * t5 / 3;
- }
- else
- {
- d_max = sqrt((v_target - v_end) * j_max);
- t7 = sqrt((v_target - v_end) / j_max);
- s7 = v_end * t7 + (j_max * t7 * t7 * t7) / 6;
- v6 = v_end + 0.5 * j_max * t7 * t7;
- t6 = 0;
- s6 = 0;
- t5 = t7;
- v5 = v6;
- s5 = v3 * t5 - j_max * t5 * t5 * t5 / 6;
- }
- L = 0.5 * ((v_start + v_target) * (t1 + t2 + t3) + (v_end + v_target) * (t5 + t6 + t7));
- }
- if (L >= s_target && v_max == 0) //如果 L >= s_target,那么达不到目标速度,需要规划出实际最大速度,只有第一遍循环会进这个程序
- {
- t4 = 0;
- s4 = 0;
- v4 = v3;
- v_high = v_target;
- /*初速度较大*/
- if (v_start > v_end)
- {
- v_low = v_start;
- }
- /*末速度较大*/
- else if (v_end > v_start)
- {
- v_low = v_end;
- end_count += 1;
- }
- v_max = 0.5 * (v_low + v_high);
- }
- /*如果 L < s_target 那么可以正常进行规划(有匀速段),标志位flag1=1*/
- else if (L < s_target)
- {
- v_max = v_target;
- flag1 = 1;
- }
- /*********************************** 根据二分处理后的速度再算位移 ******************/
- if ((v_max - v_start) > a_target * a_target / j_max) //匀加速段
- {
- a_max = a_target;
- t1 = a_max / j_max;
- v1 = v_start + 0.5 * j_max * t1 * t1;
- s1 = v_start * t1 + (j_max * t1 * t1 * t1) / 6;
- t2 = (v_max - v_start) / a_max - t1;
- v2 = v1 + a_target * t2;
- s2 = v1 * t2 + 0.5 * a_max * t2 * t2;
- t3 = t1;
- v3 = v2 + a_max * t3 - 0.5 * j_max * t3 * t3;
- s3 = v2 * t3 + j_max * t3 * t3 * t3 / 3;
- }
- else
- {
- a_max = sqrt((v_max - v_start) * j_max);
- t1 = sqrt((v_max - v_start) / j_max);
- s1 = v_start * t1 + (j_max * t1 * t1 * t1) / 6;
- v1 = v_start + 0.5 * j_max * t1 * t1;
- t2 = 0;
- v2 = v1;
- s2 = 0;
- t3 = t1;
- s3 = v2 * t3 + j_max * t3 * t3 * t3 / 3;
- v3 = v2 + a_max * t3 - 0.5 * j_max * t3 * t3;
- }
- if (v_max - v_end > (d_target * d_target) / j_max) //匀减速段
- {
- d_max = d_target;
- t7 = d_max / j_max;
- s7 = v_end * t7 + (j_max * t7 * t7 * t7) / 6;
- v6 = v_end + 0.5 * j_max * t7 * t7;
- t6 = (v_max - v_end) / d_max - t7;
- s6 = v6 * t6 + 0.5 * d_max * t6 * t6;
- v5 = v6 + d_target * t6;
- t5 = t7;
- s5 = v5 * t5 + j_max * t5 * t5 * t5 / 3;
- }
- else
- {
- d_max = sqrt((v_max - v_end) * j_max);
- t7 = sqrt((v_max - v_end) / j_max);
- s7 = v_end * t7 + (j_max * t7 * t7 * t7) / 6;
- v6 = v_end + 0.5 * j_max * t7 * t7;
- t6 = 0;
- s6 = 0;
- t5 = t7;
- v5 = v6;
- s5 = v3 * t5 - j_max * t5 * t5 * t5 / 6;
- }
- s_max = s1 + s2 + s3 + s5 + s6 + s7; //s_max是二分后的位移
- /*二分后的位移无限接近目标位移,则标志位flag=1,开始规划*/
- if (s_max - s_target < 0.001 && s_max - s_target > 0)
- {
- flag = 1;
- }
- /*否则继续二分*/
- else
- {
- //二分法思想：改变上下限
- if (s_max < s_target) //如果规划出来的位移偏小
- {
- v_low = v_max;
- }
- else { //如果规划出来的位移偏大
- v_high = v_max;
- }
- v_max = 0.5 * (v_low + v_high);
- }
- /*********************************** 只有在标志位置位时才规划(画图) ******************/
- if (flag == 1 || flag1 == 1)
- {
- if (flag1 == 1 )
- {
- /*正常进行规划*/
- t4 = (s_target - L) / v_max;
- v4 = v3;
- s4 = v4 * t4;
- }
- else if (flag == 1)
- {
- t4 = 0;
- v4 = v3;
- s4 = 0;
- }
- t_sum = t1 + t2 + t3 + t4 + t5 + t6 + t7;
- if (t >= 0 && t < t1)
- {
- j_cur = j_max;
- a_cur = j_cur * t;
- v_cur = v_start + 0.5 * j_cur * t * t;
- s_cur = v_start * t + j_cur * t * t * t / 6;
- }
- else if (t >= t1 && t < t1 + t2 && t2 != 0)
- {
- j_cur = 0;
- a_cur = a_max;
- v_cur = v1 + a_cur * (t - t1);
- s_cur = s1 + v1 * (t - t1) + 0.5 * a_cur * (t - t1) * (t - t1);
- }
- else if (t >= t1 + t2 && t < t1 + t2 + t3)
- {
- j_cur = -j_max;
- a_cur = a_max - j_max * (t - t1 - t2);
- v_cur = v2 + a_max * (t - t1 - t2) - 0.5 * j_max * (t - t1 - t2) * (t - t1 - t2);
- s_cur = s1 + s2 + v2 * (t - t1 - t2) + 0.5 * a_max * (t - t1 - t2) * (t - t1 - t2) - j_max * (t - t1 - t2) * (t - t1 - t2) * (t - t1 - t2) / 6;
- }
- else if (t >= t1 + t2 + t3 && t < t1 + t2 + t3 + t4 && t4 != 0)
- {
- j_cur = 0;
- a_cur = 0;
- v_cur = v3;
- s_cur = s1 + s2 + s3 + v3 * (t - t1 - t2 - t3);
- }
- else if (t >= t1 + t2 + t3 + t4 && t < t1 + t2 + t3 + t4 + t5)
- {
- j_cur = -j_max;
- a_cur = -j_max * (t - t1 - t2 - t3 - t4);
- v_cur = v4 - 0.5 * j_max * (t - t1 - t2 - t3 - t4) * (t - t1 - t2 - t3 - t4);
- s_cur = s1 + s2 + s3 + s4 + v4 * (t - t1 - t2 - t3 - t4) - j_max * (t - t1 - t2 - t3 - t4) * (t - t1 - t2 - t3 - t4) * (t - t1 - t2 - t3 - t4) / 6;
- }
- else if (t >= t1 + t2 + t3 + t4 + t5 && t < t1 + t2 + t3 + t4 + t5 + t6 && t6 != 0)
- {
- j_cur = 0;
- a_cur = -d_max;
- v_cur = v5 - d_target * (t - t1 - t2 - t3 - t4 - t5);
- s_cur = s1 + s2 + s3 + s4 + s5 + v5 * (t - t1 - t2 - t3 - t4 - t5) - 0.5 * d_target * (t - t1 - t2 - t3 - t4 - t5) * (t - t1 - t2 - t3 - t4 - t5);
- }
- else if (t >= t1 + t2 + t3 + t4 + t5 + t6 && t < t1 + t2 + t3 + t4 + t5 + t6 + t7)
- {
- j_cur = j_max;
- a_cur = -d_max + j_max * (t - t1 - t2 - t3 - t4 - t5 - t6);
- v_cur = v6 - d_max * (t - t1 - t2 - t3 - t4 - t5 - t6) + 0.5 * j_max * (t - t1 - t2 - t3 - t4 - t5 - t6) * (t - t1 - t2 - t3 - t4 - t5 - t6);
- s_cur = s1 + s2 + s3 + s4 + s5 + s6 + v6 * (t - t1 - t2 - t3 - t4 - t5 - t6) - 0.5 * d_max * (t - t1 - t2 - t3 - t4 - t5 - t6) * (t - t1 - t2 - t3 - t4 - t5 - t6) + j_max * (t - t1 - t2 - t3 - t4 - t5 - t6) * (t - t1 - t2 - t3 - t4 - t5 - t6) * (t - t1 - t2 - t3 - t4 - t5 - t6) / 6;
- }
- else if (t > t1 + t2 + t3 + t4 + t5 + t6 + t7)
- {
- a_cur = 0;
- j_cur = 0;
- v_cur = v_end;
- return;
- }
- }
- }
- }
-
相关阅读:
websocket与Socket的区别
Axure绘制星级评分
Go 微服务开发框架 DMicro 的设计思路
在3+1的方向上展开结构加法4a3+4a14
通过Shell脚本自动安装Hive&JDBC测试&提供CDH5网盘地址
Java Properties类
Java——》JVM对原生的锁做了哪些优化
终日乾乾，夕惕若厉
NIFI实现数据库数据增量同步
[ 红队知识库 ] 常见防火墙(WAF)拦截页面
- 原文地址：https://blog.csdn.net/david10000/article/details/133852909
