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 __init__(self, start, obstacles):
"""
initializer
start: 3d-vector with the starting coordinates of the drone
obestacles: list of 6d-vectors with the rectangular obstacles. format{pos_x, pos_y, pos_z, len_x, len_y, len_z}
"""
self.start = start
self.obs = obstacles
self.path = []
self.iteration_data = []
self.planReady = False
self.iterRunGo = False
self.pathIter = 0
self.goalIter = 0
self.QUADCOPTER = {
'q1': {
'position': start,
'orientation': [0, 0, 0],
'L': 0.175,
'r': 0.0665,
'prop_size': [8, 3.8],
'weight': 0.5,
'motorWeight': 0.035
}
}
# Controller parameters
self.CONTROLLER_PARAMETERS = {
'Motor_limits': [2000, 12000], #4000,12000
'Tilt_limits': [-10, 10],
'Yaw_Control_Limits': [-900, 900],
'Z_XY_offset': 500,
'Linear_PID': {
'P': [300, 300, 7000],
'I': [0.03, 0.03, 6],
'D': [450, 450, 4200]
},
'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]
},
}
# Make objects for quadcopter, gui and controller
self.quad = quadcopter.Quadcopter(self.QUADCOPTER)
self.gui_object = gui.GUI(quads=self.QUADCOPTER, obs=obstacles)
self.ctrl = controller.Controller_PID_Point2Point(
self.quad.get_state,
self.quad.get_time,
self.quad.set_motor_speeds,
params=self.CONTROLLER_PARAMETERS,
quad_identifier='q1')
self.rrt = RRTStar(obstacle_list=self.obs)
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()
def Multi_Point2Point():
# Set goals to go to
GOALS_1 = [(-1,-1,4),(1,1,2)]
GOALS_2 = [(1,-1,2),(-1,1,4)]
# Define the quadcopters
QUADCOPTERS={'q1':{'position':[1,0,4],'orientation':[0,0,0],'L':0.3,'r':0.1,'prop_size':[10,4.5],'weight':1.2},
'q2':{'position':[-1,0,4],'orientation':[0,0,0],'L':0.15,'r':0.05,'prop_size':[6,4.5],'weight':0.7}}
# Controller parameters
CONTROLLER_1_PARAMETERS = {'Motor_limits':[4000,9000],
'Tilt_limits':[-10,10],
'Yaw_Control_Limits':[-900,900],
'Z_XY_offset':500,
'Linear_PID':{'P':[300,300,7000],'I':[0.04,0.04,4.5],'D':[450,450,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]},
}
CONTROLLER_2_PARAMETERS = {'Motor_limits':[4000,9000],
'Tilt_limits':[-10,10],
'Yaw_Control_Limits':[-900,900],
'Z_XY_offset':500,
'Linear_PID':{'P':[300,300,7000],'I':[0.04,0.04,4.5],'D':[450,450,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 controllers
gui_object = gui.GUI(quads=QUADCOPTERS)
quad = quadcopter.Quadcopter(quads=QUADCOPTERS)
ctrl1 = controller.Controller_PID_Point2Point(quad.get_state,quad.get_time,quad.set_motor_speeds,params=CONTROLLER_1_PARAMETERS,quad_identifier='q1')
ctrl2 = controller.Controller_PID_Point2Point(quad.get_state,quad.get_time,quad.set_motor_speeds,params=CONTROLLER_2_PARAMETERS,quad_identifier='q2')
# Start the threads
quad.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
while(run==True):
for goal1,goal2 in zip(GOALS_1,GOALS_2):
ctrl1.update_target(goal1)
ctrl2.update_target(goal2)
for i in range(150):
for key in QUADCOPTERS:
gui_object.quads[key]['position'] = quad.get_position(key)
gui_object.quads[key]['orientation'] = quad.get_orientation(key)
gui_object.update()
quad.stop_thread()
ctrl1.stop_thread()
ctrl2.stop_thread()
def reset(self):
"""
Important: the observation must be a numpy array
:return: (np.array)
"""
# Initialize the agent at the right of the grid
self.quad_id += 1
QUADCOPTER = {
str(self.quad_id): {
'position': [0, 0, 0],
'orientation': [0, 0, 0],
'L': 0.3,
'r': 0.1,
'prop_size': [10, 4.5],
'weight': 1.2
}
}
self.quad = quadcopter.Quadcopter(QUADCOPTER)
#self.gui_object = gui.GUI(quads=QUADCOPTER)
self.ctrl = controller.Blended_PID_Controller(
self.quad.get_state,
self.quad.get_time,
self.quad.set_motor_speeds,
self.quad.get_motor_speeds,
self.quad.stepQuad,
self.quad.set_motor_faults,
params=BLENDED_CONTROLLER_PARAMETERS,
quad_identifier=str(self.quad_id))
self.ctrl.setController("Agent")
self.current = 0
self.ctrl.update_target(goals[self.current], safe_region[self.current])
#print(type(self.quad.get_state("q1")))
self.setRandomFault()
self.cumu_reward = 0
obs = self.ctrl.get_updated_observations()
obs_array = []
for key, value in obs.items():
obs_array.append(value)
return obs_array
def __init__(self,
num_states=10,
reward_location=RewardLocation.Ends,
failure_probability=.1):
"""Initialize QuadcopterDomain."""
#if num_states < 4:
# raise ValueError('num_states must be >= 4')
#if failure_probability < 0 or failure_probability > 1:
# raise ValueError('failure_probability must be in range [0, 1]')
self.num_states = int(num_states)
#self.reward_location = reward_location
self.failure_probability = failure_probability
self._state = QuadcopterDomain.__init_random_state(num_states)
self.key = 'q1'
QUADCOPTER = {
self.key: {
'position': [1, 0, 4],
'orientation': [0, 0, 0],
'L': 0.3,
'r': 0.1,
'prop_size': [10, 4.5],
'weight': 1.2
}
}
self.quad_dict = QUADCOPTER
self.quad = quadcopter.Quadcopter(QUADCOPTER)
self.speed_low = 4000
self.speed_high = 9000
self.num_of_possible_speeds = 5
self.possible_motor_speeds = np.linspace(
self.speed_low, self.speed_high,
num=self.num_of_possible_speeds).astype(int)
self.target_position = [1, 0, 0]
self.target_lin_vel = [0, 0, 0]
def __init__(self):
# Set goals to go to
GOALS = [(1,1,2)]
YAWS = [0]
self.STEPSIZE = 0.002
# Define the quadcopters
QUADCOPTER={'q1':{'position':[1,0,4],'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':[300,300,7000],'I':[0.04,0.04,4.5],'D':[450,450,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]},
}
self.quad = quadcopter.Quadcopter(QUADCOPTER)
self.gui_object = gui.GUI(quads=QUADCOPTER)
self.random_seed = None
def NNQuadAngleTest2D():
# Define the quadcopters
quad_list = [
quadcopter.Quadcopter([1, 0, 0], [0, 0, 0], 0.3, 0.1, [10, 4.5], 1.2)
]
# Catch Ctrl+C to stop threads
signal.signal(signal.SIGINT, signal_handler)
# Make objects for quadcopter, gui and controllers
gui_object = gui.GUI_Neural(quads=quad_list)
quad_manager = quadcopter.QuadManagerTimeOnly()
#def __init__(self, quad, dt, get_state, set_state, get_time, actuate_motors, memory_length, gamma, epsilon, epsilon_decay, epsilon_min, learning_rate, ANGLE_DISCRITIZATION, MOVEMENT_LENGTH, will_save):
ctrl1 = controller.Controller_NeuralNet_Angle_2D(
quad_list[0], 5, quad_list[0].get_state, quad_list[0].set_state,
quad_manager.get_time, quad_list[0].set_motor_speeds, 2000, 0.95, 0,
0.995, 0, 0.001, 36, 0.1, False)
#ctrl1 = controller.Controller_NeuralNet_Angle_2D(quad_list[0],5, quad_list[0].get_state,quad_list[0].set_state,quad_manager.get_time, quad_list[0].set_motor_speeds, 2000, 0.95, 1, 0.995, 0.2, 0.001, 36, 0.1, True)
# Start the threads
quad_manager.start_thread(dt=QUAD_DYNAMICS_UPDATE,
time_scaling=TIME_SCALING)
ctrl1.start_thread_graphics(update_rate=CONTROLLER_DYNAMICS_UPDATE,
time_scaling=TIME_SCALING,
path='models/nn_angle_test/angle_demo.hdf5')
while (run == True):
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()
def Multi_Point2Point():
# Set goals to go to
FORMATIONS = json.load(open('quad_sim/formations.json'))
# Define the quadcopters
QUADCOPTERS = json.load(open('quad_sim/quadcopters.json'))
# Controller parameters
CONTROLLER_PARAMETERS = {
'Motor_limits': [4000, 9000],
'Tilt_limits': [-10, 10],
'Yaw_Control_Limits': [-900, 900],
'Z_XY_offset': 500,
'Linear_PID': {
'P': [300, 300, 7000],
'I': [0.04, 0.04, 4.5],
'D': [450, 450, 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]
},
}
# Create enough controller parameters
ctrl_params = [CONTROLLER_PARAMETERS.copy() for q in QUADCOPTERS]
# Catch Ctrl+C to stop threads
signal.signal(signal.SIGINT, signal_handler)
# Make objects for quadcopter, gui and controllers
gui_object = gui.GUI(quads=QUADCOPTERS)
quad = quadcopter.Quadcopter(quads=QUADCOPTERS)
controllers = [
controller.Controller_PID_Point2Point(quad.get_state,
quad.get_time,
quad.set_motor_speeds,
params=param,
quad_identifier=q)
for q, param in zip(QUADCOPTERS, ctrl_params)
]
# ctrl1 = controller.Controller_PID_Point2Point(quad.get_state,quad.get_time,quad.set_motor_speeds,params=CONTROLLER_1_PARAMETERS,quad_identifier='q1')
# ctrl2 = controller.Controller_PID_Point2Point(quad.get_state,quad.get_time,quad.set_motor_speeds,params=CONTROLLER_2_PARAMETERS,quad_identifier='q2')
# Start the threads
quad.start_thread(dt=QUAD_DYNAMICS_UPDATE, time_scaling=TIME_SCALING)
for ctrl in controllers:
ctrl.start_thread(update_rate=CONTROLLER_DYNAMICS_UPDATE,
time_scaling=TIME_SCALING)
# Update the GUI while switching between destination poitions
while (run == True):
key = input(
"Which formation would you like to make?\noptions: {}\n:".format(
[str(key) for key in FORMATIONS.keys()]))
while key not in FORMATIONS.keys():
key = input(
"Did not recognize input. Please select an option below...\noptions: {}\n:"
.format([str(key) for key in FORMATIONS.keys()]))
# TODO: Write a smart algorithm to set targets of nearest drones
for q, ctrl in zip(QUADCOPTERS, controllers):
ctrl.update_target(tuple(FORMATIONS[key][q]))
for i in range(300):
for key in QUADCOPTERS:
gui_object.quads[key]['position'] = quad.get_position(key)
gui_object.quads[key]['orientation'] = quad.get_orientation(
key)
gui_object.update()
quad.stop_thread()
for ctrl in controllers:
ctrl.stop_thread()
'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]
},
}
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]
def __init__(self, p, i, d):
self.quad_id = 1
BLENDED_CONTROLLER_PARAMETERS = {
'Motor_limits': [0, 9000],
'Tilt_limits': [-10, 10],
'Yaw_Control_Limits': [-900, 900],
'Z_XY_offset': 500,
'Linear_PID': {
'P': [300, 300, 7000],
'I': [0.04, 0.04, 4.5],
'D': [450, 450, 5000]
},
'Linear_To_Angular_Scaler': [1, 1, 0],
'Yaw_Rate_Scaler': 0.18,
'Angular_PIDS': [{
'P': [p, p, 1500],
'I': [i, i, 1.2],
'D': [d, d, 0]
}]
}
QUADCOPTER = {
str(self.quad_id): {
'position': [0, 0, 0],
'orientation': [0, 0, 0],
'L': 0.3,
'r': 0.1,
'prop_size': [10, 4.5],
'weight': 1.2
}
}
# Make objects for quadcopter
self.quad = quadcopter.Quadcopter(QUADCOPTER)
Quads = {str(self.quad_id): QUADCOPTER[str(self.quad_id)]}
self.config = [int(p), int(i), int(d)]
# create blended controller and link it to quadcopter object
self.ctrl = controller.PID_Controller(
self.quad.get_state,
self.quad.get_time,
self.quad.set_motor_speeds,
self.quad.get_motor_speeds,
self.quad.stepQuad,
self.quad.set_motor_faults,
self.quad.setWind,
self.quad.setNormalWind,
params=BLENDED_CONTROLLER_PARAMETERS,
quad_identifier=str(self.quad_id))
# self.gui_object = gui.GUI(quads=QUADCOPTER, ctrl=self.ctrl)
#
self.setEasyPath()
self.current = 0
self.ctrl.update_target(goals[self.current], safe_region[self.current])
self.ctrl.setController("PID")
self.done = False
self.stepcount = 0
self.stableAtGoal = 0
'''
Quadcopter Parameters
'''
# Set target sequences of waypoints and yaws to go to
WAYPOINTS = [(0, 0, 4)]
YAWS = [0]
# Define quadcopter properties
QUADCOPTER = {
'q1': {
'position': [2, 2, 2],
'orientation': [0, 0, 0],
'L': 0.3,
'r': 0.1,
'prop_size': [10, 4.5],
'weight': 1.2,
'method': 'Zero_Dynamic_Stabilization'
}
}
# Make objects for quadcopter, gui and controller
quad = quadcopter.Quadcopter(QUADCOPTER,
time_horizon=time_horizon,
plot_simulation_trail=plot_sim_trail,
simulate=simulate)
# Start the threads
quad.start_thread(dt=QUAD_DYNAMICS_UPDATE, time_scaling=TIME_SCALING)
ui = gui.GUI(QUADCOPTER, quad, display_obstacles, plot_sim_trail,
plot_quad_trail, save)
def __init__(self):
super(Quad_Env, self).__init__()
self.quad_id = 1
QUADCOPTER = {
str(self.quad_id): {
'position': [0, 0, 0],
'orientation': [0, 0, 0],
'L': 0.3,
'r': 0.1,
'prop_size': [10, 4.5],
'weight': 1.2
}
}
# Make objects for quadcopter
self.quad = quadcopter.Quadcopter(QUADCOPTER)
#self.gui_object = gui.GUI(quads=QUADCOPTER)
self.sampleTime = 0.01
#create blended controller and link it to quadcopter object
self.ctrl = controller.Blended_PID_Controller(
self.quad.get_state,
self.quad.get_time,
self.quad.set_motor_speeds,
self.quad.get_motor_speeds,
self.quad.stepQuad,
self.quad.set_motor_faults,
params=BLENDED_CONTROLLER_PARAMETERS,
quad_identifier=str(self.quad_id))
self.ctrl.setController("Agent")
self.current = 0
self.ctrl.update_target(goals[self.current], safe_region[self.current])
self.setRandomFault()
self.cumu_reward = 0
# Define action and observation space
# They must be gym.spaces objects
# Example when using discrete actions, we have two: left and right
n_actions = 3
#PID LOW and HIHG
self.actionlow = 0.01
self.actionhigh = 1
self.action_low_state = np.array([self.actionlow, self.actionlow],
dtype=np.float)
self.action_high_state = np.array([self.actionhigh, self.actionhigh],
dtype=np.float)
self.action_space = spaces.Box(low=self.action_low_state,
high=self.action_high_state,
dtype=np.float)
# The observation will be the coordinate of the agent
# this can be described both by Discrete and Box space
# print(self.observation_space)
self.low_error = -1
self.high_error = 1
self.low_dest = -10
self.high_dest = 10
self.low_act = -10000
self.high_act = 10000
self.low = 0
self.high = 10000
self.low_state = np.array([
self.low_dest,
self.low_dest,
self.low_dest,
self.low_dest,
self.low_dest,
self.low_dest,
],
dtype=np.float32)
# self.low_state = np.array([self.low_dest,self.low_dest, self.low_error,self.low_error, self.low_act, self.low_act, self.low_act, self.low_act])
#self.high_state = np.array([self.high_dest,self.high_dest, self.high_error, self.high_error, self.high_act, self.high_act, self.high_act,self.high_act])
self.high_state = np.array([
self.high_dest,
self.high_dest,
self.high_dest,
self.high_dest,
self.high_dest,
self.high_dest,
],
dtype=np.float32)
self.observation_space = spaces.Box(low=self.low_state,
high=self.high_state,
dtype=np.float32)
def __init__(self):
self.quadcopter = quadcopter.Quadcopter()
self.landing_site = landing_site.LandingSite()
start_time = 0
time = start_time
num_quads = 1 # only supports 1 vehicle as of now
quad_list = []
# start_pos = np.array([0.0, 0.0, 1.0, 0, 0, 0]) # x, y, z, qx, qy, qz Straight Line Test
start_pos = np.array(
[0.5, 0.5, 0.25, 0, 0, 0]
) # x, y, z, qx, qy, qz Hover Test, YOU NEED TO SWAP TRAJECTORIES IN quadcopter.py simulation_step()
# TODO: fix the straight line trajectory
# TODO: fix bug in quat2rot line 59 if 800 sim steps and desired position is [0.5, 0.5, 1.5]
end_pos = np.array([0.5, 0.5, 1.5]) # x, y, z
quad = quadcopter.Quadcopter(
start_pos, end_pos
) # single quadcopter object, we are currently only running single vehicle simulations
state_data = np.zeros(
(sim_steps, 13)) # rows = iterations in simulation, columns = state values
time_data = np.zeros(sim_steps)
err_data = np.zeros(sim_steps)
step_size = int(cstep / tstep)
assert int(cstep % tstep) == 0, "Step size is not evenly divisible"
#
# Run simulation
#
for itr in range(0, int(sim_steps / step_size)):
timeint = np.arange(time, time + cstep, tstep)
def TwoQuadTest():
# Set goals to go to
GOALS_1 = [(-1, -1, 4), (1, 1, 2)]
GOALS_2 = [(1, -1, 2), (-1, 1, 4)]
# Define the quadcopters
# def __init__(self,position, orientation, L, r, prop_size, weight, gravity=9.81,b=0.0245):
quad_list = [
quadcopter.Quadcopter([1, 0, 4], [0, 0, 0], 0.3, 0.1, [10, 4.5], 1.2),
quadcopter.Quadcopter([-1, 0, 4], [0, 0, 0], 0.15, 0.05, [6, 4.5], 0.7)
]
# 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_Point2Point(
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]
})
ctrl2 = controller.Controller_PID_Point2Point(
quad_list[1].get_state, quad_manager.get_time,
quad_list[1].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
while (run == True):
for goal1, goal2 in zip(GOALS_1, GOALS_2):
ctrl1.update_target(goal1)
ctrl2.update_target(goal2)
for i in range(150):
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()
ctrl2.stop_thread()
