模型驱动讲解[双舵轮]

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.Vel1 ~= nil and data.msg.Vel2 ~= nil then
			local vel1 = data.msg.Vel1 * 60.0 * para.vel1Dir * para.speedratio 
			local vel2 = data.msg.Vel2 * 60.0 * para.vel2Dir * para.speedratio 
			
			rbasetime = rbasetime + 1
			if rbasetime > 100 then
				rbasetime = 0
				--读取IO信息
				readBase()
			end
			
			if motorLogic.setStop == true then
				vel1 = 0
				vel2 = 0
				print("stop")
			end
			
			if motorLogic.setPause == 1 then
				vel1 = 0
				vel2 = 0
				print("pause")
			end
			
			print("motor speed : "..vel1..'\t'..vel2)
			rtime = 0
			setMotoVel(vel1,vel2)
		end
	else
		rtime = rtime + rdelay
		rbasetime = rbasetime + rdelay
		
		--超时没有接受到电机转速,则重新加入一次域数据
		if rtime > 1000 then
			rtime = 0
			veldev:addRegion("setwheelvel")
			setMotoVel(0,0)
			
			--读取IO信息
			readBase()
			if motorLogic.setStop == true then
				--setEnable(false)
			end
			
			print("timeout")
		end
	end
end

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

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

settheta.lua部分代码

while true do
	local data = veldev:read()
	if data ~= nil and data.msg ~= nil then
		rtime = 0
		if data.msg.theta1 ~= nil and data.msg.theta2 ~= nil then
			local theta1 = (data.msg.theta1 + para.thetaOffset) / (math.pi * 2.0)
			local theta2 = (data.msg.theta2 + para.thetaOffset) / (math.pi * 2.0)
			
			theta1 = theta1 * para.theta1Dir * para.wheelratio * para.encodeValue
			theta2 = theta2 * para.theta2Dir * para.wheelratio * para.encodeValue
			
			rbasetime = rbasetime + 1
			if rbasetime > 100 then
				rbasetime = 0
				--读取IO信息
				readBase()
			end
			
			if motorLogic.setStop == true then
				theta1 = 0
				theta2 = 0
				print("stop")
			end
			
			--print("motor speed : "..theta1..'\t'..theta2)
			rtime = 0
			setMotoPosition(theta1,theta2)
		end
	else
		rtime = rtime + rdelay
		rbasetime = rbasetime + rdelay
		
		--超时没有接受到电机转速,则重新加入一次域数据
		if rtime > 1000 then
			rtime = 0
			veldev:addRegion("setwheelvel")
			
			--读取IO信息
			readBase()
			if motorLogic.setStop == true then
				--setEnable(false)
			end
			
			print("timeout")
		end
	end
end

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

--data.msg.theta1 为控制内部发出来的电机1角度，单位是弧度，范围是[-PI,PI]。
--data.msg.theta1 为控制内部发出来的电机2角度，单位是弧度，范围是[-PI,PI]。
--para.theta1Dir 电机1实际的方向，该值一般是1或者-1
--para.theta2Dir 电机2实际的方向，该值一般是1或者-1
--para.wheelratio 轮子转速比，控制器是以机器人行驶轮的单位为标准，电机还需要通过一轮减速比限速
--para.thetaOffset 轮子的偏移角
--para.encodeValue 轮子自身转一圈需要的编码数
--最终得出电机的实际位置
--不同电机的单位不一样，多数驱动代码只改此部分
local theta1 = (data.msg.theta1 + para.thetaOffset) / (math.pi * 2.0)
local theta2 = (data.msg.theta2 + para.thetaOffset) / (math.pi * 2.0)
			
theta1 = theta1 * para.theta1Dir * para.wheelratio * para.encodeValue
theta2 = theta2 * para.theta2Dir * para.wheelratio * para.encodeValue

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"]  >= 0x581 and rarr["stdid"] <= 0x58f then
		--解析can中的数据
		candel:readReceive(rarr["stdid"],rarr)
		
		local id = candel.read.id - 0x580

		--存放电机位置
		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.theta1ID] == true and
	   motorLogic.encodeRec[para.theta2ID] == true and
	   motorLogic.encodeRec[para.vel1ID] == true and
	   motorLogic.encodeRec[para.vel2ID] == true
	 then
		
		motorLogic.encodeRec[para.theta1ID] = false
		motorLogic.encodeRec[para.theta2ID] = false
		motorLogic.encodeRec[para.vel1ID] = false
		motorLogic.encodeRec[para.vel2ID] = false
		
		local dis1 = limit32Bit(motorLogic.encode[para.theta1ID])
		local dis2 = limit32Bit(motorLogic.encode[para.theta2ID])
		
		local dis3 = limit32Bit(motorLogic.encode[para.vel1ID])
		local dis4 = limit32Bit(motorLogic.encode[para.vel2ID])
		
		dis1 = dis1 / para.wheelratio * para.theta1Dir
		dis2 = dis2 / para.wheelratio * para.theta2Dir
		
		dis3 = dis3 / para.speedratio * para.vel1Dir
		dis4 = dis4 / para.speedratio * para.vel2Dir
		
		local theta1 = (dis1 / para.encodeValue) * (math.pi * 2.0)
		local theta2 = (dis2 / para.encodeValue) * (math.pi * 2.0)
		
		motorLogic.theta1 = theta1 - para.thetaOffset
		motorLogic.theta2 = theta2 - para.thetaOffset
		
	   --上传两轮里程计
		local msg = {}
		msg.wheelTheta1 = motorLogic.theta1
		msg.wheelTheta2 = motorLogic.theta2
		msg.encode1 = dis3
		msg.encode2 = dis4
		
		print("wheelencode : "..msg.wheelTheta1..'\t'..msg.wheelTheta2)
		veldev:write("wheelencode",msg)
	end

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

		motorLogic.gvelRec[para.vel1ID] = false
		motorLogic.gvelRec[para.vel2ID] = false

		motorLogic.gvel[1] = limit32Bit(motorLogic.gvel[para.vel1ID])
		motorLogic.gvel[2] = limit32Bit(motorLogic.gvel[para.vel2ID])

		local v1 = motorLogic.gvel[1]
		local v2 = motorLogic.gvel[2]
		v1 = v1 / (60.0 * para.speedratio * 2730.0)
		v2 = v2 / (60.0 * para.speedratio * 2730.0)
		
		v1 = v1 * para.vel1Dir
		v2 = v2 * para.vel2Dir
		
		local msg = {}
		msg.wheelTheta1 = motorLogic.theta1
		msg.wheelTheta2 = motorLogic.theta2
		msg.vel1 = v1
		msg.vel2 = v2
		
		--print("vel : "..msg.vel1..'\t'..msg.vel2)

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

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

startup 需要上电自启动的脚本
describe 该功能包的描述
speedratio 行走电机转速比
wheelratio 转向电机转速比
encodeValue 电机转一圈的编码值
vel1ID 行走电机1ID
vel2ID 行走电机2ID
vel1Dir 行走电机1运行方向
vel2Dir 行走电机1运行方向
theta1ID 转向电机1ID
theta2ID 转向电机2ID
theta1Dir 转向电机1运行方向
theta2Dir 转向电机2运行方向
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失能

{
    "antiCollision": -1,
    "antiCollisionTrigger": 0,
    "bps": 500,
    "describe": "OPENCAN协议",
    "dev": "CAN2",
    "enable": true,
    "encodeValue": 65536,
    "resetButton": 14,
    "resetTrigger": 0,
    "speedratio": 34,
    "thetaOffset":0.07,
    "startIO": 13,
    "startup": [
        "readmotor.lua",
        "recmotor.lua",
        "settheta.lua",
        "setvel.lua"
    ],
    "stopButton": 15,
    "stopTrigger": 1,
    "theta1Dir": 1,
    "theta1ID": 1,
    "theta2Dir": 1,
    "theta2ID": 3,
    "vel1Dir": -1,
    "vel1ID": 2,
    "vel2Dir": -1,
    "vel2ID": 4,
    "wheelratio": 122.5
}