OK, here's some really basic code for one of my subroutine programs. This one will be the one that "blindly" builds a map of where the robot is and where other things may be. The map itself is only used as a secondary source (more for seeing "am i headed in the right direction" then for planning out complex moves).

sensors used: 3 IR sensors that measure proximity to robot. They are placed facing foward, foward left, and foward right.Distance tracking: x,y, and degree values of the robot are measured by the robot. There may be a lot of imprecision here, which is why the map is a secondary source! (NOTE: the x and y values may be negative! the robot starts off in the state 0,0 )

The pseudocode:

/* NOTES: if you use this, email me...! raquib0@lycos.com i'd like to hear about your robot! (credit would be nice too..)

a map is made up of areas

an area is a matrix of doubles (real numbers) where the location of each element (x,y) represents a set distance and the value of each element represents the probability that is is "filled".

when I say "mark" an area, i mean that you will do this:

your position is x,y
add .1 (or some other constant) to the elements 1 element away from x,y
add .05 (or some other contant) to the elements 2 elements away from x,y
add .0025 (or some other contant) to the elements 3 elements away from x,y...
.... etc
.... etc
keep doing this for N elements away from xy

the elements in the matrix with a value above a certain threshold are likely to be filled... you can use this to plan a overall route to a point.

*/

constant Tx; // max x size on the map
constant Ty; // max y size on the map
constant Td = 360; // max degrees
constant Tir; // threshold for IR values to signal a mark event

var doff = 0;
var xoff = 10;
var yoff = 10;

var ir1, ir2, ir3, x,y,d

begin critical loop{
read in ir1, ir2, ir3, x,y,d values from robot
// bring the x,y values to our system
x += (Tx/2);
y += (Ty/2);

// now put x,y to our degree position
var distance = sqrt(x*x + y*y);
x = distance*sin(doff+d);
y = distance*cos(doff+d);
// check bounds, create or move to new area if need be
if(! inbounds(x,y) ){
make xoff and yoff starting positions in new area
doff = (doff+d)%Td;
go back to start of loop
} // inbounds check

// check if i should update this area of the map
if(ir1 + ir2 + ir3 > Tir){
update values around me in the area
}
} // end of crtical loop