From aed8442a31ddc7292db50ca8fb80748f19852935 Mon Sep 17 00:00:00 2001 From: Adrian Date: Fri, 7 Jul 2023 19:37:37 +0200 Subject: [PATCH] Remove unnecessary files --- depdencies/json.lua | 388 ------------------------------------- new_egoistic.lua | 460 -------------------------------------------- rat.lua | 174 ----------------- 3 files changed, 1022 deletions(-) delete mode 100644 depdencies/json.lua delete mode 100644 new_egoistic.lua delete mode 100644 rat.lua diff --git a/depdencies/json.lua b/depdencies/json.lua deleted file mode 100644 index 711ef78..0000000 --- a/depdencies/json.lua +++ /dev/null @@ -1,388 +0,0 @@ --- --- json.lua --- --- Copyright (c) 2020 rxi --- --- Permission is hereby granted, free of charge, to any person obtaining a copy of --- this software and associated documentation files (the "Software"), to deal in --- the Software without restriction, including without limitation the rights to --- use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies --- of the Software, and to permit persons to whom the Software is furnished to do --- so, subject to the following conditions: --- --- The above copyright notice and this permission notice shall be included in all --- copies or substantial portions of the Software. --- --- THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR --- IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, --- FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE --- AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER --- LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, --- OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE --- SOFTWARE. --- - -local json = { _version = "0.1.2" } - -------------------------------------------------------------------------------- --- Encode -------------------------------------------------------------------------------- - -local encode - -local escape_char_map = { - [ "\\" ] = "\\", - [ "\"" ] = "\"", - [ "\b" ] = "b", - [ "\f" ] = "f", - [ "\n" ] = "n", - [ "\r" ] = "r", - [ "\t" ] = "t", -} - -local escape_char_map_inv = { [ "/" ] = "/" } -for k, v in pairs(escape_char_map) do - escape_char_map_inv[v] = k -end - - -local function escape_char(c) - return "\\" .. (escape_char_map[c] or string.format("u%04x", c:byte())) -end - - -local function encode_nil(val) - return "null" -end - - -local function encode_table(val, stack) - local res = {} - stack = stack or {} - - -- Circular reference? - if stack[val] then error("circular reference") end - - stack[val] = true - - if rawget(val, 1) ~= nil or next(val) == nil then - -- Treat as array -- check keys are valid and it is not sparse - local n = 0 - for k in pairs(val) do - if type(k) ~= "number" then - error("invalid table: mixed or invalid key types") - end - n = n + 1 - end - if n ~= #val then - error("invalid table: sparse array") - end - -- Encode - for i, v in ipairs(val) do - table.insert(res, encode(v, stack)) - end - stack[val] = nil - return "[" .. table.concat(res, ",") .. "]" - - else - -- Treat as an object - for k, v in pairs(val) do - if type(k) ~= "string" then - error("invalid table: mixed or invalid key types") - end - table.insert(res, encode(k, stack) .. ":" .. encode(v, stack)) - end - stack[val] = nil - return "{" .. table.concat(res, ",") .. "}" - end -end - - -local function encode_string(val) - return '"' .. val:gsub('[%z\1-\31\\"]', escape_char) .. '"' -end - - -local function encode_number(val) - -- Check for NaN, -inf and inf - if val ~= val or val <= -math.huge or val >= math.huge then - error("unexpected number value '" .. tostring(val) .. "'") - end - return string.format("%.14g", val) -end - - -local type_func_map = { - [ "nil" ] = encode_nil, - [ "table" ] = encode_table, - [ "string" ] = encode_string, - [ "number" ] = encode_number, - [ "boolean" ] = tostring, -} - - -encode = function(val, stack) - local t = type(val) - local f = type_func_map[t] - if f then - return f(val, stack) - end - error("unexpected type '" .. t .. "'") -end - - -function json.encode(val) - return ( encode(val) ) -end - - -------------------------------------------------------------------------------- --- Decode -------------------------------------------------------------------------------- - -local parse - -local function create_set(...) - local res = {} - for i = 1, select("#", ...) do - res[ select(i, ...) ] = true - end - return res -end - -local space_chars = create_set(" ", "\t", "\r", "\n") -local delim_chars = create_set(" ", "\t", "\r", "\n", "]", "}", ",") -local escape_chars = create_set("\\", "/", '"', "b", "f", "n", "r", "t", "u") -local literals = create_set("true", "false", "null") - -local literal_map = { - [ "true" ] = true, - [ "false" ] = false, - [ "null" ] = nil, -} - - -local function next_char(str, idx, set, negate) - for i = idx, #str do - if set[str:sub(i, i)] ~= negate then - return i - end - end - return #str + 1 -end - - -local function decode_error(str, idx, msg) - local line_count = 1 - local col_count = 1 - for i = 1, idx - 1 do - col_count = col_count + 1 - if str:sub(i, i) == "\n" then - line_count = line_count + 1 - col_count = 1 - end - end - error( string.format("%s at line %d col %d", msg, line_count, col_count) ) -end - - -local function codepoint_to_utf8(n) - -- http://scripts.sil.org/cms/scripts/page.php?site_id=nrsi&id=iws-appendixa - local f = math.floor - if n <= 0x7f then - return string.char(n) - elseif n <= 0x7ff then - return string.char(f(n / 64) + 192, n % 64 + 128) - elseif n <= 0xffff then - return string.char(f(n / 4096) + 224, f(n % 4096 / 64) + 128, n % 64 + 128) - elseif n <= 0x10ffff then - return string.char(f(n / 262144) + 240, f(n % 262144 / 4096) + 128, - f(n % 4096 / 64) + 128, n % 64 + 128) - end - error( string.format("invalid unicode codepoint '%x'", n) ) -end - - -local function parse_unicode_escape(s) - local n1 = tonumber( s:sub(1, 4), 16 ) - local n2 = tonumber( s:sub(7, 10), 16 ) - -- Surrogate pair? - if n2 then - return codepoint_to_utf8((n1 - 0xd800) * 0x400 + (n2 - 0xdc00) + 0x10000) - else - return codepoint_to_utf8(n1) - end -end - - -local function parse_string(str, i) - local res = "" - local j = i + 1 - local k = j - - while j <= #str do - local x = str:byte(j) - - if x < 32 then - decode_error(str, j, "control character in string") - - elseif x == 92 then -- `\`: Escape - res = res .. str:sub(k, j - 1) - j = j + 1 - local c = str:sub(j, j) - if c == "u" then - local hex = str:match("^[dD][89aAbB]%x%x\\u%x%x%x%x", j + 1) - or str:match("^%x%x%x%x", j + 1) - or decode_error(str, j - 1, "invalid unicode escape in string") - res = res .. parse_unicode_escape(hex) - j = j + #hex - else - if not escape_chars[c] then - decode_error(str, j - 1, "invalid escape char '" .. c .. "' in string") - end - res = res .. escape_char_map_inv[c] - end - k = j + 1 - - elseif x == 34 then -- `"`: End of string - res = res .. str:sub(k, j - 1) - return res, j + 1 - end - - j = j + 1 - end - - decode_error(str, i, "expected closing quote for string") -end - - -local function parse_number(str, i) - local x = next_char(str, i, delim_chars) - local s = str:sub(i, x - 1) - local n = tonumber(s) - if not n then - decode_error(str, i, "invalid number '" .. s .. "'") - end - return n, x -end - - -local function parse_literal(str, i) - local x = next_char(str, i, delim_chars) - local word = str:sub(i, x - 1) - if not literals[word] then - decode_error(str, i, "invalid literal '" .. word .. "'") - end - return literal_map[word], x -end - - -local function parse_array(str, i) - local res = {} - local n = 1 - i = i + 1 - while 1 do - local x - i = next_char(str, i, space_chars, true) - -- Empty / end of array? - if str:sub(i, i) == "]" then - i = i + 1 - break - end - -- Read token - x, i = parse(str, i) - res[n] = x - n = n + 1 - -- Next token - i = next_char(str, i, space_chars, true) - local chr = str:sub(i, i) - i = i + 1 - if chr == "]" then break end - if chr ~= "," then decode_error(str, i, "expected ']' or ','") end - end - return res, i -end - - -local function parse_object(str, i) - local res = {} - i = i + 1 - while 1 do - local key, val - i = next_char(str, i, space_chars, true) - -- Empty / end of object? - if str:sub(i, i) == "}" then - i = i + 1 - break - end - -- Read key - if str:sub(i, i) ~= '"' then - decode_error(str, i, "expected string for key") - end - key, i = parse(str, i) - -- Read ':' delimiter - i = next_char(str, i, space_chars, true) - if str:sub(i, i) ~= ":" then - decode_error(str, i, "expected ':' after key") - end - i = next_char(str, i + 1, space_chars, true) - -- Read value - val, i = parse(str, i) - -- Set - res[key] = val - -- Next token - i = next_char(str, i, space_chars, true) - local chr = str:sub(i, i) - i = i + 1 - if chr == "}" then break end - if chr ~= "," then decode_error(str, i, "expected '}' or ','") end - end - return res, i -end - - -local char_func_map = { - [ '"' ] = parse_string, - [ "0" ] = parse_number, - [ "1" ] = parse_number, - [ "2" ] = parse_number, - [ "3" ] = parse_number, - [ "4" ] = parse_number, - [ "5" ] = parse_number, - [ "6" ] = parse_number, - [ "7" ] = parse_number, - [ "8" ] = parse_number, - [ "9" ] = parse_number, - [ "-" ] = parse_number, - [ "t" ] = parse_literal, - [ "f" ] = parse_literal, - [ "n" ] = parse_literal, - [ "[" ] = parse_array, - [ "{" ] = parse_object, -} - - -parse = function(str, idx) - local chr = str:sub(idx, idx) - local f = char_func_map[chr] - if f then - return f(str, idx) - end - decode_error(str, idx, "unexpected character '" .. chr .. "'") -end - - -function json.decode(str) - if type(str) ~= "string" then - error("expected argument of type string, got " .. type(str)) - end - local res, idx = parse(str, next_char(str, 1, space_chars, true)) - idx = next_char(str, idx, space_chars, true) - if idx <= #str then - decode_error(str, idx, "trailing garbage") - end - return res -end - - -return json diff --git a/new_egoistic.lua b/new_egoistic.lua deleted file mode 100644 index 4950948..0000000 --- a/new_egoistic.lua +++ /dev/null @@ -1,460 +0,0 @@ --- Problem: Program si? wiesza przy zapisywaniu zdj?? i wysy?aniu requesta do API -http = require('socket.http') -json = require('json') - -COLOR_WHITE = {255, 255, 255} -COLOR_RED = {255, 0, 0} -COLOR_GREEN = {0, 255, 0} -COLOR_BLUE = {0, 0, 255} - -OBSTACLE_AVOID_DISTANCE = 3 -MOVING_VELOCITY = 20 -TURNING_VELOCITY = 15 -PROXIMITY_STOP_DISTANCE = 1 -CONFIDENCE = 0 -IMAGE_COUNTER = 0 -CAMERA_FIELD_OF_VIEW = 60 -PHOTO_WIDTH = 1920 -BATERY_VALUE = 10 - -function sysCall_init() - - os.execute("rm -r robot") - os.execute("mkdir robot") - - ModelHandle = sim.getObject(".") - ModelName = sim.getObjectAlias(ModelHandle) - - RatHandles = {} -- List of rats on the scene - local i = 0 - while true do - local rat_handle = sim.getObject("/Rat*", {index=i, noError=true}) - if rat_handle < 0 then - break - end - table.insert(RatHandles, rat_handle) - i = i + 1 - end - - DuckHandles = {} -- List of ducks on the scene - local i = 0 - while true do - local duck_handle = sim.getObject("/Duck*", {index=i, noError=true}) - if duck_handle < 0 then - break - end - table.insert(DuckHandles, duck_handle) - i = i + 1 - end - - RightWheel = sim.getObject("./RightWheel") - LeftWheel = sim.getObject("./LeftWheel") - Camera = sim.getObject("./Camera") - VisionSensor = sim.getObject("./Vision_sensor") - SonarRight = sim.getObject("./Proximity_sensor_right") - SonarLeft = sim.getObject("./Proximity_sensor_left") - - Diodes = { -- LED_strip has to be ungrouped - D2 = sim.getObject("./diode_2"), - D3 = sim.getObject("./diode_3"), - D4 = sim.getObject("./diode_4"), - D5 = sim.getObject("./diode_5"), - D6 = sim.getObject("./diode_6"), - D7 = sim.getObject("./diode_7") - } - - sim.setShapeColor(sim.getObject("./diode_1"), nil, 0, COLOR_WHITE) - sim.setShapeColor(sim.getObject("./diode_8"), nil, 0, COLOR_WHITE) - - sim.setObjectInt32Param(RightWheel,2000, 1) -- Enable velocity control - sim.setObjectInt32Param(LeftWheel,2000, 1) - sim.setObjectInt32Param(Camera,2000, 1) - - Set_wheel_velocity(5, 5) - sim.setJointTargetVelocity(Camera, 0) - - sim.setJointTargetForce(LeftWheel, 5) - sim.setJointTargetForce(RightWheel, 5) - - Last_Color_Change_Time = sim.getSimulationTime() - Last_Camera_Rotation_Time = sim.getSimulationTime() - Last_Photo_Time = sim.getSimulationTime() - Last_Turn_Time=sim.getSimulationTime() - Turning = false - Seeing_Another_Rat = 0 - Close_To_Rat = false - CameraAngle = Get_angle(VisionSensor) - ModelAngle = Get_angle(ModelHandle) - Rat_Degrees_Offset_From_Photo_Center = 0 - - DEBUG_Sensor_Time = sim.getSimulationTime() - - --sim.addBanner("Bateria = 100", 2, sim_banner_fullyfacingcamera, nil, ModelHandle, nil, nil) - - Camera_Rotate_Counter = 0 - --IMAGE_COUNTER = 0 -end - - - -function sysCall_actuation() - -------------------- photo test -------------------- - if (sim.getSimulationTime() - Last_Photo_Time) > 0.5 then - if BATERY_VALUE > 0 then - BATERY_VALUE = BATERY_VALUE - 1 - UpdateBatery(BATERY_VALUE) - end - if not Close_To_Rat then - Take_photo() - end - Last_Photo_Time = sim.getSimulationTime() - end - - -------------------- camera test ------------------- - if (sim.getSimulationTime() - Last_Camera_Rotation_Time) > 1.7 then - if Camera_Rotate_Counter == 0 then - sim.setJointTargetVelocity(Camera, 2) - Camera_Rotate_Counter = 1 - else - sim.setJointTargetVelocity(Camera, -2) - Camera_Rotate_Counter = 0 - end - Last_Camera_Rotation_Time = sim.getSimulationTime() - end - - -------------------- moving test ------------------- - if Turning == true and Angle_difference(Get_angle(ModelHandle), Desired_angle) < 10 then - Finish_turning() - Stop_if_close(RatHandles) - end - -end - - - -function sysCall_sensing() - local _, _, _, detected_handle_left, _ = sim.readProximitySensor(SonarLeft) - local _, _, _, detected_handle_right, _ = sim.readProximitySensor(SonarRight) - if Has_Value(RatHandles, detected_handle_left) or Has_Value(RatHandles, detected_handle_right) then - if detected_handle_left > 0 and Has_Value(RatHandles, detected_handle_left) then - Handle = detected_handle_left - else - Handle = detected_handle_right - end - Close_To_Rat = true - Change_strip_color(COLOR_RED) - SensorMovement(Handle) - elseif BATERY_VALUE > 0 then - Close_To_Rat = false - VisionMovement() - else - Set_wheel_velocity(0, 0) - end -end - - - -function sysCall_cleanup() - -- do some clean-up here -end - - - - - ------------------------------- non-sysCall functions below ------------------------------ -function VisionMovement() - if Seeing_Another_Rat == 1 then - local adjusted_angle = Calculate_desired_angle(CameraAngle, Rat_Degrees_Offset_From_Photo_Center) - local angle_diff = Angle_difference(ModelAngle, adjusted_angle) - if angle_diff > 5 then - if math.abs(ModelAngle - adjusted_angle) < 180 then - if ModelAngle < adjusted_angle then - print(string.format("[%s] Rat detected on the left", ModelName)) - else - print(string.format("[%s] Rat detected on the right", ModelName)) - angle_diff = -angle_diff - end - else - if ModelAngle > adjusted_angle then - print(string.format("[%s] Rat detected on the left", ModelName)) - else - print(string.format("[%s] Rat detected on the right", ModelName)) - angle_diff = -angle_diff - end - end - DEGREES_TO_TURN = angle_diff - Start_turning(angle_diff) - ModelAngle = 0 - CameraAngle = 0 - Rat_Degrees_Offset_From_Photo_Center = 0 - end - if not Turning then - Go_Forward() - end - elseif not Turning then - Random_Walk() - end - if not Turning then - Stop_if_close(RatHandles) - end -end - -function SensorMovement(handle) - Follow(handle) -end - - -function RatCallPredictionAPI(filename) - local response = http.request(string.format("http://127.0.0.1:5000/detectRat1?img=%s", filename)) - return json.decode(response) -end - -function RobotCallPredictionAPI(filename) - local response = http.request(string.format("http://127.0.0.1:5000/detectRobot1?img=%s", filename)) - return json.decode(response) -end - -function UpdateBatery(batery_value) - sim.addBanner(batery_value, 0.4, sim_banner_fullyfacingcamera, {1.0, 0, 0, 0, 0, 0}, ModelHandle, nil, {1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}) -end - -function Has_Value (tab, val) - for index, value in ipairs(tab) do - if value == val then - return true - end - end - return false -end - - -function Change_strip_color(rgb_vals) - for _, v in pairs(Diodes) do - sim.setShapeColor(v, nil, 0, rgb_vals) - end -end - - -function Set_wheel_velocity(left_vel, right_vel) - sim.setJointTargetVelocity(LeftWheel, left_vel) - sim.setJointTargetVelocity(RightWheel, right_vel) -end - - -function Take_photo() - CameraAngle = Get_angle(VisionSensor) - ModelAngle = Get_angle(ModelHandle) - local img = sim.getVisionSensorImg(VisionSensor) - local filename = "img"..IMAGE_COUNTER..".png" - local path = "robot1/"..filename - sim.saveImage(img, {1920,1080}, 0, path, -1) - print(string.format("[%s] Photo taken 1", ModelName)) - local res = RatCallPredictionAPI(filename) - local res_robot = RobotCallPredictionAPI(filename) - print(CONFIDENCE) - if BATERY_VALUE > 0 then - if table.concat(res['0']) == 'None' then - --print(CONFIDENCE) - if CONFIDENCE > 0 then - CONFIDENCE = CONFIDENCE - 1 - end - Seeing_Another_Rat = 0 - if not Close_To_Rat and CONFIDENCE == 0 then - Change_strip_color(COLOR_GREEN) - print(string.format("[%s] No rats detected!", ModelName)) - else - print(string.format("[%s] Chase the rat!", ModelName)) - end - - else - local frame_width = res['0'][1] - local rat_pos = res['0'][2] + frame_width / 2 - Rat_Degrees_Offset_From_Photo_Center = (rat_pos - (PHOTO_WIDTH / 2)) / (PHOTO_WIDTH/CAMERA_FIELD_OF_VIEW) - if table.concat(res_robot['0']) == 'None' then - Seeing_Another_Rat = 1 - Change_strip_color(COLOR_RED) - CONFIDENCE = 3 - print(string.format("[%s] Another rat detected on camera", ModelName)) - elseif res_robot['0'][1] == 'red' then - Seeing_Another_Rat = 0 - print(string.format("[%s] I'm looking for another rat. This robot doesn't need my help", ModelName)) - if CONFIDENCE == 0 then - Change_strip_color(COLOR_GREEN) - end - - elseif res_robot['0'][1] == 'white' then - Seeing_Another_Rat = 1 - print(string.format("[%s] Robot need my help. I'm on my way!", ModelName)) - Change_strip_color(COLOR_RED) - CONFIDENCE = 3 - end - end - IMAGE_COUNTER = IMAGE_COUNTER + 1 - else - Change_strip_color(COLOR_WHITE) - print(string.format("[%s] I have to charge my battery", ModelName)) - end -end - - - -function Orientation_to_angle(orientation) - -- returns a value between [0, 360] - local angle = math.deg(orientation[2]) + 90 - if orientation[1] < 0 then - angle = 360 - angle - end - return angle -end - - -function Get_angle(handle) - local orientation = sim.getObjectOrientation(handle, sim.handle_world) - return Orientation_to_angle(orientation) -end - - -function Calculate_desired_angle(old_angle, angle_delta) - local desired = old_angle + angle_delta - if desired > 360 then - return desired % 360 - elseif - desired < 0 then - return desired + 360 - else - return desired - end -end - - -function Angle_difference(actual_angle, desired_angle) - -- returns a value between [0, 180] - local diff = math.abs(desired_angle - actual_angle) - return diff < 180 and diff or 360 - diff -end - - -function Turn_Left() - Set_wheel_velocity(0, TURNING_VELOCITY) -end - - -function Turn_Right() - Set_wheel_velocity(TURNING_VELOCITY,0) -end - - -function Go_Forward() - Set_wheel_velocity(MOVING_VELOCITY, MOVING_VELOCITY) -end - - -function Stop_Moving() - Set_wheel_velocity(0, 0) -end - - -function Start_turning(degrees) - FULL_CIRCLE_PERIOD = 2.8 -- at 2.5 speed - print(string.format("[%s] Starting to turn %.2f degrees", ModelName, degrees)) - Turning = true - Desired_angle = Calculate_desired_angle(Get_angle(ModelHandle), degrees) - if degrees > 0 then - Turn_Left() - else - Turn_Right() - end - Last_Turn_Time = sim.getSimulationTime() -end - - -function Finish_turning() - Go_Forward() - Last_Turn_Time = sim.getSimulationTime() - Turning = false - After_turning = Get_angle(ModelHandle) - print(string.format("[%s] Finished turning; Angle deviation from desired: %.2f (Desired: %.2f vs Actual: %.2f)", - ModelName, - Angle_difference(After_turning, Desired_angle), - Desired_angle, - After_turning)) -end - - -function Random_Walk() - local _, obstacle_left_distance, _, _, _ = sim.readProximitySensor(SonarLeft) - if obstacle_left_distance == 0 then - obstacle_left_distance = math.huge - end - - local _, obstacle_right_distance, _, _, _ = sim.readProximitySensor(SonarRight) - if obstacle_right_distance == 0 then - obstacle_right_distance = math.huge - end - - if obstacle_right_distance < obstacle_left_distance and obstacle_right_distance < OBSTACLE_AVOID_DISTANCE then - Turn_Left() - elseif obstacle_left_distance < obstacle_right_distance and obstacle_left_distance < OBSTACLE_AVOID_DISTANCE then - Turn_Right() - else - Go_Forward() - end -end - - -function Follow(object_handle) -- Use with flat sensors with no blind zone in the middle - local left_detected, left_distance, _, _, _ = sim.checkProximitySensor(SonarLeft, object_handle) - local right_detected, right_distance, _, _, _ = sim.checkProximitySensor(SonarRight, object_handle) - - if (sim.getSimulationTime() - Last_Photo_Time) > 0.5 then - if not Close_To_Rat then - local img = sim.getVisionSensorImg(VisionSensor) - local filename = "img"..IMAGE_COUNTER..".png" - local path = "robot1/"..filename - sim.saveImage(img, {1920,1080}, 0, path, -1) - end - Last_Photo_Time = sim.getSimulationTime() - IMAGE_COUNTER = IMAGE_COUNTER + 1 - end - - if left_detected == 1 and right_detected == 1 then - Go_Forward() - elseif left_detected == 1 then - Turn_Left() - elseif right_detected == 1 then - Turn_Right() - else - Go_Forward() - end - - if (left_distance > 0 and left_distance < PROXIMITY_STOP_DISTANCE) or (right_distance > 0 and right_distance < PROXIMITY_STOP_DISTANCE) then - Stop_Moving() - end -end - - -function Stop_if_close(handle_table) - -- Przy szczurach jest ok przy robotach z jakiegos powodu nie wykrywa jesli patrzy na kolo zamiast chassisa (kolo pewnie ma inny objecthandle?) - local left_detected, left_distance, _, left_detected_object_handle, _ = sim.readProximitySensor(SonarLeft) - local right_detected, right_distance, _, right_detected_object_handle, _ = sim.readProximitySensor(SonarRight) - if Has_Value(handle_table, left_detected_object_handle) or Has_Value(handle_table, right_detected_object_handle) then - if (left_distance > 0 and left_distance < PROXIMITY_STOP_DISTANCE) or (right_distance > 0 and right_distance < PROXIMITY_STOP_DISTANCE) then - Stop_Moving() - end - end -end - - ------------------------------- DEBUG functions below ------------------------------ -function DEBUG_What_Do_I_See(sensor_handle) - local detected, distance, detected_point, detected_object_handle, detected_surface_normal_vector = sim.readProximitySensor(sensor_handle) - if detected == 1 then - print(string.format("[%s] Object detected by %s; Handle: %d, Name: %s, Distance: %.2f", - ModelName, - sim.getObjectAlias(sensor_handle), - detected_object_handle, - sim.getObjectAlias(detected_object_handle, 1), - distance)) - end -end diff --git a/rat.lua b/rat.lua deleted file mode 100644 index 9bca105..0000000 --- a/rat.lua +++ /dev/null @@ -1,174 +0,0 @@ - -WALL_AVOID_DISTANCE = 0.5 -MOVING_VELOCITY = -20 -TURNING_VELOCITY = 2.5 -ROBOT_PROXIMITY_STOP_DISTANCE = 1.2 - - - -function sysCall_init() - - grains1 = sim.getObject("../Bump[2]/shape") - WallHandle = sim.getObject("../ExternalWall/Cuboid") - - rightWheel= sim.getObject("./RightWheelRat") - leftWheel= sim.getObject("./LeftWheelRat") - sim.setObjectInt32Param(rightWheel,2000,1) - sim.setObjectInt32Param(leftWheel,2000,1) - - Set_wheel_velocity(-20, -20) - - sim.setJointTargetForce(leftWheel, 5) - sim.setJointTargetForce(rightWheel, 5) - - last_time=sim.getSimulationTime() - turn_time=sim.getSimulationTime() - rotate = false - - sonarRight = sim.getObject("./Proximity_sensor_right_rat"); - sonarLeft = sim.getObject("./Proximity_sensor_left_rat"); - sonarCenter = sim.getObject("./Proximity_sensor_center_rat"); - max_dist = 3.0 - speed = 10 - - - RobotHandles = {} -- List of our robots on the scene - local i = 0 - while true do - local robot_handle = sim.getObject("/Bump*/shape", {index=i, noError=true}) - if robot_handle < 0 then - break - end - table.insert(RobotHandles, robot_handle) - i = i + 1 - end - -end - - -function sysCall_actuation() -end - -function sysCall_sensing() - -- put your sensing code here - - local _, _, _, detected_handle_left, _ = sim.readProximitySensor(sonarLeft) - local _, _, _, detected_handle_right, _ = sim.readProximitySensor(sonarRight) - if Has_Value(RobotHandles, detected_handle_left) then -- For demonstration purposes this robot will follow any robot of the same type - Follow(detected_handle_left) - elseif Has_Value(RobotHandles, detected_handle_right) then - Follow(detected_handle_right) - else - Random_Walk() - end -end - - - -function sysCall_cleanup() - -- do some clean-up here -end - -function Has_Value (tab, val) - for index, value in ipairs(tab) do - if value == val then - return true - end - end - return false -end - -function Set_wheel_velocity(left_vel, right_vel) - sim.setJointTargetVelocity(leftWheel, left_vel) - sim.setJointTargetVelocity(rightWheel, right_vel) -end - -function Turn_Left() - Set_wheel_velocity(TURNING_VELOCITY, -TURNING_VELOCITY) -end - - -function Turn_Right() - Set_wheel_velocity(-TURNING_VELOCITY, TURNING_VELOCITY) -end - - -function Go_Forward() - Set_wheel_velocity(MOVING_VELOCITY, MOVING_VELOCITY) -end - - -function Stop_Moving() - Set_wheel_velocity(0, 0) -end - -function Random_Walk() - -- put your sensing code here - distSonarRight = getDistance(sonarRight, max_dist) - distSonarLeft = getDistance(sonarLeft, max_dist) - distSonarCenter = getDistance(sonarCenter, max_dist) - - if distSonarRight < max_dist and distSonarRight < distSonarLeft and rotate == false then - Set_wheel_velocity(0, -40) - elseif distSonarRight < max_dist and distSonarLeft < max_dist and distSonarCenter < max_dist and distSonarRight < distSonarLeft and rotate == false then - Set_wheel_velocity(0, -40) - elseif distSonarRight < max_dist and distSonarCenter < max_dist and distSonarRight < distSonarLeft and rotate == false then - Set_wheel_velocity(0, -40) - elseif distSonarLeft < max_dist and distSonarCenter < max_dist and distSonarLeft < distSonarRight and rotate == false then - Set_wheel_velocity(-40, 0) - elseif distSonarLeft < max_dist and distSonarLeft < distSonarRight and rotate == false then - Set_wheel_velocity(-40, 0) - elseif distSonarCenter < max_dist and rotate == false then - Set_wheel_velocity(0, -40) - turn_time = sim.getSimulationTime() - rotate = true - elseif rotate == false then - Set_wheel_velocity(-20, -20) - end - - if rotate == true and (sim.getSimulationTime() - turn_time) > 0.5 then - Set_wheel_velocity(-20, -20) - rotate = false - end -end - -function Follow(object_handle) -- Use with flat sensors with no blind zone in the middle - local left_detected, left_distance, _, _, _ = sim.checkProximitySensor(sonarLeft, object_handle) - local right_detected, right_distance, _, _, _ = sim.checkProximitySensor(sonarRight, object_handle) - - if left_detected == 1 and right_detected == 1 then - Go_Forward() - elseif left_detected == 1 then - Turn_Left() - elseif right_detected == 1 then - Turn_Right() - else - Go_Forward() - end - - if (left_distance > 0 and left_distance < ROBOT_PROXIMITY_STOP_DISTANCE) or (right_distance > 0 and right_distance < ROBOT_PROXIMITY_STOP_DISTANCE) then - Stop_Moving() - end - -end - -function getDistance(sensor, max_dist) - local detected, distance - detected, distance = sim.readProximitySensor(sensor) - if (detected < 1) then - distance = max_dist - end - return distance -end - -function DEBUG_What_Do_I_See(sensor_handle) - local detected, distance, detected_point, detected_object_handle, detected_surface_normal_vector = sim.readProximitySensor(sensor_handle) - if detected == 1 then - print(string.format("[%s] Object detected by %s; Handle: %d, Name: %s, Distance: %.2f", - ModelName, - sim.getObjectAlias(sensor_handle), - detected_object_handle, - sim.getObjectAlias(detected_object_handle, 1), - distance)) - end -end \ No newline at end of file