def calculate_contour_2(receive_contour_queue, send_motor_queue, p_start_lock,
p_end_lock):
global run_flag
global currentDutyCycleT
global p0, p1, p2, p3
x_diff = 0
y_diff = 0
area_diff = 0
last_areaDiff = 0
area = 0
last_xdiff = 0
last_ydiff = 0
start_time = 0
waiting_threshold = 10
count = 0
Period = 200
#print("I am here1")
tilt_lst = [0, 0]
global controller
controller = motor_controller()
while (run_flag.value):
try:
if ((not receive_contour_queue.empty())):
#last_contour_receive_time = time.time()
contours = receive_contour_queue.get() # Extract contour
# print("Main Processor 2: Get the processed contour from queue\n")
# print("2 receive_contour_queue ", receive_contour_queue.qsize())
Area_list = []
if 0 == len(contours):
# print("Calibration --- finding balloon\n\n\n\n\n")
# controller.set_control( 0, 0)
controller.set_control(0, 10)
if count < Period / 2:
tilt_lst[0] = 1
tilt_lst[1] = 1.0
if count >= Period / 2 and count < Period:
tilt_lst[0] = -1
tilt_lst[1] = 1.0
if count == Period:
count = 0
count += 1
time.sleep(0.02)
# print currentDutyCycleT
if (send_motor_queue.qsize() < 2):
send_motor_queue.put(tilt_lst) # put mask in queue
#time.sleep(waiting_threshold/1000.)
else:
run_flag.value = 0
controller.clean()
if run_flag.value == 0:
p0.terminate()
p1.terminate()
p2.terminate()
p3.terminate()
# print("processor 2 time ", time.time() - run_time)
# print("\n\n\n\n")
else:
#print("Processor 2 Didn't Receive Frame, sleep for 10ms")
time.sleep(0.01)
except KeyboardInterrupt:
run_flag.value = 0
controller.clean()
pi_hw.stop()
print(run_flag.value)
print("Quiting Processor 2")
pygame.init()
clock = pygame.time.Clock()
ot = time.time()
running = True
fow_speed = 0
turn_speed = 0
skew = 0
connection_counter = 0
speed_f = speed_filter.speed_filter()
motor_ctrl = motor_controller.motor_controller()
print "start"
while running:
zmq.synch()
man_ctrl = zmq.recv("man_ctrl")
connection_counter += 1
if len(man_ctrl) == 3:
connection_counter = 0
fow_speed = float(man_ctrl[0])
turn_speed = float(man_ctrl[1])
def calculate_contour_2(receive_contour_queue, send_motor_queue, p_start_lock,
p_end_lock):
global count2
global tilt_time
global currentDutyCycleT
global run_flag
# global p0,p1,p2,p3
x_diff = 0
y_diff = 0
area_diff = 0
last_areaDiff = 0
area = 0
last_xdiff = 0
last_ydiff = 0
start_time = 0
waiting_threshold = 20
count_no_contour = 0
#print("I am here1")
tilt_lst = [0, 0]
count = 0
Period = 200
controller = motor_controller()
while (run_flag.value):
try:
if ((not receive_contour_queue.empty())):
#last_contour_receive_time = time.time()
contours = receive_contour_queue.get() # Extract contour
# print("Main Processor 2: Get the processed contour from queue\n")
# print("2 receive_contour_queue ", receive_contour_queue.qsize())
count2 += 1
#print("count2 ", count2)
Area_list = []
if 0 == len(contours):
# print("Calibration --- finding balloon\n\n\n\n\n")
# controller.set_control( 0, 0)
controller.set_control(0, 10)
if count < Period / 2:
tilt_lst[0] = 1
tilt_lst[1] = 1.0
if count >= Period / 2 and count < Period:
tilt_lst[0] = -1
tilt_lst[1] = 1.0
if count == Period:
count = 0
count += 1
time.sleep(0.01)
# print currentDutyCycleT
if (send_motor_queue.qsize() < 2):
send_motor_queue.put(tilt_lst)
if count_no_contour > 150:
run_flag.value = 0
controller.clean()
if run_flag.value == 0:
p0.terminate()
p1.terminate()
p2.terminate()
p3.terminate()
# yeh_David
time.sleep(waiting_threshold / 1000.)
count_no_contour += 1
else:
count_no_contour = 0
run_time = time.time()
Area_list = [cv2.contourArea(c) for c in contours]
##print (Area_list)
maxindex = Area_list.index(max(Area_list))
##print maxindex
cnt = contours[maxindex] #Get the first contour
##print (contours[0])
M = cv2.moments(cnt) #Calculat M of the first contour
# #print (M)
#calcualte the center coordinate
if M['m00'] != 0:
cx = int(M['m10'] / M['m00'])
cy = int(M['m01'] / M['m00'])
area = M['m00']
#PID control Algo to calculate strength to control servo
x_diff = cx - center_x
y_diff = abs(cy - center_y)
area_diff = area_ref - area
# #print("x_bar=%f, y_bar= %f" % (cx,cy))
# #print("x_diff= %f, y_diff= %f" % (x_diff,y_diff))
# #print("area = %f" % area)
#### PID Parameters ####
kp_x = 1.0
kd_x = 0.001
### ZH PID
# Ku = 3
# kp_x = Ku*0.6
# Tu = 3
# Td = Tu/8
# kd_x = kp_x*Td
kp_z = 6
kd_z = 0
kp_y = 6
kd_y = 0.001
proportional_x = x_diff
proportional_y = y_diff / (y_res / 2.0)
proportional_z = area_diff
derivative_x = (last_xdiff - x_diff) / (time.time() -
start_time)
derivative_y = (last_ydiff - y_diff) / (time.time() -
start_time)
derivative_z = (last_areaDiff -
area_diff) / (time.time() - start_time)
start_time = time.time()
##print("derivative_x: " + str(derivative_x))
##print("derivative_x*kd_x: " + str(derivative_x*kd_x))
#start_time = time.time()
strength_x = proportional_x * kp_x + derivative_x * kd_x
strength_z = proportional_z * kp_z + derivative_z * kd_z
strength_y = proportional_y * kp_y - derivative_y * kd_y
##print "strength:"
##print strength_x
#Assume the left and top corner is (0,0)
if ((abs(area_diff) <= area_tlr
or currentDutyCycleT == 65000)
and abs(x_diff) <= x_tlr):
c = 1
controller.set_control(0, 0)
else:
controller.set_control(
strength_z * 0.005, strength_x *
0.05) #strength_z*0.005, strength_x*0.05
# print("area didn't meet the reference")
if (y_diff <= y_tlr):
# do nothing within tolerance range
b = 1
elif (cy > center_y):
tilt_lst[0] = 1
tilt_lst[1] = strength_y
##print('the camera need to raise its head up\n')
else:
tilt_lst[0] = -1
tilt_lst[1] = strength_y
##print('the camera need to low its head down\n')
length_time = time.time() - tilt_time
tilt_time = time.time()
last_area = area
last_xdiff = x_diff
last_ydiff = y_diff
last_areaDiff = area_diff
# #print(tilt_lst)
# print(send_motor_queue.qsize())
#print(tilt_lst)
if (send_motor_queue.qsize() < 2):
send_motor_queue.put(tilt_lst) # put mask in queue
#print("send_motor_queue", send_motor_queue.qsize())
#print("tilt time", length_time)
# print("processor 2 time ", time.time() - run_time)#print the running time of processor 2
else:
#print("Processor 2 Didn't Receive Frame, sleep for 10ms")
time.sleep(0.01)
except KeyboardInterrupt:
run_flag.value = 0
controller.clean()
pi_hw.stop()
def recognize_balloon(run_flag, send_frame_queue, receive_contour_queue,
p_start_turn, p_end_turn, p_start_lock, p_end_lock):
#global cx # Reference for motors angular velocity
#global area # Reference for motors velocity
last_contour_receive_time = 0
start_time = 0
start_queue = datetime.now()
count_put = 0
count_print_x = 0
global count1
global count2
global count3
global tilt_time
time_total_run = time.time()
x_diff = 0
y_diff = 0
area_diff = 0
area = 0
last_areaDiff = 0
last_xdiff = 0
last_ydiff = 0
waiting_threshold = 10
calibration = False
controller = motor_controller()
while (run_flag.value):
try:
#1. get a frame and show ret,
_, frame = cap.read()
# height 480; width 640; channel 3
#cv2.imshow('Capture', frame)
#2. change to hsv model
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
#3. Threshold the HSV image to get only red colors and get mask
mask = cv2.inRange(hsv, lower_red, upper_red) #lower than
# print(mask)
start_time_count = time.time()
#cv2.imshow('Mask_first', mask)
print('count1 %d, count2 %d, count3 %d' %
(count1, count2, count3))
# #find the contours
# contours,hierarchy = cv2.findContours(mask, 1, 2)
cur_time = datetime.now()
delta_time = cur_time - start_queue
delta_time_tol_ms = delta_time.total_seconds() * 1000
if (calibration):
print("Calibration the camera")
else:
#only put the mask in queue if it has past 10ms and there are less than 4 masks in queue
if (delta_time_tol_ms >
waiting_threshold) and (send_frame_queue.qsize() < 4):
start_queue = cur_time # Update last send to queue time
send_frame_queue.put(mask) # put mask in queue
count_put += 1
#print("put the mask to queue\n")
#check if receive_contour_queue is not qmpty
if ((not receive_contour_queue.empty())):
last_contour_receive_time = time.time()
contours = receive_contour_queue.get() # Extract contour
#print("Main Processor: Get the processed contour from queue\n")
Area_list = []
if 0 == len(contours):
time.sleep(waiting_threshold / 1000.)
else:
Area_list = [cv2.contourArea(c) for c in contours]
#print (Area_list)
maxindex = Area_list.index(max(Area_list))
#print maxindex
cnt = contours[maxindex] #Get the first contour
#print (contours[0])
M = cv2.moments(cnt) #Calculat M of the first contour
# print (M)
#calcualte the center coordinate
if M['m00'] != 0:
cx = int(M['m10'] /
M['m00']) # columns for angular velocity
cy = int(M['m01'] / M['m00']) # rows
area = M['m00'] # area for velocity
#PID control Algo to calculate strength to control servo
#x_diff = abs(cx - center_x) # should it be absolute? it can be negative as well
x_diff = center_x - cx
y_diff = abs(cy - center_y)
area_diff = area_ref - area
#print("x_bar=%f, y_bar= %f" % (cx,cy))
#print("x_diff= %f, y_diff= %f" % (x_diff,y_diff))
#print("area = %f" % area)
count_print_x += 1
kp_x = 3
kd_x = 0
# ZH PID
Ku = 3
kp_x = Ku * 0.6
Tu = 2.2
Td = Tu / 8
kd_x = kp_x * Td
kp_x_right = 3
kd_x_right = 0.005
#kp_y = 8
#kd_y = 0.00005
# ZH PID
Ku_y = 9
kp_y = Ku_y * 0.6
Tu_y = 2.5
Td_y = Tu_y / 8
kd_y = kp_y * Td_y * 0.1
kp_area = 6
kd_area = 0
proportional_x = x_diff
proportional_area = area_diff
#proportional_x_left = x_diff/(x_res/2.0)
#proportional_x_right = x_diff/(x_res/2.0)
proportional_y = y_diff #/(y_res/2.0)
derivative_x = (last_xdiff - x_diff) / (
time.time() - start_time)
derivative_area = (last_areaDiff - area_diff) / (
time.time() - start_time)
derivative_y = (last_xdiff - y_diff) / (
time.time() - start_time)
#derivative_z = (last_area - area)/(time.time() - start_time)
start_time = time.time()
#print("derivative_x: " + str(derivative_x))
#print("derivative_x*kd_x: " + str(derivative_x*kd_x))
strength_x = proportional_x * kp_x - derivative_x * kd_x
strength_area = proportional_area * kp_area - derivative_area * kd_area
#strength_x_left = proportional_x_left*kp_x_left - derivative_x*kd_x_left
#strength_x_right = proportional_x_right*kp_x_right - derivative_x*kd_x_right
strength_y = proportional_y * kp_y + derivative_y * kd_y
#print "strength:"
#print strength_x
#Assume the left and top corner is (0,0)
if (abs(x_diff) <= x_tlr):
a = 1
controller.set_control(0, 0)
#do nothing within tolerance range
else:
controller.set_control(0., strength_x * 0.05)
print(
'the car need to turn left and mvoe forward\n')
if (abs(area_diff) <= area_tlr):
c = 1
controller.set_control(0, 0)
else:
controller.set_control(strength_area * 0.005, 0.)
print("area didn't meet the reference")
if (y_diff <= y_tlr):
# do nothing within tolerance range
b = 1
elif (cy > center_y):
rotate_camera(1, strength_y * 0.01)
#print('the camera need to raise its head up\n')
else:
rotate_camera(-1, strength_y * 0.01)
#print('the camera need to low its head down\n')
length_time = time.time() - tilt_time
tilt_time = time.time()
#last_area = area
last_xdiff = x_diff
last_ydiff = y_diff
last_areaDiff = area_diff
print("tilt time", length_time)
#if ((time.time()-last_contour_receive_time) < waiting_threshold/1000.):
#cv2.circle(frame, (cx, cy), 7, (255, 255, 255), -1) #Draw center of object
#cv2.drawContours(frame,contours,-1,(255,0,0),3) #Draw contour of object
#cv2.circle(frame, (center_x, center_y), 2, (0, 0, 255), -1) #Draw center of camera
#cv2.imshow('frame',frame) #Display Frame
cal_time = time.time() - start_time_count
print("Running_time = ", cal_time)
if cv2.waitKey(1) & 0xFF == ord('q'):
run_flag.value = 0
time_total_run = time.time() - time_total_run
print('count_put %d' % count_put)
print('count_print_x %d' % count_print_x)
print('Total running time', time_total_run)
print('count1 %d, count2%d, count3%d' %
(count1, count2, count3))
except KeyboardInterrupt:
run_flag.value = 0
time_total_run = time.time() - time_total_run
print('count_put %d' % count_put)
print('count_print_x %d' % count_print_x)
print('Total running time', time_total_run)
print('count1 %d, count2%d, count3%d' % (count1, count2, count3))
print("Quit Processor 0")
##########################################################
### Yanling Wu (yw996), Yeh Dawei(ty359) ##
### This is for celebration mode and the car ##
### will turn around and move forward and backward ##
##########################################################
import time
from motor_controller import motor_controller
controller = motor_controller()
value = 1
start_time = time.time()
while 10 > (time.time() - start_time):
if value == 1:
controller.set_control(0., 30.)
time.sleep(2)
value += 1
if value == 2:
controller.set_control(0., -30.)
time.sleep(2)
value += 1
if value == 3:
controller.set_control(2000., 0.)
time.sleep(1)
value += 1
if value == 4:
controller.set_control(-2000., 0.)
time.sleep(2)
value = 1