#Get X and Y Error error_x = error_xy[0] error_y = error_xy[1] #Filter for dropped ultrasonic sensor values if error_z is None: error_z = last_error elif abs(error_z) > 3500: error_z = 800 else: error_z = error_z #Store the Last Error last_error = error_z #Close Error and move on after 50 sequences if abs(error_z) < alt_bound_err and abs(error_z) > 0 and abs(error_x) < pos_bound_err and abs(error_y) < pos_bound_err: seq3_cnt += 1 if seq3_cnt == 50: sequence = 0 #Go back to Manual control if vidro.current_rc_channels[4] < 1600: controller.vidro.rc_throttle_reset() controller.vidro_rc_pitch_reset() controller.vidro_rc_roll_reset()
