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 __init__(self, x, y, w, h, angle):
super(Obstacle, self).__init__()
self.build_rect_poly(w, h)
self.pos = pt(x, y)
mat = matrix(angle)
self.poly = mat.map(self.poly)
self.poly.translate(self.pos)
def draw_sensor(self, qp, pos):
servo_mat = matrix(self.sensor_angle)
p1 = servo_mat.map(pt(-5, -5)) + self.servo_pos
p2 = servo_mat.map(pt(5, -5)) + self.servo_pos
p1 = self.mat.map(p1)
p2 = self.mat.map(p2)
qp.setPen(QtGui.QPen(QtGui.QBrush(QtGui.QColor(0, 0, 255)), 2))
qp.drawLine(pos + p1, pos + p2)
def set_angle(self, a):
self.angle = a
self.mat = matrix(self.angle)
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)