AI Problem for vacuum cleaner robot (Agent) WIP

[bplus]

This is not machine learning but is an AI for allowing a sim of a device to act on it's own without human intervention. 
It's a WIP (Work In Progress) so far just getting tools together like finding shortest Path to closest empty cell.
I am taking a room rectangle and filling along sides furniture in rectangles, a random setup for vacuum RI (Robot Intelligence) to run through.

So far it does cover the room in a reasonable amount of steps without a lot of going over same cells.

This is either fun to watch or I need to get a life ;-))


' AI for Vacuum 2022-02-27 b+ trans
' from  "I Robot - Room Mapper 2 (IR-RM2)" ' b+ 22-02-26 QB64

Global H$, SW, SH, Cell, MapW, MapH
H$ = "gr": SW = 1021: SH = 721 ' cell @30 max 34x24
Cell = 30
MapW = 17: MapH = 17 ' min @30 cell is 17, 17

nomainwin

WindowWidth = SW + 8
WindowHeight = SH + 32
UpperLeftX = (1200 - SW) / 2
UpperLeftY = (700 - SH) / 2

open "AI for Vacuum" for graphics_nsb_nf as #gr
#gr "setfocus"
#gr "trapclose quit"
#gr "down"
#gr "fill black"

Global BeeLine, Done, StepI, StepDist, rx, ry
' BeeLine is a mode where RI is following shortest path to next empty cell
' StepI, StepDist are info for Path following
' rx,ry is current robot position

Dim StepMap(MapW, MapH) ' for making paths that you step through
Dim StepX(MapW * MapH), StepY(MapW * MapH) ' actual steps to take on path
Dim Room(MapW, MapH) ' 0 = empty cell in grid -1 = wall see MakeRoom -2 area inaccessible to vaccum

Do  'resets
   scan
   Done = 0: rx = Int(MapW / 2 + .5): ry = Int(MapH / 2 + .5) ' starts here in middle of room should be clear
   call MakeRoom
   call drawRoom
   #gr "getbmp TheRoom 0 0 1020 720"
   #gr "background TheRoom"

   Do 'sweep the room just made
       scan
       call RoomUpdate
       call RI ' ok roby make your move
       If Done Then ' need update of last move by robot
           call RoomUpdate
           Exit Do
       end if
       call pause 100
   Loop until 0
   sweeps = sweeps + 1
   notice "Robot Vacuum has made " + Str$(sweeps) + " successful room sweeps."
Loop until 0
wait

Sub RI ' the robot appraises it's current postition rx, ry in Room(x) and makes a move changing rx, ry and that ends the sub
   ' I was here!  sweeps the spot (again?)
   Room(rx, ry) = Room(rx, ry) + 1 ' put roby's presense on map by counting number of times in this cell

   ' after marking current spot see if we have swept all possible
   If swept() Then Done = -1: Exit Sub

   If BeeLine = 0 Then ' normal sweeping pattern, hey try up/down then right/left see if leaves less spots
       If Room(rx, ry - 1) = 0 Then ' one must have ones priorities
           ry = ry - 1: Exit Sub
       else
           If Room(rx + 1, ry) = 0 Then
               rx = rx + 1: Exit Sub
           Else
               If Room(rx, ry + 1) = 0 Then
                   ry = ry + 1: Exit Sub
               Else
                   If Room(rx - 1, ry) = 0 Then rx = rx - 1: Exit Sub
               end if
           end if
       End If

       ' still here ?  ========== Decide to make a BeeLine - find the closet empty and make a bee-line to it
       BeeLine = -1 'put us into BeeLine Mode
       call prepStepMap rx, ry
       mini = 10000000
       For y = 1 To MapH ' now run through step map and find the mini closest empty room
           For x = 1 To MapW
               scan
               If StepMap(x, y) > 1 Then ' has to be > 1 because 0 is robot and 1 the robot will detect!
                   If StepMap(x, y) < mini And Room(x, y) = 0 Then mini = StepMap(x, y): saveX = x: saveY = y
               End If
           Next
       Next

       'OK we have our target not find a clear short path to it from roby
       call path rx, ry, saveX, saveY ' path sets stepI, stepX(i), stepY(I) that are shared
       StepI = 1 ' roby is on it's way to closet empty cell
       rx = StepX(StepI): ry = StepY(StepI)
   Else
       ' beeline mode
       StepI = StepI + 1 ' roby is on it's way to closet empty cell
       rx = StepX(StepI): ry = StepY(StepI)

       'turn off beeLine mode when we have hit target
       If StepI = StepDist - 1 Then BeeLine = 0 ' we have arrived at our target turn off beeLine mode
   End If
End Sub

Sub drawRoom
   #gr "fill black"
   call drawGridSq
   For y = 1 To MapH
       For x = 1 To MapW
           scan
           If Room(x, y) = -1 Then
               #gr "color 0 150 200"
               #gr "backcolor 0 150 200"
               call fbox (x - 1) * 30, (y - 1) * 30, x * 30, y * 30
           Else
               If Room(x, y) > 0 Then
                   #gr "color white"
                   #gr "backcolor black"
                   s$ = Str$(Room(x, y))
                   call stext (x - 1) * 30 + (30 - Len(s$) * 8) / 2, (y - 1) * 30 + 7 + 16, s$
               end if
           End If
       Next
   Next
End Sub

sub RoomUpdate
   #gr "drawsprites" 'draw room background
   For y = 1 To MapH
       For x = 1 To MapW 'update numbers
           scan
           If Room(x, y) > 0 Then
               #gr "color white"
               #gr "backcolor black"
               s$ = Str$(Room(x, y))
               call stext (x - 1) * 30 + (30 - Len(s$) * 8) / 2, (y - 1) * 30 + 7 + 16, s$
           end if
       Next
   Next
   ' and robot
   #gr "color yellow"
   #gr "backcolor yellow"
   call fbox (rx - 1) * 30, (ry - 1) * 30, rx * 30, ry * 30
end sub

Sub MakeRoom
   Dim Room(MapW, MapH) ' 0 = empty, -1 = wall, -2 = area vac can't access because walled out

   ' Here are the walls
   For x = 1 To MapW
       Room(x, 1) = -1
       Room(x, MapH) = -1
   Next
   For y = 1 To MapH
       Room(1, y) = -1
       Room(MapW, y) = -1
   Next

   ' add random rectangles around the edges
   For i = 1 To Int(Sqr(1.5 * MapW * MapH))
       scan
       rw = Int(Rnd(0) * 4) + 1: rh = Int(Rnd(0) * 4) + 1
       wall = Int(Rnd(0) * 4)
       Select Case wall
           Case 0 ' top
               If Rnd(0) < .5 Then ys = 1 Else ys = 4
               xs = Int(Rnd(0) * (MapW - rw)) + 1
               For y = ys To ys + rh - 1
                   For x = xs To xs + rw - 1
                       Room(x, y) = -1
                   Next
               Next
           Case 1 'right
               If Rnd(0) < .5 Then xs = MapW - rw + 1 Else xs = (MapW - 4) - rw + 1
               ys = Int(Rnd(0) * (MapH - rh)) + 1
               For y = ys To ys + rh - 1
                   For x = xs To xs + rw - 1
                       Room(x, y) = -1
                   Next
               Next
           Case 2 ' bottom
               If Rnd(0) < .5 Then ys = MapH - rh + 1 Else ys = (MapH - 4) - rh + 1
               xs = Int(Rnd(0) * (MapW - rw)) + 1
               For y = ys To ys + rh - 1
                   For x = xs To xs + rw - 1
                       Room(x, y) = -1
                   Next
               Next
           Case 3 'left
               If Rnd(0) < .5 Then xs = 1 Else xs = 4
               ys = Int(Rnd(0) * (MapH - rh)) + 1
               For y = ys To ys + rh - 1
                   For x = xs To xs + rw - 1
                       Room(x, y) = -1
                   Next
               Next
       End Select
   Next

   ' before make map make sure rx, ry is set or reset
   call prepStepMap rx, ry ' see what cells not accessible to vac mark them -1
   For y = 1 To MapH
       For x = 1 To MapW
           If Room(x, y) = 0 Then
               If StepMap(x, y) = 0 Then Room(x, y) = -1
           End If
       Next
   Next
   Room(rx, ry) = 0 ' robot start not a -2 room!
End Sub

Sub drawGridSq
   #gr "color white"
   For x = 0 To Cell * MapW Step Cell
       #gr "Line ";x;" ";0;" ";x;" ";Cell * MapH
   Next
   For y = 0 To Cell * MapH Step Cell
       #gr "Line ";0;" ";y;" ";Cell * MapW;" ";y
   Next
End Sub

Function swept()
   For y = 1 To MapH
       For x = 1 To MapW
           If Room(x, y) = 0 Then Exit Function 'not swept
       Next
   Next
   swept = -1 ' all clean!
End Function

Sub path sx, sy, tx, ty ' start x, y to target x, y
   call prepStepMap tx, ty
   dist = StepMap(sx, sy)
   StepDist = dist 'for global
   If dist = 0 Or Room(tx, ty) = -1 Then
       StepI = 0
       notice "Target: " + Str$(tx) + "," + Str$(ty) + " is bad, fatal error."
       call quit H$
   End If
   'refresh
   Dim StepX(MapW * MapH), StepY(MapW * MapH)
   StepI = 0
   cx = sx: cy = sy
   While dist >= 2
       scan
       cf = 0
       #gr "color darkgreen"
       #gr "backcolor darkgreen"
       For y = cy - 1 To cy + 1
           For x = cx - 1 To cx + 1
               scan
               If StepMap(x, y) = dist - 1 Then
                   StepI = StepI + 1
                   StepX(StepI) = x: StepY(StepI) = y
                   call fbox (StepX(StepI) - 1) * 30 + 6, (StepY(StepI) - 1) * 30 + 6, StepX(StepI) * 30 - 6, StepY(StepI) * 30 - 6
                   cf = 1: Exit For
               End If
           Next
           If cf = 1 Then Exit For
       Next
       If cf = 0 Then 'lost path
           Exit Sub
       Else
           cx = StepX(StepI): cy = StepY(StepI)
           dist = dist - 1
       End If
   Wend
   call pause 500
End Sub

Sub prepStepMap tx, ty ' ========================================== no more diagonal steps
   Dim StepMap(MapW, MapH)
   If tx > 0 And tx <= MapW And ty > 0 And ty <= MapH Then
       StepMap(tx, ty) = 1: tick = 1: changes = 1
       While changes
           scan
           tick = tick + 1: changes = 0
           ystart = max(ty - tick, 1): ystop = min(ty + tick, MapH)
           For y = ystart To ystop
               xstart = max(tx - tick, 1): xstop = min(tx + tick, MapW)
               For x = xstart To xstop
                   'check out the neighbors
                   If Room(x, y) >= 0 Then ' places OK to go
                       ' ============================================================= new >>> path finder  that won't take diagonal steps
                       'need to check 4 cells around x, y for parent
                       If StepMap(x - 1, y) = tick - 1 And StepMap(x, y) = 0 Then
                           StepMap(x, y) = tick
                           changes = 1: GoTo [skip]
                       End If
                       If StepMap(x + 1, y) = tick - 1 And StepMap(x, y) = 0 Then
                           StepMap(x, y) = tick
                           changes = 1: GoTo [skip]
                       End If
                       If StepMap(x, y - 1) = tick - 1 And StepMap(x, y) = 0 Then
                           StepMap(x, y) = tick
                           changes = 1: GoTo [skip]
                       End If
                       If StepMap(x, y + 1) = tick - 1 And StepMap(x, y) = 0 Then
                           StepMap(x, y) = tick
                           changes = 1
                       End If
                       [skip]
                   End If
               Next
           Next
       Wend
   Else
       if swept() then exit sub
       Notice "Target: " + Str$(tx) + ", " + Str$(ty) + " is bad, fatal error."
       call quit H$
   End If
End Sub

sub stext x, y, message$ 'note: have to reset fore or back color after ink
   #gr "place ";x;" ";y;";|";message$
end sub

sub fbox x0, y0, x1, y1
   #gr "place ";x0;" ";y0
   #gr "boxfilled ";x1+1;" ";y1+1
end sub

sub quit H$
   close #gr '<=== this needs Global H$ = "gr"
   end       'Thanks Facundo, close graphic wo error
end sub

sub pause mil   'tsh version has scan built-in
   t0 = time$("ms")
   while time$("ms") < t0 + mil : scan : wend
end sub


Ideally or eventually I'd like the RI to map out a room with no idea where it sits in the room, just figure it out all out on it's own and then after first sweep, optimize the the round for least cells crossed over more than once. So eventually RI will be creating it's own map and be weened off what I call the Reality Map = the Room array.

This might be interesting pencil and paper problem, given a room with furniture obstacles and draw a continuous line (sorry John T ) through the cells accessible to vacuum (could be lawn mower too) with least amount of cross over.

PS I should also mention the device can only go NSEW the 4 compass directions and it also can sense only whether the squares on those 4 sides (the device is square) are empty or occupied, like having whiskers that detect resistance on the 4 sides. It knows not to go in direct of resistance. Thus when starting off in direction it "learns" the "state" of 3 new cells empty or not. It prefers not to backtrack unless it is the only move it can make (out of a one way tunnel say).

[bplus]

Reading over my words, I now see the next step for this code. As of now, the closest empty cell is given to the RI. The next step is the RI recording empty cells it passes up. I/it could review it's list when ever it gets surrounded by already swept cells.