def main():
it = 0
agents, agentLog = initDrones(uavInitPos, 5)
FC = formCtl(BHouse, Dhouse, agents, 0.1, 2)
# FC = formCtl(BHouse, Dhouse, agents, 0.15,2)
pl.close("all")
pl.ion()
fig = pl.figure(0)
axis3d = fig.add_subplot(111, projection='3d')
pl.figure(0)
for t in time:
FC.run()
i = 0
for uav in agents:
uav.step(dt)
uavLog = agentLog[i]
uavLog.append(uav.xyz)
i += 1
if it%frames==0 and FC.atAltitude:
# plotDrones(axis3d, agents, t)
plotFormation(FC, t)
pass
it+=1
if checkCrash(agents):
break
plotDrones(axis3d, agents, t)
# Error Data from the Flight Controller
data = FC.errLog
datLen = len(data)
datWid = len(data[0])
data = np.concatenate(data)
data = np.reshape(data, (datLen,datWid))
matLogs = []
for uavLogs in agentLog:
tmp = matReconfig(uavLogs)
matLogs.append(tmp)
# Plot errors
fig = pl.figure(1)
pl.title('Error Per Step')
for i in range(datWid):
pl.plot(data[:, i], color=uavColours[i], label='E'+str(i+1))
pl.xlabel('Iteration')
pl.ylabel('offset (M)')
pl.legend()
pl.savefig('img/2_Square_errorPerIter',format="svg")
# Plot Movement
pl.figure(2)
pl.title("2D Position [m]")
j = 0
for uav in matLogs:
pl.scatter(uav[0,0], uav[0,1], color=uavColours[j], marker='+',s=50, label='q'+str(j+1))
pl.plot(uav[:,0], uav[:,1], color=uavColours[j])
j +=1
plotFormation(FC,t)
pl.axis('equal')
pl.xlabel("East")
pl.ylabel("South")
pl.legend()
pl.grid()
pl.savefig('img/2_Square_FormationPositions',format="svg")
pl.pause(0)
def main():
it = 0
agents, agentLog = initDrones(uavInitPos, 4)
FC = formCtl(Bsquare, Dsquare, agents, 1, 2)
pl.close("all")
pl.ion()
fig = pl.figure(0)
axis3d = fig.add_subplot(111, projection='3d')
pl.figure(0)
FC.atAltitude = True
for t in time:
FC.run()
i = 0
for uav in agents:
uav.step(dt)
uavLog = agentLog[i]
uavLog.append(uav.xyz)
i += 1
if it % frames == 0 and FC.atAltitude:
plotDrones(axis3d, agents, t)
pass
it += 1
if checkCrash(agents):
break
# plotDrones(axis3d, agents, t)
# Error Data from the Flight Controller
data = FC.errLog
datLen = len(data)
datWid = len(data[0])
data = np.concatenate(data)
data = np.reshape(data, (datLen, datWid))
matLogs = []
for uavLogs in agentLog:
tmp = matReconfig(uavLogs)
matLogs.append(tmp)
# Plot errors
pl.figure(1)
# pl.subplot(1,1,1)
pl.title('Error Per Step')
for i in range(datWid):
pl.plot(data[:, i], color=uavColours[i], label='E' + str(i + 1))
pl.xlabel('Iteration')
pl.ylabel('offset (M)')
pl.legend()
pl.savefig('img/1_errorPerIter', format="svg")
# Plot Movement
pl.figure(2)
# pl.subplot(2,2,2)
pl.title("2D Position [m]")
j = 0
for uav in matLogs:
pl.scatter(uav[0, 0],
uav[0, 1],
color=uavColours[j],
marker='+',
s=50,
label='q' + str(j + 1))
pl.plot(uav[:, 0], uav[:, 1], color=uavColours[j])
j += 1
for j in range(FC.numEdges):
edgeTail = 0
edgeHead = 0
for i in range(FC.numAgents):
action = FC.CoIncMatx[i, j]
if action == -1:
edgeTail = i
elif action == 1:
edgeHead = i
if edgeHead != edgeTail:
x1 = FC.dronePositions[edgeHead, 0]
x2 = FC.dronePositions[edgeTail, 0]
y1 = FC.dronePositions[edgeHead, 1]
y2 = FC.dronePositions[edgeTail, 1]
# print(x1, x2)
pl.plot([x1, x2], [y1, y2], 'r--', lw=2)
pl.axis('equal')
pl.xlabel("East")
pl.ylabel("South")
pl.legend()
pl.grid()
pl.savefig('img/1_FormationPositions', format="svg")
pl.pause(0)