def processFrame(self, frame):
print "turn state"
path = vision.ProcessFrame(frame)
if path.found:
print "path found"
self.pathLost.restart()
"""
finding out where the start of the path is. This is the path
end point closest to the center of the camera
"""
#pt1, pt2 = path.endPoints()
pts = [[path.p1x, path.p1y], [path.p2x, path.p2y]]
self.startPt, self.endPt = sortPoints(self.startPt, pts)
center = (path.cx, path.cy)
angle = turnParallelTo(center, self.endPt)
print "Angle: %d" % angle
if abs(angle) <= MAXANGLEDIF:
sw3.RelativeYaw(0).start()
return OnwardState()
elif not self.pathLost.timeLeft():
"""if the path has been lost for too long go to path lost state"""
return OnwardState()
print "ret found"
return self
def processFrame(self, frame):
print "found state"
path = vision.ProcessFrame(frame)
if path.found:
print "path found"
self.pathLost.restart()
"""
Turning to face the end point of the path.
"""
#pt1, pt2 = path.endPoints()
pts = [[path.p1x, path.p1y], [path.p2x, path.p2y]]
self.startPt, self.endPt = sortPoints(self.startPt, pts)
center = (path.cx, path.cy)
angle = turnParallelTo(center, self.endPt)
print "Angle: %d" % angle
if abs(angle) <= MAXANGLEDIF:
sw3.RelativeYaw(0).start()
return ToEndState(self.startPt, self.endPt)
elif not self.pathLost.timeLeft():
"""if the path has been lost for too long go to path lost state"""
return PathLostState()
print "ret found"
return self
def processFrame(self, frame):
print "onward state"
path = vision.ProcessFrame(frame)
if path.found:
self.pathLost.restart()
sw3.Forward(SPEED).start()
print "Speed %.2f" % SPEED
elif not self.pathLost.timeLeft():
"""if the path has been lost for too long go to path lost state"""
return LostState()
print "ret found"
return self
def processFrame(self, frame):
path = vision.ProcessFrame(frame)
print path.found
if path.found:
frame = path.draw(frame)
self.foundCounter -= 1
if self.foundCounter <= 0:
#closest point to center is start point
h, w, _ = frame.shape
pt1, pt2 = [[path.p1x, path.p1y], [path.p2x, path.p2y]]
dist1 = math.sqrt((w / 2 - pt1[0])**2 + (h / 2 - pt1[1])**2)
dist2 = math.sqrt((w / 2 - pt2[0])**2 + (h / 2 - pt2[1])**2)
if dist1 < dist2:
return FoundState(pt1, pt2)
else:
return FoundState(pt2, pt1)
return self
def processFrame(self, frame):
path = vision.ProcessFrame(frame)
print "search state"
print path.found
if path.found:
frame = path.draw(frame)
self.foundCounter -= 1
if self.foundCounter <= 0:
#closest point to center is start point
h, w, _ = frame.shape
pt1, pt2 = [[path.p1x, path.p1y], [path.p2x, path.p2y]]
center = (path.cx, path.cy)
#ideal angle is the angle of the end plank
angle1 = getAngleFromCenter(center, pt1)
angle2 = getAngleFromCenter(center, pt2)
if abs(angle1) < abs(angle2):
return TurnState(pt2, pt1)
else:
return TurnState(pt1, pt2)
return self
def processFrame(self, frame):
print "found state"
path = vision.ProcessFrame(frame)
if path.found:
print "path found"
self.pathLost.restart()
"""
moving to the end of the path.
"""
pts = [[path.p1x, path.p1y], [path.p2x, path.p2y]]
self.startPt, self.endPt = sortPoints(self.startPt, pts)
center = (path.cx, path.cy)
if moveTo(frame, self.endPt) <= MAXDIST:
self.atEnd = True
elif not self.pathLost.timeLeft():
"""if the path has been lost for too long go to path lost state"""
return PathLostState()
print "ret found"
return self
def processFrame(self, frame):
print "found state"
path = vision.ProcessFrame(frame)
if path.found:
print "path found"
self.pathLost.restart()
"""
finding out where the start of the path is. This is the path
end point closest to the center of the camera
"""
#pt1, pt2 = path.endPoints()
pts = [[path.p1x, path.p1y], [path.p2x, path.p2y]]
self.startPt, self.endPt = sortPoints(self.startPt, pts)
cx, cy = (path.cx, path.cy)
if moveTo(frame, self.startPt) <= MAXDIST:
return FirstTurnState(self.startPt, self.endPt)
elif not self.pathLost.timeLeft():
"""if the path has been lost for too long go to path lost state"""
return PathLostState()
print "ret found"
return self