def run(self):
""" Method that runs forever """
# Get the home postition
home = vehicle.location.global_relative_frame
home.alt = home.alt + 2
print home
lidar = Lidar_Lite()
#Attempt to connect to the lidar sensor(s)
connected = lidar.connect(1)
#If not able to connect then print command
if connected < -1:
print "Not Connected"#CS people do what you want here
#turn off gpio warnings
GPIO.setwarnings(False)
#Set GPIO enumeration Type (Pi vs Board)
GPIO.setmode(GPIO.BCM)
#turn off all sensors by pulling GPIO pins to 0
for i in range(0, NUMBER_OF_SENSORS):
GPIO.setup(GPIO_NUMBERS[i], GPIO.OUT)#setting gpio mode to output
GPIO.output(GPIO_NUMBERS[i], False)#set gpio pin to 0
# Infinite loop
while True:
# This line is sensor code
if self.hazCollision(lidar):
# store the previous mode so we can switch back into it later
previousMode = vehicle.mode.name
# Switch into GUIDED mode
print "Going from %s mode into GUIDED mode" % previousMode
vehicle.mode = VehicleMode("GUIDED")
# Set the airspeed to 2
print "Set target airspeed to 2"
vehicle.airspeed = 2
# Go toward the previous GPS location
print "Going towards home"
# Go toward home for 2 seconds
vehicle.simple_goto(home)
time.sleep(2)
# Go back to the previous mode
print "Going to previous mode: %s" % previousMode
vehicle.mode = VehicleMode(previousMode)
def run(self):
""" Method that runs forever """
lidar = Lidar_Lite()
# Get the home postition
home = vehicle.location.global_relative_frame
print home
# Attempt to connect to the lidar sensor(s)
connected = lidar.connect(1)
# If not able to connect then print command
if connected < -1:
print "Not Connected" # CS people do what you want here
# turn off gpio warnings
GPIO.setwarnings(False)
# Set GPIO enumeration Type (Pi vs Board)
GPIO.setmode(GPIO.BCM)
# turn off all sensors by pulling GPIO pins to 0
for i in range(0, NUMBER_OF_SENSORS):
GPIO.setup(GPIO_NUMBERS[i], GPIO.OUT) # setting gpio mode to output
GPIO.output(GPIO_NUMBERS[i], False) # set gpio pin to 0
# Infinite loop
while True:
# If we see a object we will collide with, avoid it
if self.hazCollision(lidar):
try:
# Acquire the lock on the GPS history queue
gpsHistoryQueue.dequeLock.acquire()
# If the GPS History Queue is populated, go back toward the last safe point
if len(gpsHistoryQueue) > 1:
# store the previous mode so we can switch back into it later
previousMode = vehicle.mode.name
# Switch into GUIDED mode
print "Going from %s mode into GUIDED mode" % previousMode
vehicle.mode = VehicleMode("GUIDED")
# Set the airspeed to 2
print "Set target airspeed to 2"
vehicle.airspeed = 2
# Go toward the previous GPS location
print "Going towards previous GPS point"
# Go toward the most recent point for 2 seconds
print "popping from HistoryQueue", len(gpsHistoryQueue)
safePoint = gpsHistoryQueue.popleft()
# Start going to the last safe point we recorded
vehicle.simple_goto(safePoint)
# Get the current time so that we'll advance for 2 seconds from now.
start = time.time()
# let this loop run until two seconds have elapsed from start
while (time.time() - start) < 2:
# get the current position of the drone
currentPosition = vehicle.location.global_relative_frame
# If the current position of the drone is equal to the point we earlier defined as safe,
# start going towards the next point.
if withinThreshold(
currentPosition, safePoint, HORIZONTAL_COLLISION_THRESHOLD, VERTICAL_COLLISION_THRESHOLD
):
# get the next safe point from the queue.
print "popping from HistoryQueue", len(gpsHistoryQueue)
safePoint = gpsHistoryQueue.popleft()
# We never want to have an empty queue, so make sure that we pop
# the safePoint back on if we need to
if len(gpsHistoryQueue) <= 0:
gpsHistoryQueue.append(safePoint)
# and head to it.
vehicle.simple_goto(safePoint)
# Go back to the previous mode
print "Going to previous mode: %s" % previousMode
vehicle.mode = VehicleMode(previousMode)
# If the deque is not populated and we find an obstacle,
# just go home
else:
safePoint = gpsHistoryQueue.popleft()
# Put the safePoint back in so we don't ever have an empty queue
gpsHistoryQueue.append(safePoint)
# Go toward last safe point for 2 seconds
vehicle.simple_goto(safePoint)
time.sleep(2)
# Release the lock on the queue when we're done
finally:
gpsHistoryQueue.dequeLock.release()
def tilt(angle):
us = angleToMicroseconds(angle)
servo.set_servo(TILT_SERVO_PIN, us)
def angleToMicroseconds(angle):
us = int(((angle + 90) * ((LEFT-RIGHT)/(180.0))) + RIGHT)
us -= (us % GRANULARITY_MICROSECONDS)
return us
def cleanup():
servo.stop_servo(PAN_SERVO_PIN)
servo.stop_servo(TILT_SERVO_PIN)
PWM.set_loglevel(PWM.LOG_LEVEL_ERRORS)
servo = PWM.Servo()
lidar = Lidar_Lite()
connected = lidar.connect(1)
if connected <= -1:
print "Not Connected"
exit(1)
print "Pan_Angle, Tilt_Angle, Distance (cm), Error"
INCREMENT = 2
for t in range(TILT_MIN_ANGLE, TILT_MAX_ANGLE+1, INCREMENT):
tilt(t)
for p in range(PAN_MIN_ANGLE, PAN_MAX_ANGLE+1, INCREMENT):
pan(p)
try:
time.sleep(SLEEP_TIME)
distance = lidar.getDistance()
def tilt(angle):
us = angleToMicroseconds(angle)
servo.set_servo(TILT_SERVO_PIN, us)
def angleToMicroseconds(angle):
us = int(((angle + 90) * ((LEFT-RIGHT)/(180.0))) + RIGHT)
us -= (us % GRANULARITY_MICROSECONDS)
return us
def cleanup():
servo.stop_servo(PAN_SERVO_PIN)
servo.stop_servo(TILT_SERVO_PIN)
PWM.set_loglevel(PWM.LOG_LEVEL_ERRORS)
servo = PWM.Servo()
lidar = Lidar_Lite()
connected = lidar.connect(1)
if connected <= -1:
print "Not Connected"
exit(1)
INCREMENT = 2
W = 1 + (PAN_MAX_ANGLE - PAN_MIN_ANGLE) / INCREMENT
H = 1 + (TILT_MAX_ANGLE - TILT_MIN_ANGLE) / INCREMENT
MaxDepth = 0
depths = [[0 for x in range(W+1)] for y in range(H+1)]
i=0;
for t in range(TILT_MIN_ANGLE, TILT_MAX_ANGLE+1, INCREMENT):
tilt(t)
def run(self):
""" Method that runs forever """
# Start the C sensor code
# The C sensor code needs to be run as sudo because it uses
# wiringPi which needs root permissions, so make sure to run this python
# module as sudo
#sensorProcess = subprocess.Popen([r'./multipleSensors', '4'], stdout=subprocess.PIPE, stdin=subprocess.PIPE)
lidar = Lidar_Lite()
#Attempt to connect to the lidar sensor(s)
connected = lidar.connect(1)
#If not able to connect then print command
if connected < -1:
print "Not Connected"#CS people do what you want here
#turn off gpio warnings
GPIO.setwarnings(False)
#Set GPIO enumeration Type (Pi vs Board)
GPIO.setmode(GPIO.BCM)
#turn off all sensors by pulling GPIO pins to 0
for i in range(0, NUMBER_OF_SENSORS):
GPIO.setup(GPIO_NUMBERS[i], GPIO.OUT)#setting gpio mode to output
GPIO.output(GPIO_NUMBERS[i], False)#set gpio pin to 0
# Infinite loop
while True:
# This line is sensor code
if self.hazCollision(lidar) and len(gpsHistoryQueue) > 0:
try:
gpsHistoryQueue.dequeLock.acquire()
# store the previous mode so we can switch back into it later
previousMode = vehicle.mode.name
# Switch into GUIDED mode
print "Going from %s mode into GUIDED mode" % previousMode
vehicle.mode = VehicleMode("GUIDED")
# Set the airspeed to 2
print "Set target airspeed to 2"
vehicle.airspeed = 2
# Go toward the previous GPS location
print "Going towards previous GPS point"
# Go toward the most recent point for 2 second
print "popping from HistoryQueue", len(gpsHistoryQueue)
safePoint = gpsHistoryQueue.popleft()
vehicle.simple_goto(safePoint)
#Get the current time so that we'll advance for 2 seconds from now.
start = time.time()
#let this loop run until two seconds have elapsed from start
while((time.time() - start) < 2 and len(gpsHistoryQueue) > 0):
#get the current position of the drone
currentPosition = vehicle.location.global_relative_frame
#If the current position of the drone is equal to the point we earlier defined as safe,
#start going towards the next point.
if(self.withinThreshold(currentPosition,safePoint)):
#get the next safe point from the queue.
print "popping from HistoryQueue", len(gpsHistoryQueue)
safePoint = gpsHistoryQueue.popleft()
#and head to it.
vehicle.simple_goto(safePoint)
# Go back to the previous mode
print "Going to previous mode: %s" % previousMode
vehicle.mode = VehicleMode(previousMode)
finally:
gpsHistoryQueue.dequeLock.release()
