Path Planning in Lego Robot

/in /by

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