| S型速度曲线C程序实现 | 您所在的位置：网站首页 › 举例说明s型曲线的实践意义和价值 › S型速度曲线C程序实现 |
S型速度曲线C程序实现
|
S型速度曲线C语言程序实现
前言
S型曲线相较于梯形速度曲线具有运行更加平稳的特点，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 /***********************************根据目标速度,加速度,加加速度等规划出加速区和减速区******************/ 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 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) { flag = 1; } /*否则继续二分*/ else { //二分法思想：改变上下限 if (s_max //如果规划出来的位移偏大 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 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 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 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 a_cur = 0; j_cur = 0; v_cur = v_end; return; } } } } 思想说明可以根据目标加速度，目标减速度，目标加加速度，目标速度等求出L，L是这些给定条件下加速区和减速区的总位移，如果此时L > s_target，那么必不存在匀速区，即实际速度达不到给定速度，因此需要使用二分法对速度进行迭代，并重新进行规划，直到迭代出的速度算出的总位移和目标位移差值小于一定的误差（用户设定），则此时的速度为最大速度。 程序结果
黄色加减速度 绿色速度 蓝色位移 备注：图形是程序在信捷PLC软件 XD pro中示波器实测截图得来 |
红色加加速度【本文地址】
公司简介
联系我们