Lua Example Programs
Scene Tasks
Calling a scene with the scene interface
-- Scene call
-- Use the Lua API `scene` interface to call scene B from scene A (in this example, scene B's ID is assumed to be 10216)
-- `scene` can be understood as a wrapper function. It blocks until the logic has finished executing, so the call is blocking.
-- Content of scene A
print(scene(10216,{'{1, 2}', 3, 4}))-- Print scene return values 4, 5, 6. Note that the parameters of the scene interface {'{1, 2}', 3, 4} must be strings or numbers
print(var_test) -- Print the global variable from scene B
--Content of scene B
local params = ... --The passed parameters {'{1, 2}', 3, 4} are assigned to params, type table, where the first parameter '{1, 2}' is a string type
print("params: ", params)
print('print parameter type',type(params[1]))
-- Parse the string into a form usable in Lua
if params and next(params) then
for i=1,#params do
param = params[i]
if i ==1 then
print('print parameter 1 type',type(params[1])) --the first parameter is a string type
table_data = load("return " .. param)() -- convert to table type
print('print parameter 1 type',type(table_data))
print(table_data)
else
print(param)
end
end
end
-- Set a global variable, printed in scene A, and it can be printed out
var_test = {
name = "Xiao Wu",
age = 16
}
return 4, 5, 6
Calling a task with the start_task interface
-- Task call
--1. In scene A, use the Lua API start_task to call scene B (scene ID 10216);
--2. Note that in a Lua scene, if scene A is invoked as a serial task, when calling the start_task interface, the fourth parameter must be true (parallel execution),
-- because scene A itself is a serial task when executed serially, and serial tasks are queued. At most one serial task can run at a time,
-- and subsequent tasks only run after the preceding task completes, fails or is terminated; therefore the serial scene A task is logically stuck here and cannot complete the logic below.
--3. If scene A is invoked as a parallel task by another scene task, when start_task calls a serial task, if there is no serial task running at the moment, it returns the task ID without blocking; if a serial task is running, it blocks;
--4. When start_task calls a parallel task, the parallel task starts immediately. Multiple parallel tasks can run at the same time, and their environment variables are independent;
--5. If scene A runs as a serial task and calls scene B to run as a parallel task, note that if both scenes contain motion logic, pay attention to the logic control to avoid confusing concurrent motion execution
--Difference between start_task and scene interfaces
--1. Both start_task and scene interfaces can call a scene. The difference is that start_task starts a separate task (parallel or serial), while the scene call is like calling a function, blocking execution
--2. In addition, start_task does not return scene B's return value like scene does; it returns the ID of the executed task
--Content of scene A
task_id = start_task(10216,{'{1, 2}', 3, 4}, "", true, 1) -- Print scene return values 4, 5, 6. Note the parameters {'{1, 2}', 3, 4} must be strings or numbers
print(var_test) -- Print the global variable
--Content of scene B
local params = ... --Assign the passed parameters {'{1, 2}', 3, 4} to params, type table, where the first parameter '{1, 2}' is a string type
print("params: ", params)
print('print parameter type',type(params[1]))
-- Parse the string, see the parameter parsing example for scenes for details
if params and next(params) then
for i=1,#params do
param = params[i]
if i ==1 then
print('print parameter type',type(params[1])) --the first parameter is a string type
table_data = load("return " .. param)() -- convert to table type
print('print parameter type',type(table_data))
print(table_data)
else
print(param)
end
end
end
-- Set a global variable, printed in scene A, and it can be printed out
var_test = {
name = "Xiao Wu",
age = 16
}
return 4, 5, 6
The executed scene being called
--The executed scene being called usually contains a series of motion logic, combined with gripper opening/closing and DO output control
-- Execute motion
current_tcp_pose = get_target_tcp_pose() --current end coordinate
print('current_tcp_pose',current_tcp_pose)
offset_position = {0, 0, 0.1, 0, 0, 0} --offset relative to the end TCP position, moving forward 0.1m along the z-axis of the end TCP coordinate system
calculate_location = pose_times(current_tcp_pose, offset_position) --calculated new position
print('calculate_location',calculate_location)
movej(calculate_location, 1, 0.5, 0, 0)
--insert multiple points to implement motion
--Gripper opening/closing
set_claw(100,100)
sync() --wait for the gripper to finish
--Check the gripper opening
claw_state = get_claw_aio('Amplitude')
if claw_state > 90 then
print('Gripper opened')
end
set_claw(100,0)
--Turn on DO
set_do(1,1)
Parameter parsing in scene tasks
C_POSE = {}
J_POSE = {}
--[[
Use the Lua API: scene(scene_id, params, dir) or start_task(scene_id, params, dir, is_parallel, loop_to) to call the executed scene
Or use the SDK start_task(scene_id, params, dir, is_parallel, loop_to) to call the scene.
If params (string array) is passed in, since the parameters may be strings in the form of a dictionary or an array, the parameters need to be parsed and referenced in the Lua context.
Then, based on the passed parameter content and the agreed protocol, perform logic judgment to execute the corresponding logic.
]]--
local params =... --parameters, such as {"{'x': 0.297070,'y': 0.019994,'z': 0.385210,'rx': -1.694321,'ry': -0.116908,'rz': -0.303152} ",'{1,2}',1}
-- such as {"[0.297070, 0.019994,0.385210,-0.303152, -0.116908,-1.694321] ",'{1,2}',1}
print("params: ", params)
print('print the number of parameters',#params) --print the number of parameters
print('print the type of the first parameter',type(params[1]))
local json = require("json")
-- The python sdk passes a dictionary-form Cartesian coordinate and an array-form joint angle coordinate. Convert them to a Lua-form position, parse the string into a form usable in Lua
if params and next(params) then
for i=1,#params do
if i==1 then
----If the passed position is in dictionary style {'x': 0.297070,'y': 0.019994,'z': 0.385210,'rx': -1.694321,'ry': -0.116908,'rz': -0.303152}, handle it as follows
-- cpose = json.decode(params[i])
-- print(cpose)
-- lua_cpose={cpose.x,cpose.y,cpose.z,cpose.rz,cpose.ry,cpose.rx}
-- print(lua_cpose)
-- C_POSE[i]= lua_cpose
-- --If the position is in list style [0.297070, 0.019994,0.385210,-0.303152, -0.116908,-1.694321], the order represents
jpose = json.decode(params[i])
lua_jpose = {j1=jpose[1],j2=jpose[2],j3=jpose[3],j4=jpose[4],j5=jpose[5],j6=jpose[6]}
J_POSE[i]= lua_jpose
end
if i==2 then
str = params[i]
table_result = load("return " .. str)()
for key, value in pairs(table_result) do
print(key, value)
end
end
end
end
-- Besides JSON parsing, there are other ways to parse strings
local str = "{x = 1}"
local func, err = load("return " .. str)
if not func then
error("Failed to load string: " .. err)
end
local tbl = func()
print(tbl.x)
-- Convert the string "{1,2}" into a Lua table
local str = "{1,2}"
local table_result = load("return " .. str)()
for key, value in pairs(table_result) do
print(key, value)
end
Calling a task scene on a schedule
J_POSES = {}
C_POSES = {}
local date = os.date("*t", os.time());
--The result is date = {year = 2017, month = 8, day = 22, yday = 234, wday = 3,hour = 10, min = 46, sec = 52, isdst = false}
execution_scene_flag=false --whether to execute the specified scene A 8:00-21:00
execution_exscene_flag=false --whether to execute the extended scene B 22:00-8:00
exscene_executed_tags=0 --mark whether the extended scene B has been executed. Mark that it has been executed once
while true do
wait(100)
local date = os.date("*t", os.time())
if date['hour']>=8 and date['hour']<=21 then
execution_scene_flag=true
exscene_executed_tags=0
elseif (string.find(os.date(), " 22:")) then -- end at 22:00 every day
execution_scene_flag=false
if exscene_executed_tags == 0 then
execution_exscene_flag=true
else
execution_exscene_flag=false
end
end
if execution_scene_flag==true then
-- Execute the scene
print('Running scene')
scene(10103) --Modify the scene number as needed. It blocks here for serial execution
end
if execution_exscene_flag==true then
print('Running scene')
scene(10103) -- Execute the scene. It blocks here for serial execution
exscene_executed_tags=1
end
end
Auto-start task on boot
-- Purpose
-- Auto-start a scene at boot. Start a main scene that loops, detecting input signals in real time: it can be 485/232 TTL serial data,
-- or DI input (the DI inputs include those on the control box and the flange, as well as the signal input when the shoulder light panel and the flange button are pressed)
-- Upon detecting the corresponding signal, execute the corresponding action
-- The specific implementation is as follows
--Create a new scene A and set it as the default program (auto-start on boot), with the following content:
start_task('10026', nil, nil, true, 1) --Modify the main scene number 10026 according to the specific situation. Call the main scene as a parallel task
-- Note: '10026' called here is the main scene, which has an infinite loop logic that continuously detects input signals and makes corresponding actions
-- The fourth parameter is true, meaning parallel execution. This main task only performs logic judgment and does not execute specific action commands. It does not block. Action logic and tasks are implemented by calling sub-tasks again
-- The main task scene 10026 content is as follows:
--Redefine the underlying function on_robot_stop. When the emergency stop is pressed, only cancel the task, do not power down
function on_robot_stop(is_estop)
print("estop current TASK id:", task_id)
cancel_task(task_id)
end
--Physical serial port communication reading
local com = serial.open("/dev/ttyS1") ---Use the RS485 serial port of the control box
com:set_timeout(200)
com:set_baud_rate(9600)
function Com_read_data()
local success, rst = pcall(function() return com:read(1) end)
if success then
if rst[1] == 0xAA then
--Read according to the serial protocol. Here a simple handling is done: after finding the byte header, read a certain length
local success, rst = pcall(function() return com:read(11) end)
if success then
table.insert(rst, 1,0xAA )
return success, rst
end
end
end
return false,nil
end
-- CRC16 calculation (Modbus)
local function calc_crc(data)
local crc = 0xFFFF
for i = 1, #data do
crc = crc ~ data[i]
for _ = 1, 8 do
local j = crc & 1
crc = crc >> 1
if j == 1 then
crc = crc ~ 0xA001
end
end
end
return crc
end
-- Serial data parsing request
function parse_request(request)
if #request < 12 then return nil end
-- Check the protocol header
if request[1] ~= 0xAA or request[2] ~= 0x55 then return nil end
-- Check the slave ID
if request[3] ~= 0x01 then return nil end
-- Check the command word
if request[5] ~= 0x37 and request[5] ~= 0x38 then return nil end
-- print('check command word')
-- Verify the CRC
local crc_low = request[11]
local crc_high = request[12]
local crc_data = {}
for i=1,10 do
crc_data[i] = request[i]
end
local calc_crc_val = calc_crc(crc_data)
if crc_low ~= (calc_crc_val & 0xFF) or crc_high ~= ((calc_crc_val >> 8) & 0xFF) then
return nil
end
-- Return the basket number
return request[5], request[6] --command word, basket number(0-3)
end
--Infinite loop logic
while true do
-- Handle communication requests and feedback status
local success, request = Com_read_data()
if success then
if request and #request == 12 then
local cmd_id, basket_id = parse_request(request)
print("com received ：",request)
if basket_id ~= nil then
response = {0x55, 0xAA}
print("com sent<：", response)
local response_success, response_rst = pcall(function() return com:write(response) end)
if response_success then
print("com sent successfully")
-- Execute the corresponding logic according to the protocol content
task_id = start_task('10001', nil, nil, false, 1) -- the serially executed task
else
print("com sending failed")
end
end
end
end
if get_di(0) == 1 then --received a di signal from the control box
while true do
wait(10) --wait 100ms
if get_di(0) == 0 then --check whether the di signal has become 0
print("Received a di signal from the control box")
break
end
end
local robot_state = lebai:get_robot_state()
if robot_state == 'ESTOP' or robot_state == 'IDLE' or robot_state == 'TEACHING' then
if robot_state == 'ESTOP' then
start_sys()
sleep(2000) -- wait for the system startup to complete
end
state = get_task_state(task_id) --get the task status. The task has finished executing and is no longer running
if state ~= 'WAIT' and state ~= 'RUNNING' and state ~= 'PAUSE'
then
if robot_state == 'TEACHING' then
end_teach_mode()
end
-- Start a task. Modify scene 10024 as needed
task_id = start_task('10024', nil, nil, false, 1) -- the serially executed task
print(string.format('Robot start task, task_id: %s', task_id))
end
end
end
local button_type = "SHOULDER" --"FLANGE_BTN" is the flange flat button "SHOULDER" is the shoulder light button control
if lebai:get_di(button_type, 0)==1 then
while lebai:get_di(button_type, 0)==1 do
wait(20)
end
task_id = get_main_task_id()
cancel_task(task_id) -- stop the task
end
sleep(10) -- wait for a while in each loop to reduce CPU usage
end
Serial Communication
Serial communication interaction control
--[[
Start a Lua scene that reads serial information in a loop,
When different data is read, parse the data that conforms to the protocol,
According to the protocol agreement, execute different motion logic or other tasks upon receiving different data,
And reply with data according to the protocol.
--]]
-- Serial communication data interaction control example
com1 = serial.open("/dev/ttyS1")
com1:set_timeout(200)
com1:set_baud_rate(9600)
-- Convert a byte stream to hexadecimal representation
function table2Hex(s)
rst = ''
for i = 1, #s do
rst = rst .. string.format('0x%02X ', s[i])
-- print(s[i])
end
return rst
end
-- Convert a string to hexadecimal representation
function String2Hex(s)
local rst = ""
for i = 1, #s do
local byte = string.byte(s, i)
rst = rst .. string.format('0x%02X', byte)
if i < #s then
rst = rst .. ' ' --only add spaces between characters
end
end
return rst
end
-- Convert a single-byte unsigned integer to a signed integer
function u2s_8(num)
if num > 127 then
num = -(256 - num)
end
return num
end
-- Convert a single-byte signed integer to an unsigned integer
function s2u_8(num)
if num < 0 then
num = 256 + num
end
return num
end
-- Convert a two-byte unsigned integer to a signed integer
function usign2sign(num)
if num > 32767 then
num = -(65536 - num)
end
return num
end
-- Convert a two-byte signed integer to an unsigned integer
function sign2usign(num)
if num < 0 then
num = 65536 + num
end
return num
end
--- crc16_modbus checksum calculation
-- data data to be checked (string or byte array)
-- Returns the checksum value
function crc16_modbus(data)
local crc = 0xFFFF -- initial value
local poly = 0xA001 -- polynomial (bit-reversed form of 0x8005)
-- Check the input type (string or table)
local byte
for i = 1, #data do
if type(data) == "string" then
byte = string.byte(data, i) -- string mode: take the ASCII code byte by byte
else
byte = data[i] -- table mode: take the value directly
end
crc=((crc ~ byte) & 0xFFFF) -- XOR the current byte
-- Process 8 bits
for _ = 1, 8 do
if (crc & 1) ~= 0 then
crc = ((crc >> 1) ~ poly)& 0xFFFF -- shift right and XOR the polynomial
else
crc = crc >> 1 -- shift right only
end
end
end
-- Return the low byte and the high byte (little-endian mode)
local crc_low = crc & 0xFF
local crc_high = (crc >> 8) & 0xFF
--ret = ((crc_low << 8) | crc_high)
-- return ret
return crc_low, crc_high
end
-- Calculate and verify the sum
function calculateChecksum(data)
local sum = 0
for i = 1, #data do
sum = sum + data[i]
end
-- Take the low 8 bits as the checksum
return sum % 256
end
-- Calculate the XOR checksum
-- data input data (string or table, such as "123" or {0x01, 0x02})
-- xor_checksum XOR checksum (1 byte, 0x00~0xFF)
function xor_checksum(data)
local checksum = 0 -- initial value
-- Iterate over each byte
for i = 1, #data do
local byte
if type(data) == "string" then
byte = string.byte(data, i) -- string mode: take the ASCII code
else
byte = data[i] -- table mode: take the value directly
end
checksum = checksum ~ byte -- XOR operation
end
-- Return the 1-byte checksum value (0x00~0xFF)
return checksum & 0xFF
end
function recv_data()
-- Read the serial port, use pcall to catch errors
success, result = pcall(function() return com1:read() end)
if success and #result>2 then
--Add checksum logic here, using CRC check as an example
local subTable = {}
for i = 1, #result-2 do
subTable[i] = result[i]
end
local crc_low, crc_high = crc16_modbus(subTable)
if result[#result-1] == crc_low and result[#result] ==crc_high then
print(table2Hex(subTable)) -- print HEX
local data = string.char(table.unpack(subTable))
return data
else
return nil
end
else
print("Error: ", result)
end
end
while true
do
data = recv_data()
if data == "123456"
then
movej({j1 = 0, j2 = 0, j3 = 0, j4 = 0, j5 = 0, j6 = 0}, 0.1, 0.1, 0, 0)
sync() -- wait for the motion to finish
set_claw(100,100)
local str = "finish1"
local crc_low, crc_high = crc16_modbus(str)
com1:write({string.byte(str, 1, #str),crc_low,crc_high}) -- send the string
elseif data == "234567"
then
scene(10001) -- execute the scene, blocking logic, need to wait until the scene task completes
set_claw(100,0)
local str = "finish2"
local crc_low, crc_high = crc16_modbus(str)
com1:write({string.byte(str, 1, #str),crc_low,crc_high}) -- send the string
else
--com1:write({0xAA,0x55, 0x01, 0x08, 0xFF ,0x00, 0x00 ,0x00, 0x00, 0x00 ,0xC9 ,0xF5 })
print("unknown data:", data)
end
end
Serial control of the Xiaofei coffee machine
-- Serial communication ice maker protocol example
--CRC check function, the parameter is the data string
function Crc16(buf)
local init = 0xFFFF
local poly = 0xA001
local ret = init
local byte=0
for j=1,#buf,1 do
byte = string.byte(buf,j)
ret=((ret ~ byte) & 0xFFFF)
for i=1,8,1 do
if((ret & 0x0001)>0) then
ret = (ret >> 1)
ret = ((ret ~ poly) & 0xFFFF)
else
ret= (ret >> 1)
end
end
end
local hi = ((ret >> 8) & 0xFF)
local lo = (ret & 0xFF)
ret = ((lo << 8) | hi)
return ret
end
local com1 = serial.open("/dev/ttyS0") ---select the 232 port for serial communication
com1:set_timeout(300) --read timeout
com1:set_baud_rate(9600) --set the baud rate
com1:set_parity("None") --None: no parity check
--Function for sending commands to the serial port
function Send_Cmd_to_com(cmd)
-- print('cmd :',cmd)
com1:write(cmd) -- send the corresponding command
--Loop reading until data is read and verified and returned, or terminate and return false if no data is read after ten attempts
for i = 1,3 do
wait(100)
-- Read the serial port, use pcall to catch errors
success, result = pcall(function() return com1:read() end)
-- print(success)
if success==true and result~=nil then
-- print('receive:',result) -- print HEX
crc_response = Crc16(string.char(table.unpack(result,1,#result-2)))
-- print('crc_response:',crc_response)
-- print( crc_response>>8 ,crc_response&0xFF)
if result[#result-1]==crc_response>>8 and result[#result]==crc_response&0xFF then
return result
end
end
end
return false
end
--Ice dispensing function. The parameter is the ice dispensing time, in units of 0.1 seconds. For example, input 40 means dispensing ice for 4 seconds. Returns true if ice dispensing succeeds, returns false if it fails, and returns other values for command sending or communication hardware problems
function Make_ice(out_ice_time)
if out_ice_time==nil then
out_ice_time=10
end
makeice_cmd = {0xA5,0x5A,0x01,0x02}
out_time= math.ceil(out_ice_time) --round the time
table.insert(makeice_cmd, out_time&0xff00)
table.insert(makeice_cmd, out_time&0xff)
crc_result = Crc16(string.char(table.unpack(makeice_cmd))) --get the checksum
table.insert(makeice_cmd, crc_result>>8)
table.insert(makeice_cmd, crc_result&0xFF)
--Send the ice dispensing command
make_ice_result= Send_Cmd_to_com(makeice_cmd)
if make_ice_result ~=false then
--Normal return result of the ice dispensing command
if #make_ice_result==8 then
status_code=table.unpack(make_ice_result,5,5)
if status_code==0 then --ice dispensing succeeded
return true
else --ice dispensing failed
return false
end
--Return result caused by an erroneous command
elseif #make_ice_result==7 then
abnormal_code= table.unpack(make_ice_result,5,5)
if abnormal_code==0 then --position error causing ice dispensing failure
return 'undefined error'
elseif abnormal_code==2 then --command sending problem causing ice dispensing failure
return 'unrecognizable command'
elseif abnormal_code==3 then --command checksum error causing ice dispensing failure
return 'CRC check error'
elseif abnormal_code==4 then --system busy causing ice dispensing failure
return 'system busy'
end
end
else
--communication failure
print('Anomaly in communication with the ice maker when sending the ice dispensing command')
return 'communication error' --return false, the command has no return value
end
end
--Read the ice maker status
function Read_machine_status()
read_status_cmd = {0xA5,0x5A,0x01,0x01,0x00,0x00,0x10,0xDF}
--Send the status reading command
read_status_result= Send_Cmd_to_com(read_status_cmd)
--Return the status for a normal command
if read_status_result ~=false then
if #read_status_result==8 then
machine_status_code= table.unpack(read_status_result,5,5)
error_code = table.unpack(read_status_result,6,6)
print('Ice maker status code:', machine_status_code , ' error code:',error_code )
if machine_status_code==2 then
return 'idle standby'
elseif machine_status_code==3 then
return 'dispensing ice or water'
elseif machine_status_code==4 then
return 'discharging ice'
elseif machine_status_code==5 then
if error_code==1 then
return 'insufficient water supply'
elseif error_code==2 then
return 'ice maker internal temperature too low'
elseif error_code==3 then
return 'ice maker internal temperature too high'
elseif error_code==4 then
return 'compressor temperature too high'
elseif error_code==5 then
return 'exhaust vent temperature too low'
elseif error_code==6 then
return 'motor current too high'
elseif error_code==7 then
return 'communication error'
elseif error_code==8 then
return 'data storage error'
elseif error_code==9 then
return 'motor stall'
elseif error_code==10 then
return 'motor warning'
end
elseif machine_status_code==6 then
return 'cleaning'
elseif machine_status_code==1 or machine_status_code==0 then
return 'initializing on power-on'
elseif machine_status_code==7 then
return 'waiting to take ice'
end
--Return the status for an abnormal command
elseif #read_status_result==7 then
abnormal_code= table.unpack(read_status_result,5,5)
if abnormal_code==0 then
return 'undefined error'
elseif abnormal_code==2 then
return 'unrecognizable command'
elseif abnormal_code==3 then
return 'CRC check error'
elseif abnormal_code==4 then
return 'system busy'
end
end
--communication failure, no return value
else
print('Anomaly in communication with the ice maker when reading the ice maker status' )
return 'communication error'
end
return false
end
--Check the ice bucket status
function Read_icebucket_status()
read_icebucket_cmd = {0xA5,0x5A,0x01,0x03,0x00,0x00,0xB1,0x1F}
--Send the status reading command
read_icebucket_result= Send_Cmd_to_com(read_icebucket_cmd)
--Return the status for a normal command
if read_icebucket_result~=false then
if #read_icebucket_result==8 then
icebucket_status_code= table.unpack(read_icebucket_result,5,5)
if icebucket_status_code==1 then
return 'ice bucket full'
else
return 'ice bucket not full'
end
--Return the status for an abnormal command
elseif #read_icebucket_result==7 then
icebucket_abnormal_code= table.unpack(read_icebucket_result,5,5)
if icebucket_abnormal_code==0 then
return 'undefined error'
elseif icebucket_abnormal_code==2 then
return 'unrecognizable command'
elseif icebucket_abnormal_code==3 then
return 'CRC check error'
elseif icebucket_abnormal_code==4 then
return 'system busy'
end
end
--communication failure, no return value
else
print('Anomaly in communication with the ice maker when reading the ice bucket status' )
return 'communication error'
end
return false
end
print(Make_ice(10))
print(Read_machine_status())
print(Read_icebucket_status())
Control box Modbus RTU control
-- Control box Modbus example
-- Use the Modbus/RTU of the control box
local com1 = serial.open("/dev/ttyS1")
com1:set_timeout(200)
com1:set_baud_rate(9600)
local mb = modbus.new_rtu(com1)
-- Configure Modbus
mb:set_timeout(500)
mb:set_slave(0x01)
-- Write a single coil
success, result = pcall(function() mb:write_single_coil(0x0000, true) end)
if not success then
print("Error: "..tostring(result))
else
print("Write successful")
end
-- Write multiple coils
success, result = pcall(function()
mb:write_multiple_coils(0x0000, {true,true,true,true,true})
end)
if not success then
print("Error: ", result)
else
print("Write successful")
end
-- Read multiple coils
success, result = pcall(function() return mb:read_coils(0x0000, 5) end)
if not success then
print("Error: ", result)
else
print(result)
end
-- Write a single holding register
mb:write_single_register(0x0090, 0x0088)
-- Write multiple holding registers
mb:write_multiple_registers(0x0090, {0x0088, 0x0088})
-- Read multiple holding registers
mb:read_holding_registers(0x0090, 2)
-- Read multiple input registers
mb:read_input_registers(0x0080, 2)
Modbus control of the Sanqi servo motor
-- Sanqi servo motor control example
C_POSE = {}
J_POSE = {}
disable_auto_sync() -- disable auto sync
--Convert the read bytes into hexadecimal number representation
function String2Hex(s)
rst = ''
for i = 1, #s do
rst = rst .. string.format('0x%02X ', s[i])
end
return rst
end
function get_sign32(vx)
if (not vx) or (vx < 0x80000000) then
return vx
end
return vx - 0x100000000
end
--Convert an unsigned 2-byte integer into a signed one
function get_sign16(vx)
if (not vx) or (vx < 0x8000) then
return vx
end
return vx - 0x10000
end
--Convert a signed 4-byte integer into an unsigned one
function signed32_to_unsigned32(vx)
if not (vx<0) then
return vx
end
return vx + 0x100000000
end
--Define the serial port
com1 = serial.open("/dev/ttyS1")
com1:set_baud_rate(38400)
com1:set_parity("Even")
com1:set_timeout(200)
-- Configure Modbus
mb = modbus.new_rtu(com1)
mb:set_slave(0x01)
EI_status = 0
EI_address = 0x0000 --photoelectric address
function Servo_ON()
mb:write_multiple_registers(0x4230, {0, 1})
wait(20)
end
function Servo_OFF()
mb:write_multiple_registers(0x4230, {0, 0})
wait(20)
end
function Clear_EI()
mb:write_multiple_registers(EI_address, {0, 0})
wait(20)
end
--Position preset command when starting the servo (clear faults)
function Set_zero_EI()
Clear_EI()
mb:write_multiple_registers(EI_address, {0x0100>>16, 0x0100&0x0000FFFF})
wait(20)
end
--Home return
function Find_zero_EI()
Clear_EI()
mb:write_multiple_registers(EI_address, {0x0400>>16, 0x0400&0x0000FFFF})
wait(20)
end
--Move command
function Go_position_EI()
Clear_EI()
mb:write_multiple_registers(EI_address, {0x1>>16, 0x1&0x0000FFFF})
wait(20)
end
--EI2 = -OT, triggers when 0, EI4 = +OT, triggers when 0, EI5 = home LS detection, triggers when 1
-- Read the limit and home photoelectric sensor status
function Read_EI()
EI_status = mb:read_coils(0x0400, 5)
print(EI_status)
if EI_status ~= false then
local rst = {}
rst['home'] = not EI_status[5]
rst['OT-'] = not EI_status[2]
rst['OT+'] = not EI_status[4]
return rst
else
return false
end
end
--Set position parameters. The input is the converted pulse count
function Set_position( position_value )
position_value = signed32_to_unsigned32(position_value)
print(position_value)
print({position_value>>16, position_value&0x0000FFFF})
mb:write_multiple_registers(0x5102 , {position_value>>16, position_value&0x0000FFFF})
wait(20)
end
--Read position. The returned position unit is millimeters
function Read_position()
local rst_position = mb:read_holding_registers(0x100c, 2)
wait(20)
if rst_position == false then
return false
end
local current_position = get_sign32((rst_position[1]<<16) + rst_position[2]) * 32 / 4000
print('Current position:' ,current_position)
return current_position
end
--Set speed parameters
function Set_position_speed(speed_value)
mb:write_multiple_registers(0x5104 , {speed_value>>16, speed_value&0x0000FFFF})
wait(20)
end
--Read the instantaneous speed value
function Read_position_speed()
speed_rst = mb:read_holding_registers(0x5104, 2)
wait(20)
current_speed = get_sign32((speed_rst[1]<<16) + speed_rst[2] )
print('Current speed:' ,current_speed)
return current_speed
end
--Read the current speed
function Read_vel()
print(444)
local rst = mb:read_holding_registers(0x1000, 2)
wait(20)
print(777)
if rst == false then
return false
end
local current_vel = get_sign32((rst[1]<<16) + rst[2])
print('Current velocity:' , math.modf(current_vel))
return current_vel
end
--Set absolute position mode
function Set_abs_mode()
mb:write_multiple_registers(0x5100, {0x00FF0000>> 16,0x00FF0000&0x0000FFFF})
wait(20)
end
--Initialize and return home
function Init_servo()
if Read_vel() ~= 0 then
print('The seventh axis is moving and cannot be reset.')
return false
end
Set_zero_EI()
Find_zero_EI()
Set_abs_mode()
while true do
wait(500)
if Read_vel() == 0 and math.abs(Read_position()) < 5 then
break
end
end
return true
end
-- pos unit: millimeters
function Move(pos,move_speed)
if move_speed==nil then
move_speed=80000
end
-- if pos > 1540 or pos < -350 then
-- print('Exceeds the linear axis motion range')
-- return false
-- end
if Read_vel() ~= 0 then
print('The seventh axis is moving and cannot accept move commands.')
return false
end
pos_driver = math.modf(pos / 32 * 4000) --#set the position immediate value, 4000unit = 9mm, correction: 4000unit = 32mm
print(pos_driver)
Set_position(pos_driver)
Set_position_speed(move_speed)
Go_position_EI()
while true do
if math.abs(Read_position() - pos) < 4 then
return true
else
wait(500)
end
end
end
Flange Modbus RTU gripper control
-- Flange Modbus example
-- Generally, the gripper is controlled directly by calling the Lua API or the SDK. Here we take the flange Modbus control method as an example.
-- This helps to understand that when adding other communication modules at the end, if the 485 wire is paralleled on the gripper control wire, similar interfaces can be used to control the added communication module
local mb = modbus.new_flange() --Since the serial port address of the flange is fixed, there is no need to configure the serial port as when instantiating the control box Modbus
-- Configure Modbus
mb:set_timeout(600)
mb:set_slave(0x01)
mb:write_single_register(0x9c40, 100) --amplitude control
wait(100)
mb:write_single_register(0x9c41, 10) --force control
amplitude = mb:read_holding_registers(0x9c45, 1) --read the amplitude
print(amplitude)
force = mb:read_holding_registers(0x9c46, 1) --read the force
print(force)
--To prevent errors, use pcall to catch errors
success, result = pcall(function() return mb:read_holding_registers(0x9c46, 1) end)
if not success then
print("Error: ", result)
else:
print(result)
end
Modbus TCP example
-- Through Modbus TCP communication interaction, query the value of a fixed register, call a certain scene action according to the protocol, and query the action completion status and cancel the task according to the protocol
-- The specific implementation is as follows
--Create a new scene A and set it as the default program (auto-start on boot), with the following content:
start_task('10026', nil, nil, true, 1) --Modify the main scene number according to the specific situation. Call the main scene as a parallel task
-- Note: '10026' called here is the main scene, which has an infinite loop logic that continuously reads a fixed address and performs corresponding actions according to the protocol.
-- The fourth parameter is true, meaning parallel execution. This main task only performs logic judgment and does not execute specific action commands. It does not block. Action logic and tasks are implemented by calling sub-tasks again
--The main task scene content is as follows:
--Redefine the underlying function on_robot_stop. When the emergency stop is pressed, only cancel the task, do not power down
function on_robot_stop(is_estop)
print("estop current TASK id:", task_id)
cancel_task(task_id)
end
mb = modbus.new_tcp('192.168.4.85', 22) --the control box acts as a client master station and sends requests
-- Configure Modbus
mb:set_timeout(500)
mb:set_slave(0x01)
function recv_data()
-- read
success, result = pcall(function() return mb:read_holding_registers(0x0000, 2) end)
if success then
print('recieve data ',result)
local data = string.char(table.unpack(result))
return data
else
print("Error: ", result)
end
end
function send_data(address,data)
--after receiving the task, execute it and give feedback
success, result = pcall(function() mb:write_multiple_registers(address, data) end)
if success then
return true
else
print("Error: ", result)
return false
end
end
--Note:
--1. In start_task("10001", {}, "", false, 1), the parameter false starts a serial task
--2. Note that if scene 10001 is started upon receiving 1, and then scene 10002 is started upon receiving 2, since serial tasks are queued,
-- the logic start_task("10002", {}, "", false, 1) will block until the previous serial task finishes
--3. Because the arm cannot execute the actions of scene A and the actions of scene B at the same time, scenes with motion logic generally execute serially
while true
do
wait(50) --read once every 50ms
data = recv_data()
if(data == "1")
then
task_id = start_task("10001", {}, "", false, 1)
local str = 1
local address = 0x0000
send_data(address, {1}) --execution feedback
elseif(data == "2")
then
task_id = start_task("10002", {}, "", false, 1)
local str = 2
local address = 0x0000
send_data(address, {2}) --execution feedback
elseif (data == "check") --check the task status
then
state = get_task_state(task_id)
--state task status: "NONE" no task; "WAIT" queuing; "RUNNING" running; "PAUSE" paused; "SUCCESS" ran successfully; "INTERRUPTING" stopping; "INTERRUPT" stopped; "FAIL" ran with failure
local str = state
local address = 0x0002
send_data(address, {string.byte(str, 1, #str)}) --feedback
if state=="NONE" or state=="SUCCESS" or state=="INTERRUPTING" or state=="INTERRUPT" or state=="FAIL":
local address = 0x0000
send_data(address, {0}) --feedback
elseif (data == "cancel") --cancel the task
then
task_id = get_main_task_id() --get the serial task ID
cancel_task(task_id)
local str = 0
local address = 0x0000
send_data(address, {0}) --feedback
end
end
Spatial Position Calculation
Relative point calculation
-- Relative position point calculation
-- Find the position relative to the end TCP coordinate system in the base coordinate system, and perform motion relative to the end
current_tcp_pose = get_target_tcp_pose() --current end coordinate
print('current_tcp_pose',current_tcp_pose)
offset_position = {0, 0, 0.1, 0, 0, 0} --offset relative to the end TCP position, moving forward 0.1m along the z-axis of the end TCP coordinate system
calculate_location = pose_times(current_tcp_pose, offset_position) --calculated new position
print('calculate_location',calculate_location)
movej(calculate_location, 1, 0.5, 0, 0)
--If you want the current position and orientation to undergo a pose transformation in a certain coordinate system.
----For example, relative to the base coordinate system, increase by 0.1 along the base z-axis
base = get_target_tcp_pose() --current end coordinate
print('current_tcp_pose',base)
delta = {0, 0, 0.1,0,0, 0} --move 0.1m along the z-axis of the frame coordinate system
frame ={0, 0, 0, 0, 0, 0} -- base coordinate system. Pose offset direction, only the orientation part is effective.
pose = pose_add(base, delta, frame)
print('pose',pose)
movej(pose, 1, 0.5, 0, 0)
----For example, relative to a new coordinate system rotated 45° about the y-axis so that the z-axis points diagonally upward, increase by 0.1 along the base z-axis
base = get_target_tcp_pose() --current end coordinate
print('current_tcp_pose',base)
delta = {0, 0, 0.1, 0, 0, 0} -- move 0.1m along the z-axis of the frame coordinate system
frame = {0, 0, 0, 0 , 0.78, 0} -- new coordinate system rotated 45° about the y-axis, with the z-axis pointing diagonally upward
pose = pose_add(base, delta, frame)
print('pose',pose)
movej(pose, 1, 0.5, 0, 0)
Motion Control
Trajectory reproduction
--Trajectory reproduction
name = "test" --name this dragged trajectory "test"
function on_robot_state(state)
if state == 11
then
-- Enter teach mode, start trajectory recording
start_record_trajectory(0.01)
end
if last_state == 11
then
-- Exit teach mode, finish trajectory recording
end_record_trajectory(name)
-- Reproduce the trajectory
move_trajectory(name)
end
last_state = state
end
while true
do
sleep(1000)
end
--While the above scene is running, press the teach button to start recording, drag to form a trajectory, and release the teach button to finish recording
--Create a new scene and call move_trajectory("test") to reproduce the trajectory
Drawing a five-pointed star
-- Draw a five-pointed star
function clone(x) --clone the point
return {x[1],x[2],x[3],x[4],x[5],x[6]}
end
local p = math.rad(36)/2
local vel = 0.06
local acc = 0.1
function draw_star(a, r)
local x = r * math.sin(p)
local y = r * math.cos(p)
print('x', x, 'y', y)
print('a', a)
movej(a, 0.4, 1, 0, 1)
wait(1000)
b = clone(a)
b[1] = a[1] - x
b[2] = a[2] - y
print('b', b)
movel(b, vel, acc, 0, 0)
c = clone(a)
c[1] = a[1] + r / 2
c[2] = a[2] - r / 2 * math.cos(math.rad(36))
print('c', c)
movel(c, vel, acc, 0, 0)
d = clone(a)
d[1] = a[1] - r / 2
d[2] = c[2]
print('d', d)
movel(d, vel, acc, 0, 0)
e = clone(a)
e[1] = a[1] + x
e[2] = a[2] - y
print('e', e)
movel(e, vel, acc, 0, 0)
movel(a, vel, acc, 0, 0)
end
-- a = get_actual_tcp_pose()
a = {-0.34, -0.12, 0.4, -1.57, 0, 0.25}
for i=0,2 do
a[3] = a[3] - 0.08
draw_star(a, 0.1+i*0.02)
end
a[2] = a[2] + 0.2
a[3] = a[3] - 0.04
for i=0,2 do
a[3] = a[3] + 0.08
draw_star(a, 0.1+i*0.02)
end
Two-robot synchronization
-- Two-robot synchronization: make one arm follow another arm to perform the same action
disable_auto_sync()
lebai = lebai_sdk.connect("127.0.0.1", false) -- local robot
target = lebai_sdk.connect("192.168.4.100", false) -- the controlled robot that performs the follow action
target:start_sys() -- enable the controlled robot
wait(3000)
-- Move the controlled robot to the current position of the local robot
pose = (lebai:get_kin_data())["actual_joint_pose"]
target:movej(pose, 0.1, 0.1, 0, 0)
target:wait_move()
print("start")
last_time = timestamp() --the millisecond timestamp of the last synchronization
start_flag =0 -- the local robot calls an action scene, start flag
while true
do
wait(10)
now = timestamp()
used_time = now-last_time
last_time = now
-- Synchronize the arm position
status = lebai:get_kin_data()
target:move_pvat(status["actual_joint_pose"], status["actual_joint_speed"], status["actual_joint_acc"], used_time/1000) --synchronous motion
-- Detect whether the motion has started
if start_flag == 0 then
start_flag = 1
scene_id = 10289 -- action scene ID
start_task(scene_id, nil, '', true, 1) -- the local arm starts moving according to the scene setting, and the other arm starts to follow
end
-- This is an infinite loop logic. After the local robot finishes the scene, manually teach the local robot, and the other arm will follow the motion too
-- To stop, manually stop the scene or press the emergency stop, or stop via a third-party script
end
Synchronized execution of one scene across multiple robots
-- Implement multiple robots executing one scene at the same time (all devices have the scene ID to execute)
C_POSE = {}
J_POSE = {}
--Disable auto sync
disable_auto_sync()
-- IP address list. Add if there are new robots, delete if there are none
local robot_ips = {
"127.0.0.1", -- master
"192.168.6.109" ,
"192.168.4.57", -- controlled robot 1
"192.168.4.109"
}
-- Table storing connection objects
local robots = {}
-- Create connections in batch
for i, ip in ipairs(robot_ips) do
success, result = pcall(function() return lebai_sdk.connect(ip, false) end)
if success then
robots[i] = result
print("Connection successful: " .. ip)
robots[i]:start_sys()
else
print("Connection failed: " .. ip)
end
end
wait(3000)
-- Move the controlled robots to the current position of the local robot
local pose = (robots[1]:get_kin_data())["actual_joint_pose"]
for i =1,#robot_ips do
if robots[i] then
robots[i]:movej(pose, 0.5, 0.5, 0, 0)
end
end
for i =1,#robot_ips do
if robots[i] then
robots[i]:wait_move()
end
end
print("Start action")
for i =1,#robot_ips do
if robots[i] then
robots[i]:start_task(10010, nil, '', true, 2)
end
end
print("All started")
IO Control
DI interaction control
-- By controlling the DI input signal, call the corresponding scene action
-- And control task cancellation through the shoulder button
tasks = {}--store the started task IDs
-- Cancel all tasks
function cancel_all_tasks()
for k,v in pairs(tasks) do
print(k,v)
cancel_task(v)
tasks[k] = nil
end
end
--Note:
-- 1. In start_task("10160", {}, "", true, 1), the parameter true starts a parallel task
-- 2. Note that if DI0 is pressed to start scene 10160, and then DI1 is pressed to start scene 10161, since tasks execute in parallel,
-- if both scenes have motion logic, executing the motion of two scenes at the same time may cause motion confusion. Therefore, call cancel_all_tasks() before a new task is executed
button_type = "SHOULDER" --"FLANGE_BTN" is the flange flat button "SHOULDER" is the shoulder light button control
while true do
wait(100) --check once every 0.1s
if get_di(0) == 1 then
wait_di(0, 0, "=") -- wait for the button to be released
cancel_all_tasks()
tasks["0"] = start_task("10160", {}, "", true, 1)
end
if get_di(1) == 1 then
wait_di(1, 0, "=") -- wait for the button to be released
cancel_all_tasks()
tasks["1"] = start_task("10160", {}, "", true, 1)
end
if lebai:get_di(button_type, 0)==1 then
while lebai:get_di(button_type, 0)==1 do
wait(20)
end
cancel_all_tasks()
tasks["0"] = start_task("10160", {}, "", true, 1)
end
end
DI interaction control of the gripper
-- Implement pressing the shoulder button 3 times consecutively to control the opening/closing of the gripper
set_auto('ARM_POWER' , true) -- set auto power-on after boot
set_auto('ENABLE_JOINT' , true) --set auto-start to idle state after power-on
init_claw(true) --initialize the gripper
sync()
claw_status ='close'
set_claw(100,0)
sync()
button_type = "SHOULDER" --"FLANGE_BTN" is the flange flat button "SHOULDER" is the shoulder light button control
button_times = 0 --number of button presses. Press 3 times consecutively to open or close the gripper
interval_flag = timestamp()
while true do
wait(20) --reduce CPU usage
if timestamp()-interval_flag >2000 then
button_times = 0
end
if lebai:get_di(button_type, 0)==1 then
while lebai:get_di(button_type, 0)==1 do
wait(20)
end
button_times = button_times + 1
if button_times ==1 then
interval_flag = timestamp()
elseif button_times == 3 then
button_times = 0
if claw_status =='close' then
claw_status='open'
print(claw_status)
set_claw(100,100)
sync()
elseif claw_status=='open' then
claw_status='close'
set_claw(100,0)
print(claw_status)
sync()
end
end
end
end
DO control of the pump
-- DO control of the suction pump
-- During motion, when the suction pump sensor detects contact with an object, the suction pump starts to suck the corresponding object, places it in a suitable position, and then the suction pump releases it
-- For the specific wiring and assembly method, see the official website
--First move to above the target position of the object to be sucked
object_pose = {j1=-1.9267757433836,j2=-1.9655087582777,j3=2.2828510337716,j4=-0.34313232749017,j5=-3.9459738292373,j6=0}
object_pose_top = pose_add(object_pose, {0,0,0.15,0,0,0}) --based on the object_pose point, move 0.15m along the z-axis of the base coordinate system to form a new coordinate
movej(object_pose_top, 0.5, 0.5, 0, 0)
--Move gradually downward from above the object. When the end DI is triggered, the pump has contacted the object. Stop moving and turn on the pump
movel_until(object_pose, 0.2, 0.08, 0, function() return get_flange_di(0) == 1 end)
--Turn on the pump to suck
set_do(0, 1) -- turn on the pump
set_do(1, 0) -- close the valve
-- Move to above the target position of the object
movej(object_pose_top, 0.5, 0.5, 0, 0)
put_pose = {j1=0,j2=-1.9655087582777,j3=2.2828510337716,j4=-0.34313232749017,j5=-3.9459738292373,j6= 0}
movej(put_pose, 0.5, 0.5, 0, 0)
--Turn off the pump to release
set_do(0, 0) -- turn off the pump
set_do(1, 1) -- open the valve
wait(500)
set_do(1, 0) -- close the valve