def calc_distance_right(self):
# trig_right.on()
# sleep(UDS_PULSE_DURATION)
# trig_right.off()
# pulse_start = time()
# while echo_right.is_active == False:
# pulse_start = time()
# pulse_end = time()
# while echo_right.is_active == True:
# pulse_end = time()
# pulse = pulse_end - pulse_start
# return ((SOUND_CONSTANT * pulse) - UDS_RIGHT_OFFSET)
return CERCBot.calc_dist_cm_v2(18, 23)
def calc_distance_middle(self):
# trig_middle.on()
# sleep(UDS_PULSE_DURATION)
# trig_middle.off()
# pulse_start = time()
# while echo_middle.is_active == False:
# pulse_start = time()
# pulse_end = time()
# while echo_middle.is_active == True:
# pulse_end = time()
# pulse = pulse_end - pulse_start
# return ((SOUND_CONSTANT * pulse) - UDS_MIDDLE_OFFSET)
return CERCBot.calc_dist_cm_v2(4, 17)
def calc_distance_left(self):
#Send the pulse 10microsec
# trig_left.on()
# sleep(UDS_PULSE_DURATION)
# trig_left.off()
# pulse_start = time()
# while echo_left.is_active == False:
# pulse_start = time()
# pulse_end = time()
# while echo_left.is_active == True:
# pulse_end = time()
# pulse = pulse_end - pulse_start
# return ((SOUND_CONSTANT * pulse) - UDS_LEFT_OFFSET)
return CERCBot.calc_dist_cm_v2(15, 14)
def go_left(speed):
burt_the_robot.left(speed)
print("Turning Left")
def go_right(speed):
burt_the_robot.right(speed)
print("Turning Right")
try:
while True:
#take UDS readings
left_distance = CERCBot.calc_dist_cm_v2(left_trigger_pin, left_echo_pin)
centre_distance = CERCBot.calc_dist_cm_v2(centre_trigger_pin, centre_echo_pin)
right_distance = CERCBot.calc_dist_cm_v2(right_trigger_pin, right_echo_pin)
#print the results on the screen. Distance is in centimetres
print(left_distance, centre_distance, right_distance)
if left_distance < threshold and centre_distance > threshold and right_distance > threshold:
go_right(fast)
elif left_distance < threshold and centre_distance < threshold and right_distance > threshold:
go_right(fast)
elif left_distance > threshold and centre_distance > threshold and right_distance < threshold:
go_left(fast)
elif left_distance > threshold and centre_distance < threshold and right_distance < threshold:
go_left(fast)
def main_loop():
global colourDone
print("centre")
centre_distance = CERCBot.calc_dist_cm_v2(centre_trigger_pin,
centre_echo_pin)
print("right")
right_distance = CERCBot.calc_dist_cm_v2(right_trigger_pin, right_echo_pin)
print("left")
left_distance = CERCBot.calc_dist_cm_v2(left_trigger_pin, left_echo_pin)
# In the specific order ('red','blue','yellow','green'), go to each zone
#and stop when it is less than 15cm away.
led.off()
print("starting")
for k in range(len(colours)):
direction = "left"
colourDone = False
got_pixel = False
pic_on_left = False
myColour = colours[k]
led.off()
filecnt = 1
done = False
turn_speed = turn_speed_fast
while done == False:
if colourDone == True:
done = True
burt_the_robot.backward(fwd_speed)
sleep(sl)
sleep(sl)
sleep(sl)
sleep(sl)
print('Finished looking for colour')
print('Finding colour', myColour)
sleep_val = sl
burt_the_robot.stop()
# find the colors within the specified boundaries and apply
# the mask
camera.capture(img_dest, format='png')
# Depending on colour, we need different masks
# load the img
img = cv2.imread(img_dest)
#img = img[200:400, 0:400]
img = img[0:x_mod, 0:y_mod]
img = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
img = cv2.GaussianBlur(
img, (11, 11), 100
) # The last value changes the amount of blur, in our case the highest
img_right = img[0:x_mod, 0:round(y_mod / 2) - 1]
img_left = img[0:x_mod, round(y_mod / 2):y_mod]
# Get how many red pixels in the image
maskr_left = cv2.inRange(img_left, lower_red1,
upper_red1) + cv2.inRange(
img_left, lower_red2, upper_red2)
pixr_left_cnt = (maskr_left == 255).sum()
maskr_right = cv2.inRange(img_right, lower_red1,
upper_red1) + cv2.inRange(
img_right, lower_red2, upper_red2)
pixr_right_cnt = (maskr_right == 255).sum()
pixr_cnt = pixr_left_cnt + pixr_right_cnt
# Get how many blue pixels in the image
maskb_left = cv2.inRange(img_left, lower_blue, upper_blue)
pixb_left_cnt = (maskb_left == 255).sum()
maskb_right = cv2.inRange(img_right, lower_blue, upper_blue)
pixb_right_cnt = (maskb_right == 255).sum()
pixb_cnt = pixb_left_cnt + pixb_right_cnt
# Get how many green pixels in the image
maskg_left = cv2.inRange(img_left, lower_green, upper_green)
pixg_left_cnt = (maskg_left == 255).sum()
maskg_right = cv2.inRange(img_right, lower_green, upper_green)
pixg_right_cnt = (maskg_right == 255).sum()
pixg_cnt = pixg_left_cnt + pixg_right_cnt
# Get how many gyello2 pixels in the image
masky_left = cv2.inRange(img_left, lower_yellow, upper_yellow)
pixy_left_cnt = (masky_left == 255).sum()
masky_right = cv2.inRange(img_right, lower_yellow, upper_yellow)
pixy_right_cnt = (masky_right == 255).sum()
pixy_cnt = pixy_left_cnt + pixy_right_cnt
# Now see pixels of which colour are the highest and assign thos evalues for use in the later
# part of the program
if ((pixr_cnt > pixb_cnt) and (pixr_cnt > pixg_cnt) and
(pixr_cnt > pixy_cnt)) and (myColour == 'red'):
print("Found Red Image")
pix_left_cnt = pixr_left_cnt
pix_right_cnt = pixr_right_cnt
pix_cnt = pixr_cnt
elif ((pixb_cnt > pixr_cnt) and (pixb_cnt > pixg_cnt) and
(pixb_cnt > pixy_cnt)) and (myColour == 'blue'):
print("Found Blue image")
pix_left_cnt = pixb_left_cnt
pix_right_cnt = pixb_right_cnt
pix_cnt = pixb_cnt
elif ((pixg_cnt > pixr_cnt) and (pixg_cnt > pixb_cnt) and
(pixg_cnt > pixy_cnt)) and (myColour == 'green'):
print("Found Green Image")
pix_left_cnt = pixg_left_cnt
pix_right_cnt = pixg_right_cnt
pix_cnt = pixg_cnt
elif ((pixy_cnt > pixr_cnt) and (pixy_cnt > pixg_cnt) and
(pixy_cnt > pixb_cnt)) and (myColour == 'yellow'):
print("Found Yellow Image")
pix_left_cnt = pixy_left_cnt
pix_right_cnt = pixy_right_cnt
pix_cnt = pixy_cnt
else:
print("NO IMAGE FOUND")
pix_left_cnt = 0
pix_right_cnt = 0
pix_cnt = 0
#output = cv2.bitwise_and(img, img, mask = mask)
#row,col= mask.shape
#print("row = ",row," col = ",col)
#print ("pix_cnt ",pix_cnt)
#print ("pix_cnt = ",pix_cnt, " red = ",red, "green = ",green,"blue = ",blue)
r = CERCBot.calc_dist_cm_v2(right_trigger_pin, right_echo_pin)
l = CERCBot.calc_dist_cm_v2(left_trigger_pin, left_echo_pin)
m = CERCBot.calc_dist_cm_v2(centre_trigger_pin, centre_echo_pin)
print(' l= ', l, " m = ", m, " r = ", r, " pix_cnt = ", pix_cnt,
" threshold = ", threshhold)
## If colour is greater than threshold (i.e. it is looking at it clearly
## and any of the distabnce is too less, i.e. it is either in the corner
## or head on to the colour that means it is done for that colour
if colourDone == False:
if (pix_cnt > threshhold):
got_pixel = True
turn_speed = turn_speed_default
if pix_left_cnt > pix_right_cnt:
pic_on_left = True
else:
pic_on_left = False
print("pix_left_cnt = ", pix_left_cnt, " pix_right_cnt = ",
pix_right_cnt)
print("Going forward")
burt_the_robot.forward(fwd_speed)
else:
if (got_pixel): # that means turn towards the picture
print("got_pixel = ", got_pixel, " pic_on_left = ",
pic_on_left)
if (pic_on_left) and (l >= dis):
direction = "left"
else:
if (r >= dis):
direction = "right"
else:
direction = "backward"
else:
if l < dis and r < dis:
direction = "backward"
else:
if l < dis and direction == "left":
direction = "right"
if r < dis and direction == "right":
direction = "left"
if m < dis and direction == "forward":
direction = "backward"
if direction == "backward":
if l > dis:
direction = "left"
elif r > dis:
direction = "right"
elif m > dis:
direction = "forward"
else:
direction = "backward"
got_pixel = False
print("Changing direction to ", direction)
if direction == "left":
burt_the_robot.left(turn_speed)
elif direction == "right":
burt_the_robot.right(turn_speed)
elif direction == "forward":
burt_the_robot.forward(fwd_speed)
else:
burt_the_robot.backward(fwd_speed)
sleep(sleep_val)
image_tg = "/home/pi/Pictures/" + str(myColour) + "-" + str(
filecnt) + "-" + str(pix_cnt) + "-" + str(l) + "-" + str(
m) + "-" + str(r) + "-" + ".png"
filecnt = filecnt + 1
call(["cp", img_dest, image_tg])
# Remove pictures afterwards.
# show the imgs
#cv2.imshow("img", np.hstack([img, output]))
#cv2.waitKey(0)
##
burt_the_robot.stop()
def calc_distance(self):
self.left_distance = CERCBot.calc_dist_cm_v2(15, 14)
self.middle_distance = CERCBot.calc_dist_cm_v2(4, 17)
self.right_distance = CERCBot.calc_dist_cm_v2(18, 23)
return 0
def main_loop():
global colourDone
print("centre")
centre_distance = CERCBot.calc_dist_cm_v2(centre_trigger_pin, centre_echo_pin)
print("right")
right_distance = CERCBot.calc_dist_cm_v2(right_trigger_pin, right_echo_pin)
print("left")
left_distance = CERCBot.calc_dist_cm_v2(left_trigger_pin, left_echo_pin)
# In the specific order ('red','blue','yellow','green'), go to each zone
#and stop when it is less than 15cm away.
led.off()
print("starting")
for k in range(len(colours)):
direction = "left"
colourDone = False
got_pixel = False
pic_on_left = False
myColour = colours[k]
led.off()
filecnt = 1
done = False
while done = False:
if colourDone == True:
done = True
burt_the_robot.backward(speed)
sleep(speed)
sleep(speed)
print('Finished looking for colour')
print('Finding colour',myColour)
sleep_val = sl
speed_val = speed
burt_the_robot.stop()
# find the colors within the specified boundaries and apply
# the mask
camera.capture(img_dest,format='png')
# Depending on colour, we need different masks
# load the img
img = cv2.imread(img_dest)
#img = img[200:400, 0:400]
img = img[0:x_mod, 0:y_mod]
img = cv2.cvtColor(img,cv2.COLOR_BGR2HSV)
img = cv2.GaussianBlur(img,(11,11),100) # The last value changes the amount of blur, in our case the highest
img_right = img[0:x_mod,0:round(y_mod/2)-1]
img_left = img[0:x_mod,round(y_mod/2):y_mod]
if myColour == 'red':
mask_left = cv2.inRange(img_left, lower_red, upper_red)
mask_right = cv2.inRange(img_right, lower_red, upper_red)
elif myColour == 'blue':
mask_left = cv2.inRange(img_left, lower_blue, upper_blue)
mask_right = cv2.inRange(img_right, lower_blue, upper_blue)
elif myColour == 'green':
mask_left = cv2.inRange(img_left, lower_green, upper_green)
mask_right = cv2.inRange(img_right, lower_green, upper_green)
else:
mask_left = cv2.inRange(img_left, lower_yellow, upper_yellow)
mask_right = cv2.inRange(img_right, lower_yellow, upper_yellow)
pix_left_cnt = (mask_left==255).sum()
pix_right_cnt = (mask_right==255).sum()
pix_cnt = pix_left_cnt + pix_right_cnt
#output = cv2.bitwise_and(img, img, mask = mask)
#row,col= mask.shape
#print("row = ",row," col = ",col)
#print ("pix_cnt ",pix_cnt)
#print ("pix_cnt = ",pix_cnt, " red = ",red, "green = ",green,"blue = ",blue)
r = CERCBot.calc_dist_cm_v2(right_trigger_pin, right_echo_pin)
l = CERCBot.calc_dist_cm_v2(left_trigger_pin,left_echo_pin)
m = CERCBot.calc_dist_cm_v2(centre_trigger_pin, centre_echo_pin)
print(' l= ',l," m = ",m," r = ",r," pix_cnt = ",pix_cnt," threshold = ",threshhold)
## If colour is greater than threshold (i.e. it is looking at it clearly
## and any of the distabnce is too less, i.e. it is either in the corner
## or head on to the colour that means it is done for that colour
if colourDone == False:
if (pix_cnt > threshhold):
got_pixel = True
if pix_left_cnt > pix_right_cnt:
pic_on_left = True
else:
pic_on_left = False
print("pix_left_cnt = ",pix_left_cnt," pix_right_cnt = ",pix_right_cnt)
print("Going forward")
burt_the_robot.forward(0.4)
else:
if (got_pixel): # that means turn towards the picture
print("got_pixel = ",got_pixel," pic_on_left = ",pic_on_left)
if (pic_on_left) and (l >= dis):
direction= "left"
else:
if (r >= dis ):
direction = "right"
else:
direction = "backward"
else:
if l < dis and r < dis:
direction = "backward"
elif l < dis and direction == "left":
direction = "right"
elif r < dis and direction == "right":
direction = "left"
elif m < dis and direction == "forward":
direction = "backward"
else:
direction = "left"
got_pixel = False
print("Changing direction to ",direction)
if direction == "left":
burt_the_robot.left(speed_val)
elif direction == "right":
burt_the_robot.right(speed_val)
elif direction == "forward":
burt_the_robot.forward(speed_val)
else:
burt_the_robot.backward(speed_val)
sleep(sleep_val)
image_tg= "/home/pi/Pictures/" + str(myColour) + "-" + str(filecnt) + "-" + str(pix_cnt) + "-" + str(l) + "-" + str(m) + "-" + str(r) + "-" + ".png"
filecnt = filecnt + 1
call(["cp", img_dest, image_tg])
def go_left(speed):
burt_the_robot.left(speed)
print("Turning Left")
def go_right(speed):
burt_the_robot.right(speed)
print("Turning Right")
try:
while True:
#take UDS readings
left_distance = CERCBot.calc_dist_cm_v2(
left_trigger_pin,
left_echo_pin) # updeted to "v2" as per Rishan/Alok's change
centre_distance = CERCBot.calc_dist_cm_v2(centre_trigger_pin,
centre_echo_pin)
right_distance = CERCBot.calc_dist_cm_v2(right_trigger_pin,
right_echo_pin)
#print the results on the screen. Distance is in centimetres
print(left_distance, centre_distance, right_distance)
#look for left turns if way ahead is available
if left_distance > threshold and centre_distance > threshold and right_distance < threshold:
go_left(fast)
elif left_distance > threshold and centre_distance < threshold and right_distance < threshold:
go_left(fast)