def safe_right(cap):
right()
time.sleep(a_moment)
img = cam.get_calibrated_img(cap)
x, y, distance1, x_aruco = ID.getarucoPosition(img, 1)
if distance1 == 0:
time.sleep(a_moment)
img = cam.get_calibrated_img(cap)
x, y, distance3, x_aruco = ID.getarucoPosition(img, 3)
if distance3 == 0:
halfright()
time.sleep(a_moment)
img = cam.get_calibrated_img(cap)
x, y, distance1, x_aruco = ID.getarucoPosition(img, 1)
if distance1 == 0:
time.sleep(a_moment)
img = cam.get_calibrated_img(cap)
x, y, distance3, x_aruco = ID.getarucoPosition(img, 3)
if distance3 == 0:
left()
img = cam.get_calibrated_img(cap)
x, y, distance1, x_aruco = ID.getarucoPosition(img, 1)
if distance1 == 0:
time.sleep(a_moment)
img = cam.get_calibrated_img(cap)
x, y, distance3, x_aruco = ID.getarucoPosition(img, 3)
if distance3 == 0:
halfright()
def vel_to_marker(cap, id, desired_distance, x_old):
#Settings
on_top = 15 #10
distancetoline_Limit = 0.06 # in m #0.06
img = cam.get_calibrated_img(cap)
x, y, distance, x_aruco = ID.getarucoPosition(img, id)
x = int(x)
y = int(y)
if x > 720:
x = 720
if y > 540:
y = 540
if distance > desired_distance: #normal usage
speedl, speedr = vel_control.followball(x, y, x_old)
elif distance == 0 and x_old != int(720 / 2): #nothing found
speedl, speedr = vel_control.followball(x, y, x_old)
speedl = speedl / 1.5
speedr = speedr / 1.5
else: #too close
speedl = -30
speedr = -30
if x_aruco > distancetoline_Limit and x < 720 - 80 and abs(
distance - desired_distance) > 0.02:
speedr = speedr - on_top
speedl = speedl + on_top
if x_aruco > distancetoline_Limit * 2:
speedr = speedr - on_top / 2
speedl = speedl + on_top / 2
elif x_aruco < -distancetoline_Limit and x > 80:
speedr = speedr + on_top
speedl = speedl - on_top
if x_aruco < -distancetoline_Limit * 2:
speedr = speedr + on_top / 2
speedl = speedl - on_top / 2
#reducing velocity at the last centimeters
#4 cm
if abs(distance - desired_distance) < 0.05:
speedl = speedl * 0.5
speedr = speedr * 0.5
#2cm
if abs(distance - desired_distance) < 0.03:
speedl = speedl * 0.2
speedr = speedr * 0.2
#reduccing velocity at high distance and high turn rate to not loose the marker in the image
if distance > 1 and abs(speedl - speedr) > 20:
speedl = speedl * 0.5
speedr = speedr * 0.5
print("")
print("x-value: " + str(x_aruco))
return speedl, speedr, distance, x
def checkthisbush(cap):
left()
time.sleep(
0.25
) #waiting before capturing the next image as this shows if red berry is there or not
x, area, y = find.firstImage(cam.get_calibrated_img(cap))
if area / 10000 > Size_threshhold: # red berry on this bush
print("RED Berry found!")
if findandpickberry(cap) == True: #pick it
driveback(cap)
return True
else: # green berry on this bush
print('\033[92m' + "green berry" + '\033[0m')
safe_right(cap)
return False
def findandpickberry(cap):
#Settings
drivingspeed = 60 #60 or 85 # in %
pickingspeed = 20000 # speed of the arm
agg = 4 #4 #aggressitivity for picking # the safer the lower
supersavedrivingspeed = 10 # in %
supersave_agg = 0
#testedsetting:
# 80,200,5 works
# 90.2500,3 works
# 100,3000, 15 works but with overshoots
# 90,3500,15 works better than 100
#developersettings
devmode = 0 #always 0, 1 for output of stream and more information
if devmode == 1:
import cv2
drivingspeed = drivingspeed / 4
window_handle = cv2.namedWindow('Calibrated camera',
cv2.WINDOW_AUTOSIZE)
y_old = int(540 - 100)
x_old = int(720 / 2)
reachedberry = False
secondtry = False
#tic = time.time()
while True:
berry_far_away = False
img = cam.get_calibrated_img(cap)
if x_old == int(720 / 2) and y_old == int(540 - 100):
x_old, area, y_old = find.firstImage(img)
x, area, y = find.berry(img, x_old, y_old)
#x,area,y = find.berryAI(img)
#x,area,y = find.berry_old(img)
if area > 0:
berry_far_away = True
area = int(area / 10000)
if area == 0 and berry_far_away == True:
area = 1
berry_far_away = False
if area > 600:
area = 600
if area == 0: #nothing found
speedl = 0 #don't move
speedr = 0
x = int(720 / 2)
y = int(540 - 100)
else:
speedl, speedr = vel_control.followstrawberry(x, area, x_old)
if devmode == 1:
#output image
cv2.circle(img, (x, y), 5, (255, 255, 255), -1)
cv2.putText(img, "centroid", (x - 25, y - 25),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
cv2.imshow('Calibrated camera', img)
print(
"area: ", area, " x: ", x, " Y: ", y,
" | {:1.0f} %% vel left | {:2.0f} %% vel right ".format(
speedl, speedr))
if secondtry == True:
print("second try:safe mode")
if secondtry == False:
speedl = speedl * (drivingspeed / 100)
speedr = speedr * (drivingspeed / 100)
else:
speedl = speedl * (supersavedrivingspeed / 100)
speedr = speedr * (supersavedrivingspeed / 100)
x_old = x
y_old = y
motor.drive_l(speedl)
motor.drive_r(speedr)
if (abs(speedl) <= agg and abs(speedr) <= agg and reachedberry == True
and area != 0
and secondtry == False) or (abs(speedl) == supersave_agg
and abs(speedr) == supersave_agg
and reachedberry == True
and area != 0):
#pick.dummy()
motor.drive(0)
pick.berry(pickingspeed) #safe mode
y_old = int(540 - 100)
x_old = int(720 / 2)
reachedberry = False
#doublecheck
time.sleep(
0.25
) #waitng before capturing the next image as this shows if the berry is still there or not
x_old, area, y_old = find.firstImage(cam.get_calibrated_img(cap))
if area / 10000 > 200: #Berrypicking failed
secondtry = True
# till next picking:
# agg = supersave_agg
# speed = supersavespeed
print("FAILED picking, trying again")
else:
secondtry = False
print('PICKED berry')
return True
#end(cap)
elif (abs(speedl) <= agg and abs(speedr) <= agg and area != 0
and secondtry == False) or (abs(speedl) == supersave_agg
and abs(speedr) == supersave_agg
and area != 0):
#berry.pick() #fast mode
reachedberry = True
else:
reachedberry = False
def camtest():
while True:
tic = time.time()
img = cam.get_calibrated_img(cap)
print("time of that frame in ms: " + str((time.time() - tic) * 1000))