Maze solving - with L#

Korean company maker of Robot kits and servos designed for of articulated robots. Re-incarnation of Megarobotics.
4 postsPage 1 of 1
4 postsPage 1 of 1

Maze solving - with L#

Post by l3v3rz » Fri Jan 08, 2010 7:27 pm

Post by l3v3rz
Fri Jan 08, 2010 7:27 pm

In the Robobuilder C programming tutorial there is an application written in C to enable the robot to traverse a maze using the distance sensor. You have to compile it in AVR C and download to the RBC. To make this easier to do, I've converted it to L#/Lsharp.Net. A simple scripting language. It uses the lisp code explained here http://robosavvy.com/forum/viewtopic.php?t=4800. (Or downloaded as a word document here : http://robobuildervc.googlecode.com/files/final.doc )

You need final.lisp from here http://robobuildervc.googlecode.com/files/Lisp.zip to connect to Robobuilder. The maze application code is available here http://robobuildervc.googlecode.com/files/maze2.lisp

The code builds a map on screen of the maze as the robot moves around. It uses the distance sensor to identify the walls and then the following algorithm:

1. If there is no wall, move forward.
2. If the wall is detected, check left side.
3. If front and left side wall are detected, check right side.
4. If front, left and right side walls detected, go to opposite way.

You can run it in a simulate mode without the robot. Here's an example of it in action:

Code: Select all
 > (maze)
  ........
  ........
  ........
  ........
  ....^...
  ........
  ........
  ........
  distance ?
  : 20
  LT90
  Run motion 3
  Run motion 3
  Run motion 3
  ........
  ........
  ........
  ........
  ....<...
  ........
  ........
  ........


The program defines a number of simple primitive motions based on the built in motions:

Code: Select all
(def leftturn90   () (prn "LT90")   ( runMotion 3) (runMotion 3) (runMotion 3))
(def leftturn180 () (prn "LT180") ( runMotion 3) (runMotion 3) (runMotion 3) (runMotion 3) (runMotion 3))
(def rightturn90 () (prn "RT90")  ( runMotion 5) (runMotion 5) (runMotion 5) (runMotion 5))
(def forward      () (prn "FWD")   ( runMotion 4) (runMotion 4) (runMotion 5) )
(def back           () (prn "BACK")  ( runMotion 10))


The maps is created using a simple string to represent 8x8 grid. And some functions that display and show the robot a simple character.

The function maze then uses these routines to solve the maze, by walking the robot around, and measuring the distances to the nearest wall using the PSD.

The code is written so that it uses two key routines from final.lisp runMotion that plays the motion turn left etc and readdistance that reads the PSD sensor. These routines are initially defined in maze2.lisp to allow user to simulate by entering the distance and then printing the motion to be called - so it can be run without a robot attached.

To run fully all that is required is to load final.lisp which will redefine runMotion and readdistance to be the actual routines. Call (run_robobuilder) and connect the PC to robot. Select "Basic Posture" and then exit the menu. Now type (maze) and off it goes, showing its its progress on the map!

Code: Select all
;maze algorithm
(def maze ()
  (= px 4) (= py 4)          ; assume center of maze
  (= pd 0)                   ; facing up
  (= pt '("^" ">" "v" "<"))  ; maze turtle character
  (= cs 0)                   ; current state = 0
  (clrmap)                   ; reset map
 
  (while true
     (= pd (mod pd 4))
     (putmap px py (nth pt pd)) (showmap)
     (= d (readDistance))
     (if  (> 12 d)                   (back)
     
          (and (is cs 0) (> 30 d))   (do (leftturn90)  (= pd (+ pd 3)) (= cs 1))

          (and (is cs 1) (> 30 d))   (do (leftturn180) (= pd (+ pd 2)) (= cs 2))

          (and (is cs 2) (> 30 d))   (do (rightturn90) (= pd (+ pd 1)) (= cs 0))
         
          (do
             (= cs 0) 
             (forward)
             
             ; update map       
             (putmap px py "*")
             
             (= pd (mod pd 4))
             (if (is pd 0) (= py (- py 1))
                   (is pd 1) (= px (+ px 1))
                   (is pd 2) (= py (+ py 1))
                   (is pd 3) (= px (- px 1)))
             (if (< px 0) (= px 0) (< py 0) (= py 0) (> px 7) (= px 7) (> py 7) (= px 7))
           )
      )
  )
)


You may well have to tune the motion routines to optimise performance. You you could add a test to see if the robot has fallen over - and if it does, get it to stand up, reset the maze and start again.

Have fun!
In the Robobuilder C programming tutorial there is an application written in C to enable the robot to traverse a maze using the distance sensor. You have to compile it in AVR C and download to the RBC. To make this easier to do, I've converted it to L#/Lsharp.Net. A simple scripting language. It uses the lisp code explained here http://robosavvy.com/forum/viewtopic.php?t=4800. (Or downloaded as a word document here : http://robobuildervc.googlecode.com/files/final.doc )

You need final.lisp from here http://robobuildervc.googlecode.com/files/Lisp.zip to connect to Robobuilder. The maze application code is available here http://robobuildervc.googlecode.com/files/maze2.lisp

The code builds a map on screen of the maze as the robot moves around. It uses the distance sensor to identify the walls and then the following algorithm:

1. If there is no wall, move forward.
2. If the wall is detected, check left side.
3. If front and left side wall are detected, check right side.
4. If front, left and right side walls detected, go to opposite way.

You can run it in a simulate mode without the robot. Here's an example of it in action:

Code: Select all
 > (maze)
  ........
  ........
  ........
  ........
  ....^...
  ........
  ........
  ........
  distance ?
  : 20
  LT90
  Run motion 3
  Run motion 3
  Run motion 3
  ........
  ........
  ........
  ........
  ....<...
  ........
  ........
  ........


The program defines a number of simple primitive motions based on the built in motions:

Code: Select all
(def leftturn90   () (prn "LT90")   ( runMotion 3) (runMotion 3) (runMotion 3))
(def leftturn180 () (prn "LT180") ( runMotion 3) (runMotion 3) (runMotion 3) (runMotion 3) (runMotion 3))
(def rightturn90 () (prn "RT90")  ( runMotion 5) (runMotion 5) (runMotion 5) (runMotion 5))
(def forward      () (prn "FWD")   ( runMotion 4) (runMotion 4) (runMotion 5) )
(def back           () (prn "BACK")  ( runMotion 10))


The maps is created using a simple string to represent 8x8 grid. And some functions that display and show the robot a simple character.

The function maze then uses these routines to solve the maze, by walking the robot around, and measuring the distances to the nearest wall using the PSD.

The code is written so that it uses two key routines from final.lisp runMotion that plays the motion turn left etc and readdistance that reads the PSD sensor. These routines are initially defined in maze2.lisp to allow user to simulate by entering the distance and then printing the motion to be called - so it can be run without a robot attached.

To run fully all that is required is to load final.lisp which will redefine runMotion and readdistance to be the actual routines. Call (run_robobuilder) and connect the PC to robot. Select "Basic Posture" and then exit the menu. Now type (maze) and off it goes, showing its its progress on the map!

Code: Select all
;maze algorithm
(def maze ()
  (= px 4) (= py 4)          ; assume center of maze
  (= pd 0)                   ; facing up
  (= pt '("^" ">" "v" "<"))  ; maze turtle character
  (= cs 0)                   ; current state = 0
  (clrmap)                   ; reset map
 
  (while true
     (= pd (mod pd 4))
     (putmap px py (nth pt pd)) (showmap)
     (= d (readDistance))
     (if  (> 12 d)                   (back)
     
          (and (is cs 0) (> 30 d))   (do (leftturn90)  (= pd (+ pd 3)) (= cs 1))

          (and (is cs 1) (> 30 d))   (do (leftturn180) (= pd (+ pd 2)) (= cs 2))

          (and (is cs 2) (> 30 d))   (do (rightturn90) (= pd (+ pd 1)) (= cs 0))
         
          (do
             (= cs 0) 
             (forward)
             
             ; update map       
             (putmap px py "*")
             
             (= pd (mod pd 4))
             (if (is pd 0) (= py (- py 1))
                   (is pd 1) (= px (+ px 1))
                   (is pd 2) (= py (+ py 1))
                   (is pd 3) (= px (- px 1)))
             (if (< px 0) (= px 0) (< py 0) (= py 0) (> px 7) (= px 7) (> py 7) (= px 7))
           )
      )
  )
)


You may well have to tune the motion routines to optimise performance. You you could add a test to see if the robot has fallen over - and if it does, get it to stand up, reset the maze and start again.

Have fun!
l3v3rz offline
Savvy Roboteer
Savvy Roboteer
Posts: 473
Joined: Fri Jul 18, 2008 2:34 pm

Distance Sensor Error

Post by ebrand » Tue Mar 16, 2010 4:28 pm

Post by ebrand
Tue Mar 16, 2010 4:28 pm

I've set this up on my robobuilder and it is pretty sweet. I've just run into one problem. I get this exception when the robobuilder gets too close to an object:

Exception : Input string was not in a correct format.

If you can just try to point me in the right direction in the code I bet I can figure out the error. I am using maze2.lisp and LSharpConsole.exe.

Any help will be greatly appreciated!
I've set this up on my robobuilder and it is pretty sweet. I've just run into one problem. I get this exception when the robobuilder gets too close to an object:

Exception : Input string was not in a correct format.

If you can just try to point me in the right direction in the code I bet I can figure out the error. I am using maze2.lisp and LSharpConsole.exe.

Any help will be greatly appreciated!
ebrand offline
Robot Builder
Robot Builder
Posts: 9
Joined: Mon Mar 15, 2010 9:32 pm

Post by l3v3rz » Tue Mar 16, 2010 6:43 pm

Post by l3v3rz
Tue Mar 16, 2010 6:43 pm

Its great hear someone try and using it. The problem might be down to the lack of parameter checking on putmap - and the .substring command fails. Try putting a test for x and y values between 0 and 7.
Its great hear someone try and using it. The problem might be down to the lack of parameter checking on putmap - and the .substring command fails. Try putting a test for x and y values between 0 and 7.
l3v3rz offline
Savvy Roboteer
Savvy Roboteer
Posts: 473
Joined: Fri Jul 18, 2008 2:34 pm

Post by l3v3rz » Thu Mar 18, 2010 11:41 pm

Post by l3v3rz
Thu Mar 18, 2010 11:41 pm

Another potential problem is if the readdistance command fails - bad comms for instance - a null string (rather than a 0) is returned that also generates this same message about "Input String Format .."

This is easily fixed and I've just uploaded a new version of maze2,lisp. The essential difference being it now checks and set to 0 if a null or empty string is returned:

Code: Select all
(def readDistance   ()
    (with (x (readdistance))
        (do (exit?)
              (if (or (is x "") (is x null)) (= x 0) )
              (prn "distance = " x)
      )))


regards
Another potential problem is if the readdistance command fails - bad comms for instance - a null string (rather than a 0) is returned that also generates this same message about "Input String Format .."

This is easily fixed and I've just uploaded a new version of maze2,lisp. The essential difference being it now checks and set to 0 if a null or empty string is returned:

Code: Select all
(def readDistance   ()
    (with (x (readdistance))
        (do (exit?)
              (if (or (is x "") (is x null)) (= x 0) )
              (prn "distance = " x)
      )))


regards
l3v3rz offline
Savvy Roboteer
Savvy Roboteer
Posts: 473
Joined: Fri Jul 18, 2008 2:34 pm


4 postsPage 1 of 1
4 postsPage 1 of 1
cron