Final code used at Robocon India - 2014