while vidro.current_rc_channels[5] > 1600:
controller.rc_alt(500)
controller.rc_yaw(0)
controller.rc_xy(500,500)
if round((round(time.clock(),3) % .05),2) == 0:
screen.clear()
screen.refresh()
#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(controller.base_rc_throttle+controller.error_alt*controller.alt_K_P+controller.I_error_alt*controller.alt_K_I)) + " = "+ str(controller.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]), 6, 0)
curses_print("Error: " + str(controller.error_yaw), 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(controller.base_rc_yaw+controller.error_yaw*controller.yaw_K_P+controller.I_error_yaw*controller.yaw_K_I)) + " = "+ str(controller.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)
plot_error_throttle_I.append(controller.I_error_alt)
plot_time_throttle.append(controller.previous_time_alt)
plot_error_pitch.append(controller.error_pitch)
plot_error_pitch_I.append(controller.I_error_pitch)
plot_time_pitch.append(controller.previous_time_xy)
plot_error_pitch_D.append(controller.D_error_pitch)
plot_rc_pitch.append(vidro.current_rc_channels[1])
plot_error_roll.append(controller.error_roll)
plot_error_roll_I.append(controller.I_error_roll)
plot_time_roll.append(controller.previous_time_xy)
plot_rc_roll.append(vidro.current_rc_channels[0])
plot_error_roll_D.append(controller.D_error_roll)
plot_x_current.append(vidro.get_position()[0])
plot_y_current.append(vidro.get_position()[1])
switch = True
vidro.update_mavlink()
vidro.rc_all_reset()
vidro.update_mavlink()
#Erase Plots
if switch == True:
"""
plot.figure(1).clf()
plot.figure(1)
plot.xlabel("Time(sec)")
plot.ylabel("Error(rads)")
controller.vidro.set_rc_throttle(controller.base_rc_throttle)
controller.vidro.set_rc_roll(controller.base_rc_roll)
controller.vidro.set_rc_pitch(controller.base_rc_pitch)
controller.vidro.set_rc_yaw(controller.base_rc_yaw)
curses_print("ERROR",4,0)
#Printing to screen.
if round((round(time.time(),3) % .05),2) == 0:
#screen.clear()
#screen.refresh()
if print_to_screen == True:
try:
curses_print("Under quad control",0,0)
curses_print("Position: X: " + str(vidro.get_position()[0]) + " Y: " + str(vidro.get_position()[1]) + " Z: " + str(vidro.get_position()[2]),1,0)
curses_print("Wand: X: " + str(vidro.get_vicon()[4]) + " Y: " + str(vidro.get_vicon()[5]) + " Z: " + str(vidro.get_vicon()[6]),2,0)
curses_print("Target: X: " + str(target_x) + " Y: " + str(target_y) + " Z: " + str(target_z),3,0)
curses_print("Vicon Error: " + str(vidro.vicon_error),4,0)
#Print alt data
curses_print("Throttle RC Override: " + str(vidro.current_rc_overrides[2]), 6, 1)
curses_print("Throttle RC Level: " + str(vidro.current_rc_channels[2]), 7, 1)
curses_print("Error: " + str(controller.error_alt), 8, 1)
curses_print("Altitude:" + str(vidro.get_position()[2]), 9, 1)
curses_print("T: "+ str(int(controller.base_rc_throttle+controller.error_alt*controller.alt_K_P+controller.I_error_alt*controller.alt_K_I)) + " = "+ str(controller.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]), 6, 0)
curses_print("Error: " + str(controller.error_yaw), 7, 0)
curses_print("raw vicon : " + str(vidro.get_vicon()[9]), 8, 0)
