#Get PID gains
controller.update_gains()
# Grab updated rc channel values
vidro.update_mavlink()
error_x = error_xy[0]
error_y = error_xy[1]
#print('Inner Loop')
#Seq. 0: Takeoff to 1 m and (0,0)
if sequence == 0:
#Assign Commands
error_z = controller.rc_alt(1000)
error_xy = controller.rc_xy(0, 0)
#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
vidro.previous_error_yaw = 0
vidro.previous_error_roll = 0
vidro.previous_error_pitch = 0
desc_alt = 1000
area_max = 0
yaw_coarse_step = 0.5236 # 30 degrees to get good, overlapping coverage
print('outer loop'+' Roll: '+repr(vidro.current_rc_channels[0])+' Pitch: '+repr(vidro.current_rc_channels[1]))
#Update of gains before going into control loop
if vidro.current_rc_channels[5] > 1600:
controller.update_gains()
while vidro.current_rc_channels[5] > 1600:
vidro.update_mavlink() # Grab updated rc channel values
#On ground -> Takeoff to 1 m
if sequence == 0:
error_z = controller.rc_alt(alt_com)
error_yaw = controller.rc_yaw(yaw_com)
error_x_y = controller.rc_xy(0, 0)
#error_z = 0
#error_yaw = 0
err_x = error_x_y[0]
err_y = error_x_y[1]
#controller.vidro.set_rc_pitch(1450)
#controller.vidro.set_rc_roll(1370)
print('Seq: '+repr(sequence)+' Err Z: '+repr(error_z)+' Err Yaw: '+repr(error_yaw)+' Err X: '+repr(err_x)+' Err y: '+repr(err_y)+' Roll: '+repr(vidro.current_rc_channels[0])+' Pitch: '+repr(vidro.current_rc_channels[1]))
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 for takeoff
seq0_cnt += 1 # just update the sequence if the loop is closed for 3 software loops
#print('update')
if seq0_cnt == 10:
sequence = 1
roll_idle = vidro.current_rc_channels[0]
controller.previous_time_alt = (time.time()-controller.timer)*10
controller.previous_time_yaw = (time.time()-controller.timer)*10
controller.previous_time_xy = (time.time()-controller.timer)*10
vidro.previous_error_alt = 0
vidro.previous_error_yaw = 0
vidro.previous_error_roll = 0
vidro.previous_error_pitch = 0
if vidro.current_rc_channels[5] > 1600:
controller.update_gains()
while vidro.current_rc_channels[5] > 1600:
#~ try:
controller.rc_alt(1000)
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()
controller.I_error_pitch = 0
controller.I_error_roll = 0
controller.I_error_yaw = 0
controller.previous_time_alt = (time.clock()-controller.timer)*10
controller.previous_time_yaw = (time.clock()-controller.timer)*10
controller.previous_time_xy = (time.clock()-controller.timer)*10
vidro.previous_error_alt = 0
vidro.previous_error_yaw = 0
vidro.previous_error_roll = 0
vidro.previous_error_pitch = 0
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)
controller.previous_time_alt = (time.time() - controller.timer) * 10 controller.previous_time_yaw = (time.time() - controller.timer) * 10 controller.previous_time_xy = (time.time() - controller.timer) * 10 vidro.previous_error_alt = 0 vidro.previous_error_yaw = 0 vidro.previous_error_roll = 0 vidro.previous_error_pitch = 0 #Update of gains before going into control loop if vidro.current_rc_channels[5] > 1600: controller.update_gains() while vidro.current_rc_channels[5] > 1600: #On ground if sequence == 0: error_z = controller.rc_alt(1000) error_yaw = controller.rc_yaw(0) error_pitch, error_roll = controller.rc_xy(0, 0) if error_z < 50 and error_yaw < 1 and error_pitch < 50 and error_roll < 50: if (time.time() % 1) == 0: seq0_cnt += 1 if seq0_cnt == 3: sequence = 1 #Hover in center of space if sequence == 1: error_z = controller.rc_alt(1000) error_yaw = controller.rc_yaw(0) error_x_y = controller.rc_xy(0,0) if error_z < 50 and error_yaw < 1 and error_pitch < 50 and error_roll < 50: if (time.time() % 1) == 0:
try:
target_x = vidro.get_vicon()[4]
target_y = vidro.get_vicon()[5]
target_z = vidro.get_vicon()[6]
except:
logging.error('Unable to get position data from the vicon for wand')
pass
#filter position of wand between values
target_z = filter_value(1000,5000,target_z)
target_x = filter_value(-2000,2000,target_x)
target_y = filter_value(-2000,2000,target_y)
#Send control values
try:
controller.rc_alt(target_z)
controller.rc_yaw(0)
controller.rc_xy(target_x,target_y)
except:
logging.error('Something went wrong in the control system. Setting to hover')
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)
curses_print("ERROR",4,0)
#Printing to screen.
if round((round(time.time(),3) % .05),2) == 0:
screen.clear()
screen.refresh()
vidro.previous_error_alt = 0
vidro.previous_error_yaw = 0
vidro.previous_error_roll = 0
vidro.previous_error_pitch = 0
desc_alt = 1000
print('outer loop'+' Roll: '+repr(vidro.current_rc_channels[0])+' Pitch: '+repr(vidro.current_rc_channels[1]))
#Update of gains before going into control loop
if vidro.current_rc_channels[5] > 1600:
controller.update_gains()
while vidro.current_rc_channels[5] > 1600:
vidro.update_mavlink() # Grab updated rc channel values
#On ground -> Takeoff to 1 m
if sequence == 0:
error_z = controller.rc_alt(1000)
error_yaw = controller.rc_yaw(0)
error_x_y = controller.rc_xy(0, 0)
#error_z = 0
#error_yaw = 0
err_x = error_x_y[0]
err_y = error_x_y[1]
#controller.vidro.set_rc_pitch(1450)
#controller.vidro.set_rc_roll(1370)
print('Seq: '+repr(sequence)+' Err Z: '+repr(error_z)+' Err Yaw: '+repr(error_yaw)+' Err X: '+repr(err_x)+' Err y: '+repr(err_y)+' Roll: '+repr(vidro.current_rc_channels[0])+' Pitch: '+repr(vidro.current_rc_channels[1]))
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 for takeoff
seq0_cnt += 1 # just update the sequence if the loop is closed for 3 software loops
print('update')
if seq0_cnt == 20:
roll_idle = vidro.current_rc_channels[0]
controller.update_gains()
sequence = 0
seq0_cnt = 0
seq1_cnt = 0
seq2_cnt = 0
seq3_cnt = 0
yaw = 0
alt = 1000
while vidro.current_rc_channels[5] > 1600:
#On ground
if sequence = 0:
error_z = controller.rc_alt(0)
error_yaw = controller.rc_yaw(0)
error_pitch, error_roll = controller.rc_xy(0,0)
if error_z < 50 and error_yaw < 1 and error_pitch < 50 and error_roll < 50:
if (time.time() % 1) == 0:
seq0_cnt += 1
if seq0_cnt == 3:
sequence = 1
#Hover in center of space
if sequence = 1:
error_z = controller.rc_alt(1000)
error_yaw = controller.rc_yaw(0)
error_x_y = controller.rc_xy(0,0)
if error_z < 50 and error_yaw < 1 and error_pitch < 50 and error_roll < 50:
if (time.time() % 1) == 0:
