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 testleftright():
while True:
left()
motor.drive(0)
time.sleep(0.5)
right()
motor.drive(0)
time.sleep(1)
def orient_in_direction(frame, new_direction):
logging.info("Inside orient_in_direction")
# DONE_TODO: rotate in new direction
retval = mk.marker_solve(frame)
if retval is None:
raise Exception("No qr in orient_in_direction")
_, top, center = retval
orient_deg = hp.taninv(top[0] - center[0], top[1] - center[1])
new_direction = config.ANGLES[hp.get_direction_from_name(new_direction)]
rot_dir, rot_angle = hp.get_rotation_info(orient_deg, new_direction)
logging.debug("Going %s to %s degrees orient_in_direction", orient_deg, new_direction)
motor.drive(rot_dir, rot_angle * config.DELAY_PER_ANGLE)
def orient_on_qr(retval):
# DONE_TODO: reorient itself
#frame = mk.get_frame(cap)
#retval = mk.marker_solve(frame)
if retval is None:
raise Exception("Not standing on qr but orient_on_qr is called")
_, top, center = retval
orientx, orienty = top[0] - center[0], top[1] - center[1]
orient_deg = hp.taninv(orienty, orientx)
#! orient_hor, orient_vert = 1024 - center[0], 768 - center[1]
direction_name, rotation_direction, rot_deg = hp.get_nearest_direction(orient_deg)
logging.debug("Going %s to %s degrees orient_on_qr",
orient_deg, direction_name)
motor.drive(rotation_direction, rot_deg * config.DELAY_PER_ANGLE)
def pick_all_in_line(cap, ID, point):
## Settings
distance_to_next_bush = 0.17
picked_berrys = 0
for x in range(5):
if drivetomarker(cap, ID, point) == True:
motor.drive(0)
#time.sleep(1.5)
if checkthisbush(cap) == True:
picked_berrys = picked_berrys + 1
point = point - distance_to_next_bush
return picked_berrys
def goto_next_marker():
# DONE_TODO: goto new location
global cap
cap.release()
logging.info("Inside goto_next_marker")
#motor.start_fw()
motor.drive("F", 0.5)
retval = None
cap = mk.setup_webcam()
while retval is None:
motor.drive("F", 0.1)
mk.get_frame(cap)
print("Chala", retval)
retval = mk.marker_solve(mk.get_frame(cap))
png = ping.get_distance()
print("PING", retval)
if png < config.MIN_DIST:
while png < config.MIN_DIST:
logging.info("Ping distance < %s", config.MIN_DIST)
cap.release()
sleep(2000)
cap = mk.setup_webcam()
png = ping.get_distance()
return retval
raw=mes.decode('utf-8')
print('mes',mes,'raw',raw)
if raw == 'q':
print('Sub process is terminated')
break
elif raw is not '' : #and self.is_json(raw) ==True:
pwm_str=raw.split(',')
for pwm in map(int,pwm_str):
self.data[cnt]=pwm
cnt+=1
print(self.data)
else:
print('empty message or not json ')
# time.sleep(1)
if __name__ == '__main__' :
motor=motor.Motor([[14,15],[23,24],[8,7],[16,20]])
pwms=[0 for i in range(28)]
# print(pwms,pwms[4::])
led=matrix.LED()
server=SubUdpServer(pwms)
server.setDaemon(True)
server.start()
print('=== Main Thread Starts ===')
while True:
print('motor',pwms[0:4:],'led',pwms[4:13:])
for i in range(4):
motor.drive(i,pwms[i]);
led.upall(pwms[4:13:]);
def start_main():
init()
print("init")
while True:
# 距離が15cm以上だったら1秒間前に進む
print("while")
if sensor.getDistance() >=KYORI:
motor.drive(motor.FORWARD)
print("GO Forward")
sound.CLOCK()
led.blue_blink(0.8)
time.sleep(0.5)
# 距離が5cm以下だったら止まって、方向転換
else: #sensor.getDistance() <= KYORI:
sound.NG()
motor.stop()
led.red_blink(0.8)
# 左を確認
servo.drive_servo(servo.LEFT)
# 距離が6cm以上だったら方向転換
if sensor.getDistance() >= KYORI:
sound.OK()
led.blue_blink(0.8)
motor.drive(motor.TURN_LEFT)
print("TURN LEFT")
led.seq_back(3)
time.sleep(1)
servo.drive_servo(servo.FRONT)
######=================----------------------------------------------------------
else:
sound.NG()
led.red_blink(0.8)
# 右を確認
servo.drive_servo(servo.RIGHT)
# 距離が6cm以上だったら方向転換
if sensor.getDistance() >= KYORI:
sound.OK()
led.blue_blink(0.8)
motor.drive(motor.TURN_RIGHT)
print("TURN RIGHT")
led.seq_forward(3)
time.sleep(1)
servo.drive_servo(servo.FRONT)
# もう進めない場合は後ろに下がって回れ右
else:
sound.NG()
led.red()
print("led.red")
motor.drive(motor.BACKWARD)
print("motor.drive")
time.sleep(2)
servo.drive_servo(servo.FRONT)
# End
if sensor.getDistance() <= KYORI:
sound.END()
motor.end()
led.seq_forward(1.2)
led.seq_back(1.2)
time.sleep(1)
servo.drive_servo(servo.RIGHT)
servo.drive_servo(servo.LEFT)
servo.drive_servo(servo.RIGHT)
servo.drive_servo(servo.FRONT)
sensor.end()
servo.end()
GPIO.cleanup()
else:
sound.OK()
motor.drive(motor.TURN_RIGHT)
time.sleep(1)
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 end(cap):
motor.drive(0)
pick.armdown()
cam.closecam(cap)
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))
#################################################################################################
########################### BEGIN OF PROGRAM ######################################################
#################################################################################################
print("----- Program start -----")
collectedberrys = 0
motor.drive(0)
pick.armdown()
print("opening camera stream")
cap = cam.opencam()
if cap.isOpened():
try:
## MAIN Program is here ##
start_point = ((4 * 17 + 50) / 100) + turning_distance #setpoint
collectedberrys = collectedberrys + pick_all_in_line(
cap, 1, start_point)
if drivetomarker(cap, 1, 0.27 + turning_distance) == True:
left()
if drivetomarker(cap, 2, 0.27 + turning_distance) == True:
left()
start_point = ((4 * 17 + 50) / 100) + turning_distance #setpoint
collectedberrys = collectedberrys + pick_all_in_line(