I made a basic navigation algo, it just checks if any samples are within 18" and if it happens more than a certain number of times (in case of bad ToF measurements). It still needs work. This does not include the IMU. The sampling is very course. The beam is narrow, so often the outer legs run into something... I tried to correct that later by increasing the sensing distance threshold eg. from 12" away to 18".

At this time I have to replace all of the servos on this robot because many of them died. I think primarily from when it was running into a wall/tipped over and it had to push itself back upright. Overall I've mentioned before my disappointment in this robot due to picking cheap parts (weak servos, inaccurate sensors). But hey, that's what you get for 75% off (servo price). I'm half tempted to find 9G servos with the exact same dimensions but are more powerful/possibly using metal gear however I can't guarantee that they're actually good (unless I buy from someone like Adafruit I suppose). Ultimately I'm also not sure if it's worth dumping more money into this project. I do want to follow through with the software and finish the slated goals.

Here's a long video talking about the first obstacle detection "algo" and then the various trials.

I mostly had dumb lazy code mistakes. I think I can polish it up/make it work decent, with the IMU correction for stall/being stuck that can help improve its reliability. I had another thought too where an overhead camera/server would steer it.

There are obvious improvements to make like pre-emptively terminating the rest of a sweep sample if I already found something in the way. This is a very crude/coarse obstacle detection, if something's detected that's treated as a big cube that the robot has to go around. I don't know about the dimensions yet.