def champ_perso(self,x_depart,x_arrivee,a,b): self.Potentiel() Ex1,Ey1=pt.Gradient(self.potentiel) Ex1,Ey1=pt.Grad(Ex1,Ey1,self.x,self.y,x_depart,x_arrivee,a,b) potentiel_repulsion=pt.Potentiel(+10,[self.x,self.y],p.x_grid,p.y_grid,p.r_evit/1.5) Ex10,Ey10=pt.Gradient(potentiel_repulsion) self.Ex,self.Ey=p.k*Ex10+Ex1,p.k*Ey10+Ey1
def champ_perso(self, x_depart, x_arrivee, a, b): #permet de calculer le champ produit par ce robot self.Potentiel() Ex1, Ey1 = pt.Gradient(self.potentiel) Ex1, Ey1 = pt.Grad(Ex1, Ey1, self.x, self.y, x_depart, x_arrivee, a, b) potentiel_repulsion = pt.Potentiel(+10, [self.x, self.y], x_grid, y_grid, r_evit / 1.5) Ex10, Ey10 = pt.Gradient(potentiel_repulsion) self.Ex, self.Ey = k * Ex10 + Ex1, k * Ey10 + Ey1
def commande_position( self, x, y, o, spin=False): #commande vers une position avec orientation position_arrivee = Balle(x, y) Exb, Eyb = pt.Gradient(position_arrivee.potentiel) xd = int((self.x + longueur) / (2 * longueur) * nbPoints) yd = int((self.y + largeur) / (2 * largeur) * nbPoints) # print(Ex[yd,xd],xd,self.x,self.y) a, b = np.polyfit([x, self.x], [y, self.y], 1) self.champ_autre(self.x, x, a, b) Ex, Ey = Exb + k * self.Ex_autre, Eyb + k * self.Ey_autre Ex, Ey = pt.norme(Ex, Ey) vecteur = position_arrivee.position - self.positionc distance, phi = c.polar(vecteur) delta = o - self.orientation vitesse_tangente = K * (np.sin(self.orientation) * Ey[yd, xd] + np.cos(self.orientation) * Ex[yd, xd]) vitesse_normale = K * (-np.sin(self.orientation) * Ex[yd, xd] + np.cos(self.orientation) * Ey[yd, xd]) if (abs(distance) > 35) | (abs(delta) > 0.05): if distance < seuil_distance: vitesse_tangente = vitesse_tangente * distance / seuil_distance vitesse_normale = vitesse_normale * distance / seuil_distance p = psl.packetCommandBot(self.team == 'Y', id=self.id, veltangent=vitesse_tangente, velnormal=vitesse_normale, velangular=delta, spinner=spin) grSim.send(p) return 'EN COURS' # time.sleep(0.05) else: p = psl.packetCommandBot(self.team == 'Y', id=self.id, veltangent=0., velnormal=0, velangular=0., spinner=True) grSim.send(p) return 'DONE'
def commande_balle(self): #permet de recuperer la balle ballon = self.match.balle Exb, Eyb = pt.Gradient(ballon.potentiel) a, b = np.polyfit([ballon.x, self.x], [ballon.y, self.y], 1) self.champ_autre(self.x, ballon.x, a, b) Ex, Ey = Exb + k * self.Ex_autre, Eyb + k * self.Ey_autre Ex, Ey = pt.norme(Ex, Ey) vecteur = ballon.position - self.positionc distance, phi = c.polar(vecteur) delta = phi - self.orientation xd = int((self.x + longueur) / (2 * longueur) * nbPoints) yd = int((self.y + largeur) / (2 * largeur) * nbPoints) spin = False vitesse_tangente = K * (np.sin(self.orientation) * Ey[yd, xd] + np.cos(self.orientation) * Ex[yd, xd]) vitesse_normale = K * (-np.sin(self.orientation) * Ex[yd, xd] + np.cos(self.orientation) * Ey[yd, xd]) if not self.hasTheBall(): if distance < seuil_distance: vitesse_tangente = vitesse_tangente * distance / seuil_distance vitesse_normale = vitesse_normale * distance / seuil_distance spin = True p = psl.packetCommandBot(self.team == 'Y', id=self.id, veltangent=vitesse_tangente, velnormal=vitesse_normale, velangular=delta / 0.2, spinner=spin) grSim.send(p) return 'EN COURS' else: p = psl.packetCommandBot(self.team == 'Y', id=self.id, veltangent=0, velnormal=0, velangular=0, spinner=True) grSim.send(p) return 'DONE'
def commande_position(self, x, y, xo, yo, balle=False, spin=False): position_arrivee = Balle(x, y) vecteur = position_arrivee.position - self.positionc distance, phi = c.polar(vecteur) o = c.polar(complex(xo, yo) - self.positionc)[1] delta = (o - self.orientation) delta += np.pi delta = delta % (2 * np.pi) delta -= np.pi vitesse_angulaire = min(sat_vitesse_angulaire, abs(delta) * K_angulaire) * np.sign(delta) distance_autres_joueurs = [] distance_autres_joueurs_balle = [] for robot in self.match.joueurs: if robot != self: distance_autres_joueurs.append( self.distanceToXY(robot.positionc)) distance_autres_joueurs_balle.append( robot.distanceToXY(robot.match.balle.position)) #si l'objectif est proche mais un autre robot aussi alors on fait abstraction des champs repulsifs if (min(distance_autres_joueurs) > distance) & (min(distance_autres_joueurs_balle) < 300) & (distance < 300) & balle: # print("oui") p = psl.packetCommandBot(self.team == 'Y', id=self.id, veltangent=K_proche * np.cos(delta), velnormal=K_proche * np.sin(delta), velangular=vitesse_angulaire, spinner=True) grSim.send(p) return 'EN COURS' elif (abs(distance) > 35) | (abs(delta) > 0.05): #champ créé par l'objectif Exb, Eyb = pt.Gradient(position_arrivee.potentiel) #Calcul des positions x,y discrètes sur la grille xd = int((self.x + longueur) / (2 * longueur) * nbPoints) yd = int((self.y + largeur) / (2 * largeur) * nbPoints) # print(Ex[yd,xd],xd,self.x,self.y) a, b = np.polyfit([x, self.x], [y, self.y], 1) self.champ_autre(self.x, x, y, a, b) Ex, Ey = Exb + k * self.Ex_autre, Eyb + k * self.Ey_autre #champ surfaces interdites Ex += 2 * surface_x Ey += 2 * surface_y Ex, Ey = pt.norme(Ex, Ey) # #Affichage du champ # fig = plt.figure() # ax1 = fig.add_subplot(1, 1, 1) # ax1.quiver(x_grid,y_grid,Ex,Ey,width=0.0008) #Saturation vitesse_tangente = min( saturation_vitesse_tangente, K_max * (np.sin(self.orientation) * Ey[yd, xd] + np.cos(self.orientation) * Ex[yd, xd])) vitesse_normale = min( saturation_vitesse_normale, K_max * (-np.sin(self.orientation) * Ex[yd, xd] + np.cos(self.orientation) * Ey[yd, xd])) if distance < seuil_distance: vitesse_tangente = vitesse_tangente * distance / seuil_distance vitesse_normale = vitesse_normale * distance / seuil_distance spin = True p = psl.packetCommandBot(self.team == 'Y', id=self.id, veltangent=vitesse_tangente, velnormal=vitesse_normale, velangular=vitesse_angulaire, spinner=spin) grSim.send(p) return 'EN COURS' #Objectif atteint else: p = psl.packetCommandBot(self.team == 'Y', id=self.id, veltangent=0., velnormal=0, velangular=0., spinner=True) grSim.send(p) return 'DONE'
def commande_position(self, x, y, xo, yo, balle=False, spin=False): self.goto = complex(x, y) position_arrivee = match.Balle(x, y) vecteur = position_arrivee.position - self.positionc distance, phi = c.polar(vecteur) o = c.polar(complex(xo, yo) - self.positionc)[1] delta = (o - self.orientation) delta += np.pi delta = delta % (2 * np.pi) delta -= np.pi vitesse_angulaire = min(p.sat_vitesse_angulaire, abs(delta) * p.K_angulaire) * np.sign(delta) distance_autres_joueurs = [] distance_autres_joueurs_balle = [] for robot in self.match.joueurs: if robot != self: distance_autres_joueurs.append( self.distanceToXY(robot.positionc)) distance_autres_joueurs_balle.append( robot.distanceToXY(position_arrivee.position)) #si l'objectif est proche mais un autre robot aussi alors on fait abstraction des champs repulsifs if (min(distance_autres_joueurs) > distance) & ( min(distance_autres_joueurs_balle) < 300) & (distance < 300): self.commande_robot(p.K_proche * np.cos(phi - self.orientation), p.K_proche * np.sin(phi - self.orientation), vitesse_angulaire, spinner=(balle or spin)) return 'EN COURS' elif (abs(distance) > 35) | (abs(delta) > 0.05): #champ créé par l'objectif Exb, Eyb = pt.Gradient(position_arrivee.potentiel) #Calcul des positions x,y discrètes sur la grille xd = int((self.x + p.longueur) / (2 * p.longueur) * p.nbPoints) yd = int((self.y + p.largeur) / (2 * p.largeur) * p.nbPoints) # print(Ex[yd,xd],xd,self.x,self.y) #test si les robots sont sur le terrain if xd < p.nbPoints and yd < p.nbPoints: a, b = np.polyfit([x, self.x], [y, self.y], 1) self.champ_autre(self.x, x, y, a, b) Ex, Ey = Exb + p.k * self.Ex_autre, Eyb + p.k * self.Ey_autre #champ surfaces interdites Ex += 2 * p.surface_x Ey += 2 * p.surface_y Ex, Ey = pt.norme(Ex, Ey) # #Affichage du champ # fig = plt.figure() # ax1 = fig.add_subplot(1, 1, 1) # ax1.quiver(x_grid,y_grid,Ex,Ey,width=0.0008) #Saturation vitesse_tangente = min( p.saturation_vitesse_tangente, p.K_max * (np.sin(self.orientation) * Ey[yd, xd] + np.cos(self.orientation) * Ex[yd, xd])) vitesse_normale = min( p.saturation_vitesse_normale, p.K_max * (-np.sin(self.orientation) * Ex[yd, xd] + np.cos(self.orientation) * Ey[yd, xd])) if distance < p.seuil_distance: vitesse_tangente = vitesse_tangente * distance / p.seuil_distance vitesse_normale = vitesse_normale * distance / p.seuil_distance spin = (balle or spin) self.commande_robot(vitesse_tangente, vitesse_normale, vitesse_angulaire, spinner=spin) return 'EN COURS' #Objectif atteint else: self.commande_robot(0, 0, 0, spinner=True) return 'DONE'
def commande_position(self,x,y,xo,yo,balle=False,spin=False): position_arrivee=Balle(x,y) vecteur=position_arrivee.position-self.positionc distance,phi=c.polar(vecteur) o=c.polar(complex(xo,yo)-self.positionc)[1] delta=(o-self.orientation) delta+=np.pi delta=delta%(2*np.pi) delta-=np.pi vitesse_angulaire=min(sat_vitesse_angulaire,abs(delta)*K_angulaire)*np.sign(delta) distance_autres_joueurs=[] for robot in self.match.joueurs: if robot!=self: distance_autres_joueurs.append(self.distanceToXY(robot.positionc)) #si l'objectif est proche mais un autre robot aussi alors on fait abstraction des champs repulsifs if (min(distance_autres_joueurs)<R_non_evitement) & (distance<250)&balle: p = psl.packetCommandBot(self.team=='Y', id=self.id, veltangent=K_proche*np.cos(delta), velnormal=K_proche*np.sin(delta), velangular=vitesse_angulaire, spinner=True) grSim.send(p) return 'EN COURS' elif (abs(distance)>35)|(abs(delta)>0.05): Exb,Eyb=pt.Gradient(position_arrivee.potentiel) xd=int((self.x+longueur)/(2*longueur)*nbPoints) yd=int((self.y+largeur)/(2*largeur)*nbPoints) # print(Ex[yd,xd],xd,self.x,self.y) a,b=np.polyfit([x,self.x],[y,self.y],1) self.champ_autre(self.x,x,a,b) Ex,Ey=Exb+k*self.Ex_autre,Eyb+k*self.Ey_autre Ex,Ey=pt.norme(Ex,Ey) vitesse_tangente=min(saturation_vitesse_tangente,K_max*(np.sin(self.orientation)*Ey[yd,xd]+np.cos(self.orientation)*Ex[yd,xd])) vitesse_normale=min(saturation_vitesse_normale,K_max*(-np.sin(self.orientation)*Ex[yd,xd]+np.cos(self.orientation)*Ey[yd,xd])) if distance<seuil_distance: vitesse_tangente=vitesse_tangente*distance/seuil_distance vitesse_normale=vitesse_normale*distance/seuil_distance spin=True # if distance<seuil_distance: # vitesse_tangente=vitesse_tangente*distance/seuil_distance # vitesse_normale=vitesse_normale*distance/seuil_distance p = psl.packetCommandBot(self.team=='Y', id=self.id, veltangent=vitesse_tangente, velnormal=vitesse_normale, velangular=vitesse_angulaire, spinner=spin) grSim.send(p) return 'EN COURS' # time.sleep(0.05) else : p = psl.packetCommandBot(self.team=='Y', id=self.id, veltangent=0., velnormal=0, velangular=0., spinner=True) grSim.send(p) return 'DONE'
if (a*x+b)>y: Ex,Ey=Ey,-Ex else : Ex,Ey=-Ey,Ex return Ex,Ey r_evit=120 k=2 #facteur de repulsion for bot in blueBots: if bot[5]==1: botInfo=bot xbot1,ybot1,obot=botInfo[6],botInfo[7],botInfo[8] potentielBlue1=pt.Potentiel(+5,[xbot1,ybot1],x,y,r_evit) Ex1,Ey1=pt.Gradient(potentielBlue1) Ex10,Ey10=grad(Ex1,Ey1,xbot1,ybot1) potentielBlue1=pt.Potentiel(+5,[xbot1,ybot1],x,y,r_evit/1.5) Ex1,Ey1=pt.Gradient(potentielBlue1) Ex1,Ey1=k*Ex1+Ex10,k*Ey1+Ey10 for bot in yellowBots: if bot[5]==1: botInfo=bot xbot2,ybot2,obot=botInfo[6],botInfo[7],botInfo[8] potentielYel1=pt.Potentiel(+5,[xbot2,ybot2],x,y,r_evit) Ex2,Ey2=pt.Gradient(potentielYel1) Ex20,Ey20=grad(Ex2,Ey2,xbot2,ybot2) potentielYel1=pt.Potentiel(+5,[xbot2,ybot2],x,y,r_evit/1.5) Ex2,Ey2=pt.Gradient(potentielYel1)