def __init__(self): #drone manuver limits self.maxVelocity = 100 #m/s self.maxAccel = 1000 #current state self.position = point3D(0,0,0) self.velocity = point3D() self.acceleration = point3D() self.distToTarget = 0 self.uid = None #externaly set targets self.targetPositon = point3D() #internally set targets to reach targets #note that we are using a velocity controller which will always try to #move to the target position. self.commandVelocity = point3D()
def updateDrone(drone, deltaTime, canvas): tartag = drone.uid + "_target" lastPos = drone.position drone.update_state(deltaTime) canvas.move(drone.uid, drone.position.x - lastPos.x, drone.position.y - lastPos.y) if drone.distToTarget < 5: ltp = drone.targetPositon drone.set_target_pos(point3D(random.random()*750+25, random.random()*750+25)) canvas.move(tartag, drone.get_target_pos().x - ltp.x, drone.get_target_pos().y - ltp.y)
self.distToTarget = (self.position - self.targetPositon).magnitude() def tick(self, deltaTime): self.update_state(deltaTime) if __name__ == "__main__": import time import Tkinter as tk import random random.seed() #then we run some tests. drone1 = drone() drone1.set_target_pos(point3D(100,100,0)) lastTime = time.time() window = tk.Tk() canvas = tk.Canvas(window, width = 800, height = 800) canvas.pack() canvas.create_oval(drone1.position.x,drone1.position.y,drone1.position.x+10,drone1.position.y+10,fill="red", tag='drone') canvas.create_oval(drone1.get_target_pos().x, drone1.get_target_pos().y, drone1.get_target_pos().x + 5, drone1.get_target_pos().y + 5, fill="blue", tag='target') lastPos = drone1.position while True: if drone1.distToTarget < 10: print "moving target" ltp = drone1.get_target_pos() drone1.set_target_pos(point3D(random.random()*750+25, random.random()*750+25)) canvas.move('target', drone1.get_target_pos().x - ltp.x, drone1.get_target_pos().y - ltp.y)
window = tk.Tk() canvas = tk.Canvas(window, width = 800, height = 800) canvas.pack() master = mcp() for i in range(100): master.addDrone() i = 0 for drone in master.drones: drone.set_uid("drone" + str(i)) tartag = drone.uid + "_target" drone.position = point3D(random.random()*750+25, random.random()*750+25) drone.set_target_pos(point3D(random.random()*750+25, random.random()*750+25)) canvas.create_oval(drone.position.x-5,drone.position.y-5,drone.position.x+5,drone.position.y+5,fill="red", tag=drone.uid) canvas.create_oval(drone.targetPositon.x-3, drone.targetPositon.y-3, drone.targetPositon.x + 3, drone.targetPositon.y + 3, fill="blue", tag=tartag) i = i+1 lastTime = time.time() deltaTime = 0 while True: for drone in master.drones: updateDrone(drone, deltaTime, canvas) canvas.after(20) canvas.update() deltaTime = time.time() - lastTime