From d768193275f4c29048e0e745aaaebec675712fc4 Mon Sep 17 00:00:00 2001 From: Redo Date: Fri, 28 Oct 2022 15:25:25 -0600 Subject: [PATCH] add robot prototype --- bricks/special/robot-init.lua | 12 ++ bricks/special/robot-input.lua | 31 +++++ bricks/special/robot-update.lua | 63 +++++++++ bricks/special/robot.cs | 219 +++++++++++++++----------------- bricks/special/rom.cs | 24 ++-- 5 files changed, 221 insertions(+), 128 deletions(-) create mode 100644 bricks/special/robot-init.lua create mode 100644 bricks/special/robot-input.lua create mode 100644 bricks/special/robot-update.lua diff --git a/bricks/special/robot-init.lua b/bricks/special/robot-init.lua new file mode 100644 index 0000000..0bd3480 --- /dev/null +++ b/bricks/special/robot-init.lua @@ -0,0 +1,12 @@ + +return function(gate) + gate.brickcolor = 0 + gate.brickcolorfx = 0 + gate.brickshapefx = 0 + gate.brickray = 0 + gate.brickcol = 0 + gate.brickren = 0 + gate.waiting = true + gate.robotpos = {0, 0, 0} + gate.robotdir = 0 +end diff --git a/bricks/special/robot-input.lua b/bricks/special/robot-input.lua new file mode 100644 index 0000000..2ca6230 --- /dev/null +++ b/bricks/special/robot-input.lua @@ -0,0 +1,31 @@ + +local function intToPort(gate, port, len, v) + for i = 1, len do + local p = math.pow(2, i-1) + Gate.setportstate(gate, port+i-1, math.floor(v/p)%2) + end +end + +return function(gate, argv) + for word in argv[1]:gmatch("[^\t]+") do + local first, rest = word:sub(1, 1), word:sub(2, #word) + local vec = {} + for a in rest:gmatch("[^ ]+") do table.insert(vec, tonumber(a) or error("invalid number "..a)) end + if first=="P" then -- set position + assert(#vec==3, "invalid position given to robot: "..word) + brick.robotpos = vec + elseif first=="B" then -- detected brick info + assert(#vec==7, "invalid brick info given to robot: "..word) + gate.brickexists = vec[1] + gate.brickcolor = vec[2] + gate.brickcolorfx = vec[3] + gate.brickshapefx = vec[4] + gate.brickray = vec[5] + gate.brickcol = vec[6] + gate.brickren = vec[7] + intToPort(gate, 15, 6, gate.brickcolor) + Gate.setportstate(gate, 21, gate.brickexists) + else error("invalid control word given to robot: "..word) end + end + gate.waiting = false +end diff --git a/bricks/special/robot-update.lua b/bricks/special/robot-update.lua new file mode 100644 index 0000000..674d712 --- /dev/null +++ b/bricks/special/robot-update.lua @@ -0,0 +1,63 @@ + +local function intFromPort(gate, port, len) + local v = 0 + for i = 1, len do + v = v + Gate.getportstate(gate, port+i-1)*math.pow(2, i-1) + end + return v +end + +local function rotateVector(vec, rot) + if rot==0 then return { vec[1], vec[2], vec[3]} + elseif rot==1 then return { vec[2], -vec[1], vec[3]} + elseif rot==2 then return {-vec[1], -vec[2], vec[3]} + elseif rot==3 then return {-vec[2], vec[1], vec[3]} + else error("invalid rot "..rot) end +end + +return function(gate) + if not gate.waiting then + local action = "" + if Gate.getportstate(gate, 7)~=0 then -- remove brick + action = action.."R\t" + end + if Gate.getportstate(gate, 8)~=0 then -- plant brick + local color = intFromPort(gate, 1, 6) + local colorfx = 0 + local shapefx = 0 + local ray = 1 + local col = 1 + local ren = 1 + action = action.."P "..color.." "..colorfx.." "..shapefx.." "..ray.." "..col.." "..ren.."\t" + end + if Gate.getportstate(gate, 22)~=0 then -- detect brick + action = action.."D\t" + end + + local movePos = {0, 0, 0} + if Gate.getportstate(gate, 9)~=0 then movePos[3] = movePos[3]-1 end -- down + if Gate.getportstate(gate, 10)~=0 then movePos[3] = movePos[3]+1 end -- up + if Gate.getportstate(gate, 11)~=0 then movePos[3] = movePos[1]+1 end -- right + if Gate.getportstate(gate, 12)~=0 then movePos[3] = movePos[1]-1 end -- left + if Gate.getportstate(gate, 13)~=0 then movePos[3] = movePos[2]-1 end -- back + if Gate.getportstate(gate, 14)~=0 then movePos[3] = movePos[2]+1 end -- fore + local moveRotation = 0 + --if Gate.getportstate(gate, 13)~=0 then moveRotation = moveRotation+1 end -- right + --if Gate.getportstate(gate, 14)~=0 then moveRotation = moveRotation-1 end -- left + gate.robotdir = (gate.robotdir + moveRotation)%4 + + if movePos[1]~=0 or movePos[2]~=0 or movePos[3]~=0 then + movePos = rotateVector(movePos, gate.robotdir) + gate.robotpos = { gate.robotpos[1]+movePos[1], gate.robotpos[2]+movePos[2], gate.robotpos[3]+movePos[3] } + action = action.."M "..gate.robotpos[1].." "..gate.robotpos[2].." "..gate.robotpos[3].."\t" + end + if moveRotation~=0 then + action = action.."A "..gate.robotdir.."\t" + end + + if action~="" then + Gate.cb(gate, action:sub(1, #action-1)) + gate.waiting = true + end + end +end diff --git a/bricks/special/robot.cs b/bricks/special/robot.cs index 0209c0a..b1193f6 100644 --- a/bricks/special/robot.cs +++ b/bricks/special/robot.cs @@ -21,14 +21,14 @@ datablock fxDTSBrickData(LogicGate_RobotController_Data) { hasPrint = 1; printAspectRatio = "Logic"; orientationFix = 3; - + isLogic = true; isLogicGate = true; isLogicInput = true; logicInit = lualogic_readfile($LuaLogic::Path @ "bricks/special/robot-init.lua" ); logicUpdate = lualogic_readfile($LuaLogic::Path @ "bricks/special/robot-update.lua"); - logicGlobal = lualogic_readfile($LuaLogic::Path @ "bricks/special/robot-global.lua"); + logicInput = lualogic_readfile($LuaLogic::Path @ "bricks/special/robot-input.lua" ); logicUIName = "Robot Controller"; logicUIDesc = "Creates and controls a mobile robot that can detect and place bricks"; @@ -109,161 +109,144 @@ datablock fxDTSBrickData(LogicGate_RobotController_Data) { logicPortPos[14] = "15 3 0"; logicPortDir[14] = 1; logicPortUIName[14] = "ColorOut0"; - + logicPortType[15] = 0; logicPortPos[15] = "13 3 0"; logicPortDir[15] = 1; logicPortUIName[15] = "ColorOut1"; - + logicPortType[16] = 0; logicPortPos[16] = "11 3 0"; logicPortDir[16] = 1; logicPortUIName[16] = "ColorOut2"; - + logicPortType[17] = 0; logicPortPos[17] = "9 3 0"; logicPortDir[17] = 1; logicPortUIName[17] = "ColorOut3"; - + logicPortType[18] = 0; logicPortPos[18] = "7 3 0"; logicPortDir[18] = 1; logicPortUIName[18] = "ColorOut4"; - + logicPortType[19] = 0; logicPortPos[19] = "5 3 0"; logicPortDir[19] = 1; logicPortUIName[19] = "ColorOut5"; - + logicPortType[20] = 0; logicPortPos[20] = "1 3 0"; logicPortDir[20] = 1; logicPortUIName[20] = "Brick Detected"; - + logicPortType[21] = 1; logicPortPos[21] = "-1 3 0"; logicPortDir[21] = 1; logicPortUIName[21] = "Detect Brick"; }; -function LogicGate_RobotController_Data::getRelativeVector(%this, %obj, %vec) -{ - %rot = %obj.angleID; - %x = getWord(%vec, 0); - %y = getWord(%vec, 1); - %z = getWord(%vec, 2); - %ax = %x; - switch(%rot) - { - case 1: - %x = %y; - %y = -%ax; - case 2: - %x = -%x; - %y = -%y; - case 3: - %x = -%y; - %y = %ax; +function LogicGate_RobotController_Data::LuaLogic_Callback(%this, %brick, %data) { + %robot = %brick.luaLogicRobot; + if(!isObject(%robot)) { talk("brick " @ %brick @ " has no robot!"); return; } + %pos = %robot.getPosition(); + + initContainerBoxSearch(%pos, "0.49 0.49 0.19", $TypeMasks::FxBrickAlwaysObjectType); + %tbrick = containerSearchNext(); + + %output = ""; + + for(%i=0; %i> %i) & 1); - - %obj.illogicRobotBrick = %sobj; - } - else - { - %obj.Logic_SetOutput(20, false); - - for(%i = 0; %i < 6; %i++) - %obj.Logic_SetOutput(14+%i, false); - - %obj.illogicRobotBrick = 0; - } - } - - if(!$LBC::Ports::LastBrickState[%obj, 6] && $LBC::Ports::BrickState[%obj, 6]) - { - initContainerBoxSearch(%pos, "0.49 0.49 0.19", $TypeMasks::FxBrickObjectType); - if(isObject(%sobj = containerSearchNext()) && %sobj.getGroup() == nameToID(LuaLogic_RobotBrickGroup)) - %sobj.delete(); - } - - if(!$LBC::Ports::LastBrickState[%obj, 7] && $LBC::Ports::BrickState[%obj, 7]) - { - for(%i = 0; %i < 6; %i++) - %color += mPow(2, %i) * $LBC::Ports::BrickState[%obj, %i]; - - %brick = new fxDTSBrick() - { - datablock = brick1x1fData; - position = %pos; - colorID = %color; - isPlanted = 1; - }; - - %err = %brick.plant(); - if(%err != 0 && %err != 2) - %brick.delete(); - else - { - LuaLogic_RobotBrickGroup.add(%brick); - %brick.setTrusted(true); - } - } -} - -function LogicGate_RobotController_Data::Logic_onGateAdded(%this, %obj) -{ - if(isObject(%obj.illogicRobot)) - %obj.illogicRobot.delete(); - +function LogicGate_RobotController_Data::createRobot(%this, %obj) { + if(isObject(%obj.luaLogicRobot)) + %obj.luaLogicRobot.delete(); + %pos = %obj.getPosition(); %rpos = vectorAdd(%pos, %this.getRelativeVector(%obj, "0.25 7.75 0")); - %robot = new StaticShape() - { - datablock = LuaLogic_BrickData; + %robot = new StaticShape() { + datablock = LuaLogic_RobotShapeData; position = %rpos; }; %robot.setNodeColor("ALL", "1 1 1 1"); missionCleanup.add(%robot); - - %obj.illogicRobot = %robot; + + %obj.luaLogicRobot = %robot; + + lualogic_sendinput(%brick, 1, "P" @ %rpos); +} + +function LogicGate_RobotController_Data::onDeath(%this, %obj) { + if(isObject(%obj.luaLogicRobot)) + %obj.luaLogicRobot.delete(); +} + +function LogicGate_RobotController_Data::onRemove(%this, %obj) { + if(isObject(%obj.luaLogicRobot)) + %obj.luaLogicRobot.delete(); +} + +function LogicGate_RobotController_Data::onPlant(%this, %obj) { + if(!isObject(%obj.luaLogicRobot)) + %this.createRobot(%obj); +} +function LogicGate_RobotController_Data::onLoadPlant(%this, %obj) { + if(!isObject(%obj.luaLogicRobot)) + %this.createRobot(%obj); } diff --git a/bricks/special/rom.cs b/bricks/special/rom.cs index 24f45a2..31ebfc7 100644 --- a/bricks/special/rom.cs +++ b/bricks/special/rom.cs @@ -30,17 +30,19 @@ package lualogic_rom_update { }; schedule(0, 0, activatePackage, lualogic_rom_update); -function lualogic_rom_updatebrick(%brick, %instant) { +function lualogic_rom_updatebrick(%brick) { cancel(%brick.lualogic_rom_updatebrick_schedule); - if(%instant) lualogic_rom_updatebrick_final(%brick); - else %brick.lualogic_rom_updatebrick_schedule = schedule(100, 0, lualogic_rom_updatebrick_final, %brick); + %brick.lualogic_rom_updatebrick_schedule = schedule(100, 0, lualogic_rom_updatebrick_final, %brick, %brick.getWorldBox()); } -function lualogic_rom_updatebrick_final(%brick) { - if(!isObject(%brick)) return; - if(%brick.lualogic_rom_delete) { if(%brick.lualogic_rom_deletedone) return; %brick.lualogic_rom_deletedone = 1; } +function lualogic_rom_updatebrick_final(%brick, %box) { + if(isObject(%brick)) { + if(%brick.lualogic_rom_delete) { + if(%brick.lualogic_rom_deletedone) return; + %brick.lualogic_rom_deletedone = 1; + } + } // update any roms below - %box = %brick.getWorldBox(); %boxl = getWords(%box, 0, 2); %boxlz = getWord(%boxl, 2); %boxh = getWords(%box, 3, 5); %boxs = vectorSub(%boxh, %boxl); @@ -60,9 +62,11 @@ function lualogic_rom_updatebrick_final(%brick) { } // if rom, update - if(%brick.lualogic_rom_delete) return; - if(%brick.getDatablock().isLogicRom) { - lualogic_rom_updatedata(%brick); + if(isObject(%brick)) { + if(%brick.lualogic_rom_delete) return; + if(%brick.getDatablock().isLogicRom) { + lualogic_rom_updatedata(%brick); + } } }