-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathmap.py
134 lines (94 loc) · 4.37 KB
/
map.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
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
import numpy as np
import math
import ai
#handles mapping out the area (another object will keep track of the position)
class AreaMap(object):
def __init__(self, x, y):
#self.mapArr = np.zeros((10,10))
#amount of square cm each array element represents
self.arrWidth = 10
self.x = self.scale(x)
self.y = self.scale(y)
def initTestMap(self):
self.mapArr = np.array( [ [0,0,0,0,0,0,0,0,0,0],
[0,0,0,0,0,0,0,0,0,0],
[0,0,0,0,0,0,0,0,0,0],
[0,0,0,0,0,0,0,0,0,0],
[0,0,0,0,0,0,0,0,0,0],
[0,1,1,0,0,0,0,0,0,0],
[0,0,1,0,0,0,0,0,0,0],
[0,0,1,1,0,0,0,0,0,0],
[0,3,0,1,1,1,0,0,0,0],
[0,0,0,0,0,0,0,0,0,0]] )
#return a map of the immediate area only
#value of r is currently in array unit scale, not CM
def prox(self, r):
#the MAX builtin is required to make sure no negative array indicies are requested
print self.mapArr[(max(0,self.y-r)):(self.y+r),(max(0,self.x-r)):(self.x+r)]
#update the car's current position on the map
def setPosition(self, x, y):
self.x = self.scale(x)
self.y = self.scale(y)
#nifty feature from numpy. Find and remove the existing position
self.mapArr[self.mapArr == 2] = 0
self.mapArr[self.y][self.x] = 2
#convert any units the car provides into units based on the map array
def scale(self, val):
out = int((float(val)/float(self.arrWidth)))
return out
#this function adds a known obstacle to the map array
#this assumes we have 4 sensors on the car
#the cardinal specifies the direction of the sensor as a string, "left", "right", "forward", or "backward"
#direction finds the current rotation of the vehicle (in degrees), which should be kept track of by the position object
#this assumes that the default direction of the car when it begins moving is looking DOWN the array.
#distance is the raw reading from the sensor
def plot(self, cardinal, direction, distance):
if cardinal == "forward":
cx = round(self.scale(distance*(math.sin(math.radians(direction)))),1)
cy = round(self.scale(distance*(math.cos(math.radians(direction)))),1)
elif cardinal == "backward":
cx = round(self.scale(distance*(math.sin(math.radians(direction+180)))),1)
cy = round(self.scale(distance*(math.cos(math.radians(direction+180)))),1)
elif cardinal == "left":
cx = round(self.scale(distance*(math.sin(math.radians(direction+90)))),1)
cy = round(self.scale(distance*(math.cos(math.radians(direction+90)))),1)
elif cardinal == "right":
cx = round(self.scale(distance*(math.sin(math.radians(direction-90)))),1)
cy = round(self.scale(distance*(math.cos(math.radians(direction-90)))),1)
#this is a temporary solution. Really the map should expand as we travel
try:
self.mapArr[self.y+cy][self.x+cx] = 1
except:
print "plot was outside of array range"
def prettyDirections(directions):
print directions
new = ""
directions.insert(0,'')
i = 1
for n, direc in enumerate(directions):
if n == 0:
pass
else:
if direc == directions[n-1]:
i = i + 1
else:
new = new + (directions[n-1]+' x'+str(i)+', ')
i = 1
return new[5:]
tinst = AreaMap(0,0)
tinst.initTestMap()
tinst.setPosition(50,50)
tinst.plot('right',-45,30)
tinst.prox(5)
pathfinder = ai.PathCalc()
result = pathfinder.calculateBestPath(tinst.x, tinst.y, tinst.mapArr)
'''
tinst.prox(3)
result = tinst.calculateBestPath()
'''
print "--------------------------------------------------------"
print result
print "--------------------------------------------------------"
#tinst.setPosition(50,50)
#print tinst.mapArr
#tinst.prox(3)