def Single_Velocity():
# Set goals to go to
GOALS = [(0.5,0,2),(0,0.5,2),(-0.5,0,2),(0,-0.5,2)]
# Define the quadcopters
QUADCOPTER={'q1':{'position':[0,0,0],'orientation':[0,0,0],'L':0.3,'r':0.1,'prop_size':[10,4.5],'weight':1.2}}
# Controller parameters
CONTROLLER_PARAMETERS = {'Motor_limits':[4000,9000],
'Tilt_limits':[-10,10],
'Yaw_Control_Limits':[-900,900],
'Z_XY_offset':500,
'Linear_PID':{'P':[2000,2000,7000],'I':[0.25,0.25,4.5],'D':[50,50,5000]},
'Linear_To_Angular_Scaler':[1,1,0],
'Yaw_Rate_Scaler':0.18,
'Angular_PID':{'P':[22000,22000,1500],'I':[0,0,1.2],'D':[12000,12000,0]},
}
# Catch Ctrl+C to stop threads
signal.signal(signal.SIGINT, signal_handler)
# Make objects for quadcopter, gui and controller
quad = quadcopter.Quadcopter(QUADCOPTER)
gui_object = gui.GUI(quads=QUADCOPTER)
ctrl = controller.Controller_PID_Velocity(quad.get_state,quad.get_time,quad.set_motor_speeds,params=CONTROLLER_PARAMETERS,quad_identifier='q1')
# Start the threads
quad.start_thread(dt=QUAD_DYNAMICS_UPDATE,time_scaling=TIME_SCALING)
ctrl.start_thread(update_rate=CONTROLLER_DYNAMICS_UPDATE,time_scaling=TIME_SCALING)
# Update the GUI while switching between destination poitions
while(run==True):
for goal in GOALS:
ctrl.update_target(goal)
for i in range(150):
gui_object.quads['q1']['position'] = quad.get_position('q1')
gui_object.quads['q1']['orientation'] = quad.get_orientation('q1')
gui_object.update()
quad.stop_thread()
ctrl.stop_thread()
def PIDExperiment():
# Set goals to go to
GOALS_1 = [(0, -0.5, 0), (0, 0.5, 0)]
# Define the quadcopters
quad_list = [
quadcopter.Quadcopter([0, 0, 0], [0, 0, 0], 0.3, 0.1, [10, 4.5], 1.2)
]
# Controller parameters
# Catch Ctrl+C to stop threads
signal.signal(signal.SIGINT, signal_handler)
# Make objects for quadcopter, gui and controllers
gui_object = gui.GUI(quads=quad_list)
#quad = quadcopter.Quadcopter(quads=QUADCOPTERS)
quad_manager = quadcopter.QuadManager(quad_list)
ctrl1 = controller.Controller_PID_Velocity(quad_list[0].get_state,
quad_manager.get_time,
quad_list[0].set_motor_speeds,
[4000, 9000], [-10, 10],
[-900, 900], 500, {
'P': [300, 300, 7000],
'I': [0.04, 0.04, 4.5],
'D': [450, 450, 5000]
}, [1, 1, 0], 0.18, {
'P': [22000, 22000, 1500],
'I': [0, 0, 1.2],
'D': [12000, 12000, 0]
})
# Start the threads
quad_manager.start_thread(dt=QUAD_DYNAMICS_UPDATE,
time_scaling=TIME_SCALING)
ctrl1.start_thread(update_rate=CONTROLLER_DYNAMICS_UPDATE,
time_scaling=TIME_SCALING)
# ctrl2.start_thread(update_rate=CONTROLLER_DYNAMICS_UPDATE,time_scaling=TIME_SCALING)
# Update the GUI while switching between destination poitions
update_rate = 0.01
while (run == True):
for goal1 in GOALS_1:
ctrl1.update_target(goal1)
last_update = datetime.datetime.now()
#ctrl2.update_target(goal2)
#for i in range(10):
while ((datetime.datetime.now() - last_update).total_seconds() <
update_rate):
for q in gui_object.quads:
q.position = q.get_position()
q.orientation = q.get_orientation()
gui_object.update()
quad_manager.stop_thread()
ctrl1.stop_thread()
'P': [2000, 2000, 7000],
'I': [0.25, 0.25, 4.5],
'D': [50, 50, 5000]
},
'Linear_To_Angular_Scaler': [1, 1, 0],
'Yaw_Rate_Scaler': 0.18,
'Angular_PID': {
'P': [22000, 22000, 1500],
'I': [0, 0, 1.2],
'D': [12000, 12000, 0]
},
}
quad = quadcopter.Quadcopter(QUADCOPTER)
ctrl = controller.Controller_PID_Velocity(quad.get_state,
quad.get_time,
quad.set_motor_speeds,
params=CONTROLLER_PARAMETERS,
quad_identifier='q1')
def dynamics(state, u, dt):
"""
Returns next state given last state x, wrench u, and timestep dt.
Very car-like characteristics.
"""
# # Velocity in world frame
# vwx = np.cos(x[3]) * x[4]
# vwy = np.sin(x[3]) * x[4]
# vwz = 0.0
# Actuator saturation
# u = np.clip(u[:2], [-u_max[0]/10, -u_max[1]], u_max)