Advertisement
VSZM

Untitled

May 2nd, 2017
173
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. while visitable_coordinates.isNotEmpty():# Megnézzük, van-e még érintendő pont
  2.     actual_position = PositioningService.getActualPosition()
  3.     closest_visitable = DescartesService.getNearestPositionToActual(actual_position, visitable_coordinates)
  4.     distance_to_goal = DescartesService.getDistanceInMeters(actual_position, closest_visitable)
  5.    
  6.     if distance_to_goal < 5.0 :
  7.         visitable_coordinates.remove(closest_visitable) # Elértük a célt, ezért ezt töröljük
  8.     else:# Kiszámoljuk, hogy a jelenlegi irány jó-e, ha nem, korrigálunk
  9.         actual_direction_vector = PositioningService.getActualDirection()
  10.         desired_direction_vector = DescartesService.getVectorBetweenTwoPoints(actual_position, closest_visitable)
  11.         angle_difference = DescartesService.getAngleBetweenTwoVectors(actual_direction_vector, desired_direction_vector)
  12.        
  13.        
  14.         if abs(angle_difference) > 5: # Elég nagy-e az eltérés?
  15.             # Ha igen, negatív szögnél balra, pozitívnál jobbra korrigálunk
  16.             if angle_difference < 0:
  17.                 MotorFunctions.TurnLeft()
  18.             else:
  19.                 MotorFunctions.TurnRight()
Advertisement
RAW Paste Data Copied
Advertisement