Init., pin assignments and mapping

Initialize Motors and Sensors, assign pins, adjust mapping on sonar
distance