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
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()
