seq0_cnt += 1 # just update the sequence if the loop is closed for 3 software loops if seq0_cnt == 10: sequence = 1 #Seq. 1: Coarse Search if sequence == 1: error_z = controller.rc_alt(alt_com) error_yaw = controller.rc_yaw(yaw_com) #error_x_y = controller.rc_xy(x_com, y_com) err_x = 0#error_x_y[0] err_y = 0#error_x_y[1] print('Seq: '+repr(sequence)+' Err Z: '+repr(round(error_z))+' Err Yaw: '+repr(round(error_yaw))+' Err X: '+repr(round(err_x))+' Err y: '+repr(round(err_y))) if ((abs(error_z) < pos_bound_err) and (abs(error_yaw) < yaw_bound_err) and (abs(err_y) < pos_bound_err) and (abs(err_x) < pos_bound_err)):# Closes Error # Hold her steady while we img proc vidro.set_rc_throttle(vidro.base_rc_throttle) vidro.set_rc_roll(vidro.base_rc_roll) vidro.set_rc_pitch(vidro.base_rc_pitch) vidro.set_rc_yaw(vidro.base_rc_yaw) # Run img proc yaw_com +=yaw_coarse_step yaw_pos = vidro.get_yaw_radians() # Grab current yaw val, assuming picture taking could be a while so grab it here son get_camera_frame() cx_val,cy_val,area_val,num_objects_val = get_object(frame) if(num_objects_val>0): balloon_found = True # If we have something, we'll assume we've found the balloon if(yaw_pos<0): yaw_pos+=(2*math.pi) # Keep our bearing estimate between 0 and 2pi if(area_val>area_max): # If our current imag has a bigger red area, update our estimate area_max_val = area_val img_balloon_ber = (cx_val-cx_mid)*cx_fov/640 # Basic camera model, somebody check this max_bear_val = yaw_pos+img_balloon_ber