Have a working algorithm using wall sensing, no odometry yet