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 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
showNumber(x)
else:
for y in range(5, 0, -1): #countdown
showNumber(y)
time.sleep(0.9)
showNumber(00)
n = 0
m = []
while(n<15):
vel = lidar.getVelocity() #get lidar data
velPos = abs(vel) #clean up data
print(velPos)
m.append(velPos)
n += 1
time.sleep(.1)
dist = lidar.getDistance()
velMax = max(m)
if velMax > 99: # ''
velMax %= 10
velMax *= 1.60934 #in MPH
velMax = round(velMax)
print(m)
print("Max speed recoded: " + str(velMax))
firebase.post('/data', { "Distance":str(dist), "velocity":str(velMax), "User ID":x, "Date":today}) #post to database
j = 0
while(midButtonPos == True):
midButtonPos =GPIO.input(backButton)
while(j<=velMax):
showNumber(j) #print to display
j += 1
time.sleep(.002)
from lidar_lite import Lidar_Lite
lidar = Lidar_Lite()
connected = lidar.connect(1)
if connected < -1:
print "Not Connected"
print lidar.getDistance()
print lidar.getVelocity()
# main body
with picamera.PiCamera() as camera:
if os.stat("/media/pi/WEIZHE/capstone101/16thRun.csv").st_size == 0:
# creating a csv file with six columns
file.write("Time,Distance(m),Speed(km/h),Relative_Speed(km/h),Rear_Speed(km/h),Safe_Distance(m)\n")
while True:
now = dt.datetime.now()
# reading the speed data from sensoduino and distance data from LIDAR-Lite v3
read_serial = float(ser.readline())
distance = float(lidar.getDistance())
# Giving condition to data from LIDAR-Lite
if distance < 0:
distance = 0
# convert the distance into meter
distance_in_m = distance/100
print("Distance to target = %s" % (distance))
# Using the continous distance data to calculate the relative speed of rear vehicle using 1 second rule
i += 1
if i%2 == 0:
from picamera import PiCamera
from time import sleep
from lidar_lite import Lidar_Lite
camera = PiCamera()
camera.resolution = (1024, 768)
lidar = Lidar_Lite()
connected = lidar.connect(1)
if connected < -1:
print("Not Connected")
camera.start_preview(fullscreen=False, window=(100, 20, 640, 480))
while True:
print(lidar.getDistance())
print("finish test")
#Todo: remove once all sensors are on Arduino.
ser=serial.Serial(port='/dev/ttyACM0',baudrate=4800)
lidar=Lidar_Lite()
connected=lidar.connect(1)
if connected<-1:
raise valueError('Lidar not connected.')
while True:
GPIO.output(ERROR_PIN,True) # If this is alive, then it means loop is running.
switchState = GPIO.input(SWITCH_PIN)
if switchState: # This means you have to record.
GPIO.output(STATUS_PIN,status)
status = not status
ser.flushInput()
ser.flush()
serialLine=ser.readline()
lidarData=lidar.getDistance()
arduinoData=serialLine.split()
timeNow=datetime.datetime.now()
dataTime=timeNow.strftime('%m-%d-%Y-%H-%M-%S')
if len(arduinoData) !=3:
logFile = open(logFileName,'a')
logFile.write("Could not write data at time " + str(dataTime) + " Arduino data: " + ''.join(arduinoData) + "\n")
logFile.close()
continue # Incomplete data
dataList = [dataTime] + arduinoData + [lidarData]
#print dataList
#dataFile = open('/home/pi/data/current_trip.csv','a')
#dataWriter = csv.writer(dataFile)
#dataWriter.writerow(dataList)
#dataFile.close()
lidar = Lidar_Lite()
connected = lidar.connect(1)
if connected < -1:
print("Not Connected")
l_samples = {}
camera.start_preview(fullscreen=False, window=(100, 20, 640, 480))
for i in range(5):
try:
input("sample number {}".format(i + 1))
except SyntaxError:
pass
s = []
for j in range(5):
s.append(lidar.getDistance())
t = datetime.now().time()
l_samples[str(t)] = (np.median(s))
camera.capture('{}.jpg'.format(str(t)))
#sleep(2)
print(l_samples)
with open('data.json', 'w') as fp:
#feeds = json.load(fp)
json.dump(l_samples, fp)
#camera.capture('test1_{0}_{1}.jpg'.format(time, date))
#camera.capture("{}.jpg".format(time))
print("finish test")
from lidar_lite import Lidar_Lite lidar = Lidar_Lite() connected = lidar.connect(1) if connected < -1: print "Not Connected" print lidar.getDistance() print lidar.getVelocity()
if __name__ == '__main__':
lidar = Lidar_Lite()
connected = lidar.connect(1)
if connected < -1:
print("Not Connected")
exit()
vehicle = connect('udp:127.0.0.1:14551', wait_ready=True)
vehicle.mode = VehicleMode('GUIDED')
vehicle.armed = True
time.sleep(5)
if vehicle.armed:
print("Taking off")
vehicle.simple_takeoff(10)
time.sleep(15)
take_pic()
height = lidar.getDistance()
print("Height from LiDAR: ", height)
time.sleep(4)
print("Sea level alt: ", vehicle.location.global_frame.alt)
print("Global location: ", vehicle.location.global_frame)
print("Battery level: ", vehicle.battery.level)
else:
print("Vehicle not armed")
print("Landing")
vehicle.mode = VehicleMode('LAND')
time.sleep(15)
vehicle.close()
view = 0
ldata = [0,0,0,0,0]
lpos = [1312,1282,1252,1222,1192]
while 1:
#'RF','LF','FF','PX','PY','LR','DF','PR'
rawdata = readsensor()
PServo.MoveServo(5,lpos[lc])
if lc < 4:
lc += 1
else:
lc =0
ldata[lc] = lidar.getDistance()
print (ldata[lc],":", lc)
sensordata = {
"US" : int(rawdata[0]),
"LF" : int(rawdata[1]),
"RF" : int(rawdata[2])}
print sensordata
if c > 50:
s = 0
l = 0
r = 0
c = 0
c += 1
espeak.synth("Clear the area")
if (ldata[3] > 65 and sensordata["LF"] > 40 and sensordata["RF"] > 40):
class Drone:
def __init__(self):
self.alt = None
self.cam = None
self.lidar = None
self.logs = {}
self.h_logs = {}
self.vehicle = self._connect_drone()
self.vehicle.mode = VehicleMode('GUIDED')
self.f = open("dump/logs.txt", "w")
self.num_samples = 0
self.num_rows = 0
self.row_size = 0
def _connect_drone(self):
'''
Connect to the drone's control unit (Pixhawk4)
:return: dronekit lib object.
'''
print("connecting...")
print("Mode: ", MODE)
if MODE == DEBUG:
str = DEBUG_CONNECTION_STR
print(str)
else: # MODE == LIVE:
str = LIVE_CONNECTION_STR
vehicle = connect(str, wait_ready=True)
print("connected successfully")
return vehicle
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 take_off(self, height):
'''
Arming the drone and taking off to the given height.
:param height: in meters
'''
if not self._arming():
return
print("Taking off...")
self.vehicle.simple_takeoff(height)
self.alt = height
time.sleep(height * 1.4)
self.goto(-14.14, 14.14) #TODO: Delete this line
def land(self):
'''
Landing.
'''
print("Landing...")
self.vehicle.mode = VehicleMode('LAND')
time.sleep(20)
def goto(self, dNorth, dEast):
'''
Go x meters to North and y meters to East.
'''
currentLocation = self.vehicle.location.global_relative_frame
targetLocation = get_location_metres(currentLocation, dNorth, dEast,
self.alt)
targetDistance = get_distance_metres(currentLocation, targetLocation)
self.vehicle.simple_goto(targetLocation)
while self.vehicle.mode.name == "GUIDED": # Stop action if we are no longer in guided mode.
remainingDistance = get_distance_metres(self.vehicle.location.global_frame, targetLocation)
print("Distance to target: ", remainingDistance, ", Heading: ",
self.vehicle.heading)
# if remainingDistance <= targetDistance * 0.05: # Just below target, in case of undershoot.
if remainingDistance <= 0.4:
print("Reached target")
break
time.sleep(2)
def take_sample(self):
'''
Capture image and takes height sample with the Lidar sensor.
'''
print("taking sample...")
if MODE == DEBUG:
sea_level_alt = self.vehicle.location.global_frame.alt
print("sea level alt:" + str(sea_level_alt))
return
# *** LiDAR ****
self.num_samples += 1
s = []
for j in range(5):
s.append(self.lidar.getDistance())
t = datetime.now().time()
sea_level_alt = self.vehicle.location.global_frame.alt
height_median = np.median(s) / 100
relative_alt = sea_level_alt - height_median
# self.h_logs[str(t)] = (height_median, sea_level_alt, relative_alt)
self.h_logs["s_" + str(self.num_samples)] = {"Time" : str(t), "Height" : str(relative_alt), "Hading" : str(self.vehicle.heading)}
# ==== write to file - DEBUG ====
self.f.write(str(self.num_samples) + "- LiD height:" + str(height_median) + "| Sea level:" + str(
sea_level_alt) + "| Relative: " + str(relative_alt) + "| Heading: " +
str(self.vehicle.heading) +'\n')
# *** Camera ****
self.cam.capture('dump/{}.jpg'.format("im_"+str(self.num_samples)))
time.sleep(2)
def close_connection(self):
'''
Close the connection of the dronekit object and save the flight logs.
'''
if MODE == LIVE:
self.logs["Logs"] = self.h_logs
self.logs["num_imgs"] = self.num_samples
self.logs["num_rows"] = self.num_rows
self.logs["row_size"] = self.row_size
with open('dump/logs.json', 'w') as fp:
json.dump(self.logs, fp)
self.vehicle.close()
self.f.close()
time.sleep(2)
print("drone disconnected")
def _scan_straight(self, height, length, width, adv_size):
raw_input("Place the drone in the South-East corner of the scan "
+ "area, and press any key to start the scan")
self.take_off(height)
for i in range(int(length // (2 * adv_size))):
self.take_sample()
for j in range(int(width // adv_size)):
self.goto(0, -adv_size)
self.take_sample()
self.goto(adv_size, 0)
self.take_sample()
for j in range(int(width // adv_size)):
self.goto(0, adv_size)
self.take_sample()
self.goto(adv_size, 0)
takeoff_return = int(length // adv_size) * adv_size + adv_size
self.goto(int(-takeoff_return), 0)
self.land()
raw_input("Press any key to turn off the drone")
self.close_connection()
def _scan_diagonal(self, height, length, width, north, east, adv_size):
fly_size = math.sqrt(adv_size ** 2 / 2)
self.take_off(height)
for i in range(int(length // (2 * adv_size))):
self.take_sample()
for j in range(int(width // adv_size)):
self.goto(north * fly_size, -fly_size)
self.take_sample()
self.goto(fly_size, east * fly_size)
self.take_sample()
time.sleep(7)
for j in range(int(width // adv_size)):
self.goto(-north * fly_size, fly_size)
self.take_sample()
self.goto(fly_size, east * fly_size)
takeoff_return = int(length // adv_size) * adv_size
fly_return = math.sqrt(takeoff_return ** 2 / 2)
self.goto(-fly_return, -east * fly_return)
self.land()
raw_input("Press any key to turn off the drone")
self.close_connection()
def scan(self, height, width, length):
pic_size = get_pic_size(height)
adv_size = 0.6 * pic_size
self.num_rows = int(length // (2 * adv_size))
self.row_size = int(width // adv_size)
found = False
while not found:
print("Select the orientation of the scan area: \n" +
"1. North \n" +
"2. North - East \n" +
"3. North - West")
direction = int(input())
if direction == 1:
found = True
self._scan_straight(height, length, width, adv_size)
elif direction == 2:
found = True
raw_input("Place the drone in the Southern corner of the scan "
+ "area, and press any key to start the scan")
self._scan_diagonal(height, length, width, 1, 1, adv_size)
elif direction == 3:
found = True
raw_input("Place the drone in the Eastern corner of the scan "
+ "area, and press any key to start the scan")
self._scan_diagonal(height, length, width, -1, -1, adv_size)
else:
print("Bad direction")
Location = "SJSU West"
Row = "B"
SpaceTopic = "car/space"
Floor = 15
if connected < -1:
print("Not Connected")
else:
print("Lidar scan bgins")
while True:
space = 0
for i in range(7): # 7 different positions for servo motor to stop
pwm.setPWM(0, 0, position[i])
time.sleep(1)
distance = lidar.getDistance()
if (distance < 200):
space = space + 1
print("Currnet degree = %s" % degree[i])
for j in range(3): # Lidar will collect data 3 times for one position
distance = lidar.getDistance()
print("Distance to target = %s" % (distance))
if (distance < 1000):
print("Start Recognizing at position:", j + 1)
camera.capture(rawCapture, format="bgr")
image = cv2.cvtColor(rawCapture.array, cv2.COLOR_BGR2GRAY)
results = alpr.recognize_ndarray(image)
for plate in results['results']:
for candidate in plate['candidates']:
# The Prefix is not necessary for the program.
if (candidate['confidence'] > 85):
for i in range(0, 32):
try:
strData += chr(bus.read_byte(comms900))
time.sleep(0.05)
except IOError:
print("Failed Read")
subprocess.call(['i2cdetect', '-y', '1'])
return
time.sleep(0.1)
return strData
def convertReadStringToIntArray(myStr):
readStr = readStr.split("_")[0]
strArray = readStr.split(" ")
intArray = [int(x) for x in strArray]
return intArray
while True:
writeNumber(100)
readStr = readString()
intArray = convertReadStringToIntArray(readStr)
print "Lidar info: ", lidar.getDistance(), lidar.getVelocity()
print("intArray", intArray)
time.sleep(.1)
# Gear display
if reverse == True:
gear = 0x1D
else:
gear = 0x1B
# For Lead Vehicle Start Alert - track lowest distance achieved while throttle is not pressed.
if throttle == True:
lastDistance = 0
lvsaTriggered = False
if esObstacleData == 0x04:
esObstacleData = 0
# Get the distance.
try:
distance = lidar.getDistance() / 30.48
prevDst = distance
except:
distance = prevDst
pass
#print("LD: " + str(lastDistance) + ", D: " + str(distance))
# Set lowest distance to object if throttle is false and a forward gear is chosen
if throttle == False and lvsaTriggered == False and reverse == False:
if lastDistance == 0:
lastDistance = distance
if distance < lastDistance:
lastDistance = distance
if distance - lastDistance > 15:
lidarController[i-1].off()
lidarController[i].on()
if i == 0:
self.getFR()
elif i == 1:
self.getFL()
elif i == 2:
self.getBL()
elif i == 3:
self.getBR()
print( str(i) + " " + str(lidarController[i].value))
j = 0
while j != 50:
try:
print("Distance: "+ str(i) + " " + str(lidar.getDistance()))
sleep(0.01)
except:
print("\tRemote I/O Error Occur, Check connection")
sleep(0.01)
j += 1
def getFR():
print("FRONT RIGHT")
return lidar.getDistance()
def getFL():
print("FRONT LEFT")
return lidar.getDistance()
def getBL():
