Skip to content

Commit

Permalink
Tested
Browse files Browse the repository at this point in the history
  • Loading branch information
@shuhan
shuhan committed Aug 31, 2019
1 parent e188f51 commit f318fcf
Show file tree
Hide file tree
Showing 3 changed files with 43 additions and 33 deletions.
46 changes: 28 additions & 18 deletions src/game_master/scripts/autonomous_controller.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,9 +50,9 @@ class AutonomousController:
INTENT_PREPARE_TO_LAND = 8
INTENT_LAND = 9

SEARCH_HEIGHT = 1.5
SEARCH_HEIGHT = 1.6
TAKE_OFF_HEIGHT = 1.3
LANDING_HEIGHT = 1.0
LANDING_HEIGHT = 0.8
MIN_BATTERY_LEVEL = 15

def __init__(self):
Expand Down Expand Up @@ -175,7 +175,7 @@ def begin_search_swipe(self, func, height):
'''
self.groundSwipeCount = 0
if callable(func):
self.goalTracker.setHeightTarget(height, False, func)
self.goalTracker.setHeightTarget(height, True, func)

def take_a_photo(self, name):
home = expanduser("~")
Expand All @@ -187,7 +187,10 @@ def return_to_base(self):
if self.intent < self.INTENT_RETURN_TO_BASE:
self.intent = self.INTENT_RETURN_TO_BASE
print("Looking for gate\n\n")
self.begin_search_swipe(self.look_for_landing_gate, self.SEARCH_HEIGHT)
self.goalTracker.setOrientationTarget(self.visualScale.southWall, False, self.begin_look_for_landing_gate)

def begin_look_for_landing_gate(self):
self.begin_search_swipe(self.look_for_landing_gate, self.SEARCH_HEIGHT)
#-----------------------------------------------------------------------
# End Common Functions
#-----------------------------------------------------------------------
Expand Down Expand Up @@ -293,17 +296,31 @@ def look_for_landing_gate(self):
self.drone.cameraControl(-15, 0)
elif self.groundSwipeCount == 1:
self.drone.cameraControl(-30, 0)
elif self.groundSwipeCount == 2:
self.drone.cameraControl(-50, 0)

if self.groundSwipeCount < 3:
if self.groundSwipeCount < 2:
self.goalTracker.setSwipeTarget(self.look_for_landing_gate)
else:
self.landing_gate_was_missing()
self.groundSwipeCount += 1

def landing_gate_was_missing(self):
'''
Handle special case of landing
'''
# first cancle all previous targets
self.goalTracker.reset()
self.intent = self.INTENT_RETURNING_TO_BASE

if self.vision.eastGateDistance is None and self.vision.northGateDistance is None:
print("Couldn't find landing gate\n\n")
# If gate marker isn't visible it's safe to just land where you are
self.drone.land()

self.groundSwipeCount += 1
elif self.vision.eastGateDistance is None:
print("Navigating to north gate\n\n")
self.goalTracker.setOrientationTarget(self.vision.northGateAngle, False, self.wait_and_navigate_to_north)
else:
print("Navigating to east gate\n\n")
self.goalTracker.setOrientationTarget(self.vision.eastGateAngle, False, self.wait_and_navigate_to_east)

def wait_and_navigate_to_north(self):
self.landingGateTarget = 'north'
Expand Down Expand Up @@ -618,19 +635,12 @@ def run(self):
target.angular.z = self.vision.landingPadOrientation
self.target_pub.publish(target)

elif self.intent == self.INTENT_RETURN_TO_BASE and (self.vision.eastGateVisible or self.vision.northGateVisible):
elif self.intent == self.INTENT_RETURN_TO_BASE and (self.vision.eastGateDistance is not None and self.vision.northGateDistance is not None):
# first cancle all previous targets
self.goalTracker.reset()
self.intent = self.INTENT_RETURNING_TO_BASE

if not self.vision.eastGateVisible:
print("Navigating to north gate\n\n")
self.goalTracker.setOrientationTarget(self.vision.northGateAngle, False, self.wait_and_navigate_to_north)
elif not self.vision.northGateVisible:
print("Navigating to east gate\n\n")
self.goalTracker.setOrientationTarget(self.vision.eastGateAngle, False, self.wait_and_navigate_to_east)
# Both gates are visible
elif self.vision.eastFrameDistance > self.vision.northFrameDistance:
if self.vision.eastGateDistance > self.vision.northGateDistance:
print("Navigating to north gate\n\n")
self.goalTracker.setOrientationTarget(self.vision.northGateAngle, False, self.wait_and_navigate_to_north)
else:
Expand Down
28 changes: 14 additions & 14 deletions src/game_master/scripts/target_tracker.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -209,7 +209,7 @@ def adjustPointTarget(self):
errorVector = self.getPointError(currentPoint, targetPoint)
errorMagnitude = np.linalg.norm(errorVector)

#Lets try to set a 150 pixel error max we can make it configureable
#Lets try to set a 100 pixel error max we can make it configureable
if errorMagnitude > 100 or self.drone.camera_tilt > -70:

if abs(errorVector[0]) > 40:
Expand All @@ -218,21 +218,21 @@ def adjustPointTarget(self):
#self.drone.moveY(moveY)
self.drone.turn(moveY)
else: # Lets control the angle and forward seperately
if errorVector[1] > 0:
if errorVector[1] > 0 or self.drone.camera_tilt <= -70:
signX = errorVector[1]/abs(errorVector[1])
moveX = signX * min([0.06, abs(errorVector[1])])
self.drone.moveX(moveX)
else:
if self.drone.camera_tilt > -70:
if self.lastTiltChanged > 10:
# print(targetPoint)
# print(currentPoint)
# print(errorVector)
# print("Looking further down\n\n")
self.drone.cameraControl(self.drone.camera_tilt - 5, self.drone.camera_pan)
self.lastTiltChanged = 0
else:
self.lastTiltChanged += 1
elif self.drone.camera_tilt > -70:
if self.lastTiltChanged > 10:
# print(targetPoint)
# print(currentPoint)
# print(errorVector)
# print("Looking further down\n\n")
self.drone.cameraControl(self.drone.camera_tilt - 5, self.drone.camera_pan)
self.lastTiltChanged = 0
else:
self.lastTiltChanged += 1


else:
if not self.pointAchived:
Expand All @@ -259,7 +259,7 @@ def adjustCentreLock(self):
errorVector = self.getPointError(currentPoint, targetPoint)
errorMagnitude = np.linalg.norm(errorVector)

#Lets try to set a 150 pixel error max we can make it configureable
#Lets try to set a 100 pixel error max we can make it configureable
if errorMagnitude > 100:
if abs(errorVector[0]) > 40:
signY = (errorVector[0]/abs(errorVector[0]))
Expand Down
2 changes: 1 addition & 1 deletion src/game_master/scripts/vision.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,7 @@ def __init__(self, drone, vfov=45, hfov=80, expectedDistance= 6.50, northToSouth
self.bearFrameDistance = None
# New frame flag
self.newFrameProcessed = False
self.processLine = False
self.processLine = True
self.processMarker = False
self.processBear = False

Expand Down

0 comments on commit f318fcf

Please sign in to comment.