/
navigation.py
executable file
·120 lines (94 loc) · 3.26 KB
/
navigation.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
import numpy as np
import math as math
import heapq
from robot import Robot as robot
from Canvas import Canvas
import time
from place_rec_bits import SignatureContainer
def square(dist=40):
offset = 100
wpts = np.array([[offset, offset, 0],
[offset + dist, offset, 0],
[offset + dist, offset - dist, 0],
[offset, offset - dist, 0],
[offset, offset, 0],
[offset+ 10, offset, 0]])
navigateThroughWaypoints(wpts)
def line(dist=100):
offset = 100
wpts = np.array([[offset, offset, 0],
[offset + dist, offset, 0],
[offset, offset, 0],
[offset + 10, offset, 0]])
navigateThroughWaypoints(wpts)
def userInput():
myLoc = np.array([0, 0, 0])
r = robot(myLoc)
while True:
x = float(input("Enter a x coordinate (in cm): "))
y = float(input("Enter a y coordinate (in cm): "))
dest = np.array([x, y, 0])
myLoc = navigateToWaypoint(myLoc, dest)
print(myLoc)
def cwWaypoints():
wpts = np.array([[84, 30, 0], #XXX
[132, 30, 0],
[180, 30, 0], #XXX
[180, 42, 0],
[180, 54, 0], #XXX
[159, 54, 0],
[138, 54, 0], #XXX
[138, 111, 0],
[138, 168, 0], #XXX
[126, 168, 0],
[114, 168, 0], #XXX
[114, 126, 0],
[114, 84, 0], #XXX
[99, 84, 0],
[84, 84, 0], #XXX
[84, 57, 0],
[84, 30, 0]])
navigateThroughWaypoints(wpts)
def compWaypoints():
wpts = np.array([[84, 30, 0], # 1
[132, 30, 0],
[180, 30, 0], # 2
[180, 54, 0], # 3
[138, 54, 0], # 4
[138, 111, 0],
[138, 168, 0], # 5
[111, 99, 0], #XXX
[84, 30, 0] # 1
])
navigateThroughWaypoints(wpts)
def compWaypoints_rt():
wpts = np.array([[84, 30, 0], # 1
[180, 30, 0], # 2
[180, 54, 0], # 3
[138, 54, 0], # 4
[138, 168, 0], # 5
[84, 30, 0] # 1
])
navigateThroughWaypoints(wpts)
def navigateThroughWaypoints(wpts):
# myLoc = wpts[0]
canvas = Canvas()
r = robot(wpts, canvas)
startingLoc = r.getStartingLocation()
n_wpts = len(wpts)
for wp in wpts:
canvas.drawWaypoint(wp)
for i in range(n_wpts):
myLoc = r.navigateToWaypoint(wpts[(i + 1 + startingLoc) % n_wpts])
# myLoc = r.navigateToWaypoint_rt(wpts[(i + 1 + startingLoc) % n_wpts])
print("At location: {0}".format(myLoc))
time.sleep(1)
print '\a'
if __name__ == "__main__":
# line(80)
# userInput()
#square(100)
compWaypoints()
# cwWaypoints()
# square(80)
# compWaypoints_rt()