def collisionProbability(distance): #drone.hover() a1 = sensor.get_distance() a2 = sensor.get_distance() a3 = sensor.get_distance() event = 0 print "Distance found: ", distance print "amostra1: ", a1, "amostra2: ", a2, "amostra3: ", a3 if a1 > 50: event = event + 1 if a2 > 50: event = event + 1 if a3 > 50: event = event + 1 collision = event * 33 # (event / 3) * 100 => Percentage of measure is correct print "event: ", event print "No collision occurs: ", collision, "%" if collision == 99: t = distance / 50 # speed: 50 cm/s (10% of max speed) t = t + 0.5 # Get out of inertia #drone.moveForward(0.1) print "Drone is move forward with 50cm/s durant", t, "s" time.sleep(t) #drone.hover() return True elif collision == 66: t = distance / 50 # speed: 50 cm/s (10% of max speed) #drone.moveForward(0.1) print "Drone is move forward with 50cm/s durant", t, "s" time.sleep(t) #drone.hover() return True else: return False
def AG(distance): "Population - random individual" #moveForward(val) #moveBackward(val) #moveLeft(val) #moveRight(val) #moveUp(val) #moveDown(val) turnLeft = 0 turnRight = 1 FITNESS = collisionProbability(distance) print "FITNESS:", FITNESS while not FITNESS: individual = random.randint(0, 1) if individual == 1: #drone.turnAngle(-90,0.5,1) time.sleep(2) #drone.hover() distance = sensor.get_distance() FITNESS = collisionProbability(distance) print "turn left" print "FITNESS: ", FITNESS else: #drone.turnAngle(90,0.5,1) time.sleep(2) #drone.hover() distance = sensor.get_distance() FITNESS = collisionProbability(distance) print "turn right" print "FITNESS: ", FITNESS
def HC(distance): "Population - random individual" #moveBackward(val) #moveLeft(val) #moveRight(val) #moveUp(val) #moveDown(val) turnRight = 0 turnLeft = 1 moveForward = 2 STUCK = 0 COLLISION = True while COLLISION and STUCK <= 3: individual = random.randint(0, 2) if individual == 1: print "Im turning left" drone.turnAngle(-90, 1, 1) distance = sensor.get_distance() COLLISION = collisionProbability(distance) print "COLLISION: ", COLLISION elif individual == 0: print "Im turning right" drone.turnAngle(90, 1, 1) # time.sleep(1) # drone.hover() distance = sensor.get_distance() COLLISION = collisionProbability(distance) print "COLLISION: ", COLLISION else: print "Im turning forward: " distance = sensor.get_distance() COLLISION = collisionProbability(distance) print "COLLISION: ", COLLISION STUCK = STUCK + 1 if STUCK > 3: print "Oh my God , Im stuck!" drone.moveUp(0.3) time.sleep(2) drone.hover() distance = sensor.get_distance() COLLISION = collisionProbability(distance) if not COLLISION: drone.moveDown(0.3) time.sleep(2) drone.hover() else: drone.land() sys.exit()
def collisionProbability(distance): print "Im checking distance..." distance = sensor.get_distance() print "It is: ", distance, "cm" pmenor50 = 0.95 pmaior50menor70 = 0.75 pmaior70 = 0.5 pobstsala = 0.5 if distance < 50: pcollision = pmenor50 * pobstsala * 100 # speed: 50 cm/s (10% of max speed) elif distance >= 50 and distance < 70: pcollision = pmaior50menor70 * pobstsala * 100 else: pcollision = pmaior70 * pobstsala * 100 print pcollision, "% of NO COLLISION" if pcollision < 45 and pcollision > 25: t = distance / 50 + 1 print "Yaaaay! I can go forward!!" drone.doggyHop() time.sleep(2) # Get out of inertia drone.moveForward(0.1) print "Im moving forward with 50cm/s during", t, "s" time.sleep(t) drone.hover() STUCK = 0 return False elif pcollision <= 25: t = distance / 50 + 1.5 print "Yaaaay! I can go forward!!" drone.doggyHop() time.sleep(2) # Get out of inertia drone.moveForward(0.1) print "Im moving forward with 50cm/s during", t, "s" time.sleep(t) drone.hover() STUCK = 0 return False else: print "Oh no! I cant go forward" drone.doggyWag() time.sleep(2) print "I found an obstacle at ", distance, "cm" return True
dweet_data = {'Name': 'Sensors'} def send_dweet(data = dweet_data, thing_name = Dweet_Thing_Name): if Dweet_Thing_Name == "": print ("Error: Thing name not defined. Use some random name for your Thing") sys.exit(0) r = requests.post(Dweet_For_URL, data=dweet_data) return r if __name__ == "__main__": try: print ("Dweeting various sensor readings") while True: distance = hcsr04.get_distance() print ("Distance is {0} cm".format(distance)) dweet_data.update({'Distance': distance}) dweet_response = send_dweet(Dweet_Thing_Name, dweet_data) if (dweet_response.status_code == requests.codes.ok): print ("Dweet successful. Please check {0}{1}".format(Dweet_Follow_URL, Dweet_Thing_Name)) else: print ("Dweet failed, please check your data") time.sleep(1) except KeyboardInterrupt: hcsr04.cleanup() print ("Done")
0.1 ) # Wait until the drone is really flying (not in landed-mode anymore) ######## Mainprogram begin ############# print "The Drone is flying now, land it with any key" drone.moveUp(0.1) time.sleep(1) drone.hover() print "Are you ready drone?" print "Yeah!" drone.doggyHop() time.sleep(1) ######### auto flight ############# try: while True: distance = sensor.get_distance() # Get distance from sensor print "Hill Climbing is running..." HC(distance) print "Batterie: " + str(drone.getBattery()[0]) + "% " + str( drone.getBattery()[1]) # Gives a battery-status except KeyboardInterrupt: print "User cancelled" except: print "Unexpected error:", sys.exit() raise finally: print "Drone is landing"
if Dweet_Thing_Name == "": print( "Error: Thing name not defined. Use some random name for your Thing" ) sys.exit(0) r = requests.post(Dweet_For_URL, data=dweet_data) return r if __name__ == "__main__": try: print("Dweeting various sensor readings") while True: distance = hcsr04.get_distance() print("Distance is {0} cm".format(distance)) dweet_data.update({'Distance': distance}) dweet_response = send_dweet(Dweet_Thing_Name, dweet_data) if (dweet_response.status_code == requests.codes.ok): print("Dweet successful. Please check {0}{1}".format( Dweet_Follow_URL, Dweet_Thing_Name)) else: print("Dweet failed, please check your data") time.sleep(1) except KeyboardInterrupt: hcsr04.cleanup() print("Done")
from car import car_control from hcsr04 import get_distance from time import sleep c = car_control() try: while 1: dis = get_distance(27, 17) if dis > 30: c.forward() else: #print(dis) c.left() #sleep(0.1) except KeyboardInterrupt: c.still() c = car_control() print('關閉程式 ') #c.__version__() #forward() c.still()