Now moves straight when mapping
Added goalDirection (not implemented)