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