def xyzCenLand(self, timer_event=None):
""" the finite-state machine that centers using the downward camera and then lands"""
data = self.xyzCenLandData
print self.state
if self.state == "keyboard":
# user stopped our state machine!
self.land()
return
elif self.state == "start":
self.takeoff()
self.state = "scanning"
elif self.state == "scanning":
if self.noBox:
print "ascending"
self.up(data["zPower"])
else:
self.state = "yCenter"
elif self.state == "yCenter":
if abs(data["tarY"]-self.boxY) < data["tolY"]:
# Y is centered, now do X
self.state = "xCenter"
elif self.boxY < data["tarY"]:
print "Going forward"
self.forward(data["yPower"])
self.state = "scanning"
else:
print "Going backwards"
self.backward(data["yPower"])
self.state = "scanning"
elif self.state == "xCenter":
if abs(data["tarX"]-self.boxX)< data["tolX"]:
self.state = "centered"
elif self.boxX < data["tarX"]:
print "Going left"
self.strafeLeft(data["xPower"])
self.state = "scanning"
else:
print "Going right"
self.strafeRight(data["xPower"])
self.state = "scanning"
elif self.state == "centered":
if self.area > data["landingArea"]:
print self.area
self.hover()
self.land()
self.state = "keyboard"
else:
print "Going Down, current area: %f" % self.area
self.down(data["zPower"])
rospy.sleep(0.2)
self.state = "scanning"
rospy.Timer( rospy.Duration(0.01), self.xyzCenLand, oneshot=True )
def xyAppr(self,timer_event=None):
""" the finite state machine that finds the landmark in the xy-plane
and approaches it, then lands """
data = self.xyApprData
print self.state + " " + str(self.boxX) + " " + str(self.boxHeight)
if self.state == "keyboard":
return
elif self.state == "start":
#self.takeoff()
self.state = "searching"
elif self.state == "searching":
if self.noBox:
self.spinLeft(data["spinPower"])
else:
self.state = "closing_in"
elif self.state == "closing_in":
if abs(self.boxX - data["tarX"]) < data["tolX"]:
self.state = "approaching"
elif self.boxX < data["tarX"]:
print "right"
self.spinRight(data["spinPower"]/2)
else:
print "left"
self.spinLeft(data["spinPower"]/2)
elif self.state == "approaching":
if self.noBox:
self.state = "searching"
elif abs(self.boxX - data["tarX"]) > data["tolX"]:
self.state = "closing_in"
else:
if data["tarY"] - self.boxY > data["tolY"]:
self.up(data["zPower"])
elif self.boxY - data["tarY"] > data["tolY"]:
self.down(data["zPower"])
elif data["tarHeight"] - self.boxHeight > data["tolHeight"]:
self.forward(data["forwardPower"])
else:
self.state = "landing"
elif self.state == "landing":
self.hover()
rospy.sleep(.25)
self.land()
self.state = "keyboard"
rospy.Timer( rospy.Duration(0.01), self.xyAppr, oneshot=True )
def fsm(self,timer_event=None):
print self.state + " " + str(self.boxX) + " " + str(self.boxHeight)
if self.state == "keyboard":
return
elif self.state == "start":
self.takeoff()
self.state = "searching"
elif self.state == "searching":
if self.noBox:
self.spinLeft(self.spinPower)
else:
self.state = "closing_in"
elif self.state == "closing_in":
if abs(self.boxX - self.tarX) < self.toleranceX:
self.state = "approaching"
elif self.boxX > self.tarX:
print "right"
self.spinRight(self.spinPower/2)
else:
print "left"
self.spinLeft(self.spinPower/2)
elif self.state == "approaching":
if self.noBox:
self.state = "searching"
elif abs(self.boxX - self.tarX) > self.toleranceX:
self.state = "closing_in"
elif self.tarHeight - self.boxHeight > self.toleranceHeight:
self.forward(self.forwardPower)
else:
self.state = "landing"
elif self.state == "landing":
self.hover()
rospy.sleep(.25)
self.land()
self.state = "keyboard"
rospy.Timer( rospy.Duration(0.01), self.fsm, oneshot=True )
