Project2a: Implement algorithm with sbRIO robots