Part3: Implement algorithm with sbRIO robots