--[[ With this controller, a robot performs random walk with obstacle avoidance mimicking a gas particle. The robot keeps going straight until an obstacle is too close. When an obstacle is close enough, the robot simulates a "particle collision" and turns away from it. The turning direction is dictated by the angle at which the obstacle is located with respect to the robot. ]] --[[ Constants ]] --[[ This parameter is to check whether is close enough to react to it. Remember that the proximity sensor readings decrease with the distance of the sense obstacle. ]] DELTA = 0.001 --[[ Actual code ]] --[[ Controller init ]] function init() -- nothing to do end --[[ Controller step ]] function step() -- We treat each proximity reading as a vector whose length is given by the value -- and whose angle is given by the angle corresponding to the reading -- First, we sum all these vectors into a variable called 'accum' accum = { x=0, y=0 } for i = 1,24 do -- This is the way to initialize a vector from length and angle local vec = { x = robot.proximity[i].value * math.cos(robot.proximity[i].angle), y = robot.proximity[i].value * math.sin(robot.proximity[i].angle) } -- Sum the 'vec' to the accumulator 'accum' accum.x = accum.x + vec.x accum.y = accum.y + vec.y end -- Now 'accum' is a vector that points to the direction of the closest obstacle -- OK, this is not always true. In fact, if two obstacles are located at symmetric -- positions around a robot, 'accum' will be zero. However, in practice these -- cases don't really count. Try it yourself to see that it's true! -- Get the angle of the accumulator angle = math.atan2(accum.y, accum.x) -- Get the length of the accumulator / 24 (in this way, we are sure that it is -- between 0 and 1 and setting a good value for delta is easy) length = math.sqrt(accum.x*accum.x + accum.y*accum.y) / 24 -- Is the closest obstacle very close? if (length < DELTA) then -- No, keep going straight robot.wheels.set_velocity(5,5) else -- Yes, bounce away depending on the direction of the obstacle if angle > 0 then -- The obstacle is on the left, turn to the right robot.wheels.set_velocity(5,-2) else -- The obstacle is on the right, turn to the left robot.wheels.set_velocity(-2,5) end end end --[[ Controller reset ]] function reset() -- nothing to do end --[[ Controller destroy ]] function destroy() -- nothing to do end