LUA读取德森特力传感器脚本


根据德森特 力传感器手册,机器人通过485通讯,参照以下指令,可以获取传感器返回的当前FX,FY,FZ和MX,MY,MZ

读浮点数格式的测量值:

发送:01 03 04 00 00 0c 44 77
返回:01 03 18 46 23 A3 14 C6 23 B7 14 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 5B 70
返回数据的格式同上,从18后开始,每四个字节一个数据,但是这个数据是浮点数的格式,按照标准
浮点数的格式发送。出厂时浮点数高字在前,低字在后。可以通过参数设置低字在前。最后2个数据是校验码。


机器人LUA程序中,通过485发送和接受到的均为16进制数据的 字符串,可以参考以下代码获取到传感器数据

Fx = "D30"
Fy = "D31"
Fz = "D32"
Tx = "D33"
Ty = "D34"
Tz = "D35"
-- 将对应的力和扭矩显示到D变量

function Str2float(d, BigEndian)
	-- 传入的是4个字节的连续字符串,默认小端编码,即低字节在前高字节在后,
	-- 若传入的是大端编码,即高字节在前,低字节在后,使用BigEndian
	out = tonumber(string.sub(d, 1, 2), 16)
	out = (tonumber(string.sub(d, 3, 4), 16) << 8) | out
	out = (tonumber(string.sub(d, 5, 6), 16) << 16) | out
	out = (tonumber(string.sub(d, 7, 8), 16) << 24) | out

	if BigEndian == nil then
		out = string.pack("I", out)
	else
		out = string.pack(">I", out)
	end
	out = string.unpack("f", out)
	return out
end

sleep(1)
spd = 115200
bits = 8
event = "N"
stop = 1

local open = rs485_open()
elite_print(open)

if open >= 0 then
	local set = rs485_setopt(spd, bits, event, stop)
	sleep(1)
	while set >= 0 do
		rs485_flush()
		repeat
			local td = "01030400000C4477" -- 发送获取数据的命令
			local lenth = rs485_send(td, 1)
			sleep(0.1)
			if (lenth == 1) then
				elite_print("successed")
			end
			if (lenth == -1) then
				elite_print("failure")
			end
			ret, recv_buff = rs485_recv(50, 1)
			elite_print(recv_buff)
		until (ret ~= 0 and string.sub(recv_buff, 1, 6) == "010318")
		local hex = string.sub(recv_buff, 7, 54)
		local f1 = string.sub(hex, 1, 8) --收到的数值类似于"4623A314" 
		local f2 = string.sub(hex, 9, 16)
		local f3 = string.sub(hex, 17, 24)
		local f4 = string.sub(hex, 25, 32)
		local f5 = string.sub(hex, 33, 40)
		local f6 = string.sub(hex, 41, 48)
	
		F1 = Str2float(f1,true)
		 --f1为'4623A314'的4个字节数据(16进制),此处为高字节在前,对应的浮点数是10472.76953125
		 -- 若低字节在前,直接使用str2float(f1)函数
		F2 = Str2float(f2,true)
		F3 = Str2float(f3,true)
		F4 = Str2float(f4,true)
		F5 = Str2float(f5,true)
		F6 = Str2float(f6,true)
		set_global_variable(Fx, F1)
		set_global_variable(Fy, F2)
		set_global_variable(Fz, F3)
		set_global_variable(Tx, F4)
		set_global_variable(Ty, F5)
		set_global_variable(Tz, F6)
	end
else
	elite_print("open dead")
end
rs485_close()
点击显示全文
赞同0
发表评论
分享

手机扫码分享
0
277
收藏
举报
收起
登录
  • 密码登录
  • 验证码登录
还没有账号,立即注册
还没有账号,立即注册
注册
已有账号,立即登录
选择发帖板块
举报
请选择举报理由
举报
举报说明