def sweep_routine(self, approach_angle):
return sw3.LoopRoutine(
sw3.Forward(BACKWARD_SPEED, BACKUP_TIME),
sw3.Forward(0, 2),
sw3.SetYaw(self.reference_angle),
#sw3.SetYaw(approach_angle),
)
def findpizza(self, pizzabox):
# go forward until pizza box, then slightly more.
if not pizzabox:
sw3.nav.do(sw3.Forward(.3, 1))
else:
sw3.nav.do(sw3.Forward(0, 0))
self.nextState()
def pass_with_style(self):
"""the code for turning the robot, and passing through the hedge
with style."""
turn_routine = sw3.RelativeYaw(180)
stop_routine = sw3.Forward(0, 0.1)
backup_routine = sw3.Forward(BACKUP_SPEED, BACKUP_TIME)
# stop
sw3.nav.do(stop_routine)
time.sleep(1)
# 180
sw3.nav.do(
stop_routine,
turn_routine,) # do a 180
time.sleep(10)
sw3.nav.do(backup_routine) # backup
time.sleep(BACKUP_TIME)
time.sleep(1)
sw3.nav.do(stop_routine) # stop
time.sleep(1)
print "doing the 2nd 180"
sw3.nav.do(sw3.RelativeYaw(180)) # do another 180
time.sleep(10)
print "done"
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 init(self):
# Set depth to hedge hight, but keep heading at same time
sw3.nav.do(sw3.CompoundRoutine(
sw3.Forward(0),
sw3.HoldYaw(),
sw3.SetDepth(HEDGE_DEPTH),
))
# give some time for the dive to complete
time.sleep(4)
"""
current_depth = sw3.data.depth()
while current_depth <= HEDGE_DEPTH - 3:
print "changing depth ({}) to {}".format(current_depth, HEDGE_DEPTH)
current_depth = sw3.data.depth()
"""
# start vision
self.process_manager.start_process(entities.GateEntity, "gate", "forward", debug=True)
# go forward
sw3.nav.do(sw3.CompoundRoutine(
#sw3.HoldYaw(),
sw3.Forward(FORWARD_SPEED),
))
def seek(self, bins):
# print "seek"
# print bins
if bins and self.orientdata:
sw3.nav.do(sw3.Forward(0, 1))
# print bins
pos_x = math.atan2(bins[0].theta, bins[0].phi) * (180 / math.pi)
print pos_x
pos_rho = math.sqrt(bins[0].theta**2 + bins[0].phi**2)
# sw3.nav.do(sw3.Forward(0,0))
print "center"
center = sw3.CompoundRoutine(sw3.RelativeYaw(pos_x),
sw3.Forward(.2),
sw3.SetDepth(BIN_DEPTH),
timeout=3)
sw3.nav.do(center)
# print pos_x
# sw3.nav.do(sw3.Forward(FORWARD_SPEED, CENTER_TIME))
# print "centering"
#self.orientdata *= 180/math.pi
if pos_x <= CENTER_THRESH:
# print "centered"
#orient_angle = self.orientdata
sw3.nav.do(sw3.Forward(0, 1))
# sw3.nav.do(sw3.RelativeYaw(20))
# sw3.nav.do(sw3.RelativeYaw(self.orientdata))
# sw3.nav.do(sw3.RelativeDepth(BIN_DEPTH))
self.nextState()
def step(self, vision_data):
buoys = []
if vision_data is not None:
buoys = vision_data['buoy'].buoys
if len(buoys) > 0:
looking_for = 'red'
if self.bumped == 1:
looking_for = 'green'
selected = None
for buoy in buoys:
if buoy.color == looking_for:
selected = buoy
if selected is not None:
sw3.nav.do(
sw3.CompoundRoutine(
sw3.SetDepth(7),
sw3.RelativeYaw(selected.theta),
sw3.Forward(.2, 6),
))
self.bumped += 1
sw3.nav.do(sw3.Forward(-.2, 6))
if self.bumped == 2:
self.finish_mission()
else:
sw3.nav.do(sw3.Forward(.1, 1))
def orient(self, bins):
if bins and self.orientdata:
orient_angle = self.orientdata * (180 / math.pi)
# print "forward"
#center = sw3.Forward(0.5,2)
#centering = sw3.nav.do(center)
# sw3.nav.do(center)
# sw3.nav.do(sw3.Forward(FORWARD_SPEED,1))
# sw3.nav.do(sw3.Forward(0,0))
print "orient"
orient = sw3.CompoundRoutine(sw3.Forward(0, 5),
sw3.RelativeYaw(orient_angle))
# sw3.nav.do(sw3.CompoundRoutine(sw3.Forward(0,0),(sw3.RelativeYaw(self.orientdata),timeout =5)))
sw3.nav.do(orient)
#orient.on_done(lambda z: sw3.nav.do(sw3.Forward(FORWARD_SPEED, 1)))
#center.on_done(lambda b: sw3.nav.do(orient))
# sw3.nav.do(sw3.RelativeDepth(BIN_DEPTH))
# print sw3.data.imu.yaw()
# print self.orientdata
if (abs(abs(sw3.data.imu.yaw()) - orient_angle) <= ORIENT_THRESH):
print "done orienting"
orient.on_done(
lambda z: sw3.nav.do(sw3.Forward(FORWARD_SPEED, 1)))
self.turn_count += 1
self.nextState()
def step(self, vision_data):
self.mission_timeout -= 1
if not vision_data:
return
gate_data = vision_data['gate']
if not gate_data:
return
print gate_data
if gate_data and gate_data.left_pole and gate_data.right_pole:
gate_center = DEGREE_PER_PIXEL * (gate_data.left_pole + gate_data.right_pole) / 2 # degrees
# If both poles are seen, point toward it then go forward.
self.gate_seen += 1
self.gate_lost = 0
if abs(gate_center) < STRAIGHT_TOLERANCE:
sw3.nav.do(sw3.CompoundRoutine([
sw3.Forward(FORWARD_SPEED),
sw3.HoldYaw()
]))
else:
print "Correcting Yaw", gate_center
sw3.nav.do(sw3.CompoundRoutine([
sw3.RelativeYaw(gate_center),
sw3.Forward(0.4)
]))
elif self.gate_seen >= 15:
self.gate_lost += 1
if self.gate_lost > 1 or self.mission_timeout <= 0:
print "Heading Locked"
self.finish_mission()
return
def update_axis(event):
global yaw_heading
angle = event.angle_radians
mag = max(min(event.magnitude, 1.0), -1.0)
forward = -(mag * sin(angle))
rate = (mag * cos(angle))
total = abs(forward) + abs(rate)
if total == 0:
yaw_heading = sw3.data.imu.yaw()
sw3.nav.do(
sw3.CompoundRoutine(
(sw3.SetYaw(yaw_heading), sw3.Forward(forward))))
else:
for_p = forward / total
rate_p = rate / total
total = min(total, 1.0)
forward = for_p * total
rate = rate_p * total
sw3.nav.do(
sw3.CompoundRoutine((sw3.SetRotate(rate), sw3.Forward(forward))))
def set_course(self, forward, rel_yaw, get_obj=False):
"""Changes the course of the robot as it's searching
for the pinger.
ARGS:
forward: how fast to go forward
rel_yaw: how much to change the relative yaw heading"""
# change heading and move forward (or backward)
if not get_obj:
sw3.nav.do(sw3.CompoundRoutine(
sw3.RelativeYaw(rel_yaw),
sw3.Forward(forward),
)
)
return
else:
set_course_routine = sw3.CompoundRoutine(
sw3.RelativeYaw(rel_yaw),
sw3.Forward(forward),
)
)
# deliver an object to the caller
return set_course_routine
def state_setup_pulldown(self):
# start: buoy has dissappeard b/c we're so close to it
# ###
sw3.nav.do(
sw3.SequentialRoutine(sw3.Forward(-0.7, 1), sw3.Forward(0, 0.1),
sw3.RelativeDepth(-2),
sw3.Forward(0.7, 1 * 1.5),
sw3.Forward(0, 0.1)))
self.wait_stop_mark = 4
self.next_state()
def state_findpath(self):
# TODO: check if the second buoy was center, if so, dont do another approach
riseup_routine = sw3.RelativeDepth(OVER_DEPTH)
runover_routine = sw3.Forward(FORWARD_SPEED)
stop_routine = sw3.Forward(0)
depth_goto = sw3.SetDepth(self.depth_seen)
print "findpath"
sw3.nav.do(
sw3.SequentialRoutine(depth_goto, riseup_routine, runover_routine))
self.finish_mission()
def findpath(self, holes):
# swim around target?
turn = sw3.CompoundRoutine(sw3.RelativeYaw(TURN_ANGLE),
sw3.Forward(TURN_SPEED),
timeout=TURN_TIMER)
finish = sw3.CompoundRoutine(sw3.RelativeYaw(-TURN_ANGLE),
sw3.Forward(TURN_SPEED),
timeout=TURN_TIMER)
sw3.nav.do(turn)
sw3.nav.do(finish)
self.finish_mission()
def state_approach(self, buoy_to_bump, buoys):
# TODO: include depth routine
if len(buoys) == 3:
buoys.sort(key=lambda x: x.theta)
self.tracking_id = buoys[buoy_to_bump].id
if self.depth_seen is None:
self.depth_seen = sw3.data.depth()
track_buoy = None
for buoy in buoys:
if buoy.id == self.tracking_id:
track_buoy = buoy
if not track_buoy:
return
#self.set_timer("Approach_Timeout", APPROACH_TIMEOUT, self.approach_timeout, sw3.data.imu.yaw())
# print "State: BuoyBump dist: ",track_buoy.r, " x angle from current: ",track_buoy.theta, " y angle from current: ", track_buoy.phi
yaw_routine = sw3.RelativeYaw(track_buoy.theta)
forward_routine = sw3.Forward(FORWARD_SPEED)
stop_routine = sw3.Forward(0, 0)
backup_routine = sw3.Forward(BACKWARD_SPEED)
reset_routine = sw3.SetYaw(self.reference_angle)
track_depth_angle = (track_buoy.phi)
if abs(track_depth_angle) > DEPTH_THRESHOLD:
if (track_depth_angle > 0):
depth_routine = sw3.RelativeDepth(-DEPTH_UNIT)
if (track_depth_angle < 0):
depth_routine = sw3.RelativeDepth(DEPTH_UNIT)
else:
depth_routine = sw3.NullRoutine()
centered = False
if abs(track_buoy.theta) <= CENTER_THRESHOLD:
centered = True
if centered:
sw3.nav.do(
sw3.CompoundRoutine(forward_routine, yaw_routine,
depth_routine))
'''
if abs(track_buoy.r) <= DIST_THRESHOLD:
self.delete_timer("Approach_Timeout")
self.next_state()
'''
self.next_state()
else:
sw3.nav.do(sw3.CompoundRoutine(stop_routine, yaw_routine))
def step(self, vision_data):
if not vision_data:
return
if vision_data['path']:
path_data = vision_data['path']
print path_data
theta_x = path_data.x * FIELD_OF_VIEW * math.pi / 180 # path_data.x is percent of fram view . multiplying them gives you theta_x
theta_y = path_data.y * FIELD_OF_VIEW * math.pi / 180 # path_data.y is percent of frame view . multiplying them gives you theta
d = PATH_DEPTH - sw3.data.depth(
) # depth between path and camera
x = d * math.sin(
theta_x
) # g1ves you the x distance from the frame center to path center
y = d * math.sin(
theta_y
) # gives you the y distance from the frame center to path center
print "Status:Step x ", x, " y ", y
if self.state == "centering":
self.state_centering(x, y)
if self.state == "orienting":
return self.state_orienting(path_data)
if vision_data['gate']:
gate_data = vision_data['gate']
print gate_data
if gate_data.left_pole and gate_data.right_pole:
gate_center = DEGREE_PER_PIXEL * (
gate_data.left_pole +
gate_data.right_pole) / 2 # degrees
if abs(gate_center) < STRAIGHT_TOLERANCE:
sw3.nav.do(
sw3.CompoundRoutine(
[sw3.Forward(FORWARD_SPEED),
sw3.HoldYaw()]))
else:
print "Correcting Yaw", gate_center
sw3.nav.do(
sw3.CompoundRoutine([
sw3.RelativeYaw(gate_center),
sw3.Forward(0.4)
]))
def sweep(self, bins):
# print "sweep"
# print self.orientdata
sweep = sw3.Forward(FORWARD_SPEED, 1)
turning = sw3.CompoundRoutine(sw3.Forward(0, TURNING_TIME),
sw3.RelativeYaw(180))
turnaround = lambda: sw3.nav.do(turning)
#self.id_holder = self.highest_id
# iif self.x == 0:
# print "nav.sweep"
# sw3.nav.do(sweep)
#self.x = 1
if bins:
for bincount in bins:
if bincount.id > self.highest_id:
self.highest_id = bincount.id
current_bin = None
if self.orientdata is not None:
# if self.highest_id > self.id_holder:
# this means weve seen a new bini
# print self.highest_id
# print self.id_holder
#self.id_holder = self.highest_id
# sw3.nav.do(sw3.Forward(0,0))
# sw3.nav.do(turning)
print "turning"
self.set_timer("bin_timeout", TURNAROUND_TIMER, turnaround)
turning.on_done(lambda y: sw3.nav.do(sweep))
# print "I turned!"
# sw3.nav.do(sw3.Forward(FORWARD_SPEED))
# print "turning"
self.turn_count += 1
# print self.turn_countF none
for bina in bins:
if bina.id == self.highest_id:
current_bin = bina
print current_bin.shape
#self.highest_id = bina.id
if current_bin:
if current_bin.shape is "A" or current_bin.shape is "C":
if self.dropped is "A" and current_bin.shape is "C":
self.nextState()
if self.dropped is "C" and current_bin.shape is "A":
self.nextState()
if self.dropped is "E":
self.nextState()
self.dropped = current_bin.shape
def findpath(self):
if self.turn_count % 2:
sw3.nav.do(sw3.RelativeYaw(-90))
else:
sw3.nav.do(sw3.RelativeYaw(90))
sw3.nav.do(sw3.Forward(.1, 2))
self.finish_mission()
def change_heading(self, forward, rel_yaw):
if abs(rel_yaw) > YAW_TOLERANCE:
sw3.nav.do(sw3.CompoundRoutine(
sw3.RelativeYaw(-rel_yaw),
sw3.Forward(forward)
)
)
def state_findpath(self):
sw3.nav.do(sw3.Forward(0))
# TODO: check if the second buoy was center, if so, dont do another approach
riseup_routine = sw3.RelativeDepth(REL_OVER_DEPTH)
runover_routine = sw3.Forward(FORWARD_SPEED)
stop_routine = sw3.Forward(0)
depth_goto = sw3.SetDepth(self.depth_seen)
print "findpath"
sw3.nav.do(
sw3.SequentialRoutine(sw3.Forward(-0.6, 0.1), riseup_routine,
sw3.SetYaw(self.reference_angle),
runover_routine))
self.wait_stop_mark = 3
self.next_state()
def bump_buoy(self, buoys, depth=None):
# Check if we saw all 3 buoys
# found_buoys = filter(lambda x: x.found, buoys)
found_buoys = buoys
if found_buoys:
found_buoys.sort(key=lambda x: x.theta)
if len(found_buoys) == 3:
print "found 3 buoys"
# Assign tracking_buoy_id to middle buoy
self.tracking_buoy_id = found_buoys[1].id
if len(found_buoys) == 1:
print "found 1 buoy"
self.tracking_buoy_id = found_buoys[0].id
"""
elif found_buoys:
for buoy in found_buoys:
absolute_theta = buoy.theta + sw3.data.imu.yaw()
buoy.center_distance = sw3.util.circular_distance(absolute_theta, self.start_angle, 180, -180)
found_buoys.sort(key=lambda x: x.center_distance)
if found_buoys[0].center_distance < BUOY_CENTER_THRESHOLD:
self.tracking_buoy_id = found_buoys[0].id
"""
# what does this conditional do?
if self.tracking_buoy_id is not None:
self.delete_timer("mission_timeout")
# Get tracking buoy from its id
tracking_buoy = None
for buoy in buoys:
if buoy.id == self.tracking_buoy_id:
tracking_buoy = buoy
# Correct if we saw it
if tracking_buoy is not None:
print tracking_buoy
self.last_seen = time.time()
if depth:
depth_routine = sw3.SetDepth(depth)
else:
depth_routine = sw3.RelativeDepth(-(tracking_buoy.phi) / 5)
print "Adjusting depth:", -tracking_buoy.phi / 5
print "id: ", tracking_buoy.id
print "Theta:", tracking_buoy.theta
print "Adjusting Yaw:", DEGREE_PER_PIXEL * tracking_buoy.theta
sw3.nav.do(sw3.CompoundRoutine(
depth_routine,
sw3.Forward(FORWARD_SPEED),
sw3.RelativeYaw(DEGREE_PER_PIXEL * tracking_buoy.theta),
))
# Consider ending the mission if we didn't see it
elif time.time() - self.last_seen > 10:
self.tracking_buoy_id = None
print "Lost buoy, finishing"
return True
def state_centering(self, x, y):
position_rho = math.sqrt(
x**2 +
y**2) # hypotenuse-distance from frame center to path center
position_phi = math.atan2(x, y) * (
180 / pi) # angle of center of path from current position
print "State:Centering dist ", position_rho, " angle from current ", position_phi
sw3.nav.do(sw3.Forward(0))
yaw_routine = sw3.RelativeYaw(position_phi)
forward_routine = sw3.Forward(FORWARD_SPEED, CENTER_TIME)
sw3.nav.do(yaw_routine)
yaw_routine.on_done(lambda x: sw3.nav.do(forward_routine))
if position_rho <= CENTER_THRESHOLD:
self.state = "orienting"
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 state_orienting(self, path_data):
current_yaw = sw3.data.imu.yaw() * (pi / 180) % (2 * pi)
path_angle = (path_data.theta + current_yaw) % pi
sw3.nav.do(sw3.Forward(0))
opposite_angle = (pi + path_angle) % (2 * pi)
print "Status: Orienting yaw ", current_yaw, " path_angle ", path_angle, " opposite_angle ", opposite_angle
if util.circular_distance(self.reference_angle,
opposite_angle) < util.circular_distance(
self.reference_angle, path_angle):
path_angle = opposite_angle
if path_angle > math.pi:
path_angle = path_angle - 2 * pi
print "Orienting to", (180 / pi) * path_angle
routine = sw3.SetYaw((180 / pi) * path_angle, timeout=15)
routine.on_done(self.finish_mission)
sw3.nav.do(routine)
self.state = 'done'
degree = path_data.theta * (180 / pi)
# if degree <= MIN_ANGLE_THRESHOLD or degree >= MAX_ANGLE_THRESHOLD:
'''
def init(self):
# set depth to bouy level
sw3.nav.do(sw3.SetDepth(DEPTH_BUMP))
time.sleep(DELAY)
# startup buoy vision code
self.process_manager.start_process(entities.BuoyHoughEntity,
"buoy",
"forward",
debug=True)
# Go forward
sw3.nav.do(
sw3.CompoundRoutine(
sw3.Forward(FORWARD_SPEED),
sw3.HoldYaw(),
))
# Captures yaw heading for ???
self.start_angle = sw.var.get("YawPID.Heading")
# starts a watchdog timer for the entire mission. After 15 seconds,
# the mission will terminate, regardless of what is happening.
self.set_timer("mission_timeout", 15, self.finish_mission)
self.tracking_buoy_id = None # State variable for bump_buoy()
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 state_disconnect(self):
sw3.nav.do(
sw3.SequentialRoutine(
sw3.Forward(-0.5, 3),
sw3.NullRoutine(),
))
self.wait_stop_mark = 1
self.next_state()
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 step(self, vision_data):
self.mission_timeout -= 1
if not vision_data:
return
hedge_data = vision_data['hedge']
if not hedge_data:
return
print hedge_data
if hedge_data:
hedge_center = None
if hedge_data.right_pole and hedge_data.left_pole:
hedge_center = DEGREE_PER_PIXEL * (
hedge_data.left_pole +
hedge_data.right_pole) / 2 # degrees
elif hedge_data.center_pole is not None:
hedge_center = hedge_data.center_pole
self.hedge_seen += 1
self.hedge_lost = 0
if hedge_center is not None:
if abs(gate_center) < STRAIGHT_TOLERANCE:
sw3.nav.do(
sw3.CompoundRoutine(
[sw3.Forward(FORWARD_SPEED),
sw3.HoldYaw()]))
else:
print "Correcting Yaw", hedge_center
sw3.nav.do(
sw3.CompoundRoutine([
sw3.RelativeYaw(hedge_center + 2),
sw3.Forward(FORWARD_SPEED),
]))
elif self.hedge_seen >= 5:
self.hedge_lost += 1
if self.hedge_lost > 1 or self.mission_timeout <= 0:
sw3.nav.do(
sw3.SequentialRoutine(sw3.RelativeYaw(180),
sw3.Forward(-1 * FORWARD_SPEED, 5)))
time.sleep(self.mission_timeout)
self.finish_mission()
