def left():
#motor.drive_l(-30)
#motor.drive_r(100)
motor.drive_l(-15)
motor.drive_r(50)
time.sleep(turningtime_left)
motor.drive(0)
def right():
#motor.drive_l(30)
#motor.drive_r(-100)
motor.drive_l(15)
motor.drive_r(-50)
time.sleep(turningtime_right)
motor.drive(0)
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 driveback(cap):
motor.drive_l(-100)
motor.drive_r(-100)
time.sleep(driveback_time)
safe_right(cap)
motor.drive(0)
def halfright():
motor.drive_l(15)
motor.drive_r(-50)
time.sleep(turningtime_right / 2)
motor.drive(0)
def drivetomarker(cap, id, desired_distance):
#Settings
drivingspeed = 100
tolerance = 2 / 100 #+- in m
x_old = int(720 / 2)
distance = 0
newlylost = True
distancereached = False
distancecorrect = False
while not distancereached:
speedl, speedr, distance, x = vel_to_marker(cap, id, desired_distance,
x_old)
print("distance to arucomarker:" + str(distance))
if x == int(720 / 2): #nothing found
if newlylost == True:
timelost = 0
tic = time.time()
newlylost = False
if timelost < 5000:
toc = time.time()
timelost = (toc - tic) * 1000 #ms
#print("lost since " + str(timelost) + " ms")
if x_old > 720 / 2 + 100: #lost it to the right
if timelost <= 1000:
speedl = 25 #turn right
speedr = 0
#print("search sequence 1")
elif timelost < 3000: # turn the other way
#print("search sequence 2")
speedl = 0
speedr = 25
else:
#print("search sequence3")
speedl = 10
speedr = 0
elif x_old < 720 / 2 - 100: #lost it to the left
if timelost <= 1000:
speedl = 0 #turn left
speedr = 25
elif timelost < 3000: # turn the other way
speedl = 25
speedr = 0
else:
speedl = 10
speedr = 0
elif x_old == int(720 / 2): #beginning
pass
else: #lost in the middle
speedl = -5
speedr = -5
else:
newlylost = True
x_old = x
speedl = speedl * (drivingspeed / 100)
speedr = speedr * (drivingspeed / 100)
motor.drive_l(speedl)
motor.drive_r(speedr)
if distance > desired_distance - tolerance and distance < desired_distance + tolerance and distancecorrect == True:
distancereached = True
elif distance > desired_distance - tolerance and distance < desired_distance + tolerance:
distancecorrect = True
else:
distancecorrect = False
return True