s478873/Projekt-PBR_Sztuczna_Empatia
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Remove unnecessary files

Browse Source
This commit is contained in:
Adrian 2023-07-07 19:37:37 +02:00
parent a4b160ab43
commit aed8442a31
3 changed files with 0 additions and 1022 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

388
depdencies/json.lua
View File

@ -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

460
new_egoistic.lua
View File

@ -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

174
rat.lua
View File

@ -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