diff --git a/urRobotApp/Db/path.db b/urRobotApp/Db/path.db
deleted file mode 100644
index 893fbb7..0000000
--- a/urRobotApp/Db/path.db
+++ /dev/null
@@ -1,11 +0,0 @@
-record(stringout, "$(P)Path$(N)") {
- field(DESC,"Description of path")
- field(VAL, "$(DESC=Path $(N))")
-}
-
-record(luascript, "$(P)Path$(N):Go") {
- field(DESC, "Executes path")
- field(CODE, "@paths.lua path_go({prefix='$(P)',N=$(N),kmax=$(KMAX),port='$(CTRL_PORT)'})")
- field(PREC, 0)
- field(SYNC, "Async")
-}
diff --git a/urRobotApp/iocsh/load_paths.lua b/urRobotApp/iocsh/load_paths.lua
index f922a8a..9f88686 100644
--- a/urRobotApp/iocsh/load_paths.lua
+++ b/urRobotApp/iocsh/load_paths.lua
@@ -1,3 +1,5 @@
+local db = require("db")
+
NPATHS = tonumber(N)
KMAX = tonumber(K)
for n = 1, NPATHS do
@@ -7,6 +9,13 @@ for n = 1, NPATHS do
end
end
for n = 1, NPATHS do
- local macros = string.format("P=%s,N=%d,KMAX=%d,DESC='Path %d',CTRL_PORT='%s'", P, n, KMAX, n, CTRL_PORT)
- dbLoadRecords("$(URROBOT)/urRobotApp/Db/path.db", macros)
+ db.record("stringout", P .. "Path" .. n) {
+ DESC = "Description of path",
+ VAL = "Path " .. n
+ }
+ db.record("bo", P .. "Path" .. n .. ":Go") {
+ ZNAM = "Idle",
+ ONAM = "Go",
+ }
+ luaLoadFile("paths_seq.lua", string.format("P=%s,N=%d,KMAX=%d,CTRL_PORT=%s,STATE=path_seq_%d", P, n, KMAX, CTRL_PORT, n))
end
diff --git a/urRobotApp/iocsh/paths.iocsh b/urRobotApp/iocsh/paths.iocsh
index d1e9815..469ecf3 100644
--- a/urRobotApp/iocsh/paths.iocsh
+++ b/urRobotApp/iocsh/paths.iocsh
@@ -9,6 +9,7 @@
# Add $(URROBOT)/urRobotApp/src/lua to LUA_SCRIPT_PATH
epicsEnvSet("LUA_SCRIPT_PATH", "$(LUA_SCRIPT_PATH=):$(URROBOT)/urRobotApp/src/lua")
+luaAddModule("$(LUA)")
# Load waypoint and path databases
luash("$(URROBOT)/urRobotApp/iocsh/load_waypoints.lua", "P=$(PREFIX),N=$(N_WP=30)")
diff --git a/urRobotApp/op/adl/path10x.adl b/urRobotApp/op/adl/path10x.adl
index 802d61e..acbb987 100644
--- a/urRobotApp/op/adl/path10x.adl
+++ b/urRobotApp/op/adl/path10x.adl
@@ -1068,7 +1068,7 @@ menu {
height=20
}
control {
- chan="$(P)Path$(N):Go.PROC"
+ chan="$(P)Path$(N):Go.VAL"
clr=14
bclr=51
}
diff --git a/urRobotApp/op/adl/path30x.adl b/urRobotApp/op/adl/path30x.adl
index 6364aeb..a1ad0e8 100644
--- a/urRobotApp/op/adl/path30x.adl
+++ b/urRobotApp/op/adl/path30x.adl
@@ -2888,7 +2888,7 @@ menu {
height=20
}
control {
- chan="$(P)Path$(N):Go.PROC"
+ chan="$(P)Path$(N):Go.VAL"
clr=14
bclr=51
}
diff --git a/urRobotApp/op/adl/path_top10x.adl b/urRobotApp/op/adl/path_top10x.adl
index 80dc848..dc3b7ac 100644
--- a/urRobotApp/op/adl/path_top10x.adl
+++ b/urRobotApp/op/adl/path_top10x.adl
@@ -95,7 +95,7 @@ display {
height=20
}
control {
- chan="$(P)Path$(N1):Go.PROC"
+ chan="$(P)Path$(N1):Go.VAL"
clr=14
bclr=51
}
@@ -110,7 +110,7 @@ display {
height=20
}
control {
- chan="$(P)Path$(N2):Go.PROC"
+ chan="$(P)Path$(N2):Go.VAL"
clr=14
bclr=51
}
@@ -125,7 +125,7 @@ display {
height=20
}
control {
- chan="$(P)Path$(N3):Go.PROC"
+ chan="$(P)Path$(N3):Go.VAL"
clr=14
bclr=51
}
@@ -140,7 +140,7 @@ display {
height=20
}
control {
- chan="$(P)Path$(N4):Go.PROC"
+ chan="$(P)Path$(N4):Go.VAL"
clr=14
bclr=51
}
@@ -155,7 +155,7 @@ display {
height=20
}
control {
- chan="$(P)Path$(N5):Go.PROC"
+ chan="$(P)Path$(N5):Go.VAL"
clr=14
bclr=51
}
@@ -397,7 +397,7 @@ text {
height=20
}
control {
- chan="$(P)Path$(N6):Go.PROC"
+ chan="$(P)Path$(N6):Go.VAL"
clr=14
bclr=51
}
@@ -442,7 +442,7 @@ text {
height=20
}
control {
- chan="$(P)Path$(N7):Go.PROC"
+ chan="$(P)Path$(N7):Go.VAL"
clr=14
bclr=51
}
@@ -487,7 +487,7 @@ text {
height=20
}
control {
- chan="$(P)Path$(N8):Go.PROC"
+ chan="$(P)Path$(N8):Go.VAL"
clr=14
bclr=51
}
@@ -532,7 +532,7 @@ text {
height=20
}
control {
- chan="$(P)Path$(N9):Go.PROC"
+ chan="$(P)Path$(N9):Go.VAL"
clr=14
bclr=51
}
@@ -577,7 +577,7 @@ text {
height=20
}
control {
- chan="$(P)Path$(N10):Go.PROC"
+ chan="$(P)Path$(N10):Go.VAL"
clr=14
bclr=51
}
diff --git a/urRobotApp/op/adl/path_top5x.adl b/urRobotApp/op/adl/path_top5x.adl
index 96eadbd..8bcd14f 100644
--- a/urRobotApp/op/adl/path_top5x.adl
+++ b/urRobotApp/op/adl/path_top5x.adl
@@ -95,7 +95,7 @@ display {
height=20
}
control {
- chan="$(P)Path$(N1):Go.PROC"
+ chan="$(P)Path$(N1):Go.VAL"
clr=14
bclr=51
}
@@ -110,7 +110,7 @@ display {
height=20
}
control {
- chan="$(P)Path$(N2):Go.PROC"
+ chan="$(P)Path$(N2):Go.VAL"
clr=14
bclr=51
}
@@ -125,7 +125,7 @@ display {
height=20
}
control {
- chan="$(P)Path$(N3):Go.PROC"
+ chan="$(P)Path$(N3):Go.VAL"
clr=14
bclr=51
}
@@ -140,7 +140,7 @@ display {
height=20
}
control {
- chan="$(P)Path$(N4):Go.PROC"
+ chan="$(P)Path$(N4):Go.VAL"
clr=14
bclr=51
}
@@ -155,7 +155,7 @@ display {
height=20
}
control {
- chan="$(P)Path$(N5):Go.PROC"
+ chan="$(P)Path$(N5):Go.VAL"
clr=14
bclr=51
}
diff --git a/urRobotApp/op/ui/autoconvert/path10x.ui b/urRobotApp/op/ui/autoconvert/path10x.ui
index 68150f1..2bc1923 100644
--- a/urRobotApp/op/ui/autoconvert/path10x.ui
+++ b/urRobotApp/op/ui/autoconvert/path10x.ui
@@ -2820,7 +2820,7 @@ caMenu{
EPushButton::WidthAndHeight
- $(P)Path$(N):Go.PROC
+ $(P)Path$(N):Go.VAL
@@ -5707,4 +5707,4 @@ caMenu{
caTextEntry_20
-
\ No newline at end of file
+
diff --git a/urRobotApp/op/ui/autoconvert/path30x.ui b/urRobotApp/op/ui/autoconvert/path30x.ui
index 36f2222..28f1b52 100644
--- a/urRobotApp/op/ui/autoconvert/path30x.ui
+++ b/urRobotApp/op/ui/autoconvert/path30x.ui
@@ -7860,7 +7860,7 @@ caMenu{
EPushButton::WidthAndHeight
- $(P)Path$(N):Go.PROC
+ $(P)Path$(N):Go.VAL
@@ -15407,4 +15407,4 @@ caMenu{
caTextEntry_60
-
\ No newline at end of file
+
diff --git a/urRobotApp/op/ui/autoconvert/path_top10x.ui b/urRobotApp/op/ui/autoconvert/path_top10x.ui
index 201d56e..5d02b7a 100644
--- a/urRobotApp/op/ui/autoconvert/path_top10x.ui
+++ b/urRobotApp/op/ui/autoconvert/path_top10x.ui
@@ -123,7 +123,7 @@ caMenu{
EPushButton::WidthAndHeight
- $(P)Path$(N1):Go.PROC
+ $(P)Path$(N1):Go.VAL
@@ -162,7 +162,7 @@ caMenu{
EPushButton::WidthAndHeight
- $(P)Path$(N2):Go.PROC
+ $(P)Path$(N2):Go.VAL
@@ -201,7 +201,7 @@ caMenu{
EPushButton::WidthAndHeight
- $(P)Path$(N3):Go.PROC
+ $(P)Path$(N3):Go.VAL
@@ -240,7 +240,7 @@ caMenu{
EPushButton::WidthAndHeight
- $(P)Path$(N4):Go.PROC
+ $(P)Path$(N4):Go.VAL
@@ -279,7 +279,7 @@ caMenu{
EPushButton::WidthAndHeight
- $(P)Path$(N5):Go.PROC
+ $(P)Path$(N5):Go.VAL
@@ -999,7 +999,7 @@ caMenu{
EPushButton::WidthAndHeight
- $(P)Path$(N6):Go.PROC
+ $(P)Path$(N6):Go.VAL
@@ -1131,7 +1131,7 @@ caMenu{
EPushButton::WidthAndHeight
- $(P)Path$(N7):Go.PROC
+ $(P)Path$(N7):Go.VAL
@@ -1263,7 +1263,7 @@ caMenu{
EPushButton::WidthAndHeight
- $(P)Path$(N8):Go.PROC
+ $(P)Path$(N8):Go.VAL
@@ -1395,7 +1395,7 @@ caMenu{
EPushButton::WidthAndHeight
- $(P)Path$(N9):Go.PROC
+ $(P)Path$(N9):Go.VAL
@@ -1527,7 +1527,7 @@ caMenu{
EPushButton::WidthAndHeight
- $(P)Path$(N10):Go.PROC
+ $(P)Path$(N10):Go.VAL
@@ -1696,4 +1696,4 @@ caMenu{
caMessageButton_10
-
\ No newline at end of file
+
diff --git a/urRobotApp/op/ui/autoconvert/path_top5x.ui b/urRobotApp/op/ui/autoconvert/path_top5x.ui
index 9dd35b3..6f91804 100644
--- a/urRobotApp/op/ui/autoconvert/path_top5x.ui
+++ b/urRobotApp/op/ui/autoconvert/path_top5x.ui
@@ -123,7 +123,7 @@ caMenu{
EPushButton::WidthAndHeight
- $(P)Path$(N1):Go.PROC
+ $(P)Path$(N1):Go.VAL
@@ -162,7 +162,7 @@ caMenu{
EPushButton::WidthAndHeight
- $(P)Path$(N2):Go.PROC
+ $(P)Path$(N2):Go.VAL
@@ -201,7 +201,7 @@ caMenu{
EPushButton::WidthAndHeight
- $(P)Path$(N3):Go.PROC
+ $(P)Path$(N3):Go.VAL
@@ -240,7 +240,7 @@ caMenu{
EPushButton::WidthAndHeight
- $(P)Path$(N4):Go.PROC
+ $(P)Path$(N4):Go.VAL
@@ -279,7 +279,7 @@ caMenu{
EPushButton::WidthAndHeight
- $(P)Path$(N5):Go.PROC
+ $(P)Path$(N5):Go.VAL
@@ -1021,4 +1021,4 @@ caMenu{
caMessageButton_5
-
\ No newline at end of file
+
diff --git a/urRobotApp/src/lua/paths_seq.lua b/urRobotApp/src/lua/paths_seq.lua
new file mode 100644
index 0000000..04a753e
--- /dev/null
+++ b/urRobotApp/src/lua/paths_seq.lua
@@ -0,0 +1,166 @@
+local epics = require("epics")
+local asyn = require("asyn")
+local seq = require("seq")
+
+local N = math.floor(tonumber(N))
+local KMAX = math.floor(tonumber(KMAX))
+
+luaRegisterState(STATE)
+
+local go_pv = epics.pv(P .. "Path" .. N .. ":Go")
+local stop_path_pv = epics.pv(P .. "Control:stop_path")
+local safety_pv = epics.pv(P .. "Receive:SafetyStatusBits")
+local sync_joint_disa_pv = epics.pv(P .. "Control:sync_joint_cmd.DISA")
+local sync_pose_disa_pv = epics.pv(P .. "Control:sync_pose_cmd.DISA")
+
+local wp_idx = 0
+local count_before = 0
+local wp_action_original = 0
+
+local wp = {}
+
+local function restore_action_override()
+ if wp.action_override ~= 0 then
+ wp.action_opt_val.VAL = wp_action_original
+ wp.set_action_opt.LNK0 = wp.action_opt.name .. " PP"
+ wp.set_action_opt.PROC = 1
+ end
+end
+
+local function finish_path(msg)
+ print(msg)
+ go_pv.VAL = 0
+ sync_joint_disa_pv.VAL = 0
+ sync_pose_disa_pv.VAL = 0
+end
+
+local prog = seq.program("path" .. N, { poll = 0.01 })
+
+prog:state("idle", {
+ seq.when(function() return go_pv.VAL == 1 end) {
+ action = function()
+ sync_joint_disa_pv.VAL = 1
+ sync_pose_disa_pv.VAL = 1
+ stop_path_pv.VAL = 0
+ wp_idx = 1
+ end,
+ next = "check_waypoint",
+ },
+})
+
+prog:state("check_waypoint", {
+ options = { always_enter = true },
+
+ entry = function()
+ wp = { action_override = 0 }
+ end,
+
+ seq.when(function() return wp_idx > KMAX end) {
+ action = function() finish_path("Path " .. N .. " completed!") end,
+ next = "idle",
+ },
+
+ seq.when(function() return stop_path_pv.VAL == 1 end) {
+ action = function() finish_path("Path " .. N .. " stopped") end,
+ next = "idle",
+ },
+
+ seq.when(function()
+ local path_wp = P .. "Path" .. N .. ":" .. wp_idx
+ local num = epics.get(path_wp .. ":Number")
+ if num <= 0 then return false end
+ return epics.get(path_wp .. ":Enabled") ~= 0
+ end) {
+ next = "configure_move",
+ },
+
+ seq.when() {
+ action = function() wp_idx = wp_idx + 1 end,
+ next = "check_waypoint",
+ },
+})
+
+prog:state("configure_move", {
+ entry = function()
+ local path_wp = P .. "Path" .. N .. ":" .. wp_idx
+ local wp_type_val = epics.get(path_wp .. ":Type")
+ local wp_type = (wp_type_val == 0.0) and "L" or "J"
+ local wp_num = math.floor(epics.get(path_wp .. ":Number"))
+ local wp_base = P .. "Waypoint" .. wp_type .. ":" .. wp_num
+
+ wp = {
+ action_override = math.floor(epics.get(path_wp .. ":ActionOverride")),
+ action_opt = epics.pv(wp_base .. ":ActionOpt"),
+ action_opt_val = epics.pv(path_wp .. ":action_opt_val"),
+ set_action_opt = epics.pv(path_wp .. ":set_action_opt"),
+ move = epics.pv(wp_base .. ":move" .. wp_type),
+ busy = epics.pv(path_wp .. ":Busy"),
+ }
+
+ if wp.action_override ~= 0 then
+ wp_action_original = math.floor(wp.action_opt.VAL)
+ wp.action_opt_val.VAL = wp.action_override
+ wp.set_action_opt.LNK0 = wp.action_opt.name .. " PP"
+ wp.set_action_opt.PROC = 1
+ end
+
+ count_before = asyn.getIntegerParam(CTRL_PORT, "MOTION_DONE_COUNT")
+ print("Moving to waypoint " .. wp_base .. "...")
+ wp.busy.VAL = 1
+ wp.move.PROC = 1
+ end,
+
+ seq.when() {
+ next = "wait_motion",
+ },
+})
+
+prog:state("wait_motion", {
+ seq.when(function()
+ return asyn.getIntegerParam(CTRL_PORT, "MOTION_DONE_COUNT") ~= count_before
+ end) {
+ next = "finish_waypoint",
+ },
+
+ seq.when(function() return stop_path_pv.VAL == 1 end) {
+ action = function()
+ wp.busy.VAL = 0
+ restore_action_override()
+ finish_path("Path " .. N .. " stopped")
+ end,
+ next = "idle",
+ },
+
+ seq.when(function() return safety_pv.VAL ~= 1 end) {
+ action = function()
+ stop_path_pv.VAL = 1
+ wp.busy.VAL = 0
+ restore_action_override()
+ finish_path("Path " .. N .. " stopped (safety)")
+ end,
+ next = "idle",
+ },
+
+ seq.when(seq.delay(300.0)) {
+ action = function()
+ print("Error: Timeout exceeded waiting for motion to complete")
+ wp.busy.VAL = 0
+ finish_path("Path " .. N .. " timed out")
+ end,
+ next = "idle",
+ },
+})
+
+prog:state("finish_waypoint", {
+ entry = function()
+ wp.busy.VAL = 0
+ restore_action_override()
+ wp_idx = wp_idx + 1
+ end,
+
+ seq.when() {
+ next = "check_waypoint",
+ },
+})
+
+seq.register(prog)