def processFrame(self, frame):
buoys = vision.ProcessFrame(frame)
if buoys.found:
print "BUOYS found -------------------------------------------------"
(x, y) = buoys.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)
if distance <= DISTANCE_ERROR:
print("Centered on wheel. Halting.")
sw3.Forward(0).start()
sw3.Strafe(0).start()
#drop balls
else:
print('modifying depth by: %.3f' % (heightError / PIXTODEPTH))
sw3.Forward(heightError / PIXTODEPTH).start()
print('setting strafe to: %.3f' % (widthError / PIXTODEPTH))
sw3.Strafe(widthError / PIXTODEPTH).start()
return self.runtime > (time.time() - self.time)
def moveTo(frame, pt):
x, y = pt
h, w, _ = frame.shape
heightError = h / 2 - y
widthError = x - w / 2
sw3.Forward(heightError / PIXTODEPTH).start()
sw3.Strafe(widthError / PIXTODEPTH).start()
return math.sqrt(heightError**2 + widthError**2)
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 __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"
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 init(self):
self.process_manager.start_process(entities.GateEntity,
"gate",
"forward",
debug=True)
sw3.nav.do(
sw3.CompoundRoutine(
sw3.Strafe(STRAFE_SPEED),
sw3.SetDepth(2),
sw3.HoldYaw(),
))
def update_Raxis(event):
global yaw_heading
angle = event.angle_radians
mag = max(min(event.magnitude, 1.0), -1.0)
#determine control vectors
pitch_stk = (mag * sin(angle))
strafe_stk = (mag * cos(angle))
pitch = (-1) * pitch_stk * MAX_PITCH
strafe = strafe_stk * MAX_STRAFE
#Send commands to Seawolf
sw.var.set("PitchPID.Heading",pitch)
sw3.nav.do(sw3.Strafe(strafe))
print("pitch %2.f, strafe %.2f" % (pitch,strafe))
def processFrame(self, frame):
buoys = vision.ProcessFrame(frame)
if buoys.found:
(x, y) = buoys.loc()
h, w, _ = frame.shape
heightError = h / 2 - y
print('modifying depth by: %.3f' % (heightError / PIXTODEPTH))
sw3.RelativeDepth(heightError / PIXTODEPTH).start()
widthError = x - w / 2
print('setting strafe to: %.3f' % (widthError / PIXTODEPTH))
sw3.Strafe(widthError / PIXTODEPTH).start()
return self.runtime > (time.time() - self.time)