def rotate(self, callback, angle, relative=True):
if self.mission == None:
self.callback = callback
self.mission = "rotate"
if relative:
self.target_rot += angle
else:
self.target_rot = angle
# print("Angle: %d" %angle)
realangle = angle_normalize(self.target_rot - self.rot)
# print("Pos: %d, Target: %d, Rotate: %d"%(self.rot, self.target_rot,
# realangle))
if realangle > 17000 and angle < 0:
realangle = realangle - 36000
elif realangle < -17000 and angle > 0:
realangle = realangle + 36000
# print("Rectified angle: %d" %realangle)
self.missions["rotate"].start(self, realangle)
def process_event(self, event):
if event.name == "odo" and event.type == "answer" and self.mission == None:
if self.fonction == "forward":
self.mission = "forward"
# print("Position actuelle : %s %d" %(event.pos, event.rot))
# print("Target actuelle : %s %d" %(self.odo.target_pos, self.odo.target_rot))
deplacement = Vertex(
self.value["dist"] * cos(self.odo.target_rot / 18000 * pi),
self.value["dist"] * sin(self.odo.target_rot / 18000 * pi),
)
# print("Distance : %d" %self.value["dist"])
# print("Vecteur de deplacement : %s" %deplacement)
self.odo.target_pos += deplacement
# print("Nouvelle target : %s %d" %(self.odo.target_pos, self.odo.target_rot))
distance = copysign(deplacement.norm(), self.value["dist"])
# print("Consigne : %d" %distance)
self.missions["forward"].start(self, distance)
elif self.fonction == "reach_x":
self.mission = "forward"
self.logger.info("[reach_x] Position actuelle : %s %d" % (self.odo.pos, event.rot))
self.logger.info("[reach_x] Consigne: x=%d" % self.value["x"])
dx = self.value["x"] - event.pos.x
dtheta = event.rot
dist = dx / cos(dtheta / 18000 * pi)
self.logger.info("[reach_x] dx: %d, dtheta: %d, dist: %d" % (dx, dtheta, dist))
self.odo.target_pos += Vertex(dist * cos(event.rot / 18000 * pi), dist * sin(event.rot / 18000 * pi))
if dist > 12500:
self.dist = dist / 2
dist = self.dist
self.fonction == "reach_x_2"
self.missions["forward"].start(self, dist)
elif self.fonction == "reach_y":
self.mission = "forward"
self.logger.info("[reach_y] Position actuelle : %s %d" % (event.pos, event.rot))
self.logger.info("[reach_y] Consigne: y=%d" % self.value["y"])
dy = self.value["y"] - event.pos.y
dtheta = event.rot
dist = dy / sin(dtheta / 18000 * pi)
self.logger.info("[reach_y] dy: %d, dtheta: %d, dist: %d" % (dy, dtheta, dist))
self.odo.target_pos += Vertex(
dist * cos(self.odo.rot / 18000 * pi), dist * sin(self.odo.rot / 18000 * pi)
)
self.missions["forward"].start(self, dist)
elif self.fonction == "rotate":
self.mission = "rotate"
if self.value["absolute"]:
self.odo.target_rot = self.value["angle"]
else:
self.odo.target_rot += self.value["angle"]
# print("Angle: %d" %self.value["angle"])
realangle = angle_normalize(self.odo.target_rot - event.rot)
# print("Pos: %d, Target: %d, Rotate: %d"%(event.rot, self.odo.target_rot, realangle))
if realangle > 17000 and self.angle < 0:
realangle = realangle - 36000
elif realangle < -17000 and self.angle > 0:
realangle = realangle + 36000
# print("Rectified angle: %d" %realangle)
self.missions["rotate"].start(self, realangle)
elif event.name == self.mission and event.type == "done":
if self.fonction == "reach_x_2" and self.dist != 0:
self.missions["forward"].start(self, dist)
self.fonction = "reach_x"
else:
if self.mission == "speed":
self.odo.target_pos += Vertex(
event.value * cos(self.odo.target_rot / 18000 * pi),
event.value * sin(self.odo.target_rot / 18000 * pi),
)
self.state = None
self.fonction = None
self.mission = None
self.send_event(Event("move", "done", [self.callback]))
