forked from Ridhwanluthra/NLMS
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathbot_movement.py
175 lines (158 loc) · 3.78 KB
/
bot_movement.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
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
from gpiozero import Motor
#from gpiozero.Pin import
#import RPi.GPIO as GPIO
from encoders import d_move, refresh
from ultrasonic import ultra
from anomaly import check
mr = Motor(2, 3)
ml = Motor(14, 15)
ml.stop()
mr.stop()
"""
I am assuming that the initial location of the bot
is facing upwards at 0,0
the bot can make only 90 degree turns
"""
lm = 20
rm = 14.6
direction = 'n' # n,e,w,s for different locations that it is facing
try:
def forward():
ml.forward(0.553)
mr.forward(0.5)
def sstop():
ml.stop()
mr.stop()
def turn_left():
while d_move()[0] < rm and d_move()[1] < rm:
ml.backward(0.555)
mr.forward(0.5)
sstop()
refresh()
def turn_right():
while d_move()[0] < rm and d_move()[1] < rm:
ml.forward(0.555)
mr.backward(0.5)
sstop()
refresh()
def soft_right():
while d_move()[0] < 15:
ml.forward(0.555)
mr.stop()
sstop()
refresh()
def soft_left():
while d_move()[0] < 15:
ml.stop()
mr.forward(0.5)
sstop()
refresh()
def up():
look_up()
if check() == True:
while d_move()[0] <= lm and d_move()[1] <= lm:
forward()
sstop()
refresh()
return True
else:
return False
def left():
look_left()
if check() == True:
while d_move()[0] <= lm and d_move()[1] <= lm:
forward()
sstop()
refresh()
return True
else:
return False
def right():
look_right()
if check() == True
while d_move()[0] <= lm and d_move()[1] <= lm:
forward()
sstop()
refresh()
return True
else:
return False
def down():
look_down()
if check() == True:
while d_move()[0] <= lm and d_move()[1] <= lm:
forward()
sstop()
refresh()
return True
else:
return False
def look_up():
global direction
if (direction == 'n'):
pass
elif (direction == 'e'):
turn_left()
#soft_left()
elif (direction == 'w'):
turn_right()
#soft_right()
elif (direction == 's'):
turn_left()
turn_left()
#soft_left()
#soft_left()
direction = 'n'
def look_down():
global direction
if (direction == 'n'):
turn_left()
turn_left()
#soft_left()
#soft_left()
elif (direction == 'e'):
turn_right()
#soft_right()
elif (direction == 'w'):
turn_left()
#soft_left()
elif (direction == 's'):
pass
direction = 's'
def look_left():
global direction
if (direction == 'n'):
turn_left()
#soft_left()
elif (direction == 'e'):
turn_left()
turn_left()
#soft_left()
#soft_left()
elif (direction == 'w'):
pass
elif (direction == 's'):
turn_right()
#soft_right()
direction = 'w'
def look_right():
global direction
if (direction == 'n'):
turn_right()
#soft_right()
elif (direction == 'e'):
pass
elif (direction == 'w'):
turn_left()
turn_left()
#soft_left()
#soft_left()
elif (direction == 's'):
turn_left()
#soft_left()
direction = 'e'
except KeyboardInterrupt:
print "cleaning"
finally:
#GPIO.cleanup()
print"check it"