-
Notifications
You must be signed in to change notification settings - Fork 0
/
Planner.py
executable file
·40 lines (35 loc) · 1.28 KB
/
Planner.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
import ClassCode.Utilities
import math
import PilotUtils
TOLERANCE = 2.0
FINAL_TOLERANCE = 2.0
class Planner:
def __init__(sf, waypoints):
sf.waypoints = waypoints
sf.shouldResetIntegral = False
def updatePosition (sf, lat, lon):
sf.shouldResetIntegral = False
sf.currPosition = [lat, lon]
if len(sf.waypoints) == 1:
return
dist = PilotUtils.distanceApproximate(sf.currPosition[0], sf.currPosition[1], sf.waypoints[0][0], sf.waypoints[0][1])
if dist < TOLERANCE:
# Pop one waypoint
sf.waypoints.pop(0)
sf.shouldResetIntegral = True
print('>>>>>>> REACHED waypoint <<<<<<<')
print('>>>>>>> REACHED waypoint <<<<<<<')
print('>>>>>>> REACHED waypoint <<<<<<<')
print('>>>>>>> REACHED waypoint <<<<<<<')
print('>>>>>>> REACHED waypoint <<<<<<<')
print('>>>>>>> REACHED waypoint <<<<<<<')
print('>>>>>>> REACHED waypoint <<<<<<<')
print(sf.waypoints)
def getTargetHeading (sf):
return PilotUtils.desiredHeadingCalib(sf.currPosition[0], sf.currPosition[1], sf.waypoints[0][0], sf.waypoints[0][1])
def shouldStop (sf):
# Less than 5 meters from the target
if len(sf.waypoints) == 1 and PilotUtils.distanceApproximate(sf.currPosition[0], sf.currPosition[1], sf.waypoints[0][0], sf.waypoints[0][1]) < FINAL_TOLERANCE:
return True
else:
return False