#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: