Started coding the robot algorithm