This is the first H bridge program I have made using sensors. So far so good. It is not fast, but has alot of torque. This is the geared up version that works. The Lego motors are rated at 9V, but using 7.2V through a H bridge. Similar modified Whisker program as the Lego Boe Bot just listed.
Here is a short video