ttss_weight/source/asp/asp_move.c

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



|------------------M-------------------A|
|A<<<<<<<<<<<<<<<<<M<<<<<<<<<<<<<<<<<<<<|
|>>>>>>>>>>>>>>>>>>M>>>>>>>>>>>>>>>>>>>A|


|-----------A------M--------------------|
|A<<<<<<<<<<<------M--------------------|
|>>>>>>>>>>>>>>>>>>M>>>>>>>>>>>>>>>>>>>A|
|-----------A<<<<<<M<<<<<<<<<<<<<<<<<<<<|




|------------------M-----A--------------|
|------------------M------>>>>>>>>>>>>>A|
|A<<<<<<<<<<<<<<<<<M<<<<<<<<<<<<<<<<<<<<|
|>>>>>>>>>>>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