API tools faq
paste
View difference between Paste ID: D59reuGc and tt6nak5v
SHOW: | | - or go back to the newest paste.
1-
local altitude = peripheral.find("altitude_sensor")
1+
local altitude = peripheral.find("altitude_sensor")
2-
local relay = peripheral.find("redstone_relay")
2+
local relay = peripheral.find("redstone_relay")
3-
local speedRotation = peripheral.find("Create_RotationSpeedController")
3+
local speedRotation = peripheral.find("Create_RotationSpeedController")
4-
local swivel = peripheral.find("swivel_bearing")
4+
local swivel = peripheral.find("swivel_bearing")
5-
local PID = require ".pid"
5+
local PID = require ".pid"
6-
6+
7-
term.clear()


7+
term.clear()

8-
term.setCursorPos(1, 1)


8+
term.setCursorPos(1, 1)

9-


9+
10-
local TARGET_HEIGHT = 100


10+
local TARGET_HEIGHT = 100

11-
local TARGET_ANGLE  = 0


11+
local TARGET_ANGLE = 0

12-


12+
13-
parallel.waitForAny(


13+
local function clamp(value, minValue, maxValue)

14-
    -- Module: AutoHeight


14+
    return math.max(minValue, math.min(maxValue, value))

15-
    function ()


15+
end

16-
        local burnerSide = "top"


16+
17-


17+
parallel.waitForAny(

18-
        local lastHeight = altitude.getHeight()


18+
    -- Module: AutoHeight

19-
        local lastTime = os.clock()


19+
    function()

20-


20+
        local burnerSide = "top"

21-
        local velocity = 0


21+
22-


22+
        local lastHeight = altitude.getHeight()

23-
        local DEADZONE = 2           -- зона покоя


23+
        local lastTime = os.clock()

24-
        local STRONG_ZONE = 40       -- сильный недолёт


24+
25-
        local PREDICT_TIME = 6       -- прогноз вперёд


25+
        local velocity = 0

26-
        local MAX_BURN = 4           -- максимум работы горелки


26+
27-
        local MIN_BURN = 0.15        -- минимальный импульс


27+
        local DEADZONE = 2

28-


28+
        local STRONG_ZONE = 40

29-
        local function burner(state)


29+
        local PREDICT_TIME = 6

30-
            redstone.setOutput(burnerSide, state)


30+
        local MAX_BURN = 4

31-
        end


31+
        local MIN_BURN = 0.15

32-


32+
33-
        while true do


33+
        local function burner(state)

34-
            local now = os.clock()


34+
            redstone.setOutput(burnerSide, state)

35-
            local dt = now - lastTime


35+
        end

36-
            lastTime = now


36+
37-
            local height = altitude.getHeight()


37+
        while true do

38-
            velocity = (height - lastHeight) / dt


38+
            local now = os.clock()

39-
            lastHeight = height


39+
            local dt = now - lastTime

40-
            local predictedHeight = height + velocity * PREDICT_TIME


40+
            lastTime = now

41-
            local error = TARGET_HEIGHT - predictedHeight


41+
42-
            if velocity > 3 and height > TARGET_HEIGHT then


42+
            local height = altitude.getHeight()

43-
                burner(false)


43+
            velocity = (height - lastHeight) / dt

44-
            elseif math.abs(error) <= DEADZONE then


44+
            lastHeight = height

45-
                burner(false)


45+
46-
            elseif error > 0 then


46+
            local predictedHeight = height + velocity * PREDICT_TIME

47-


47+
            local error = TARGET_HEIGHT - predictedHeight

48-
                local burnTime


48+
49-
                if error > STRONG_ZONE then


49+
            if velocity > 3 and height > TARGET_HEIGHT then

50-
                    burnTime = MAX_BURN


50+
                burner(false)

51-
                else


51+
            elseif math.abs(error) <= DEADZONE then

52-
                    burnTime = math.max(


52+
                burner(false)

53-
                        MIN_BURN,


53+
            elseif error > 0 then

54-
                        math.min(MAX_BURN, error * 0.5)


54+
                local burnTime

55-
                    )


55+
56-
                end


56+
                if error > STRONG_ZONE then

57-


57+
                    burnTime = MAX_BURN

58-
                burner(true)


58+
                else

59-
                sleep(burnTime)


59+
                    burnTime = math.max(

60-
                burner(false)


60+
                        MIN_BURN,

61-
            else


61+
                        math.min(MAX_BURN, error * 0.5)

62-
                burner(false)


62+
                    )

63-
            end


63+
                end

64-
            sleep(0.05)


64+
65-
        end


65+
                burner(true)

66-
    end,


66+
                sleep(burnTime)

67-


67+
                burner(false)

68-
    -- Module: AutoAngle


68+
            else

69-
    function ()


69+
                burner(false)

70-
        local pid = PID:new(0.5, 0.001, 0.5, -5, 5)


70+
            end

71-
        local block = false


71+
72-
        while true do


72+
            sleep(0.05)

73-
            local angle = swivel.getTargetAngle()


73+
        end

74-
            local raw = pid:compute(TARGET_ANGLE, angle, 1)


74+
    end,

75-
            local limited = -math.max(-20, math.min(raw, 20))


75+
76-
            if math.abs(limited) > 0.02 and math.abs(limited) < 1 then


76+
    -- Module: AutoAngle

77-
                if block == false then


77+
    function()

78-
                    if (limited > 0) then


78+
        local pid = PID:new(0.5, 0.001, 0.5, -5, 5)

79-
                        speedRotation.setTargetSpeed(math.ceil(limited))


79+
        local block = false

80-
                    else


80+
81-
                        speedRotation.setTargetSpeed(math.floor(limited))


81+
        local function getSwivelAngle()

82-
                    end


82+
            if swivel.getAngle then

83-
                    block = true


83+
                return swivel.getAngle()

84-
                else


84+
            end

85-
                    speedRotation.setTargetSpeed(0)


85+
            return swivel.getTargetAngle()

86-
                end


86+
        end

87-
            else


87+
88-
                if block then block = false end


88+
        local startAngle = getSwivelAngle()

89-
                speedRotation.setTargetSpeed(limited)


89+
90-
            end


90+
        while true do

91-
            sleep(0.1)


91+
            local angle = getSwivelAngle()

92-
        end


92+
            local wantedAngle = clamp(startAngle + TARGET_ANGLE, startAngle - 45, startAngle + 45)

93-
    end,


93+
            local raw = pid:compute(wantedAngle, angle, 1)

94-


94+
            local limited = -math.max(-20, math.min(raw, 20))

95-
    -- Module: Command handler + auto‑навигация


95+
96-
    function ()


96+
            if math.abs(limited) > 0.02 and math.abs(limited) < 1 then

97-
        local lastCommand = ""


97+
                if block == false then

98-
        while true do


98+
                    if limited > 0 then

99-
            local cmd = read()


99+
                        speedRotation.setTargetSpeed(math.ceil(limited))

100-
            if cmd and cmd ~= "" and cmd ~= lastCommand then


100+
                    else

101-
                lastCommand = cmd


101+
                        speedRotation.setTargetSpeed(math.floor(limited))

102-
                local colonPos = string.find(cmd, ":")


102+
                    end

103-
                if colonPos then


103+
                    block = true

104-
                    local param = string.sub(cmd, 1, colonPos-1):lower()


104+
                else

105-
                    local valueStr = string.sub(cmd, colonPos+1)


105+
                    speedRotation.setTargetSpeed(0)

106-


106+
                end

107-
                    -- Обработка auto:x,z


107+
            else

108-
                    if param == "auto" then


108+
                if block then

109-
                        local commaPos = string.find(valueStr, ",")


109+
                    block = false

110-
                        if commaPos then


110+
                end

111-
                            local targetX = tonumber(string.sub(valueStr, 1, commaPos-1))


111+
                speedRotation.setTargetSpeed(limited)

112-
                            local targetZ = tonumber(string.sub(valueStr, commaPos+1))


112+
            end

113-
                            if targetX and targetZ and sublevel then


113+
114-
                                while true do


114+
            sleep(0.1)

115-
                                    local pose = sublevel.getLogicalPose()


115+
        end

116-
                                    local curX = pose.position.x


116+
    end,

117-
                                    local curZ = pose.position.z


117+
118-
                                    local w = pose.orientation.a


118+
    -- Module: Command handler + auto‑навигация

119-
                                    local x = pose.orientation.v.x


119+
    function()

120-
                                    local y = pose.orientation.v.y


120+
        local lastCommand = ""

121-
                                    local z = pose.orientation.v.z


121+
122-


122+
        while true do

123-
                                    local siny_cosp = 2 * (w*y + x*z)


123+
            local cmd = read()

124-
                                    local cosy_cosp = 1 - 2 * (y*y + z*z)


124+
125-
                                    local yaw = math.deg(math.atan(siny_cosp, cosy_cosp))


125+
            if cmd and cmd ~= "" and cmd ~= lastCommand then

126-
                                    


126+
                lastCommand = cmd

127-
                                    local direction = { x = targetX - curX, z = targetZ - curZ }


127+
128-
                                    local length = math.sqrt(direction.x^2+direction.z^2)


128+
                local colonPos = string.find(cmd, ":")

129-
                                    local normilized = { x = direction.x/length, z = direction.z/length }


129+
130-
                                    local art = math.atan(normilized.x, normilized.z) * (180 / math.pi)


130+
                if colonPos then

131-
                                    local error = yaw - art


131+
                    local param = string.sub(cmd, 1, colonPos - 1):lower()

132-


132+
                    local valueStr = string.sub(cmd, colonPos + 1)

133-
                                    TARGET_ANGLE = error * 2


133+
134-
                                    print(string.format("art=%.1f, yaw=%.1f, TARGET_ANGLE=%.1f", art, yaw, TARGET_ANGLE))


134+
                    if param == "auto" then

135-
                                    sleep(0.05)


135+
                        local commaPos = string.find(valueStr, ",")

136-
                                end


136+
137-
                            else


137+
                        if commaPos then

138-
                                print("Error")


138+
                            local targetX = tonumber(string.sub(valueStr, 1, commaPos - 1))

139-
                            end


139+
                            local targetZ = tonumber(string.sub(valueStr, commaPos + 1))

140-
                        end


140+
141-
                    else


141+
                            if targetX and targetZ and sublevel then

142-
                        local value = tonumber(valueStr)


142+
                                while true do

143-
                        if value then


143+
                                    local pose = sublevel.getLogicalPose()

144-
                            if param == "a" or param == "angle" then


144+
145-
                                TARGET_ANGLE = value


145+
                                    local curX = pose.position.x

146-
                            elseif param == "h" or param == "height" then


146+
                                    local curZ = pose.position.z

147-
                                TARGET_HEIGHT = value


147+
148-
                            end


148+
                                    local w = pose.orientation.a

149-
                        end


149+
                                    local x = pose.orientation.v.x

150-
                    end


150+
                                    local y = pose.orientation.v.y

151-
                end


151+
                                    local z = pose.orientation.v.z

152-
            end


152+
153-
        end


153+
                                    local siny_cosp = 2 * (w * y + x * z)

154-
    end


154+
                                    local cosy_cosp = 1 - 2 * (y * y + z * z)

155
                                    local yaw = math.deg(math.atan(siny_cosp, cosy_cosp))

156
157
                                    local direction = {

158
    									x = curX - targetX,

159
    									z = curZ - targetZ

160
									}

161
162
                                    local length = math.sqrt(direction.x ^ 2 + direction.z ^ 2)

163
164
                                    if length > 0 then

165
                                        local normilized = {

166
                                            x = direction.x / length,

167
                                            z = direction.z / length

168
                                        }

169
170
                                        local art = math.atan(normilized.x, normilized.z) * (180 / math.pi)

171
172
                                        local raw = art - yaw

173
                                        local error = raw % 360

174
                                        if error > 180 then

175
                                            error = error - 360

176
                                        end

177
178
                                        TARGET_ANGLE = clamp(error, -45, 45)

179
180
                                        print(

181
                                            string.format(

182
                                                "curX=%.1f curZ=%.1f yaw=%.1f art=%.1f err=%.1f",

183
                                                curX,

184
                                                curZ,

185
                                                yaw,

186
                                                art,

187
                                                error

188
                                            )

189
                                        )

190
                                    else

191
                                        TARGET_ANGLE = 0

192
                                    end

193
194
                                    sleep(0.05)

195
                                end

196
                            else

197
                                print("Error")

198
                            end

199
                        end

200
                    else

201
                        local value = tonumber(valueStr)

202
203
                        if value then

204
                            if param == "a" or param == "angle" then

205
                                TARGET_ANGLE = clamp(value, -45, 45)

206
                            elseif param == "h" or param == "height" then

207
                                TARGET_HEIGHT = value

208
                            end

209
                        end

210
                    end

211
                end

212
            end

213
        end

214
    end

215
)
        

    


            


                                


        

            



                
    

        Public Pastes
    

    
            

    

                    

        

    





    


    
    
    
    
    
    
    
    




    


    



                

            We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.             OK, I Understand
        

    
                

            

                
                    
                
            

            

                Not a member of Pastebin yet?

                Sign Up, it unlocks many cool features!