Skip to content

Instantly share code, notes, and snippets.

@kudkudak
Last active August 29, 2015 13:55
Show Gist options
  • Save kudkudak/8705183 to your computer and use it in GitHub Desktop.
Save kudkudak/8705183 to your computer and use it in GitHub Desktop.
if self.state == Turner.STATE_SCANNING:
x_disc, y_disc = int(self.x + 0.5), int(self.y + 0.5)
# +0.5 because axis origin is in the middle of a field
print "Scanning on ",x_disc, " ",y_disc
import math
# Determine vector in which direction we are scanning
vector = (math.cos((self.angle - 90.0)/180.0 * pi), \
math.sin((self.angle - 90.0)/180.0 * pi))
scanned = (x_disc-round(vector[1]), y_disc + round(vector[0]))
# Check if scanned field is not *discovered* yet
if (x_disc-round(vector[1]+0.01), y_disc + \
round(vector[0]+0.01)) not in self.map_visited:
if self.last_distance < 0.9:
# Strange indexing because x runs vertically
# and y runs horizontally
# Set big number so that it won't be visited
self.map_visited[(x_disc-round(vector[1]+0.01),\
y_disc + round(vector[0]+0.01))] = 1000
else:
self.map_visited[(x_disc-round(vector[1]+0.01), \
y_disc + round(vector[0]+0.01))] = 0
# Keep track of scanned fields
self.state_helper += 1
# It means that we have reached the last rotation
if self.state_helper == 4:
self.command_queue.append(["STATE_CHANGE", Turner.STATE_DECIDE_NEXT])
else:
self.command_queue.append([TURN, int(0.5*pi / TICK_ROTATE)])
self.angle = (self.angle + 90.0) % 360
self.command_queue.append([SENSE_SONAR])
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment