Chang branch

New:
- get_current_robot_position.py (to get the current position)
- get_current_robot_position_super.py (test state machine that calls get_current_position)