path planning lego robot

I have a big square, this square divided into 5×5 squares,

any square is 25cm, total 125cm

i want to program the Lego robot to find the minimum path and avoid the objects

for example this code it is for two dimensional array

int array[10][10];

i use the robotC

you can download from:

http://www.robotc.net/

i connect the Lego with my labtop, and work the Lego in real world

i uploaded an example for report

also i uploaded a lecture for path planning

these links show code

http://www.education.rec.ri.cmu.edu/previews/robot_c_products/teaching_rc_lego_v2_preview/

http://www.robotc.net/education/curriculum/nxt/

and another 2 link to help you

http://www.societyofrobots.com/programming_wavefront.shtml

http://en.wikipedia.org/wiki/Dijkstra%27s_algorithm