AI for Vacuum cleaner WIP

[plus]

Not optimized yet just getting tools together but I do have it working so a room with lots of obstacles is completely covered by robot with fairly small amount of redundant returns.

Fun to watch because I don't have to do the work

' 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


MapW = 17: MapH = 17 ' min @30 cell is 17, 17 is the smallest room to make where you are sure the vacuum doesn't start in/on furniture (blue colors), the robot vacuum is the yellow square. The occasional green path is the AI finding the next closest empty square to go. The numbers are a count of times the vacuum has passed over that spot.

You can change MapW up to 34 and MapH up to 24 and cover the full screen as shown below:

Attachments: