def main():
from lidar_lite import Lidar_Lite
lidar = Lidar_Lite()
connected = lidar.connect(1)
if connected < -1:
print("Not Connected")
return
else:
print("Connected")
for i in range(100):
distance = lidar.getDistance()
print("Distance to target = %s" % (distance))
if int(distance) < 300:
commandimage = 'raspistill -n --timeout 1 -h 183 -w 275 -o imgtmp.jpg'
os.system(commandimage)
device = open_ncs_device()
graph = load_graph(device)
img_draw = skimage.io.imread(ARGS.image)
img = pre_process_image(img_draw)
infer_image(graph, img, distance)
close_ncs_device(device, graph)
def takeoffLand():
print("connect...")
vehicle = connect('/dev/ttyUSB0', wait_ready=True)
vehicle.mode = VehicleMode('GUIDED')
vehicle.armed = True
sleep(5)
if not vehicle.armed:
print("Vehicle not armed")
return
print("tern on devices...")
camera = PiCamera()
camera.resolution = (1024, 768)
lidar = Lidar_Lite()
connected = lidar.connect(1)
if connected < -1:
print("Lidar not Connected")
return
takeSample(lidar, camera)
vehicle.close()
with open('takeoff_dump/takeoff_land_logs.json', 'w') as fp:
json.dump(l_samples, fp)
def takeoffLand(connect_str):
print("connect to: " + connect_str)
vehicle = connect('udp:172.29.113.100:14550', wait_ready=True)
vehicle.mode = VehicleMode('GUIDED')
vehicle.armed = True
sleep(5)
if not vehicle.armed:
print("Vehicle not armed")
return
print("tern on devices...")
camera = PiCamera()
camera.resolution = (1024, 768)
lidar = Lidar_Lite()
connected = lidar.connect(1)
if connected < -1:
print("Lidar not Connected")
return
print("Taking off")
vehicle.simple_takeoff(10)
sleep(15)
takeSample(lidar, camera)
takeSample(lidar, camera)
print("Landing")
vehicle.mode = VehicleMode('LAND')
sleep(15)
vehicle.close()
with open('takeoff_dump/takeoff_land_logs.json', 'w') as fp:
json.dump(l_samples, fp)
def range_dist():
sonar = False
lidar_sens = False # lidar sensing True, else EZ4 sonar or VL53L1X
if sonar:
# does not work well on grass
from smbus import SMBus
## i2cbus = SMBus(1)
while True:
try:
i2cbus = SMBus(1)
i2cbus.write_byte(0x70, 0x51)
time.sleep(0.12)
val = i2cbus.read_word_data(0x70, 0xE1)
distance_in_cm = val >> 8 | (val & 0x0F) << 8
## print distance_in_cm, 'cm'
print(distance_in_cm, 'cm', file=f)
msg_sensor(distance_in_cm, 25, 400), #sonar facing down
except IOError as err:
print(err)
time.sleep(0.1)
if lidar_sens:
from lidar_lite import Lidar_Lite
lidar = Lidar_Lite()
connected = lidar.connect(1)
if connected < 0:
print("\nlidar not connected")
else:
print("\nlidar connected")
while True:
dist = lidar.getDistance()
print(dist, 'cm')
print(dist, 'cm', file=f)
msg_sensor(dist, 25, 700), #lidar facing down
time.sleep(0.2)
else:
# does not work in sunlight
import VL53L1X
tof = VL53L1X.VL53L1X(i2c_bus=1, i2c_address=0x29)
tof.open() # Initialise the i2c bus and configure the sensor
tof.start_ranging(
2
) # Start ranging, 1 = Short Range, 2 = Medium Range, 3 = Long Range
# Short range max:1.3m, Medium range max:3m, Long range max:4m
while True:
distance_in_cm = tof.get_distance(
) / 10 # Grab the range in cm (mm)
time.sleep(0.2) # timeout mavlink rangefinder = 500 ms
print(distance_in_cm, 'cm', file=f)
msg_sensor(distance_in_cm, 25, 250), #sensor facing down
tof.stop_ranging() # Stop ranging
def _arming(self):
self.vehicle.armed = True
self.alt = 0
time.sleep(5)
if not self.vehicle.armed:
print("Vehicle not armed")
return False
if MODE == LIVE:
print("Activating devices...")
self.cam = PiCamera()
self.cam.resolution = CAM_RES
self.lidar = Lidar_Lite()
connected = self.lidar.connect(1)
if connected < -1: #TODO: check this cond
print("Lidar not Connected")
return False
print("Armed successfully")
return True
def four_points(connect_str):
print("connect to: " + connect_str)
vehicle = connect(connect_str, wait_ready=True)
vehicle.mode = VehicleMode('GUIDED')
vehicle.armed = True
sleep(5)
if not vehicle.armed:
print("Vehicle not armed")
return
print("tern on devices...")
camera = PiCamera()
camera.resolution = (1024, 768)
lidar = Lidar_Lite()
connected = lidar.connect(1)
if connected < -1:
print("Lidar not Connected")
return
print("Taking off")
vehicle.simple_takeoff(10)
time.sleep(15)
goto(10, 0, vehicle.simple_goto)
point_handler(1, camera, lidar)
goto(0, 10, vehicle.simple_goto)
point_handler(2, camera, lidar)
goto(-10, 0, vehicle.simple_goto)
point_handler(3, camera, lidar)
goto(0, -10, vehicle.simple_goto)
point_handler(4, camera, lidar)
print("Landing")
vehicle.mode = VehicleMode('LAND')
sleep(15)
raw_input("Press any key to turn off the drone")
vehicle.close()
with open('4pts_dump/logs.json', 'w') as fp:
json.dump(l_samples, fp)
def takeoffLand():
print("connect...")
vehicle = connect('/dev/ttyUSB0', wait_ready=True)
vehicle.mode = VehicleMode('GUIDED')
vehicle.armed = True
sleep(5)
if not vehicle.armed:
print("Vehicle not armed")
return
print("turn on devices...")
camera = PiCamera()
camera.resolution = (1024, 768)
lidar = Lidar_Lite()
connected = lidar.connect(1)
if connected < -1:
print("Lidar not Connected")
return
print("Taking off")
vehicle.simple_takeoff(10)
sleep(15)
takeSample(lidar, camera)
takeSample(lidar, camera)
print("Landing")
vehicle.mode = VehicleMode('LAND')
sleep(15)
raw_input("Press any key to turn off the drone")
vehicle.close()
with open('takeoff_dump/takeoff_land_logs.json', 'w') as fp:
json.dump(l_samples, fp)
from lidar_lite import Lidar_Lite
lidar = Lidar_Lite()
connected = lidar.connect(1)
tooClose = False
def scan_li(c):
for i in range(c):
distance = lidar.getDistance()
print ("Distance to target = ", distance)
if (int(distance) < 20):
tooClose = True
print ("\nToo Close!!! Back Off!!\n")
if (int(distance) > 35):
tooClose = False
print("\nProceed\n")
if (__name__) == "__main__":
if connected < -1:
print ("Not Connected")
else:
print ("Connected")
while (connected >= -1):
def init(self):
self.lidar = Lidar_Lite()
if connected < -1:
print("Not Connected")
