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]))