Пример #1
0
class Move:
    def __init__(self):
        # initialize the status
        self.dist = 1
        self.dist_prev = 2
        self.act_prev = 'r'
        # init motors, establish bluetooth
        self.motors = Motors()
        self.motors.arm_move((90,90))
    
    def targetDist(self,targetCenter, imgCenter):
        # range from 0 to 1
        h = float(imgCenter[0]-targetCenter[0])/imgCenter[0]
        v = float(imgCenter[1]-targetCenter[1])/imgCenter[1]
        return h
        
    def findmarker(self, img):
        # return > 0 to end finding process  
        print self.dist
 
        if abs(self.dist)>abs(self.dist_prev):
        # the result got worse
            if self.act_prev == 'r':
                self.act = 'l'
            if self.act_prev == 'l':
                self.act = 'r'
        if abs(self.dist)<=abs(self.dist_prev):
        # the result got better or same, keep doing
            self.act = self.act_prev          
        
        self.motors.turn(self.act)
        import time
        time.sleep(0.1)
        self.act_prev = self.act
        self.dist_prev = self.dist
             
        mlist = marker().find(img, debug = 0, show = 0)
        #default dist, which means the not found response
           
        if len(mlist)==1:
            num, pos = mlist[0]
            center = tuple(np.int0( np.mean( pos.reshape(4,2), axis = 0)))
            img_center = tuple( img.shape[:2])
            img_center = (img_center[1]/2, img_center[0]/2)
            self.dist = self.targetDist(center, img_center)
        else:
            self.dist = 2
        return 0  
Пример #2
0
class Motob:
    """Interface between a behavior and one or more motors."""
    def __init__(self):
        """Initialize Motob object.

        Parameters
        ----------
        action : Holder of the action whose settings will be determined by the motob.
        duration : Holder of the most recent duration the of the action.
        """
        self.m = Motors()
        self.action = None
        self.duration = None

    def update(self, recommendation):
        # print('Motor recommendation:', recommendation)
        """Update object.

        Receive a new motor recommendation.
        Load it into the instance variables.
        Operationalize it.

        Example
        -------
        recommendation = ('L', 1)
        """
        self.m.stop()
        self.action = recommendation[0]
        self.duration = recommendation[1]
        self.operationalize(self.action, self.duration)

    def operationalize(self, action, duration):
        """Convert motor recommendation into motor setting and send to corresponding motor(s).

        Parameters
        ----------
        action : Action the motors will perform.
        dur : Duration of the operation.

        Example of actions
        ------------------
        F = Drive forward
        B = Drive backward
        Z = Boost forward
        X = Flee
        T = Turn around 180 degrees
        L = Turn left while driving forward
        R = Turn right while driving forward
        S = Stop
        """
        if action == 'F':
            self.m.set_value((0.5, 0.5), duration)
        elif action == 'B':
            self.m.backward(0.25, duration)
        elif action == 'Z':
            self.m.set_value((1, 1), duration)
        elif action == 'X':
            self.m.flee()  # Turn around, must be tuned for 180 degree turn.
        elif action == 'T':
            self.m.turn()
        elif action == 'L':
            self.m.left((0.1, 0.25), duration)
        elif action == 'R':
            self.m.right((0.25, 0.1), duration)
        elif action == 'S':
            self.m.stop()