模型驱动讲解[单舵轮]

ego

本文以CANOPEN协议对接电机为例，大概讲解一下驱动的原理

首先介绍一下每个文件的作用

candev.lua 是can驱动的文件。
opencan.lua 是opencan协议解析。
readmotor.lua 定时发送读取电机寄存器报文。
recmotor.lua 解析读取到的电机速度和编码器，转变成里程计的部分。[重要]
setvel.lua 接受控制器发过来的电机速度，转换成电机需要的速度，发到电机上。[重要]
settheta.lua 接受控制器发过来的电机角度，转换成电机需要的电机位置。[重要]

setvel.lua 部分代码

--循环发送电机速度
while true do
	local data = veldev:read()
	if data ~= nil and data.msg ~= nil then
		rtime = 0
		if data.msg.wheelvel ~= nil then
			local vel = data.msg.wheelvel * 60.0 * para.walk_dir * para.walk_ratio
			rbasetime = rbasetime + 1
			if rbasetime > 100 then
				rbasetime = 0
				--读取IO信息
				readBase()
			end
			
			if motorLogic.setStop == true then
				vel = 0
				print("stop")
			end
			
			print("motor vel : "..data.msg.wheelvel..'\t'..vel)
			setMotoVel(vel)
		end
	else
		rtime = rtime + rdelay
		rbasetime = rbasetime + rdelay
		
		--超时没有接受到电机转速,则重新加入一次域数据
		if rtime > 1000 then
			rtime = 0
			readBase()
			setMotoVel(0.0)
			veldev:addRegion("setwheelvel")
			print("timeout")
		end
		
	end
end

其中最为重要是电机转速单位

--data.msg.wheelvel 为控制内部发出来的电机速度，单位是转/秒 r/s 相对机器人方向往前为正，往后为负
--para.walk_dir 轮子实际的方向，该值一般是1或者-1
--para.walk_ratio 轮子转速比，控制器是以机器人行驶轮的单位为标准，电机还需要通过一轮减速比限速
--60代表秒转换分钟，大部分的电机是以RPM为单位，即是单位 转每分钟
--最终得出电机的实际转速
--不同电机的单位不一样，多数驱动代码只改此部分
local vel = data.msg.wheelvel * 60.0 * para.walk_dir * para.walk_ratio

settheta.lua部分代码

while true do
	local data = veldev:read()
	if data ~= nil and data.msg ~= nil then
		rtime = 0
		if data.msg.wheelTheta ~= nil then
			local position = (data.msg.wheelTheta / (math.pi * 2.0)) * para.wheel_encode * para.speedratio
			controlTheta = data.msg.wheelTheta
			
			rbasetime = rbasetime + 1
			if rbasetime > 100 then
				rbasetime = 0
				--读取IO信息
				readBase()
			end
			
			print("motor position : "..data.msg.wheelTheta..'\t'..position)
			setMotoPosition(position)
		end
	else
		rtime = rtime + rdelay
		rbasetime = rbasetime + rdelay
		
		--超时没有接受到电机转速,则重新加入一次域数据
		if rtime > 300 then
			rtime = 0
			readBase()
			veldev:addRegion("setwheelvel")
			print("timeout")
		end
		
	end
end

其中最为重要是电机转速单位

--data.msg.wheelTheta 为控制内部发出来的电机角度，单位是弧度，范围是[-PI,PI]。
--para.speedratio轮子转速比，控制器是以机器人行驶轮的单位为标准，电机还需要通过一轮减速比限速
--para.wheel_encode 轮子自身转一圈需要的编码数
--最终得出电机的实际转速
--不同电机的单位不一样，多数驱动代码只改此部分
local position = (data.msg.wheelTheta / (math.pi * 2.0)) * para.wheel_encode * para.speedratio

recmotor.lua部分代码,该部分则是把电机的速度和编码器上传到控制器算法上。
注意单位转换，上传的单位是编码器转一圈的单位。速度则是转/秒 r/s

--获取can的数据
while true do
	--读取CAN接受到的数据,100毫秒超时一次
	local rarr = candev.read()

	if rarr ~= nil then
		for i = 1,8,1 do
			if rarr[i] == nil then
				rarr = nil
				break
			end
		end
	end
	
	if rarr ~= nil and rarr["stdid"] ~= nil then
	
	if rarr ~= nil and #rarr >= 8 and rarr["stdid"]  == 0x181 then
		local data = rarr[5] + bit.lshift(rarr[6],8) + bit.lshift(rarr[7],16) + bit.lshift(rarr[8],24)
		motorLogic.encode[para.wheel_id] = limit32Bit(data)
		motorLogic.encodeRec[para.wheel_id] = true
	end
	
	if rarr ~= nil and #rarr >= 8 and rarr["stdid"]  >= 0x581 and rarr["stdid"] <= 0x587 then
		--解析can中的数据
		candel:readReceive(rarr["stdid"],rarr)
		
		local id = candel.read.id - 0x580

		--存放电机位置
		if candel.read.index == 0X28B0 and candel.read.sunindex == 0x04 then
			motorLogic.encode[para.walk_id] = candel.read.data
			motorLogic.encodeRec[para.walk_id] = true
		end

		--存放电机速度
		if candel.read.index == 0X28B0 and candel.read.sunindex == 0x03 then
			motorLogic.gvel[para.walk_id] = candel.read.data
			motorLogic.gvelRec[para.walk_id] = true
		end

		--存放电机位置
        if candel.read.index == 0x6063 then
            motorLogic.encode[id] = candel.read.data
            motorLogic.encodeRec[id] = true
        end

        --存放电机速度
        if candel.read.index == 0x606c then
            motorLogic.gvel[id] = candel.read.data
            motorLogic.gvelRec[id] = true
        end
	end
	end

	--如果电机编码器都更新了，则更新里程计
	if motorLogic.encodeRec[para.walk_id] == true and
	   motorLogic.encodeRec[para.wheel_id] == true
	 then
		
		motorLogic.encodeRec[para.walk_id] = false
		motorLogic.encodeRec[para.wheel_id] = false
		
		local dis1 = limit32Bit(motorLogic.encode[para.wheel_id])
		local dis2 = limit32Bit(motorLogic.encode[para.walk_id])

		
	   --上传两轮里程计
		local msg = {}
		msg.wheelTheta = dis1 / para.speedratio

		currentTheta = (msg.wheelTheta / para.wheel_encode) * math.pi * 2.0
		
		msg.wheelTheta = currentTheta 
		msg.encode = limit32Bit(motorLogic.encode[para.walk_id]) / para.walk_ratio
		--print("wheelencode : "..msg.wheelTheta..'\t'..msg.encode)
		veldev:write("wheelencode",msg)
	end

	--如果电机的速度都更新了，则更新速度
	if motorLogic.gvelRec[para.walk_id] == true or
		motorLogic.gvelRec[para.wheel_id] == true
		then

		motorLogic.gvelRec[para.walk_id] = false
		motorLogic.gvelRec[para.wheel_id] = false
		
		local msg = {}
		msg.wheelTheta = currentTheta
		msg.vel = limit32Bit(motorLogic.gvel[para.walk_id]) / (para.walk_ratio * 60.0 * 2730.0)

		--print("vel : "..msg.wheelTheta..'\t'..msg.vel)

		--上传两轮速度
		veldev:write("wheelvel",msg)
	end
end

以下是电机模块的配置参数describe.json

startup 需要上电自启动的脚本
describe 该功能包的描述
speedratio 行走电机转速比
speedratio 转向电机转速比
encodeValue 行走电机转一圈的编码值
wheel_encode 转向电机转一圈的编码值
walk_id 行走电机ID
walk_dir 行走电机运行方向
wheel_id 转向电机1ID
bps 电机波特率
dev 电机接到CAN1上还是CAN2上
stopButton 急停接到的IO上[0-15]
stopTrigger 急停触发的电平[0 1] -1失效
resetButton 复位按钮接到的IO上[0-15]
resetTrigger 复位按钮触发的电平[0-1] -1失效
antiCollision 触碰防撞接到的IO上[0-15]
antiCollisionTrigger 触碰防撞的触发电平[0-1] -1失效
startIO 电机模块启动需要打开的IO[0-15]

enable 是否使能改模块，true使能 false失能

{
	"startup": [
		"readmotor.lua",
		"recmotor.lua",
		"setVel.lua",
		"setTheta.lua"
	],
	"describe": "opencan协议",
	"speedratio": 175,
	"wheel_encode": 10000,
	"wheel_id": 7,
	"wheel_dir": 1,
	"walk_id": 1,
	"walk_dir": 1,
	"walk_ratio": 30,
	"walk_encode": 10000,
	"bps": 500,
	"dev": "CAN1",
	"stopButton": 15,
	"stopTrigger": 0,
	"resetButton": 14,
	"resetOut": 14,
	"resetTrigger": 0,
	"antiCollision": -1,
	"antiCollisionTrigger": 0,
	"startIO": 9,
	"enable": true
}