print('Armed Loop')
#Update of gains before going into control loop
if vidro.current_rc_channels[5] > 1600:
controller.update_gains() # Potentially move this into the main loop
# Auto Loop
while vidro.current_rc_channels[5] > 1600:
vidro.update_mavlink() # Grab updated rc channel values
print('Auto Loop')
# Seq. 0: Takeoff to 1 m and origin ###############=> Separate these commands maybe....maybe in all of them, just make a positioning thread maybe
if sequence == 0:
# Assign Commands
alt_com = take_off_alt
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
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]
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: seq1_cnt += 1
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()
curses_print("Position: X: " + str(vidro.get_position()[0]) + " Y: " + str(vidro.get_position()[1]) + " Z: " + str(vidro.get_position()[2]),0,0)
print('Seq: '+repr(sequence)+' Err Z: '+repr(error_z)+' Err Yaw: '+repr(error_yaw)+' Err X: '+repr(err_x)+' Err y: '+repr(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 for takeoff
yaw_com +=0.3
seq1_cnt += 1 # again just update the sequence once we've closed the loop
print('update')
if yaw > 2 * math.pi:
yaw -= (2 * math.pi)
if seq1_cnt==5: # once we've closed the loop for 20 steps, we're done
sequence = 2
time2 = time.time()
print 'Duration: '+repr(time2-time1)
#Move to landing position
if sequence == 2:
error_z = controller.rc_alt(desc_alt)
error_yaw = controller.rc_yaw(1.57079633)
error_x_y = controller.rc_xy(0, 0)
err_x = error_x_y[0]
err_y = error_x_y[1]
print('Seq: '+repr(sequence)+' Err Z: '+repr(error_z)+' Err Yaw: '+repr(error_yaw)+' Err X: '+repr(err_x)+' Err y: '+repr(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 for takeoff
print('update')
seq2_cnt += 1
#if seq2_cnt==3: # once we've closed the loop 5 times, advance!
desc_alt-=10
if(desc_alt<170):
sequence = 3
# Land
if sequence == 3:
controller.vidro.set_rc_throttle(0)# it'll round it to minimum which is like 1100
