--- Auto generated by 'aubo_scope' v0.29.5-rc.5 2025-03-12 14:27:43
--- DO NOT MODIFY IT!
local app = {}
local aubo = require('aubo')
local sched = sched or aubo.sched
local math = aubo.math or math
local realtime = aubo.realtime or realtime

local sleep = sched.sleep
local thread = sched.thread
local sync = sched.sync
local run = sched.run
local kill = sched.kill
local halt = sched.halt

app.PRIORITY = 1000 -- set the app priority, which determines app execution order
app.VERSION = "0.1"
app.VENDOR = "Aubo Robotics"

function p_spring()
    local _ENV = sched.select_robot(1)
    setCollisionStopType(0)
    setCollisionLevel(10)
    setFreedriveDamp({0.5,0.5,0.5,0.5,0.5,0.5})
    INF = 9999
    setHomePosition({0,-0.2617993877991494,1.74532925199433,0.4363323129985824,1.570796326794897,0}    )
    modbusDeleteAllSignals()
    setDigitalInputActionDefault()
    setDigitalOutputRunstateDefault()
    modbusAddSignal("192.168.1.1,502", 1, 55, 0, "ok", false)
    modbusAddSignal("192.168.1.1,502", 1, 70, 1, "start", false)
    setPayload(0.322612, {0,0,0}, {0,0,0}, {0,0,0,0,0,0,0,0,0})
    setTcpOffset({0,0,0.12,0,0,0})
    setToolVoltageOutputDomain(24)
    setToolIoInput(0, true)
    setToolIoInput(1, true)
    setToolIoInput(2, true)
    setToolIoInput(3, true)
    u5e73u9762_0= {-0.08588358002836446,-0.5674440021646662,-0.1081947151074858,3.128722971588715,0.0309080693545616,1.572465810756998}
    hikvision_feature= {0,0,0,0,0,0}
    hikvision_feature_0= {0,0,0,0,0,0}
    hikvision_feature_1= {0,0,0,0,0,0}
    hikvision_feature_2= {0,0,0,0,0,0}
    hikvision_feature_3= {0,0,0,0,0,0}
    hikvision_feature_4= {0,0,0,0,0,0}
    hikvision_feature_5= {0,0,0,0,0,0}
    hikvision_feature_6= {0,0,0,0,0,0}
    hikvision_feature_7= {0,0,0,0,0,0}
    hikvision_feature_8= {0,0,0,0,0,0}
    hikvision_feature_9= {0,0,0,0,0,0}
    hikvision_feature_10= {0,0,0,0,0,0}
    hikvision_feature_11= {0,0,0,0,0,0}
    hikvision_feature_12= {0,0,0,0,0,0}
    u5e73u9762_1= {0.132279266494938,-0.2332497245440143,0.3848683795899941,3.141209752120758,-0.0001298877750615784,1.240573333891147}
    Point_judgment = false
    xn_search_ok = false
    u5faau73af_2 = nil
    u5faau73af_3 = nil
    u5faau73af_4 = nil
    u5faau73af_5 = nil
    setPlanContext(sched.current_thread_id(), 1, "初始变量")
    Base = {0,0,0,0,0,0}
    Tool = {0,0,0,0,0,0}
    u5e73u9762_0 = {-0.0858835800283645,-0.567444002164666,-0.108194715107486,3.12872297158871,0.0309080693545616,1.572465810757}
    hikvision_feature = {0,0,0,0,0,0}
    hikvision_feature_0 = {0,0,0,0,0,0}
    hikvision_feature_1 = {0,0,0,0,0,0}
    hikvision_feature_2 = {0,0,0,0,0,0}
    hikvision_feature_3 = {0,0,0,0,0,0}
    hikvision_feature_4 = {0,0,0,0,0,0}
    hikvision_feature_5 = {0,0,0,0,0,0}
    hikvision_feature_6 = {0,0,0,0,0,0}
    hikvision_feature_7 = {0,0,0,0,0,0}
    hikvision_feature_8 = {0,0,0,0,0,0}
    hikvision_feature_9 = {0,0,0,0,0,0}
    hikvision_feature_10 = {0,0,0,0,0,0}
    hikvision_feature_11 = {0,0,0,0,0,0}
    hikvision_feature_12 = {0,0,0,0,0,0}
    u5e73u9762_1 = {0.132279266494938,-0.233249724544014,0.384868379589994,3.14120975212076,-0.000129887775061578,1.24057333389115}
    u8defu70b9_2_p = {0.4398227633407494,-0.2299381835642074,0.3840640645876587,-3.141571647263504,1.275676625145365e-05,1.068885690294249}    
    u8defu70b9_2_q = {-0.2343911374976218,-0.02924621001872988,1.319569433212051,-0.222004314809486,1.570789576604411,0.2675194992167701}    
    
    setPlanContext(sched.current_thread_id(), 4, "机器人编程")
    setPlanContext(sched.current_thread_id(), 5, "设置工具负载偏置和初始化参数")
    -- 设置工具负载偏置和初始化参数
        
        setPlanContext(sched.current_thread_id(), 7, "init.lua")
        --subscript context start
        INF = 9999
        M_PI = 3.1415926
        start_record = false
        function init()
            timeout = -1
            ret = 0
            sleep(0) -- 等待前面动作全部执行完成
            setCollisionLevel(0)
            -- 选择传感器
            selectTcpForceSensor("embedded")
            --selectTcpForceSensor("xinjingcheng_ftsensor")
            -- 传感器安装位姿
            sensor_pose = {0, 0, 0, 0, 0, 0}
            --sensor_pose = {0, 0, 0.028, 0, 0, 0}
            setTcpForceSensorPose(sensor_pose)
            
            -- 传感器偏置
            -- setTcpForceOffset(force_offset)
            
            usercoord_in_base = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}
            -- 力控坐标系选择 用户坐标系
            feature = usercoord_in_base
            frame_type = 2
            -- 力控速度限制
            speed_limits = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}
            -- 设置力控参数
            setDynamicModel(M, D, K)
            -- 设置目标力
            setTargetForce(feature, enable, goal_wrench, speed_limits, frame_type)
            min_force = -INF
            select = {0.0, 0.0, 1.0, 0.0, 0.0, 0.0}
            feature1 = usercoord_in_base
            type1 = 2
            timeout = -1
            count = 1.0
            outside = 1.0
            args = {}
            table.insert(args, min_force)
            table.insert(args, 5)
            for i = 1, 6 do
                table.insert(args, select[i])
            end
            for i = 1, 6 do
                table.insert(args, feature1[i])
            end
            table.insert(args, type1)
            table.insert(args, count)
            table.insert(args, outside)
            setCondAdvanced("ConditionContactForceOnUsercoord", args, timeout)
        end
        function condFullfiled(timeout)
            local ret = 0
            condition_timer = 0
            local pose_size = 0
            for k, v in pairs(target_pose) do
                pose_size = pose_size + 1
            end
            while not (isCondFullfiled()) do
                if ((timeout > 0) and (condition_timer > timeout)) then
                    textmsg(" 条件等待超时 !  timer = " .. condition_timer)
                    ret = -1
                    break
                end
                sync()
            end
            if (isCondFullfiled()) then
                -- 条件满足
                textmsg(" 力控终止条件满足 !")
                ret = 1
            end
            condition_timer_is_counting = false
            return ret
        end
        function startForceControl()
            start_record = true
            sleep(0)
            fcEnable()
            -- 激活终止条件
            setCondActive()
        end
        function stopForceControl()
            condFullfiled(15)
            stopJoint(2)
            sleep(0)
            fcDisable()
        end
        -- CSV 文件路径
        local currentTime = os.date("%Y-%m-%d %H.%M.%S")
        filename = "test_data.csv"
        local data = {
            {"Program start time", currentTime},
            {
                "pos_x",
                "pos_y",
                "pos_z",
                "pos_rx",
                "pos_ry",
                "pos_rz",
                "vel_x",
                "vel_y",
                "vel_z",
                "vel_rx",
                "vel_ry",
                "vel_rz",
                "force_x",
                "force_y",
                "force_z",
                "force_rx",
                "force_ry",
                "force_rz"
            }
        }
        -- 写入数据到 CSV 文件
        file = io.open(filename, "w")
        for _, row in ipairs(data) do
            file:write(table.concat(row, ",") .. "\n")
        end
        file:flush()
        textmsg("机器人数据写入到 " .. filename)
        handler_record =
            thread(
            function()
                while true do
                    if (start_record) then
                        local tcp_pose = getTcpPose()
                        local tcp_speed = getTcpSpeed()
                        local tcp_force = getTcpForce()
                        local frame_force=forceTrans(poseInverse(tcp_pose), tcp_force);
                        file:write(table.concat(tcp_pose, ", ").. ", ")
                        file:write(table.concat(tcp_speed, ", ").. ", ")
                        file:write(table.concat(frame_force, ", ") .. "\n")
                        file:flush()
                    end
                    sync()
                end
            end,
            "handler_record"
        )
        run(handler_record)
        --subscript context end
    -- 设置工具负载偏置和初始化参数
    
    setPlanContext(sched.current_thread_id(), 8, "设置力控参数MDK，目标力大小goal_wrench和开启力控方向enable")
    -- 设置力控参数MDK，目标力大小goal_wrench和开启力控方向enable
        setPlanContext(sched.current_thread_id(), 9, "M = {30.0, 30.0, 30.0, 10.0, 10.0, 10.0}")
        --subscript context start
        M = {30.0, 30.0, 30.0, 10.0, 10.0, 10.0}
        --subscript context end
        
        setPlanContext(sched.current_thread_id(), 10, "D = {1000.0, 1000.0, 100.0, 100.0, 100.0, 100.0}")
        --subscript context start
        D = {1000.0, 1000.0, 100.0, 100.0, 100.0, 100.0}
        --subscript context end
        
        setPlanContext(sched.current_thread_id(), 11, "K = {1000.0, 1000.0, 100.0, 100.0, 100.0, 100.0}")
        --subscript context start
        K = {1000.0, 1000.0, 100.0, 100.0, 100.0, 100.0}
        --subscript context end
        
        setPlanContext(sched.current_thread_id(), 12, "goal_wrench = {0, 0, 0, 0, 0, 0}")
        --subscript context start
        goal_wrench = {0, 0, 0, 0, 0, 0}
        --subscript context end
        
        setPlanContext(sched.current_thread_id(), 13, "enable ={false, false, true, false, false, false}")
        --subscript context start
        enable ={false, false, true, false, false, false}
        --subscript context end
        
        setPlanContext(sched.current_thread_id(), 14, "init()")
        --subscript context start
        init()
        --subscript context end
    -- 设置力控参数MDK，目标力大小goal_wrench和开启力控方向enable
    
    setPlanContext(sched.current_thread_id(), 15, "运动到初始点")
    -- 运动到初始点
        setPlanContext(sched.current_thread_id(), 16, "直线运动")
        setPlanContext(sched.current_thread_id(), 17, "路点_2")
        moveLine(u8defu70b9_2_p, 0.5, 0.01, 0, 0)
    -- 运动到初始点
    
    setPlanContext(sched.current_thread_id(), 18, "开启力控")
    -- 开启力控
        setPlanContext(sched.current_thread_id(), 19, "startForceControl()")
        --subscript context start
        startForceControl()
        --subscript context end
    -- 开启力控
    
    setPlanContext(sched.current_thread_id(), 20, "结束力控")
    -- 结束力控
        setPlanContext(sched.current_thread_id(), 21, "stopForceControl()")
        --subscript context start
        stopForceControl()
        --subscript context end
    -- 结束力控
end

function app:call_after_stop(name)
    if name == "rob1" then
        setPlanContext(sched.current_thread_id(), 3, "fcDisable()")
        --subscript context start
        fcDisable()
        --subscript context end
    end
end

function app:start(api)
  --
  self.api = api
  print("start---")
  p_spring()
end

function app:robot_error_handler(name, err)
  --
  print("An error hanppen to robot "..name)
end

-- return our app object
return app