def __init__(self):
super(Robot, self).__init__()
self.obstacles = []
self.orig_pic = QtGui.QImage('robot.png')
self.build_rect_poly(self.orig_pic.width(), self.orig_pic.height())
self.orig_poly = self.poly
self.start_pose = (150, 150, 0)
self.pos = pt(0, 0)
self.width = self.orig_pic.width()
self.angle = 0
self.restart()
self.sensor_angle = 0
self.mat = matrix(self.angle)
self.velocity = vec2(0.0, 0.0)
self.encoders = vec2(0.0, 0.0)
self.encoder_clicks = (0, 0)
self.pic = self.orig_pic.transformed(QtGui.QTransform())
self.servo_pos = pt(0, -20)
self.api = server.API(self.process_command)
self.commands = {
'V': self.command_velocity,
'SA': self.command_sensor_angle,
'S': self.command_sensor,
'E': self.command_encoders,
'RESET': self.command_reset
}
def restart(self):
self.set_pos(self.start_pose[0], self.start_pose[1])
self.set_angle(self.start_pose[2])
self.set_sensor_angle(0.0)
self.velocity = vec2(0.0, 0.0)
self.encoders = vec2(0.0, 0.0)
self.encoder_clicks = (0, 0)
def __init__(self, *args):
if isinstance(args[0], QtGui.QPolygonF):
poly = args[0]
i = args[1]
self.p1 = vec2(poly.at(i))
self.p2 = vec2(poly.at((i + 1) % poly.size()))
if isinstance(args[0], vec2):
self.p1 = args[0]
self.p2 = args[1]
D = self.p2 - self.p1
self.N = vec2(-D.y, D.x).normalized()
self.d = self.p1 * self.N
def advance(self, full_dt):
start = (math.floor(self.encoders[0]), math.floor(self.encoders[1]))
while full_dt > 0:
dt = min(0.001, full_dt)
full_dt -= dt
pl = dt * self.velocity[0]
pr = dt * self.velocity[1]
da = math.atan2(pr - pl, self.width)
dp = 0.5 * (pl + pr)
self.set_pos(self.pos + dp * self.mat.map(pt(0, -1)))
self.rotate(-da * RAD2DEG)
self.encoders += vec2(pl, pr)
stop = (math.floor(self.encoders[0]), math.floor(self.encoders[1]))
self.encoder_clicks = (self.encoder_clicks[0] +
int(stop[0] - start[0]),
self.encoder_clicks[1] +
int(stop[1] - start[1]))
def build_rect_poly(self, w, h):
self.poly.append(0.5 * pt(-w, -h))
self.poly.append(0.5 * pt(w, -h))
self.poly.append(0.5 * pt(w, h))
self.poly.append(0.5 * pt(-w, h))
self.radius = vec2(0.5 * w, 0.5 * h).norm()
def get_sensor_direction(self):
mat = matrix(self.sensor_angle)
mat.rotate(self.angle)
return vec2(mat.map(pt(0, -1)))
def get_sensor_position(self):
mat = matrix(self.sensor_angle)
p = mat.map(pt(0, -5)) + self.servo_pos
mat = matrix(self.angle)
p = mat.map(p) + self.pos
return vec2(p)
def command_velocity(self, args):
if len(args) == 2:
if abs(args[0]) <= 250 and abs(args[1]) <= 250:
self.velocity = vec2(args[0], args[1])
return ''
def norm(p):
return vec2(p.x(), p.y()).norm()