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 )