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()
