--- Auto generated by 'aubo_scope' v0.29.1-rc.25 2024-07-09 15:07:26
--- DO NOT MODIFY IT!
local app = {}
local aubo = require('aubo')
local sched = sched or aubo.sched
local math = aubo.math or math

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_guide()
    local _ENV = sched.select_robot(1)
    setCollisionStopType(1)
    setCollisionLevel(6)
    setHardwareCustomParameters("[over_force_config] \n    is_enable =true")
    setFreedriveDamp({0.5,0.5,0.5,0.5,0.5,0.5})
    setHomePosition({0,-0.2617993877991494,1.74532925199433,0.4363323129985824,1.570796326794897,0}    )
    modbusDeleteAllSignals()
    setDigitalInputActionDefault()
    setDigitalOutputRunstateDefault()
    setPayload(0, {0,0,0}, {0,0,0}, {0,0,0,0,0,0,0,0,0})
    setTcpOffset({0,0,0,0,0,0})
    setToolVoltageOutputDomain(0)
    setToolIoInput(0, true)
    setToolIoInput(1, true)
    setToolIoInput(2, true)
    setToolIoInput(3, true)
    local function add(num1,num2)
        return num1 + num2
    end
    local function subtract(num1,num2)
        return num1 - num2
    end
    local function multiply(num1,num2)
        return num1 * num2
    end
    local function divide(num1,num2)
        return num1 / num2
    end
    u5faau73af_2 = nil
    u5faau73af_3 = nil
    u5faau73af_4 = nil
    u5faau73af_5 = nil
    setPlanContext(sched.current_thread_id(), 1, "初始变量")
    u57fau5ea7 = {0,0,0,0,0,0}
    u5de5u5177 = {0,0,0,0,0,0}
    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(), 6, "force_offset={10.9, 0.35, 3.66, 0.02, -0.12, 0.0}")
        --subscript context start
        force_offset={10.9, 0.35, 3.66, 0.02, -0.12, 0.0}
        --subscript context end
        
        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)
        end
        function startForceControl()
            start_record = true
            sleep(0)
            fcEnable()
            -- 激活终止条件
            setCondActive()
        end
        function stopForceControl()
            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, 0.0, 100.0, 100.0, 100.0}")
        --subscript context start
        K = {1000.0, 1000.0, 0.0, 100.0, 100.0, 100.0}
        --subscript context end
        
        setPlanContext(sched.current_thread_id(), 12, "goal_wrench = {0, 0, -5, 0, 0, 0}")
        --subscript context start
        goal_wrench = {0, 0, -5, 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_guide()
end

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

-- return our app object
return app