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)