////////////////////////////////////////////////////////////////////////// /// COPYRIGHT NOTICE /// Copyright (c) 2023, 传控科技 /// All rights reserved. /// /// @file asp_move.c /// @brief asp_move /// ///（本文件实现的功能的详述） /// moved的任务调度，是角度传感器和限位开关、的上层 /// moto工作 限位开关限位 角度传感器指示位置 ///move--|--adc /// |--key /// |--moto #include "asp_move.h" #include "bsp_moto.h" #include "msp_UART1.h" #include "asp_expara.h" TS_task_move_ ts_task_move; TS_task_step_ ts_task_step; ////ts_angle2val[1].val ts_angle2val[2].val很重要 需要出厂前配置，换板子和安装都应该配置下，否则上位机应该配置 ////ts_angle2val[0].val 经验值 取决adc比例 和结构 TS_angel2val_ ts_angle2val[5]= { -1,100, ///0,1 限位 <= 0度的直接定位到该值 ts_angle2val[0].val 0,268, ///1,2 /u8 loc_min_real;////校准后的最小位置；adc比例 限位<= 0度的 直接定位到 - ts_angle2val[0].val 90,1285, ///2,3 /u8 loc_max_real;////校准后的最大位置；adc比例限位>= 90度的 直接定位到+ts_angle2val[0].val 99,1333, ///3,4 100,1333 ///4,5 限位 }; /// angle 0-90 0,90, 1-89 vU16 L2_angel2adc(u8 angle) { if(angle > 0 ) { if(angle >= 90 ) { return (ts_angle2val[2].val+ts_angle2val[0].val);///1258+200=1458 } }else { angle = 0;///直接到达限位 return ts_angle2val[1].val - ts_angle2val[0].val; } return ts_angle2val[1].val; } void L2_task_step_register(void) { L1_task_reg_clear(&ts_task_step.task); ///L3_task_s_go(ts_task_step,D_task_init); } void L2_task_move_register(void) { L1_task_reg_clear(&ts_task_move.task); L3_task_s_go(ts_task_move,D_task_init); L2_task_step_register(); } /* 3个命令： 1 前进到某一角度 2 自检 A-loc_now M loc_mid loc_min loc_max |A-----------------M--------------------| |>>>>>>>>>>>>>>>>>>M>>>>>>>>>>>>>>>>>>>A| |A<<<<<<<<<<<<<<<<>>>>>>>>>>>>>>>>>M>>>>>>>>>>>>>>>>>>>A| |-----------A------M--------------------| |A<<<<<<<<<<<------M--------------------| |>>>>>>>>>>>>>>>>>>M>>>>>>>>>>>>>>>>>>>A| |-----------A<<<<<>>>>>>>>>>>>A| |A<<<<<<<<<<<<<<<<>>>>>>>>>>A>>>>>>M>>>>>A--------------| 真实real 0------------------90 实际采样 240 263 1285 1308/1333 转换角度 Key 0-------10----------------90-----100 key 输入： 0度默认关闭 一直到key 90度 默认打开 直到key 其他角度 收到后按默认配置执行 上位机可以调整 出厂根据阀门调整下（发送协议完成验证），上位机具备参数分类校准调整更方便 出厂ok 除非控制机构，同时弹片上加可调 2.1 判断当前位置在上还是下， 如果下 先向下走 直到限位 如果上 先往上走 直到限位 向相反的方向运动，然后 */ #define D_ok BITN7/// 1=成功 0 =失败 #define D_angle BITN6///角度逼近1= 发生 0未发生 #define D_locok BITN5///到达指定位置 #define D_modemove BITN4 #define D_locmax BITN3////标志位：到达最大位置； #define D_locmin BITN2///标志位：到达最小位置； #define D_locminOV BITN1////标志位：到达最小位置超时 #define D_locmaxOV BITN0////标志位：到达最大位置超时 #define D_moto_diff 1 #define D_angel_over 100 void L2_task_step_handle(TS_task_step_ *s) {///fixme:需要添加超时保护 unsigned char d = 0; TTSS_Task_init(): printf("\r\nL2_task_step_handle TTSS_Task_init"); L2_task_go(D_task_step_init); TTSS_Task_step(D_task_step_init): printf("\r\nL2_task_step_handle D_task_step_dir"); s->ok_want = 0; L2_task_go(D_task_step_dir); TTSS_Task_step(D_task_step_dir):////移动到指定的位置 if(s->loc_want > s->loc_now)//// 1>0 3>2 90>89 34>2 67>45 { if( (s->loc_want-s->loc_now) >= D_moto_diff)////避免出现一个角度抖动 { ///BITS_1(s->ok_want,D_locok); L0_moto_run(D_MOTO_FORWARD); L2_task_go(D_task_step_moto); }else { BITS_1(s->ok_want,D_angle); L2_task_go(D_task_step_ok); } }else { if( (s->loc_now-s->loc_want) >= D_moto_diff)////避免出现一个角度抖动 { L0_moto_run(D_MOTO_BACKWARD); L2_task_go(D_task_step_moto); }else { BITS_1(s->ok_want,D_angle); L2_task_go(D_task_step_ok); } } TTSS_Task_step(D_task_step_moto): if(ABS(s->loc_want, s->loc_now) <= D_moto_diff) {////到达指定位置 L0_moto_run(D_MOTO_STOP); BITS_1(s->ok_want,D_locok); L2_task_go(D_task_step_ok); } ///printf("\r\nF =%d<-%d ", (int)s->loc_want,s->loc_now); TTSS_Task_step(D_task_step_ok): s->ok_want = 1; /// L2_task_go(0); printf("\r\nstep_ok =%d<-%d ", (int)s->loc_want,s->loc_now); L2_task_Tdelay_go(D_Tdelay_30s,D_task_step_init);///测试使用： TTSS_Task_end(); /// 7 6 5 4 3 2 1 0 /// 7 D_OK BITN7 ///6 D_angle BITN6 ///5 D_locok BITN5 ///4 ///3 D_locmax BITN3 ///2 D_locmin BITN2 ///1 D_locminOV BITN1 ///0 D_locmaxOV BITN0 /// /// //>>>>>>>>>>>>>保护代码 if (ts_moto.mode > 0x80 )////运动中 { BITS_1(s->ok_want,D_modemove); if((D_MOTO_FORWARD|0x80) == ts_moto.mode) { if(L0_Mmax_arrive()) { L0_moto_run(D_MOTO_STOP); BITS_1(s->ok_want,D_locmax); printf("\r\n##Protect:moto max arrive "); } else { BITS_0(s->ok_want,D_locmaxOV); TTSS_overtime_get(s->overtimes) printf("\r\n@Protect:moto overtime "); BITS_1(s->ok_want,D_locmaxOV); L0_moto_run(D_MOTO_STOP); TTSS_overtime_end } }else { if(L0_Mmin_arrive()) { L0_moto_run(D_MOTO_STOP); BITS_1(s->ok_want,D_locmin); printf("\r\n##Protect:moto min arrive %d ",(int)ts_moto.mode); } else { BITS_0(s->ok_want,D_locminOV); TTSS_overtime_get(s->overtimes) printf("\r\n@Protect:moto overtime "); BITS_0(s->ok_want,D_locminOV); L0_moto_run(D_MOTO_STOP); TTSS_overtime_end } } }else { BITS_0(s->ok_want,D_modemove); TTSS_overtime_init(s->overtimes,D_Tdelay_15s); /////快速设置（每次设置小于1s或更少时间间隔）如果累计超过15s 电机自动停止，下条指令后重新计时 } } ////L2_task_move_handle(&ts_task_move); void L2_task_move_handle(TS_task_move_ *s) {///fixme:需要添加超时保护 unsigned char d = 0; TTSS_Task_init(): printf("\r\nL2_task_move_handle TTSS_Task_init"); L2_task_go(D_task_move_init); TTSS_Task_step(D_task_move_init): s->loc_angle[0] = 34; s->loc_angle[1] = 67; s->loc_angle[2] = 90; s->loc_angle[3] = 100; /**/ ts_expara.angle.en = 1;///打开角度采集 s->loc_i = 0; /// L2_task_Tdelay_go(D_Tdelay_2s,D_task_move_cmd_netid); L2_task_Tdelay_go(D_Tdelay_100ms,D_task_move_next);///等待adc采样稳定 ///L2_task_go(D_task_move_next); TTSS_Task_step(D_task_move_next): if(100 != s->loc_angle[s->loc_i]) { s->loc_want = L2_angel2adc(s->loc_angle[s->loc_i]); printf("\r\ni =%d %d<-%d",(int)s->loc_i,(int)s->loc_want,s->loc_angle[s->loc_i]); s->loc_i ++; L2_task_go(D_task_move_angle); L3_task_s_go(ts_task_step,D_task_init); }else { } printf("\r\n over %d %d ",s->loc_i,s->loc_m); TTSS_Task_step(D_task_move_angle):////移动到指定的位置 if(ts_task_step.ok_want) { }else { } TTSS_Task_step(D_task_move_stop): ts_expara.angle.en = 0;///关闭角度采集 /// L2_task_go(0); L2_task_Tdelay_go(D_Tdelay_30s,D_task_move_init);///测试使用： TTSS_Task_end(); L2_task_step_handle(&ts_task_step); } #if 0 []发送→??D"1608 2956 1357 1973 D_UART1 ok 6 F1 01 DC 33 44 22 1->cmd=F num=1 len = 6 cmd = DC 33 44 22 00 00 B 22: moto=(1)3331608 2874 1340 1971 1608 2946 1315 1969 1608 2944 1301 1967 1608 2943 1276 1967 1608 2942 1261 1968 1609 2945 1235 1973 1609 2946 1201 1969 1608 2942 1183 1968 1608 2944 1149 1974 1608 2945 1130 1974 1609 2947 1101 1976 1608 2946 1070 1970 1608 2944 1051 1971 1609 2945 1022 1966 1608 2944 1007 1972 1608 2947 977 1974 1608 2945 958 1969 1608 2944 925 1972 1608 2945 896 1967 1609 2944 877 1969 1609 2945 848 1975 1608 2946 818 1969 1608 2944 799 1971 1608 2946 771 1974 1608 2946 750 1964 1609 2944 721 1971 1608 2946 699 1971 1608 2944 671 1976 1608 2946 637 1971 1608 2944 619 1967 1608 2944 593 1973 1608 2947 566 1970 1608 2945 548 1976 1608 2946 520 1968 1608 2944 507 1966 1608 2944 483 1971 1608 2946 457 1974 1608 2945 441 1971 1608 2946 412 1969 1608 2944 392 1967 1608 2945 367 1968 1608 2947 349 1976 1608 2945 325 1972 1608 2944 298 1970 1608 2945 282 1970 1608 2944 263 1963 []发送→??D" D_UART1 ok 6 F1 02 DC 33 44 22 1->cmd=F num=1 len = 6 cmd = DC 33 44 22 00 00 F 22: moto=(2)3331608 2942 258 1963 1608 2906 248 1971 1608 2946 261 1977 1608 2946 268 1972 1608 2945 294 1968 1608 2944 318 1967 1608 2945 335 1975 1608 2944 363 1971 1608 2943 378 1971 1608 2945 402 1973 1607 2946 433 1965 1608 2944 449 1971 1608 2944 475 1972 1608 2944 492 1969 1608 2944 515 1970 1608 2946 541 1968 1608 2946 557 1965 1609 2942 581 1973 1608 2945 600 1969 1608 2944 630 1973 1608 2946 661 1966 1608 2944 680 1971 1608 2946 708 1974 1607 2945 729 1967 1607 2942 763 1972 1608 2944 794 1964 1608 2945 812 1973 1608 2946 838 1975 1608 2946 856 1974 1608 2944 887 1972 1608 2944 916 1965 1608 2945 934 1971 1608 2945 967 1968 1608 2944 986 1968 1608 2944 1015 1974 1607 2944 1044 1973 1608 2945 1062 1964 1608 2942 1084 1970 1608 2944 1102 1973 1608 2945 1140 1967 1608 2940 1172 1973 1608 2944 1195 1971 1607 2945 1220 1963 1608 2941 1244 1972 1608 2944 1269 1974 1608 2944 1295 1970 1608 2945 1309 1967 1608 2945 1333 1968 []发送→??D" D_UART1 ok 6 F1 00 DC 33 44 22 1->cmd=F num=1 len = 6 cmd = DC 33 44 22 00 00 S 22: moto=(0)3331608 2945 1349 1971 1608 2955 1371 1974 转动到0的时候的值 -------------------0 1608 2905 1285 1950 1608 2905 1285 1951 1608 2904 1285 1950 1608 2904 1285 1949 1609 2905 1285 1948 1608 2905 1285 1949 1608 2905 1285 1951 误差的值 球阀自己的间隙 1608 2956 962 1961 1608 2958 962 1960 1608 2956 962 1961 1608 2956 962 1963 1608 2956 962 1962 1607 2956 962 1961 1608 2956 962 1960 1608 2956 962 1962 1608 2956 962 1963 1608 2956 962 1962 1607 2956 962 1961 1608 2956 962 1960 1608 2956 957 1962 1608 2956 957 1963 1608 2956 958 1963 1608 2956 958 1961 1608 2956 958 1960 1608 2956 958 1962 1608 2956 958 1963 1608 2956 958 1962 1608 2956 958 1961 1608 2956 958 1960 1608 2956 958 1962 #endif