class FoundState(object):
def __init__(self):
#after wheel has not been seen for 8 secs, quit
self.wheelLost = Timer(8)
#timer for being centered on the wheel
self.centeredTimer = Timer(2)
def processFrame(self, frame):
print "found state"
wheel = vision.ProcessFrame(frame)
if wheel.found:
print "wheel found"
self.wheelLost.restart()
"""
finding out how many pixels from center of down camera the wheel is
Finds difference between wheel's center in screen space and center
of the screen, then moves robot to cover that distance.
"""
(x, y) = wheel.loc()
h,w,_ = frame.shape
heightError = h/2 - y
print('Height error: %.3f' % heightError)
widthError= x - w/2
print('Width error: %.3f' % widthError)
distance = math.sqrt(heightError ** 2 + widthError ** 2)
#excluding depth
print("Distance from center of wheel: %.3f" % distance)
"""
Robot moves to center itself on the wheel until it has been centered
within DISTANCE_ERROR's threshhold long enough.
"""
print('moving forward by: %.3f' % (heightError / PIXTODEPTH))
sw3.Forward(heightError / PIXTODEPTH).start()
print('setting strafe to: %.3f' % (widthError / PIXTODEPTH))
sw3.Strafe(widthError / PIXTODEPTH).start()
"""Restart the timer for being centered on the wheel"""
if not distance <= DISTANCE_ERROR:
self.centeredTimer.restart()
if not self.centeredTimer.timeLeft():
sw3.Forward(0).start()
sw3.Strafe(0).start()
return CenteredState()
elif not self.wheelLost.timeLeft():
"""if the wheel has been lost for too long go to lost state"""
return WheelLostState()
print "ret found"
return self
def cont(self):
#wheel missions never stops in this state, only in lost or centered states
return True
class FoundState(object):
def __init__(self):
#after wheel has not been seen for 8 secs, quit
self.wheelLost = Timer(8)
#timer for being centered on the wheel
self.centeredTimer = Timer(2)
def processFrame(self, frame):
print "found state"
wheel = vision.ProcessFrame(frame)
if wheel.found:
print "wheel found"
self.wheelLost.restart()
"""
finding out how many pixels from center of down camera the wheel is
Finds difference between wheel's center in screen space and center
of the screen, then moves robot to cover that distance.
"""
(x, y) = wheel.loc()
h, w, _ = frame.shape
heightError = h / 2 - y
print('Height error: %.3f' % heightError)
widthError = x - w / 2
print('Width error: %.3f' % widthError)
distance = math.sqrt(heightError**2 + widthError**2)
#excluding depth
print("Distance from center of wheel: %.3f" % distance)
"""
Robot moves to center itself on the wheel until it has been centered
within DISTANCE_ERROR's threshhold long enough.
"""
print('moving forward by: %.3f' % (heightError / PIXTODEPTH))
sw3.Forward(heightError / PIXTODEPTH).start()
print('setting strafe to: %.3f' % (widthError / PIXTODEPTH))
sw3.Strafe(widthError / PIXTODEPTH).start()
"""Restart the timer for being centered on the wheel"""
if not distance <= DISTANCE_ERROR:
self.centeredTimer.restart()
if not self.centeredTimer.timeLeft():
sw3.Forward(0).start()
sw3.Strafe(0).start()
return CenteredState()
elif not self.wheelLost.timeLeft():
"""if the wheel has been lost for too long go to lost state"""
return WheelLostState()
print "ret found"
return self
def cont(self):
#wheel missions never stops in this state, only in lost or centered states
return True
class SearchState(object):
def __init__(self):
self.timer = Timer(SEARCHTIME)
self.foundCounter = 4
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 cont(self):
""" if true continue mission, false end mission"""
return self.timer.timeLeft()
class ToCenterState(object):
def __init__(self, startPt, endPt):
#after path has not been seen for 2 secs, quit
self.pathLost = Timer(LOSTTIME)
self.centers = []
self.startPt = startPt
self.endPt = endPt
sw3.Forward(0).start()
def processFrame(self, frame):
print "found state"
path = vision.ProcessFrame(frame)
if path.found:
print "path found"
self.pathLost.restart()
"""
moving to the center 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, center) <= MAXDIST:
return SecondTurnState(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 cont(self):
#path missions never stops while we see path
return True
class OnwardState(object):
def __init__(self):
#after path has not been seen for 2 secs, quit
self.pathLost = Timer(LOSTTIME)
self.centers = []
sw3.Forward(SPEED).start()
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 cont(self):
#path missions never stops while we see path
return True
class FoundState(object):
def __init__(self):
#after gate has not been seen for 2 secs, quit
self.pathLost = Timer(2)
self.centers = []
sw3.Forward(.3).start()
def processFrame(self, frame):
print "found state"
path = vision.ProcessFrame(frame)
if path.found:
print "path found"
self.pathLost.restart()
"""
finding out how many pixels from the center the gate is
the center obj of the gate is the number or pixels over the gate is.
Subtracting the middle pixel index from it returns a pos value if the gate is to left
and pos value if the gate is to the right
"""
print("got direction %d" % path.orientation)
sw3.RelativeYaw(path.orientation).start()
elif not self.pathLost.timeLeft():
"""if the gate has been lost for too long go to gate lost state"""
return PathLostState()
print "ret found"
return self
def cont(self):
#gate missions never stops while we see gate
return True
class SearchState(object):
def __init__(self):
self.timer = Timer(SEARCHTIME)
self.foundCounter = 4
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 cont(self):
""" if true continue mission, false end mission"""
return self.timer.timeLeft()
class SearchState(object):
def __init__(self):
self.timer = Timer(SEARCHTIME)
self.foundCounter = 4
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 cont(self):
""" if true continue mission, false end mission"""
return self.timer.timeLeft()
class OnwardState(object):
def __init__(self):
#after path has not been seen for 2 secs, quit
self.pathLost = Timer(LOSTTIME)
self.centers = []
sw3.Forward(SPEED).start()
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 cont(self):
#path missions never stops while we see path
return True
class FoundState(object):
def __init__(self):
#after gate has not been seen for 2 secs, quit
self.pathLost = Timer(2)
self.centers = []
sw3.Forward(.3 ).start()
def processFrame(self, frame):
print "found state"
path = vision.ProcessFrame(frame)
if path.found:
print "path found"
self.pathLost.restart()
"""
finding out how many pixels from the center the gate is
the center obj of the gate is the number or pixels over the gate is.
Subtracting the middle pixel index from it returns a pos value if the gate is to left
and pos value if the gate is to the right
"""
print("got direction %d" % path.orientation)
sw3.RelativeYaw(path.orientation).start()
elif not self.pathLost.timeLeft():
"""if the gate has been lost for too long go to gate lost state"""
return PathLostState()
print "ret found"
return self
def cont(self):
#gate missions never stops while we see gate
return True
class SearchState(object):
def __init__(self):
self.timer = Timer(SEARCHTIME)
self.foundCounter = 4
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 cont(self):
""" if true continue mission, false end mission"""
return self.timer.timeLeft()
class GateLostState(object):
def __init__(self):
self.stopTime = Timer(RECKONTIME)
def processFrame(self, frame):
return self
def cont(self):
return self.stopTime.timeLeft()
class PathLostState(object):
def __init__(self):
self.stopTime = Timer(RECKONTIME)
def processFrame(self, frame):
return self
def cont(self):
return self.stopTime.timeLeft()
class PathLostState(object):
def __init__(self):
self.stopTime = Timer(RECKONTIME)
def processFrame(self, frame):
print("Lost path lost lost lost")
return self
def cont(self):
return self.stopTime.timeLeft()
class FoundState(object):
def __init__(self):
#after path has not been seen for 2 secs, quit
self.diceLost = Timer(LOSTTIME)
self.centers = []
self.pastDice = False
sw3.Forward(.1).start()
def processFrame(self, frame):
print "found state"
dice = vision.ProcessFrame(frame)
if dice.found:
print "found dice"
self.diceLost.restart()
(x, y, _) = dice.closestLoc(frame)
h,w,_ = frame.shape
heightError = h/2 - y
print('modifying depth by: %.3f' % (heightError / PIXTODEPTH))
sw3.RelativeDepth(heightError / PIXTODEPTH).start()
print "x is : ", x
widthError= x - w/2
print "w is : ", widthError
print('turning: %.3f' % (widthError / PIXTODEPTH))
if widthError > 0:
print "<<"
sw3.RelativeYaw( .0001).start()
else:
print ">>"
sw3.RelativeYaw( -.0001 ).start()
#elif not self.diceLost.timeLeft():
# """if the dice has been lost for too long go to path lost state"""
# return LostState()
if not self.diceLost.timeLeft():
print "stopping seawolf"
sw3.RelativeDepth(0).start()
sw3.Strafe(0).start()
self.pastDice = True
print "ret found"
return self
def cont(self):
#path missions never stops while we see path
return not self.pastDice
class FoundState(object):
def __init__(self):
#after gate has not been seen for 2 secs, quit
self.gateLost = Timer(2)
self.centers = []
def processFrame(self, frame):
print "found state"
gate = vision.ProcessFrame(frame)
if gate.found:
print "gate found"
self.gateLost.restart()
"""
finding out how many pixels from the center the gate is
the center obj of the gate is the number or pixels over the gate is.
Subtracting the middle pixel index from it returns a pos value if the gate is to left
and pos value if the gate is to the right
"""
_, w, _ = frame.shape
center = w/2.0 - gate.cp
print("got center %d" % center)
self.centers.insert(0, center)
centers = 0
for i in self.centers:
centers += i
center = float(centers)/len(self.centers)
print(center)
print self.centers
if len(self.centers) > 10:
self.centers.pop()
print self.centers
#if less than set difference ignore it
center = center if center > SIGPIX else 0
sw3.RelativeYaw(center / PIXTODEG).start()
elif not self.gateLost.timeLeft():
"""if the gate has been lost for too long go to gate lost state"""
return GateLostState()
print "ret found"
return self
def cont(self):
#gate missions never stops while we see gate
return True
class FoundState(object):
def __init__(self):
#after gate has not been seen for 2 secs, quit
self.gateLost = Timer(2)
self.centers = []
def processFrame(self, frame):
print "found state"
gate = vision.ProcessFrame(frame)
if gate.found:
print "gate found"
self.gateLost.restart()
"""
finding out how many pixels from the center the gate is
the center obj of the gate is the number or pixels over the gate is.
Subtracting the middle pixel index from it returns a pos value if the gate is to left
and pos value if the gate is to the right
"""
_, w, _ = frame.shape
center = w / 2.0 - gate.cp
print("got center %d" % center)
self.centers.insert(0, center)
centers = 0
for i in self.centers:
centers += i
center = float(centers) / len(self.centers)
print(center)
print self.centers
if len(self.centers) > 10:
self.centers.pop()
print self.centers
#if less than set difference ignore it
center = center if center > SIGPIX else 0
sw3.RelativeYaw(center / PIXTODEG).start()
elif not self.gateLost.timeLeft():
"""if the gate has been lost for too long go to gate lost state"""
return GateLostState()
print "ret found"
return self
def cont(self):
#gate missions never stops while we see gate
return True
class SearchState(object):
def __init__(self):
self.timer = Timer(SEARCHTIME)
self.foundCounter = 4
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:
return FoundState()
return self
def cont(self):
""" if true continue mission, false end mission"""
return self.timer.timeLeft()
class SearchState(object):
def __init__(self):
self.timer = Timer(SEARCHTIME)
self.foundCounter = 4
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:
return FoundState()
return self
def cont(self):
""" if true continue mission, false end mission"""
return self.timer.timeLeft()
class SearchState(object):
def __init__(self):
self.timer = Timer(SEARCHTIME)
#number of times to see gate before going to next state
self.foundCounter = 6
def processFrame(self, frame):
gate = vision.ProcessFrame(frame)
print gate.found
if gate.found:
frame = gate.draw(frame)
self.foundCounter -= 1
if self.foundCounter <= 0:
return FoundState()
return self
def cont(self):
""" if true continue mission, false end mission"""
return self.timer.timeLeft()
class SearchState(object):
def __init__(self):
self.timer = Timer(SEARCHTIME)
#number of times to see gate before going to next state
self.foundCounter = 6
def processFrame(self, frame):
gate = vision.ProcessFrame(frame)
print gate.found
if gate.found:
frame = gate.draw(frame)
self.foundCounter -= 1
if self.foundCounter <= 0:
return FoundState()
return self
def cont(self):
""" if true continue mission, false end mission"""
return self.timer.timeLeft()
class SearchState(object):
def __init__(self):
self.timer = Timer(SEARCHTIME)
self.foundCounter = 4
self.foundCount = 0
def processFrame(self, frame):
dice = vision.ProcessFrame(frame)
print dice.found
if dice.found:
self.foundCount += 1
if self.foundCount >= self.foundCounter:
return FoundState()
return self
def cont(self):
""" if true continue mission, false end mission"""
return self.timer.timeLeft()
class TurnState(object):
def __init__(self, startPt, endPt):
#after path has not been seen for 2 secs, move to onward state
self.pathLost = Timer(2)
self.centers = []
self.startPt = startPt
self.endPt = endPt
sw3.Forward(0).start()
sw3.Strafe(0).start()
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 cont(self):
#path missions never stops while here
return True
class FirstTurnState(object):
def __init__(self, startPt, endPt):
#after path has not been seen for 2 secs, quit
self.pathLost = Timer(LOSTTIME)
self.centers = []
self.startPt = startPt
self.endPt = endPt
sw3.Forward(0).start()
sw3.Strafe(0).start()
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)
center = (path.cx, path.cy)
angle = turnParallelTo(self.startPt, center)
print "Angle: %d" % angle
if abs(angle) <= MAXANGLEDIF:
sw3.RelativeYaw(0).start()
return ToCenterState(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 cont(self):
#path missions never stops while we see path
return True
class SecondTurnState(object):
def __init__(self, startPt, endPt):
#after path has not been seen for 2 secs, quit
self.pathLost = Timer(LOSTTIME)
self.centers = []
self.startPt = startPt
self.endPt = endPt
sw3.Forward(0).start()
sw3.Strafe(0).start()
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 cont(self):
#path missions never stops while we see path
return True
class FoundState(object):
def __init__(self, startPt, endPt):
#after path has not been seen for 2 secs, quit
self.pathLost = Timer(LOSTTIME)
self.centers = []
self.startPt = startPt
self.endPt = endPt
sw3.Forward(0).start()
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
def cont(self):
#path missions never stops while we see path
return True
class FoundState(object):
def __init__(self, startPt, endPt):
#after path has not been seen for 2 secs, quit
self.pathLost = Timer(LOSTTIME)
self.centers = []
self.startPt = startPt
self.endPt = endPt
sw3.Forward(0).start()
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
def cont(self):
#path missions never stops while we see path
return True
class ToEndState(object):
def __init__(self, startPt, endPt):
#after path has not been seen for 2 secs, quit
self.pathLost = Timer(LOSTTIME)
self.centers = []
self.startPt = startPt
self.endPt = endPt
sw3.Forward(0).start()
self.atEnd = False
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 cont(self):
#path missions never stops while we see path
return not self.atEnd
