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
if(max_bear_val<0):
max_bear_val+=(2*math.pi) # Keep it positive
if (yaw_com > 2 * math.pi): # Check if we've done a full circle
yaw_com-=(2*math.pi) # Keep it positive
if(balloon_found): # Advance if we've found the balloon in our rotation
import socket, struct, threading
import numpy as np
import cv2
#Setup of vidro and controller
vidro = Vidro(False, 1)
flight_ready = vidro.connect()
controller = PositionController(vidro)
start_time = time.time()
previous_time = time.time()
cycle = 0
controller.update_gains()
sequence = 0
seq0_cnt = 0
seq1_cnt = 0
seq2_cnt = 0
seq3_cnt = 0
pos_bound_err = 300
yaw_bound_err = 0.5
yaw = 0
print('Heading to while')
while(1):
#print("Position: X: " + repr(vidro.get_position()[0]) + " Y: " + repr(vidro.get_position()[1]) + " Z: " + repr(vidro.get_position()[2])+ " Yaw: " + repr(vidro.get_yaw_radians())+ " Pitch: " + repr(vidro.get_pitch())+ " Roll: " + repr(vidro.get_roll()))
print repr(vidro.get_yaw_radians())
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)
curses_print("raw vicon : " + str(vidro.get_vicon()[6]), 7, 0)
curses_print("Heading Radians: " + str(vidro.get_yaw_radians()), 8, 0)
curses_print("Heading Degrees: " + str(vidro.get_yaw_degrees()), 9, 0)
curses_print("Y: "+ str(int(vidro.base_rc_yaw+controller.error_yaw*controller.yaw_K_P+controller.I_error_yaw*controller.yaw_K_I)) + " = "+ str(vidro.base_rc_yaw) + " + " + str(controller.error_yaw*controller.yaw_K_P) + " + " + str(controller.I_error_yaw*controller.yaw_K_I) + " + " + str(controller.D_error_yaw*controller.yaw_K_D), 20, 0)
#Print pitch and roll
curses_print("Pitch RC Level: " + str(vidro.current_rc_channels[1]), 11, 0)
curses_print("Roll RC Level: " + str(vidro.current_rc_channels[0]), 11, 1)
curses_print("Pitch: " + str(vidro.get_pitch()), 12, 0)
curses_print("Roll: " + str(vidro.get_roll()), 12, 1)
curses_print("X Error: " + str(round(controller.error_x)), 15, 0)
curses_print("Y Error: " + str(round(controller.error_y)), 15, 1)
curses_print("Roll Error: " + str(round(controller.error_roll)), 13, 1)
curses_print("Pitch Error: " + str(round(controller.error_pitch)), 13, 0)
curses_print("P: " + str(int(vidro.base_rc_pitch+controller.error_pitch*controller.pitch_K_P+controller.I_error_pitch*controller.pitch_K_I+controller.D_error_pitch*controller.pitch_K_D)) + " = " + str(vidro.base_rc_pitch) + " + " + str(controller.error_pitch*controller.pitch_K_P) + " + " + str(controller.I_error_pitch*controller.pitch_K_I) + " + " + str(controller.D_error_pitch*controller.pitch_K_D), 21, 0)
curses_print("R: " + str(int(vidro.base_rc_roll+controller.error_roll*controller.roll_K_P+controller.I_error_roll*controller.roll_K_I+controller.D_error_roll*controller.roll_K_D)) + " = " + str(vidro.base_rc_roll) + " + " + str(controller.error_roll*controller.roll_K_P) + " + " + str(controller.I_error_roll*controller.roll_K_I) + " + " + str(controller.D_error_roll*controller.roll_K_D), 22, 0)
