def check_front():
dist = distance()
if dist < 12:
print("Too close, ", dist)
reverse(2)
dist = distance()
if dist < 12:
print("Too close, ", dist)
pivot_left(2)
if dist < 12:
print("Too close, giving up", dist)
sys.exit()
def check_front():
init()
dist = distance()
while dist < 15:
print('Obstacle too close at ', dist)
init()
pivot_right(.25)
dist = distance()
if dist < 15:
pivot_right(.25)
print('Obstacle too close at ', dist)
sys.exit()
def find_nearest_visible_item(self, items):
minimum_distance = 10000
nearest_item_index = -1
view = visible_agents_items()
for i in range(0, len(items)):
if not items[i].loaded and item[i] in view:
item = items[i]
(xI, yI) = item.position.get_position()
if sensor.distance(self, item) < minimum_distance:
minimum_distance = sensor.distance(self, item)
nearest_item_index = i
return nearest_item_index
def key_input(event):
init()
print "Key:", event.char
key_press = event.char
sleep_time = 0.060
if key_press.lower() == "w":
forward(sleep_time)
elif key_press.lower() == "s":
reverse(sleep_time)
elif key_press.lower() == "a":
turn_left(sleep_time)
elif key_press.lower() == "d":
turn_right(sleep_time)
elif key_press.lower() == "p":
stop(sleep_time)
else:
pass
curDis = distance("cm") #diese Zeilen brauchen "import distance". diese bis zum naechsten Kommentar inklusiv
print("Distance:", curDis)
if curDis <15:
init()
reverse(0.5) #bis hier sollst du kommentieren
def key_input(event):
init()
print("Key:", event.char)
key_press = event.char
sleep_time = 0.03
if key_press.lower() == 'w':
forward(sleep_time)
elif key_press.lower() == 's':
reverse(sleep_time)
elif key_press.lower() == 'a':
turn_left(sleep_time)
elif key_press.lower() == 'd':
turn_right(sleep_time)
elif key_press.lower() == 'q':
pivot_left(sleep_time)
elif key_press.lower() == 'e':
pivot_right(sleep_time)
else:
pass
cur = distance('cm')
print("Current disatnce is:", cur)
if cur < 20:
init()
reverse(2)
def key_input(event):
init()
print 'Key:', event.char
key_press = event.char
sleep_time = 0.1
if key_press.lower() == 'w' or key_press.upper() =='W':
forward(sleep_time)
elif key_press.lower() == 's' or key_press.upper() =='S':
reverse(sleep_time)
elif key_press.lower() == 'a' or key_press.upper() =='A':
turn_left(sleep_time)
elif key_press.lower() == 'd' or key_press.upper() =='D':
turn_right(sleep_time)
elif key_press.lower() == 'q' or key_press.upper() =='Q':
pivot_left(sleep_time)
elif key_press.lower() == 'e' or key_press.upper() =='E':
pivot_right(sleep_time)
else:
pass
curDis = distance('cm') #current distance of object from sensor
if curDis < 25:
init()
reverse(2)
def key_input(event):
init()
print "Key:", event.char
key_press = event.char
sleep_time = 0.060
if key_press.lower() == "w":
forward(sleep_time)
elif key_press.lower() == "s":
reverse(sleep_time)
elif key_press.lower() == "a":
turn_left(sleep_time)
elif key_press.lower() == "d":
turn_right(sleep_time)
elif key_press.lower() == "p":
stop(sleep_time)
else:
pass
curDis = distance("cm")
print("Distance:", curDis)
if curDis <15:
init()
reverse(0.5)
def key_input(event):
init()
print 'Key:', event.char
key_press = event.char
sleep_time = 0.030
if key_press.lower() == 'w':
forward(sleep_time)
elif key_press.lower() == 's':
reverse(sleep_time)
elif key_press.lower() == 'a':
turn_left(sleep_time)
elif key_press.lower() == 'd':
turn_right(sleep_time)
elif key_press.lower() == 'q':
pivot_left(sleep_time)
elif key_press.lower() == 'e':
pivot_right(sleep_time)
else:
pass
curDis = distance('cm')
print('curdis is', curDis)
if curDis < 20:
init()
reverse(2)
def key_input(event):
init()
print 'KEY: ', event.char
key_press = event.char
sleep_time = 0.050
if key_press.lower() == 'w':
forward(sleep_time)
elif key_press.lower() == 's':
reverse(sleep_time)
elif key_press.lower() == 'd':
turn_right(sleep_time)
elif key_press.lower() == 'a':
turn_left(sleep_time)
elif key_press.lower() == 'q':
pivot_left(sleep_time)
elif key_press.lower() == 'e':
pivot_right(sleep_time)
else:
pass
CurrentDis = distance('cm')
print('Current Distance is: ', CurrentDis)
if CurrentDis < 20:
init()
reverse(1)
def check_front():
init()
dist = distance()
if dist < 15:
print('Too close,', dist)
init()
pivot_right(2)
dist = distance()
if dist < 15:
print('Too close,', dist)
init()
pivot_left(3)
init()
reverse(2)
dist = distance()
if dist < 15:
print('Too close, giving up', dist)
sys.exit()
def check_front():
init()
dist = distance()
if dist < 15:
print("Too Close", dist)
init()
reverse(2)
dist = distance()
if dist < 15:
print("Too close", dist)
init()
pivot_left(3)
init()
reverse(2)
dist = distance()
if dist < 15:
print("Too close", dist)
sys.exit()
def check_front():
init()
dist = distance()
if dist < 40:
print('Too close,need to move back!!', dist)
init()
reverse(2)
init()
turn_right(2)
dist = distance()
if dist < 40:
print('Too Close,Reassessing Path', dist)
init()
turn_left(2)
init()
reverse(2)
dist = distance()
if dist < 40:
print('Too Close, giving up', dist)
sys.exit()
def check_front():
# initialise gpio pins and check first distance
init()
dist = distance()
# check if distance is below 30cm, if so change direction
if dist < 30:
print("Too close, distance: ", dist)
init()
backward(1)
init()
pivotRight(0.5)
# check_back()
dist = distance()
# check new distance and again, if below 30cm change direction again
if dist < 30:
print("Too close, distance: ", dist)
init()
backward(1)
init()
pivotLeft(0.5)
dist = distance()
# check new distance and again, if below 30cm change direction again
if dist < 30:
print("Too close, distance: ", dist)
init()
backward(1)
init()
pivotLeft(2)
dist = distance()
# check new distance and again, if below 30cm give up
if dist < 30:
print("Too close, giving up with dist: ", dist)
sys.exit()
def check_front():
init()
dist = distance()
if dist < 15:
print('Too close,', dist)
init()
backwards(0.4)
init()
left_pivot(1)
init()
dist = distance()
if dist < 15:
print('Too close again,', dist)
init()
left_pivot(2)
init()
backwards(0.4)
init()
left_pivot(1)
dist = distance()
if dist < 15:
print('Too close, giviing up,', dist)
sys.exit()
def takeNscans(n, t):
if (n <= 0 or t <= 0):
print(
"Invalid scan number or scan duration provided. See takeNscans function."
)
sys.exit()
# Create list for distances to be measured
distances = []
# do a scan of n measurements towards left
for x in range(n):
ct.init()
ct.pivotLeft(t)
time.sleep(0.5)
dist = myround(int(distance() + 0.5))
distances.append(dist)
return distances
def scan(turnDur, turns):
distances = np.zeros([turns, turnDur])
for y in range(0, turns, 1):
distanceZ = []
for z in range(turnDur):
ct.init()
ct.pivotLeft(0.075)
time.sleep(0.5)
dist = round(distance(), 2)
distanceZ.append(dist)
distances[y] = distanceZ
print("Round " + str(y + 1) + " of " + str(turns) + " done.")
# mean over all turns
distances = np.mean(distances, axis=0)
return distances
0.7, 2) # Get font size
label_ymin = max(
ymin, labelSize[1] +
10) # Make sure not to draw label too close to top of window
cv2.rectangle(frame, (xmin, label_ymin - labelSize[1] - 10),
(xmin + labelSize[0], label_ymin + baseLine - 10),
(255, 255, 255),
cv2.FILLED) # Draw white box to put label text in
cv2.putText(frame, label, (xmin, label_ymin - 7),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 0),
2) # Draw label text
# Factor in motor control
# I want the car to make moves with regards to: a cup, a remote, and a banana
# You can pick different items (check the label map)
if object_name == "cup" and distance() < 15.0:
print("pivoting left")
pivot_left(1) #I decided to go with a smaller tf
elif object_name == "remote" and distance() < 15.0:
print("pivoting right")
pivot_right(1) #decided to go with a smaller tf
elif object_name == "stop sign" and distance() < 15.0:
print("stopping")
stop(1)
else:
print("moving forward!")
forward(1)
def obstacle():
def init():
gpio.setmode(gpio.BOARD)
gpio.setup(7, gpio.OUT)
gpio.setup(11, gpio.OUT)
gpio.setup(13, gpio.OUT)
gpio.setup(15, gpio.OUT)
def forward(tf):
gpio.output(7, False)
gpio.output(11, True)
gpio.output(13, False)
gpio.output(15, True)
time.sleep(tf)
gpio.cleanup()
def reverse(tf):
gpio.output(7, True)
gpio.output(11, False)
gpio.output(13, True)
gpio.output(15, False)
time.sleep(tf)
gpio.cleanup()
def turn_left(tf):
gpio.output(7, True)
gpio.output(11, True)
gpio.output(13, False)
gpio.output(15, True)
time.sleep(tf)
gpio.cleanup()
def turn_right(tf):
gpio.output(7, True)
gpio.output(11, False)
gpio.output(13, False)
gpio.output(15, False)
time.sleep(tf)
gpio.cleanup()
def pivot_left(tf):
gpio.output(7, True)
gpio.output(11, False)
gpio.output(13, False)
gpio.output(15, True)
time.sleep(tf)
gpio.cleanup()
def pivot_right(tf):
gpio.output(7, False)
gpio.output(11, True)
gpio.output(13, True)
gpio.output(15, False)
time.sleep(tf)
gpio.cleanup()
curDis = distance('cm')
print('curdis is', curDis)
if curDis < 20:
init()
reverse(2)
def check_front():
init()
dist = distance()
if dist < 15:
print('Too close,', dist)
init()
pivot_right(2)
dist = distance()
if dist < 15:
print('Too close,', dist)
init()
pivot_left(3)
init()
reverse(2)
dist = distance()
if dist < 15:
print('Too close, giving up', dist)
sys.exit()
def autonomy():
tf = 0.100
x = random.randrange(0, 5)
if x == 0:
for y in range(100):
check_front()
init()
forward(tf)
if x == 1:
for y in range(30):
check_front()
init()
pivot_left(tf)
if x == 2:
for y in range(30):
check_front()
init()
turn_right(tf)
if x == 3:
for y in range(30):
check_front()
init()
turn_left(tf)
if x == 4:
for y in range(30):
check_front()
init()
pivot_right(tf)
for z in range(100):
autonomy()
from time import sleep
leituras = []
move = Movements()
while True:
dados = raw_input(
"Informe as coordenadas x, y, theta ou digite 'fim' para encerrar: ")
print dados
if dados == 'fim':
np.save('leituras.npy', leituras)
print leituras
sys.exit()
try:
# x, y, theta = dados.split(', ')
# print x, y, theta
x, y, theta = 0, 0, 0
for i in range(8):
ang = 20 * i
#print 'a'
move.setAngle(8, ang)
#print 'b'
for j in range(5):
valor = sensor.distance()
print valor
leituras.append((int(x), int(y), int(theta), ang, valor))
sleep(0.05)
except:
print "Dados invalidos"
def lookToRight():
tmpDist = sensor.distance()
print("Distance: " + str(tmpDist) + " Look To Right Now")
if tmpDist <= 20:
movement.pivot_right(0.7)
return tmpDist
def lookToLeft():
tmpDist = sensor.distance()
print("Distance: "+ str(tmpDist) + " Look To Left Now")
if tmpDist <= 20:
movement.pivot_left(0.4)
return tmpDist
def distCheck():
tmpDist = sensor.distance()
if tmpDist <= 20:
print("Terminated by force")
return tmpDist
#coding for moving gun left goes here
gun_left()
elif (parseData[0] == "MoveGunToMiddle"):
reply = "gunMiddle"
conn.sendall(reply.encode())
conn.close()
#coding for moving gun middle goes here
gun_mid()
elif (parseData[0] == "MoveGunToRight"):
reply = "gunRight"
conn.sendall(reply.encode())
conn.close()
#coding for moving gun right goes here
gun_right()
else:
pass
curDis = distance('cm')
print('curdis is', curDis)
if curDis < 4:
reversecar(0.5)
