# Run img proc
# Check to see if we still see the Balloon
yaw_pos = vidro.get_yaw_radians() # Update our radians
get_camera_frame()
cx_val,cy_val,area_val,num_objects_val = get_object(frame)
if(num_objects_val>0): # If we've seen something, adjust our pointing
img_balloon_ber = (cx_val-cx_mid)*cx_fov/640 # Basic camera model, somebody check this
max_bear_val = yaw_pos+img_balloon_ber
if(max_bear_val<0):
max_bear_val+=(2*math.pi) # Keep it positive
yaw_com = max_bear_val
if(area_val>balloon_area_max_thresh): # If the balloon is taking up most of our FOV, advance
sequence = 4
else: # Else, update x,y com
# Grab our current x,y position
x_pos=vidro.get_position()[0]
y_pos=vidro.get_position()[1]
# Calculate step towards balloon
d_x = guid_adv*math.cos(max_bear_val)
d_y = guid_adv*math.sin(max_bear_val)
x_com = x_pos+d_x
y_com = y_pos+d_y
else:
balloon_found = False # If we don't see anything go back to coarse search
sequence = 1
area_max_val = 0
#Seq. 4: Terminal
if sequence == 4:
cv2.imwrite("terminal_pic.jpg",frame) # For now, just take a picture
sequence=5
controller.rc_yaw(0)
controller.rc_xy(0,0)
curses_print("No errors",2,0)
#~ except:
#~ controller.vidro.set_rc_throttle(vidro.base_rc_throttle)
#~ controller.vidro.set_rc_roll(vidro.base_rc_roll)
#~ controller.vidro.set_rc_pitch(vidro.base_rc_pitch)
#~ controller.vidro.set_rc_yaw(vidro.base_rc_yaw)
#~ curses_print("ERROR",2,0)
if round((round(time.time(),3) % .05),2) == 0:
screen.clear()
screen.refresh()
curses_print("Position: X: " + str(vidro.get_position()[0]) + " Y: " + str(vidro.get_position()[1]) + " Z: " + str(vidro.get_position()[2]),0,0)
if vidro.vicon_error == True:
curses_print("Vicon Error: " + str(vidro.vicon_error),1,0)
else:
curses_print("Vicon time: " + str(vidro.vicon_time),1,0)
#Print alt data
curses_print("Throttle RC Override: " + str(vidro.current_rc_overrides[2]), 5, 1)
curses_print("Throttle RC Level: " + str(vidro.current_rc_channels[2]), 6, 1)
curses_print("Error: " + str(controller.error_alt), 7, 1)
curses_print("Altitude:" + str(vidro.get_position()[2]), 8, 1)
curses_print("T: "+ str(int(vidro.base_rc_throttle+controller.error_alt*controller.alt_K_P+controller.I_error_alt*controller.alt_K_I)) + " = "+ str(vidro.base_rc_throttle) + " + " + str(controller.error_alt*controller.alt_K_P) + " + " + str(controller.I_error_alt*controller.alt_K_I) + " + " + str(controller.D_error_alt*controller.alt_K_D), 19, 0)
#Print yaw data
curses_print("Yaw RC Level: " + str(vidro.current_rc_channels[3]), 5, 0)
curses_print("Error: " + str(controller.error_yaw), 6, 0)
