# For Plotting System Response
Report1 = Report(AR1)
Report2 = Report(AR2)
# Report3 = Report(AR3)
FlyReport = FlyHistory([-6, 6], [-6, 6], [3, 5])
# Build Object for Swarm Controller
TPFconfig = {"damping_factor": 1, "gain": 1, "target_detecting_range": 1}
OPFconfig = {"positiveGain1": 1, "positiveGain2": 1, "detecting_range": 1}
SPFconfig = {"min_allowable_dist": 10}
SwarmController1 = SwarmController(TPFconfig, OPFconfig, SPFconfig)
AR1.connectToSwarmController(SwarmController1)
AR2.connectToSwarmController(SwarmController1)
# AR3.connectToSwarmController(SwarmController1)
Target1.connectToSwarmController(SwarmController1)
Obstacle1.connectToSwarmController(SwarmController1)
for i in range(1000):
SwarmController1.calculateAgentsForces()
AR1.controlSwarm(SwarmController1)
AR2.controlSwarm(SwarmController1)
# AR limit ?
AR1.updateState()
AR2.updateState()
# Plot Data
Report1.updateReport(AR1.getState(), AR1.thrust, AR1.moments)
Report2.updateReport(AR2.getState(), AR2.thrust, AR2.moments)
