ropy/rp_lib.py

import bge # Blender Game Engine (UPBGE)
import bpy # Blender
import aud # Sounds
import threading # Multithreading
import trace
import sys
import time
import math
import mathutils
import random
import serial # Liaison série (jumeau numérique)

import rp_map1 as rp_map # Map definition

###############################################################################
# rp_lib.py
# @title: Bibliothèque du Rover Ropy (rp_*)
# @project: Ropy (Blender-EduTech)
# @lang: fr
# @authors: Philippe Roy <philippe.roy@ac-grenoble.fr>
# @copyright: Copyright (C) 2020-2022 Philippe Roy
# @license: GNU GPL
#
# Bibliothèque des actions du robot
#
# Ropy est destiné à la découverte de la programmation procédurale et du language Python.
# A travers plusieurs challenges, donc de manière graduée, les élèves vont apprendre à manipuler les structures algorithmiques de base et à les coder en Python.
###############################################################################

scene = bge.logic.getCurrentScene()
debug_mvt = scene.objects['Terrain']['debug_mvt']

# Sounds
audiodev = aud.Device()
snd_click = aud.Sound('asset/sounds/rp_click.ogg')

# Threads
threads_cmd=[]
threads_gostore=[]
debug_thread = scene.objects['Terrain']['debug_thread']

# Jumeau numérique
twins_serial = None

# UPBGE constants
JUST_ACTIVATED = bge.logic.KX_INPUT_JUST_ACTIVATED
JUST_RELEASED = bge.logic.KX_INPUT_JUST_RELEASED
ACTIVATE = bge.logic.KX_INPUT_ACTIVE
# JUST_DEACTIVATED = bge.logic.KX_SENSOR_JUST_DEACTIVATED

###############################################################################
# Méthode kill pour les tâches (threads)
###############################################################################

class thread_with_trace(threading.Thread):
    def __init__(self, *args, **keywords):
        threading.Thread.__init__(self, *args, **keywords)
        self.killed = False
      
    def start(self):
        self.__run_backup = self.run
        self.run = self.__run
        threading.Thread.start(self)

    def __run(self):
        sys.settrace(self.globaltrace)
        self.__run_backup()
        self.run = self.__run_backup
     
    def globaltrace(self, frame, event, arg):
        if event == 'call':
            return self.localtrace
        else:
            return None

    def localtrace(self, frame, event, arg):
        if self.killed:
            if event == 'line':
                raise SystemExit()
        return self.localtrace

    def kill(self):
        self.killed = True

###############################################################################
# Start et stop des tâches (threads)
###############################################################################

def thread_start(threads, type_txt, fct):
    threads.append(thread_with_trace(target = fct))
    threads[len(threads)-1].start()
    if (debug_thread):
        print ("Thread",type_txt, "#", len(threads)-1, "open.")

def thread_stop(threads, type_txt):
    i=0
    zombie_flag=False
    for t in threads:
        if not t.is_alive():
            if (debug_thread):
                print ("Thread",type_txt, "#",i,"closed.")
        else:
            if (debug_thread):
                print ("Thread",type_txt, "#",i,"still open ...")
            t.kill()
            t.join()
            if not t.is_alive():
                if (debug_thread):
                    print ("Thread",type_txt, "#",i,"killed.")
            else:
                if (debug_thread):
                    print ("Thread",type_txt, "#",i,"zombie...")
                zombie_flag=True
        i +=1
    if zombie_flag==False:
        if (debug_thread):
            print ("All threads",type_txt, "are closed.")
        scene.objects['Terrain']['thread_cmd']=False
        return True
    else:
        if (debug_thread):
            print ("There are zombies threads",type_txt, ".")
        return False

def thread_cmd_start(fct):
    thread_start(threads_cmd, "commands", fct)

def thread_cmd_stop():
    thread_stop(threads_cmd, "commands")

def thread_gostore_start(fct):
    thread_start(threads_gostore, "go store", fct)

def thread_gostore_stop():
    thread_stop(threads_gostore, "go store")

def rp_end():

    # Jumeau numérique
    if scene.objects['Commands']['twins']:
        serial_msg = "FI\n"
        twins_serial.write(serial_msg.encode()) # Communication série : modele 3d -> carte communication ( arduino | micro:bit )
        rp_jumeau_close()

    # Thread
    if (debug_thread):
        print ("Thread commands is arrived.")
    time.sleep(0.125)
    scene.objects['Terrain']['thread_cmd']=False
    time.sleep(0.125)

def rp_fin():
    rp_end()

def rp_quit():
    rp_end()

###############################################################################
# Sounds
###############################################################################

# FIXME : Sound crash in Windows (very strange : blender, UPBGE, python ?), no music for Bill
def sound_play (sound):
    if scene.objects['Commands']['sound'] and sys.platform!="win32":
        audiodev.play(sound)

###############################################################################
# Rover fonction élèves
###############################################################################

##
# Avancer le rover
##

def rp_avancer ():
    obj=scene.objects['Rover']
    
    # Pas de mouvement si colision ou objectif
    if obj['stop']:
        return False

    # Contrôle colision
    x0 = obj.worldPosition.x
    y0 = obj.worldPosition.y
    z0 = obj.worldPosition.z
    if round(obj.worldOrientation.to_euler().z, 2) == 0.00:  # Sud
        x1 = x0
        y1 = y0-1
    if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi,2) or round(obj.worldOrientation.to_euler().z, 2) == - round(math.pi,2) :  # Nord
        x1 = x0
        y1 = y0+1
    if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == -round(3*(math.pi/2),2) :  # Est
        x1 = x0+1
        y1 = y0
    if round(obj.worldOrientation.to_euler().z, 2) == round(-math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == round(3*(math.pi/2),2) :  # Ouest
        x1 = x0-1
        y1 = y0
    if [x1,y1] in scene.objects['Terrain']['map_tile_montain']:
        print ("Crash dans la montagne !")
        rover_colision_montain (False)
        obj['stop'] = True
    if [x1,y1] in scene.objects['Terrain']['map_tile_station']:
        print ("Crash dans la station !")
        rover_colision_station (False)
        obj['stop'] = True
    if x1 < scene.objects['Terrain']['size'][0] or x1 > scene.objects['Terrain']['size'][2] or y1 < scene.objects['Terrain']['size'][1] or y1 > scene.objects['Terrain']['size'][3] :
        print ("Sortie de carte !")
        obj['stop'] = True
    if obj['stop']:
        return False

    # Points, console et jumeau numérique
    if debug_mvt:
        print ("rp_avancer()")
    scene.objects['Points']['step'] +=1
    if scene.objects['Commands']['twins']:
        serial_msg = "AV\n"
        twins_serial.write(serial_msg.encode()) # Communication série : modele 3d -> carte communication ( arduino | micro:bit )

    # Animation rapide
    if scene.objects['Commands']['speed'] >= 10 and scene.objects['Points']['step']> 2: #  A tendance à planter sur les premiers mouvements en rapide + balisage
        x0 = obj.worldPosition.x
        y0 = obj.worldPosition.y
        z0 = obj.worldPosition.z
        if round(obj.worldOrientation.to_euler().z, 2) == 0.00:  # Sud
            obj.worldPosition=[x0, y0-1, z0]
        if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi,2) or round(obj.worldOrientation.to_euler().z, 2) == - round(math.pi,2) :  # Nord
            obj.worldPosition=[x0, y0+1, z0]
        if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == -round(3*(math.pi/2),2) :  # Est
            obj.worldPosition=[x0+1, y0, z0]
        if round(obj.worldOrientation.to_euler().z, 2) == round(-math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == round(3*(math.pi/2),2) :  # Ouest
            obj.worldPosition=[x0-1, y0, z0]
        rp_tempo (0.1)

    # FIXME : Animation sacadée
    # step =1/100
    # print (obj.worldOrientation.to_euler().z)
    # x0 = obj.worldPosition.x
    # y0 = obj.worldPosition.y
    # z0 = obj.worldPosition.z
    # for i in range (100) :
    #     if round(obj.worldOrientation.to_euler().z, 2) == 0.00:  # Sud
    #         obj.worldPosition=[x0, y0-step*i, z0]
    #     if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi,2) or round(obj.worldOrientation.to_euler().z, 2) == - round(math.pi,2) :  # Nord
    #         obj.worldPosition=[x0, y0+step*i, z0]
    #     if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == -round(3*(math.pi/2),2) :  # Est
    #         obj.worldPosition=[x0+step*i, y0, z0]
    #     if round(obj.worldOrientation.to_euler().z, 2) == round(-math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == round(3*(math.pi/2),2) :  # Ouest
    #         obj.worldPosition=[x0-step*i, y0, z0]
    #     rp_tempo (0.1*step)

    # Animation
    if scene.objects['Commands']['speed'] < 10:
        start = 1
        end = 100
        layer = 0
        priority = 1
        blendin = 1.0
        mode = bge.logic.KX_ACTION_MODE_PLAY
        layerWeight = 0.0
        ipoFlags = 0
        speed = scene.objects['Commands']['speed']*8
        if round(obj.worldOrientation.to_euler().z, 2) == 0.00:  # Sud
            obj.playAction('Rover-Avancer-Y-', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
            # obj.worldPosition=[x0, y0-step*i, z0]
        if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi,2) or round(obj.worldOrientation.to_euler().z, 2) == - round(math.pi,2) :  # Nord
            obj.playAction('Rover-Avancer-Y+', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
            # obj.worldPosition=[x0, y0+step*i, z0]
        if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == -round(3*(math.pi/2),2) :  # Est
            obj.playAction('Rover-Avancer-X+', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
            # obj.worldPosition=[x0+step*i, y0, z0]
        if round(obj.worldOrientation.to_euler().z, 2) == round(-math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == round(3*(math.pi/2),2) :  # Ouest
            obj.playAction('Rover-Avancer-X-', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
            # obj.worldPosition=[x0-step*i, y0, z0]
        scene.objects['Rv-Wheel-right-front'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-right-mid'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-right-rear'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-front'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-mid'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-rear'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        while scene.objects['Rv-Wheel-right-front'].isPlayingAction(): # Forçage du redraw
        #     scene.objects['Camera'].applyMovement((0, 0, 0), True)
            scene.objects['Sun'].applyMovement((0, 0, 0), True)
        rp_tempo (0.1)

    # Contrôle objectif
    if rp_map.objectif_control(x1,y1):
        rover_goal ()
    return True


##
# Reculer le rover
##

def rp_reculer ():
    obj=scene.objects['Rover']
    
    # Pas de mouvement si colision ou objectif
    if obj['stop']:
        return False

    # Contrôle colision
    x0 = obj.worldPosition.x
    y0 = obj.worldPosition.y
    z0 = obj.worldPosition.z
    if round(obj.worldOrientation.to_euler().z, 2) == 0.00:  # Sud
        x1 = x0
        y1 = y0+1
    if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi,2) or round(obj.worldOrientation.to_euler().z, 2) == - round(math.pi,2) :  # Nord
        x1 = x0
        y1 = y0-1
    if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == -round(3*(math.pi/2),2) :  # Est
        x1 = x0-1
        y1 = y0
    if round(obj.worldOrientation.to_euler().z, 2) == round(-math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == round(3*(math.pi/2),2) :  # Ouest
        x1 = x0+1
        y1 = y0
    if [x1,y1] in scene.objects['Terrain']['map_tile_montain']:
        print ("Crash dans la montagne !")
        rover_colision_montain (True)
        obj['stop'] = True
    if [x1,y1] in scene.objects['Terrain']['map_tile_station']:
        print ("Crash dans la station !")
        rover_colision_station (True) # FIXME à faire en recul
        obj['stop'] = True
    if x1 < scene.objects['Terrain']['size'][0] or x1 > scene.objects['Terrain']['size'][2] or y1 < scene.objects['Terrain']['size'][1] or y1 > scene.objects['Terrain']['size'][3] :
        print ("Sortie de carte !") # FIXME à faire en recul
        obj['stop'] = True
    if obj['stop']:
        return False

    # Points, console et jumeau numérique
    if (debug_mvt):
        print ("rp_reculer()")
    scene.objects['Points']['step'] +=1
    if scene.objects['Commands']['twins']:
        serial_msg = "RE\n"
        twins_serial.write(serial_msg.encode()) # Communication série : modele 3d -> carte communication ( arduino | micro:bit )

    # Animation rapide
    if scene.objects['Commands']['speed'] >= 10 and scene.objects['Points']['step']> 2: #  A tendance à planter sur les premiers mouvements en rapide + balisage
        x0 = obj.worldPosition.x
        y0 = obj.worldPosition.y
        z0 = obj.worldPosition.z
        if round(obj.worldOrientation.to_euler().z, 2) == 0.00:  # Sud
            obj.worldPosition=[x0, y0+1, z0]
        if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi,2) or round(obj.worldOrientation.to_euler().z, 2) == - round(math.pi,2) :  # Nord
            obj.worldPosition=[x0, y0-1, z0]
        if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == -round(3*(math.pi/2),2) :  # Est
            obj.worldPosition=[x0-1, y0, z0]
        if round(obj.worldOrientation.to_euler().z, 2) == round(-math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == round(3*(math.pi/2),2) :  # Ouest
            obj.worldPosition=[x0+1, y0, z0]
        rp_tempo (0.1)

    # Animation
    if scene.objects['Commands']['speed'] < 10:
        start = 1
        end = 100
        layer = 0
        priority = 1
        blendin = 1.0
        mode = bge.logic.KX_ACTION_MODE_PLAY
        layerWeight = 0.0
        ipoFlags = 0
        speed = scene.objects['Commands']['speed']*8
        if round(obj.worldOrientation.to_euler().z, 2) == 0.00:  # Sud
            obj.playAction('Rover-Avancer-Y+', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi,2) or round(obj.worldOrientation.to_euler().z, 2) == - round(math.pi,2) :  # Nord
            obj.playAction('Rover-Avancer-Y-', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == -round(3*(math.pi/2),2) :  # Est
            obj.playAction('Rover-Avancer-X-', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        if round(obj.worldOrientation.to_euler().z, 2) == round(-math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == round(3*(math.pi/2),2) :  # Ouest
            obj.playAction('Rover-Avancer-X+', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-right-front'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-right-mid'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-right-rear'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-front'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-mid'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-rear'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        while scene.objects['Rv-Wheel-right-front'].isPlayingAction(): # Forçage du redraw
            scene.objects['Sun'].applyMovement((0, 0, 0), True)
        rp_tempo (0.1)

    # Contrôle objectif
    if rp_map.objectif_control(x1,y1):
        rover_goal ()
    return True

##
# Tourner à gauche
##

def rp_gauche ():
    obj=scene.objects['Rover']

    # Pas de mouvement si colision ou objectif
    if obj['stop']:
        return False

    # Points, console et jumeau numérique
    if (debug_mvt):
        print ("rp_gauche()")
    scene.objects['Points']['step'] +=1
    step=math.pi/2 # Pas angulaire
    if scene.objects['Commands']['twins']:
        serial_msg = "GA\n"
        twins_serial.write(serial_msg.encode()) # Communication série : modele 3d -> carte communication ( arduino | micro:bit )

    # Animation rapide
    if scene.objects['Commands']['speed'] >= 10:
        obj.applyRotation((0, 0, step), True)
        rp_tempo (0.1)
        return True

    # Animation
    start = 1
    end = 100
    layer = 0
    priority = 1
    blendin = 1.0
    mode = bge.logic.KX_ACTION_MODE_PLAY
    layerWeight = 0.0
    ipoFlags = 0
    speed = scene.objects['Commands']['speed']*8
    obj.playAction('Rover-Gauche', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    scene.objects['Rv-Wheel-right-front'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    scene.objects['Rv-Wheel-right-mid'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    scene.objects['Rv-Wheel-right-rear'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    scene.objects['Rv-Wheel-left-front'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    scene.objects['Rv-Wheel-left-mid'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    scene.objects['Rv-Wheel-left-rear'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    while scene.objects['Rv-Wheel-right-front'].isPlayingAction(): # Forçage du redraw
    #     scene.objects['Camera'].applyMovement((0, 0, 0), True)
        scene.objects['Sun'].applyMovement((0, 0, 0), True)
    rp_tempo (0.1)
    return True

##
# Tourner à droite
##

def rp_droite ():
    obj=scene.objects['Rover']

    # Pas de mouvement si colision ou objectif
    if obj['stop']:
        return False

    # Points, console et jumeau numérique
    if (debug_mvt):
        print ("rp_droite()")
    scene.objects['Points']['step'] +=1
    step=math.pi/2 # Pas angulaire
    if scene.objects['Commands']['twins']:
        serial_msg = "DR\n"
        twins_serial.write(serial_msg.encode()) # Communication série : modele 3d -> carte communication ( arduino | micro:bit )

    # Rapide
    if scene.objects['Commands']['speed'] >= 10:
        obj.applyRotation((0, 0, -step), True)
        rp_tempo (0.1)
        return True

    # Animation
    start = 1
    end = 100
    layer = 0
    priority = 1
    blendin = 1.0
    mode = bge.logic.KX_ACTION_MODE_PLAY
    layerWeight = 0.0
    ipoFlags = 0
    speed = scene.objects['Commands']['speed']*8
    obj.playAction('Rover-Droite', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    scene.objects['Rv-Wheel-right-front'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    scene.objects['Rv-Wheel-right-mid'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    scene.objects['Rv-Wheel-right-rear'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    scene.objects['Rv-Wheel-left-front'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    scene.objects['Rv-Wheel-left-mid'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    scene.objects['Rv-Wheel-left-rear'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    while scene.objects['Rv-Wheel-right-front'].isPlayingAction(): # Forçage du redraw
    #     scene.objects['Camera'].applyMovement((0, 0, 0), True)
        scene.objects['Sun'].applyMovement((0, 0, 0), True)
    rp_tempo (0.1)
    return True

##
# Marquer
##

def rp_marquer ():
    obj=scene.objects['Rover']

    # Pas de mouvement si colision ou objectif
    if obj['stop']:
        return False

    x = obj.worldPosition.x
    y = obj.worldPosition.y
    z = obj.worldPosition.z

    # Vérification de l'absence de balise sur la tuile
    if [x,y] in scene.objects['Terrain']['map_tile_beacon'] :
        print ("Case déjà marquée !")
        return False

    # Points, console et jumeau numérique
    if (debug_mvt):
        print ("rp_marquer() -> balise #"+ str(len(scene.objects['Terrain']['map_tile_beacon'])))
    if scene.objects['Commands']['twins']:
        serial_msg = "MA\n"
        twins_serial.write(serial_msg.encode()) # Communication série : modele 3d -> carte communication ( arduino | micro:bit )

    # Posage
    rp_tempo (0.1)
    if scene.objects['Points']['upgrade_beacon']:
        beacon_max= 200
    else:
        beacon_max= 20
    for i in range (beacon_max):
        beacon = scene.objects["Beacon-"+str(i)]
        if beacon['activated']==False:
            beacon.worldPosition=[x,y,0.2]
            beacon['activated']=True
            beacon.setVisible(True, True)
            scene.objects['Terrain']['map_tile_beacon'].append([x,y])
            break
    if i ==beacon_max-1 :
        print ("Plus de balise disponible !")
    rp_tempo (0.1)
    return True

##
# Détecter
##

def rp_detect ():
    obj=scene.objects['Rover']

    # Pas de mouvement si colision ou objectif
    if obj['stop']:
        return True

    # Points et console
    if (debug_mvt):
        print ("rp_detect")

    # Détection
    x0 = obj.worldPosition.x
    y0 = obj.worldPosition.y
    z0 = obj.worldPosition.z
    if round(obj.worldOrientation.to_euler().z, 2) == 0.00:  # Sud
        x1 = x0
        y1 = y0-1
    if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi,2) or round(obj.worldOrientation.to_euler().z, 2) == - round(math.pi,2) :  # Nord
        x1 = x0
        y1 = y0+1
    if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == -round(3*(math.pi/2),2) :  # Est
        x1 = x0+1
        y1 = y0
    if round(obj.worldOrientation.to_euler().z, 2) == round(-math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == round(3*(math.pi/2),2) :  # Ouest
        x1 = x0-1
        y1 = y0
    if [x1,y1] in scene.objects['Terrain']['map_tile_montain']:
        if (debug_mvt):
            print ("Présence de montage devant !")
        if scene.objects['Points']['mission']==3: # Contrôle objectif mission 3
            rover_goal ()
        return True
        
    if [x1,y1] in scene.objects['Terrain']['map_tile_station']:
        if (debug_mvt):
            print ("Présence de la station devant !")
        if scene.objects['Points']['mission']==3: # Contrôle objectif mission 3
            rover_goal ()
        return True

    if [x1,y1] in scene.objects['Terrain']['map_tile_station']:
        if (debug_mvt):
            print ("Sortie de carte devant !")
        if scene.objects['Points']['mission']==3: # Contrôle objectif mission 3
            rover_goal ()
        return True
    return False

##
# Prendre
##

def rp_prendre ():
    obj=scene.objects['Rover']

    # Pas de mouvement si colision ou objectif
    if obj['stop']:
        return False

    # Points et console
    if (debug_mvt):
        print ("rp_prendre")
    # FIXME

##
# Radar
##

def rp_radar ():
    obj=scene.objects['Rover']

    # Pas de mouvement si colision ou objectif
    if obj['stop']:
        return False

    # Points et console
    if (debug_mvt):
        print ("rp_radar")
    # FIXME

###############################################################################
# Rover fonction avancées (upgrade) élèves 
###############################################################################

##
# Peindre : station, rover, balise
##

# Couleurs par défaut
color_yellow =  (1, 0.503, 0.018, 1)
color_black =  (0.019794, 0.032076, 0.037408, 1)
color_white =  (0.799, 0.799, 0.799, 1)
color_windows_red =  (0.617, 0.037, 0.019, 1)
color_light_red =  (1, 0.003, 0.012, 1) # Balise
color_stone =  (0.191, 0.227, 0.246, 1)
color_wooddark =  (0.153, 0.117, 0.107, 1)
color_metal =  (0.401, 0.478, 0.518, 1)

# Objets 3D par groupe
paint_part ={}
all_group_part = ("Rover 1", "Rover 2", "Rover 3", "Station 1", "Station 2", "Station 3", "Station 4", "Station cube 1", "Station cube 2", "Stone", "Metal", "Black", "Red windows")
paint_part.update({"Stone" : [['Rv-Wheel-left-front', 'Rv-Wheel-right-front','Rv-Wheel-left-mid', 'Rv-Wheel-right-mid', 'Rv-Wheel-left-rear', 'Rv-Wheel-right-rear'],color_stone]}) # Stone
paint_part.update({"Metal" : [['Rv-Power source', 'Rv-Mast-arm', 'Rv-Antenna-1', 'St-Sheath', 'Rv-Arm-Segment-1', 'Rv-Arm-Segment-2'],color_metal]}) # Metal
paint_part.update({"Black" : [['St-Block1-foot1','St-Block1-foot2','St-Block1-foot3', 'St-Door-2', 'St-Stair-2','St-Cable1','St-Cable2'],color_black]}) # Black
paint_part.update({"Red windows" : [['St-Block1-window','St-Block2-window'],color_windows_red]}) # Red windows

# Rover
rover_partlist =['Rover (Rv-Body-1)', 'Rv-Body-2', 'Rv-Body-3', 'Rv-Body-4', 'Rv-Body-5', 'Rv-Body-6', 'Rv-Body-7',
                 'Rv-Mast', 'Rv-Mast-2', 'Rv-Mast-3', 'Rv-Mast-4', 'Rv-Mast-cap', 'Rv-Mast-arm','Rv-Antenna-1','Rv-Antenna-2','Rv-Antenna-3',
                 'Rv-Arm-Head', 'Rv-Arm-Segment-1', 'Rv-Arm-Segment-2',
                 'Rv-Power source', 'Rv-Power source-2', 'Rv-Power source-3', 'Rv-Power source-4', 'Rv-Power source-5',
                 'Rv-Wheel-left-front', 'Rv-Wheel-right-front','Rv-Wheel-left-mid', 'Rv-Wheel-right-mid', 'Rv-Wheel-left-rear', 'Rv-Wheel-right-rear']
paint_part.update({"Rover 1" : [['Rv-Body-2', 'Rv-Body-7', 'Rv-Power source-2', 'Rv-Power source-5', 'Rv-Mast-3', 'Rv-Mast-4', 'Rv-Mast-cap', 'Rv-Antenna-2'],color_yellow]}) # Jaune
paint_part.update({"Rover 2" : [['Rover', 'Rv-Body-6', 'Rv-Mast', 'Rv-Antenna-3', 'Rv-Arm-Head'],color_white]}) # Blanc
paint_part.update({"Rover 3" : [['Rv-Body-3', 'Rv-Body-4', 'Rv-Power source-3', 'Rv-Power source-4','Rv-Mast-2'],color_wooddark]}) # WoodDark

# Station
station_partlist =['St-Block1', 'St-Block2', 'St-Block3',
                   'St-Block1-side1', 'St-Block1-side2', 'St-Block1-roof', 'St-Block1-foot1', 'St-Block1-foot2','St-Block1-foot3',
                   'St-Door', 'St-Door-2', 'St-Block1-window', 'St-Stair', 'St-Stair-2', 'St-Stair-3','St-Block1-panel-support',
                   'St-Block2-side', 'St-Block2-window', 'St-Block2-roof',
                   'St-Block3-side', 'St-Block3-greenhouse', 'St-Block3-greenhouse-base', 'St-Tunnel', 'St-Tube', 'St-Cable1', 'St-Cable2', 'St-Sheath']
paint_part.update({"Station 1" : [[ 'St-Door','St-Block1-side1','St-Block2-side','St-Block2-roof'],color_yellow]}) # Jaune
paint_part.update({"Station 2" : [[ 'St-Block1','St-Block2','St-Block3','St-Block3-side2','St-Tunnel','St-Tube'],color_white]}) # Blanc
paint_part.update({"Station 3" : [[ 'St-Block1-roof', 'St-Block1-side2', 'St-Block3-side1'],color_stone]}) # Stone
paint_part.update({"Station 4" : [[ 'St-Stair'],color_metal]}) # Metal
paint_part.update({"Station cube 1" : [[ 'St-Cube1-hole','St-Cube2-hole','St-Cube3-hole', 'St-Cube4-hole'],color_yellow]}) # Jaune
paint_part.update({"Station cube 2" : [[ 'St-Cubes','St-Cube2','St-Cube3', 'St-Cube4'],color_white]}) # Blanc

# Balise : Objet lors de l'initialisation
# beacon_partlist =['Beacon_antenne', 'Beacon_sphere']
# paint_part.update({"Balise" : [['Beacon_antenne', 'Beacon_sphere'],color_light_red]}) # Rouge

# Mise en couleur
def rp_couleur (group_part, new_color):
    if scene.objects['Points']['upgrade_paint']:
        if (debug_mvt):
            print ("Nouvelle couleur :", str(new_color),"->", group_part)
        if "Balise" not in group_part :
            for i in range (len(paint_part[group_part][0])):
                scene.objects[paint_part[group_part][0][i]].color = new_color
        else:
            if group_part=="Balises":
                for i in range (200):
                    beacon = scene.objects["Beacon-"+str(i)]
                    beacon.children[0].color = new_color
                    beacon.children[1].color = new_color
            elif "Balise " in group_part :
                beacon = scene.objects[group_part.replace("Balise ", "Beacon-")]
                beacon.children[0].color = new_color
                beacon.children[1].color = new_color

def rp_couleur_detail (part, new_color):
    if scene.objects['Points']['upgrade_paint']:
        if (debug_mvt):
            print ("Nouvelle couleur :", str(new_color),"->", part)
        scene.objects[part].color = new_color

# Initialisation de la couleur
def rp_couleur_init (group_part=None):
    if group_part is not None:
        if scene.objects['Points']['upgrade_paint']:
            if (debug_mvt):
                print ("Réinitialisation couleur ->", group_part)
            for i in range (len(paint_part[group_part][0])):
                scene.objects[paint_part[group_part][0][i]].color = paint_part[group_part][1]
            return (paint_part[group_part][1])
    else:
        for i in range (len(all_group_part)):
            for j in range (len(paint_part[all_group_part[i]][0])):
                scene.objects[paint_part[all_group_part[i]][0][j]].color = paint_part[all_group_part[i]][1]
        for i in range (200):
            beacon = scene.objects["Beacon-"+str(i)]
            beacon.children[0].color = color_light_red
            beacon.children[1].color = color_light_red

# Affichage de la liste des composants 3D
def rp_couleur_listedetail (element, new_color):
    if scene.objects['Points']['upgrade_paint']:
        if element=="Rover":
            print ("Mise en couleur : composants 3D du Rover :", rover_partlist)
        if element=="Station":
            print ("Mise en couleur : composants 3D de la Station :", station_partlist)
        if element=="Balise":
            print ("Mise en couleur :  composants 3D des balises : Beacon-0.children[0|1] à Beacon-200.children[0|1].")

##
# Changer la vitesse
##

def rp_vitesse (new_speed):
    if scene.objects['Points']['upgrade_speed']:
        if new_speed is not None:
            if (debug_mvt):
                print ("Nouvelle vitesse :", new_speed)
            scene.objects['Text_speed']['Text']=str(new_speed)
            scene.objects['Commands']['speed']=new_speed
        return scene.objects['Commands']['speed']

##
# Connaitre le nombre de balise posées
##

def rp_balise ():
    if scene.objects['Points']['upgrade_beacon']:
        for i in range (200):
            if scene.objects["Beacon-"+str(i)]['activated']==False:
                break
        if (debug_mvt):
            print ("Nombre de balises posées :", i)
        return i

##
# Connaitre la charge de la batterie
##

def rp_batterie ():
    if scene.objects['Points']['upgrade_battery']:
        if (debug_mvt):
            print ("Charge de la batterie :", scene.objects['Points']['battery'])
        return scene.objects['Points']['battery']

###############################################################################
# Colision
###############################################################################

##
# Montagne
##

def rover_colision_montain (back):
    obj=scene.objects['Rover']

    # Jumeau numérique
    if scene.objects['Commands']['twins']:
        serial_msg = "CO\n"
        twins_serial.write(serial_msg.encode()) # Communication série : modele 3d -> carte communication ( arduino | micro:bit )

    # Animation
    start = 1
    end = 120
    layer = 0
    priority = 1
    blendin = 1.0
    mode = bge.logic.KX_ACTION_MODE_PLAY
    layerWeight = 0.0
    ipoFlags = 0
    speed = scene.objects['Commands']['speed']*2

    if back==False: # Crash en avant
        if round(obj.worldOrientation.to_euler().z, 2) == 0.00:  # Sud
            obj.playAction('Rover-Crash-Y-', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi,2) or round(obj.worldOrientation.to_euler().z, 2) == - round(math.pi,2) :  # Nord
            obj.playAction('Rover-Crash-Y+', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == -round(3*(math.pi/2),2) :  # Est
            obj.playAction('Rover-Crash-X+', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        if round(obj.worldOrientation.to_euler().z, 2) == round(-math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == round(3*(math.pi/2),2) :  # Ouest
            obj.playAction('Rover-Crash-X-', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-right-front'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-right-mid'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-right-rear'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-front'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-mid'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-rear'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    else: # Crash en arrière
        if round(obj.worldOrientation.to_euler().z, 2) == 0.00:  # Sud
            obj.playAction('Rover-Crash-Back-Y+', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi,2) or round(obj.worldOrientation.to_euler().z, 2) == - round(math.pi,2) :  # Nord
            obj.playAction('Rover-Crash-Back-Y-', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == -round(3*(math.pi/2),2) :  # Est
            obj.playAction('Rover-Crash-Back-X-', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        if round(obj.worldOrientation.to_euler().z, 2) == round(-math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == round(3*(math.pi/2),2) :  # Ouest
            obj.playAction('Rover-Crash-Back-X+', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-right-front'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-right-mid'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-right-rear'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-front'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-mid'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-rear'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)

    # Forçage du redraw
    while scene.objects['Rv-Wheel-right-front'].isPlayingAction(): # Forçage du redraw
        scene.objects['Sun'].applyMovement((0, 0, 0), True)
    rp_tempo (0.1)
    return True

##
# Station
##

def rover_colision_station (back):
    obj=scene.objects['Rover']

    # Jumeau numérique
    if scene.objects['Commands']['twins']:
        serial_msg = "CO\n"
        twins_serial.write(serial_msg.encode()) # Communication série : modele 3d -> carte communication ( arduino | micro:bit )

    # Animation
    start = 1
    end = 120
    layer = 0
    priority = 1
    blendin = 1.0
    mode = bge.logic.KX_ACTION_MODE_PLAY
    layerWeight = 0.0
    ipoFlags = 0
    speed = scene.objects['Commands']['speed']*2

    if back==False: # Crash en avant
        if round(obj.worldOrientation.to_euler().z, 2) == 0.00:  # Sud
            obj.playAction('Rover-CrashStation-Y-', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi,2) or round(obj.worldOrientation.to_euler().z, 2) == - round(math.pi,2) :  # Nord
            obj.playAction('Rover-CrashStation-Y+', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == -round(3*(math.pi/2),2) :  # Est
            obj.playAction('Rover-CrashStation-X+', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        if round(obj.worldOrientation.to_euler().z, 2) == round(-math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == round(3*(math.pi/2),2) :  # Ouest
            obj.playAction('Rover-CrashStation-X-', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-right-front'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-right-mid'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-right-rear'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-front'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-mid'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-rear'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    else:
        if round(obj.worldOrientation.to_euler().z, 2) == 0.00:  # Sud
            obj.playAction('Rover-CrashStation-Back-Y+', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi,2) or round(obj.worldOrientation.to_euler().z, 2) == - round(math.pi,2) :  # Nord
            obj.playAction('Rover-CrashStation-Back-Y-', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == -round(3*(math.pi/2),2) :  # Est
            obj.playAction('Rover-CrashStation-Back-X-', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        if round(obj.worldOrientation.to_euler().z, 2) == round(-math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == round(3*(math.pi/2),2) :  # Ouest
            obj.playAction('Rover-CrashStation-Back-X+', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-right-front'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-right-mid'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-right-rear'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-front'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-mid'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-rear'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)

    # Forçage du redraw
    while scene.objects['Rv-Wheel-right-front'].isPlayingAction():
        scene.objects['Sun'].applyMovement((0, 0, 0), True)
    rp_tempo (0.1)
    return True

###############################################################################
# Goal
###############################################################################

##
# Fin de mission
##

def rover_goal ():
    obj=scene.objects['Rover']
    if (debug_mvt):
        print ("Goal !!")
    obj['stop'] = True

    # Animation
    start = 1
    end = 160
    layer = 0
    priority = 1
    blendin = 1.0
    mode = bge.logic.KX_ACTION_MODE_PLAY
    layerWeight = 0.0
    ipoFlags = 0
    speed = scene.objects['Commands']['speed']*4
    if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi,2) or round(obj.worldOrientation.to_euler().z, 2) == - round(math.pi,2) :  # Nord
        rp_gauche()
        rp_gauche()
    if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == -round(3*(math.pi/2),2) :  # Est
        rp_droite()
    if round(obj.worldOrientation.to_euler().z, 2) == round(-math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == round(3*(math.pi/2),2) :  # Ouest
        rp_gauche()

    speed = scene.objects['Commands']['speed']
    scene.objects['Rover'].playAction('Rover-Aim1', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    scene.objects['Rv-Mast'].playAction('Mast-Aim1', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    scene.objects['Rv-Mast-cap'].playAction('Mast-cap-Aim1', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    scene.objects['Rv-Wheel-right-front'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    scene.objects['Rv-Wheel-right-mid'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    scene.objects['Rv-Wheel-right-rear'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    scene.objects['Rv-Wheel-left-front'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    scene.objects['Rv-Wheel-left-mid'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    scene.objects['Rv-Wheel-left-rear'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
    while scene.objects['Rv-Wheel-right-front'].isPlayingAction(): # Forçage du redraw
        scene.objects['Sun'].applyMovement((0, 0, 0), True)
    rp_tempo (0.1)

    # Revenir à une position propre
    x0 = obj.worldPosition.x
    y0 = obj.worldPosition.y
    obj.worldPosition.x = round(x0)
    obj.worldPosition.y = round(y0)

    # Level
    if scene.objects['Points']['mission']==scene.objects['Points']['level']:
        scene.objects['Points']['level']+=1

##
# Forage
##

def rover_drill (x,y):
    obj=scene.objects['Rover']
    if (debug_mvt):
        print ("Goal : ", [x,y])

    # Animation
    if scene.objects['Commands']['speed'] < 10:

        # Recul d'une 1/2 case
        start = 1
        end = 50
        layer = 0
        priority = 1
        blendin = 1.0
        mode = bge.logic.KX_ACTION_MODE_PLAY
        layerWeight = 0.0
        ipoFlags = 0
        speed = scene.objects['Commands']['speed']*8
        if round(obj.worldOrientation.to_euler().z, 2) == 0.00:  # Sud
            obj.playAction('Rover-Avancer-Y+', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi,2) or round(obj.worldOrientation.to_euler().z, 2) == - round(math.pi,2) :  # Nord
            obj.playAction('Rover-Avancer-Y-', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == -round(3*(math.pi/2),2) :  # Est
            obj.playAction('Rover-Avancer-X-', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        if round(obj.worldOrientation.to_euler().z, 2) == round(-math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == round(3*(math.pi/2),2) :  # Ouest
            obj.playAction('Rover-Avancer-X+', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-right-front'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-right-mid'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-right-rear'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-front'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-mid'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-rear'].playAction('Wheel-Reculer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        while scene.objects['Rv-Wheel-right-front'].isPlayingAction(): # Forçage du redraw
            scene.objects['Sun'].applyMovement((0, 0, 0), True)
        rp_tempo (0.1)

        # Forage
        start = 1
        end = 300
        layer = 0
        priority = 1
        blendin = 1.0
        mode = bge.logic.KX_ACTION_MODE_PLAY
        layerWeight = 0.0
        ipoFlags = 0
        speed = scene.objects['Commands']['speed']*8
        scene.objects['Rv-Arm-Joint-1'].playAction('Arm-Joint-1-Drill', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Arm-Segment-1'].playAction('Arm-Segment-1-Drill', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Arm-Segment-2'].playAction('Arm-Segment-2-Drill', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Arm-Joint-2'].playAction('Arm-Joint-2-Drill', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Arm-Head'].playAction('Arm-Head-Drill', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        while scene.objects['Rv-Arm-Joint-1'].isPlayingAction(): # Forçage du redraw
            scene.objects['Sun'].applyMovement((0, 0, 0), True)
        rp_tempo (0.1)

        # Jumeau numérique
        if scene.objects['Commands']['twins']:
            serial_msg = "FO\n"
            twins_serial.write(serial_msg.encode()) # Communication série : modele 3d -> carte communication ( arduino | micro:bit )

    # Tuile
    for i in range (10):
        if scene.objects['Drill_tile-'+str(i)].visible==False:
            scene.objects['Drill_tile-'+str(i)].worldPosition.x = x
            scene.objects['Drill_tile-'+str(i)].worldPosition.y = y
            scene.objects['Drill_tile-'+str(i)].setVisible(True,True)
            rp_tempo (0.1)
            break
    for obj_i in scene.objects:
        if "tile_dirtHigh" in obj_i.name:
            if round(obj_i.worldPosition.x) == x and round(obj_i.worldPosition.y) == y :
                obj_i.setVisible(False, True)
                rp_tempo (0.1)

    if scene.objects['Commands']['speed'] < 10:

        # Fin de forage
        start = 300
        end = 600
        layer = 0
        priority = 1
        blendin = 1.0
        mode = bge.logic.KX_ACTION_MODE_PLAY
        layerWeight = 0.0
        ipoFlags = 0
        speed = scene.objects['Commands']['speed']*8
        scene.objects['Rv-Arm-Joint-1'].playAction('Arm-Joint-1-Drill', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Arm-Segment-1'].playAction('Arm-Segment-1-Drill', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Arm-Segment-2'].playAction('Arm-Segment-2-Drill', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Arm-Joint-2'].playAction('Arm-Joint-2-Drill', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Arm-Head'].playAction('Arm-Head-Drill', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        while scene.objects['Rv-Arm-Joint-1'].isPlayingAction(): # Forçage du redraw
            scene.objects['Sun'].applyMovement((0, 0, 0), True)
        rp_tempo (0.1)
        
        # Avance d'une 1/2 case
        start = 1
        end = 50
        layer = 0
        priority = 1
        blendin = 1.0
        mode = bge.logic.KX_ACTION_MODE_PLAY
        layerWeight = 0.0
        ipoFlags = 0
        speed = scene.objects['Commands']['speed']*8
        if round(obj.worldOrientation.to_euler().z, 2) == 0.00:  # Sud
            obj.playAction('Rover-Avancer-Y-', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi,2) or round(obj.worldOrientation.to_euler().z, 2) == - round(math.pi,2) :  # Nord
            obj.playAction('Rover-Avancer-Y+', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        if round(obj.worldOrientation.to_euler().z, 2) == round(math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == -round(3*(math.pi/2),2) :  # Est
            obj.playAction('Rover-Avancer-X+', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        if round(obj.worldOrientation.to_euler().z, 2) == round(-math.pi/2,2) or round(obj.worldOrientation.to_euler().z, 2) == round(3*(math.pi/2),2) :  # Ouest
            obj.playAction('Rover-Avancer-X-', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-right-front'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-right-mid'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-right-rear'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-front'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-mid'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        scene.objects['Rv-Wheel-left-rear'].playAction('Wheel-Avancer', start, end, layer, priority, blendin, mode, layerWeight, ipoFlags, speed)
        while scene.objects['Rv-Wheel-right-front'].isPlayingAction(): # Forçage du redraw
            scene.objects['Sun'].applyMovement((0, 0, 0), True)
        rp_tempo (0.1)

    # # Tuile
    # for i in range (10):
    #     if scene.objects['Drill_tile-'+str(i)].visible==False:
    #         scene.objects['Drill_tile-'+str(i)].worldPosition.x = x
    #         scene.objects['Drill_tile-'+str(i)].worldPosition.y = y
    #         scene.objects['Drill_tile-'+str(i)].setVisible(True,True)
    #         break
    # for obj_i in scene.objects:
    #     if "tile_dirtHigh" in obj_i.name:
    #         if round(obj_i.worldPosition.x) == x and round(obj_i.worldPosition.y) == y :
    #             obj_i.setVisible(False, True)

###############################################################################
# Temporisation
###############################################################################

# Temporisation basée sur l'horloge de l'OS
def rp_sleep (duration):
    time.sleep(duration)

# Temporisation basée par l'horloge de UPBGE
def rp_tempo (duration):
    # time.sleep(duration*(1/scene.objects['Commands']['speed']))
    scene.objects['Commands']['time']=0
    while scene.objects['Commands']['time']<duration*(1/scene.objects['Commands']['speed']):
        # print("Temporization commands :",scene.objects['Terrain']['delay_cmd'])
        time.sleep(0.001)

###############################################################################
# Jumeau numérique
###############################################################################

# Paramétrage de la communication avec la carte de communication (Arduino, Micro:bit)

def  rp_jumeau(port, speed):
    global twins_serial
    scene.objects['Points-Twins'].setVisible(True,True)
    scene.objects['Points-Twins-text'].setVisible(True,False)

    twins_serial = getSerialOrNone(port,speed)
    if twins_serial is not None:
        # twins_serial.set_buffer_size(rx_size = 12800, tx_size = 12800)
        scene.objects['Commands']['twins'] = True
        scene.objects['Commands']['twins_port'] = port
        scene.objects['Commands']['twins_speed'] = speed
        scene.objects['Points-Twins-text']['Text'] = "Connection\nouverte :\n"+port+"\n à "+str(speed)+" baud"
        print (twins_serial)
    else:
        scene.objects['Commands']['twins'] = False
        scene.objects['Points-Twins-text']['Text'] = "Port pas \nprêt"

    # ser = serial.Serial('/dev/ttyACM0',38400) # 6 fps
    # ser = serial.Serial('/dev/ttyACM0',9600) # 2 fps
   #serial_port='COM5' # Windows
    # serial_port='/dev/ttyACM0' # GNU/Linux
    # 115200 -> 7 fps
    # 38400 -> 6 fps
    # 9600 -> 2 fps

def getSerialOrNone(port,speed):
    try:
       # return serial.Serial(port,speed, bytesize=100)
        return serial.Serial(port,speed)
    except:
       return None

def  rp_jumeau_close():
    global twins_serial
    twins_serial.close() # Fermer proprement le port série
    scene.objects['Commands']['twins'] = False
    scene.objects['Points-Twins-text']['Text'] = "Connection\nfermée"

# Configuration de la vitesse et des temps
def  rp_jumeau_config(speed, temps_avancer, temps_tourner):
    global twins_serial
    if scene.objects['Commands']['twins']:
        serial_msg1 = "CF\n"
        # print (serial_msg1)
        twins_serial.write(serial_msg1.encode())
        rp_tempo (1)
        serial_msg2 = str(speed)+"\n"
        # print (serial_msg2)
        twins_serial.write(serial_msg2.encode())
        rp_tempo (1)
        serial_msg2 = str(temps_avancer)+"\n"
        # print (serial_msg2)
        twins_serial.write(serial_msg2.encode())
        rp_tempo (1)
        serial_msg2 = str(temps_tourner)+"\n"
        # print (serial_msg2)
        twins_serial.write(serial_msg2.encode())
        rp_tempo (1)
        serial_msg1 = "FC\n"
        # print (serial_msg1)
        twins_serial.write(serial_msg1.encode())

def  rp_serie_msg(text):
    global twins_serial
    twins_serial.write(text.encode())

def  rp_serie_rcpt():
    global twins_serial
    print ("Not yet implemented") # FIXME
    return ("Not yet implemented")

###############################################################################
# Fonction bas niveau
###############################################################################

##
# Atteindre une orientation
##

def applyRotationTo(obj, rx=None, ry=None, rz=None, Local=True):
    rres=0.001 # resolution rotation

    # x
    if rx is not None:
        while (abs(rx-obj.worldOrientation.to_euler().x) > rres) :
            if obj.worldOrientation.to_euler().x-rx > rres:
                obj.applyRotation((-rres, 0, 0), Local)
            if rx-obj.worldOrientation.to_euler().x > rres:
                obj.applyRotation((rres, 0, 0), Local)
            # print ("delta x ",rx-obj.worldOrientation.to_euler().x)
       
    # y
    if ry is not None:
        while (abs(ry-obj.worldOrientation.to_euler().y) > rres) :
            if obj.worldOrientation.to_euler().y-ry > rres:
                obj.applyRotation((0, -rres, 0), Local)
            if ry-obj.worldOrientation.to_euler().y > rres:
                obj.applyRotation((0, rres, 0), Local)
            # print ("delta y ",ry-obj.worldOrientation.to_euler().y)

    # z
    if rz is not None:
        while (abs(rz-obj.worldOrientation.to_euler().z) > rres) :
            if obj.worldOrientation.to_euler().z-rz > rres:
                obj.applyRotation((0, 0, -rres), Local)
            if rz-obj.worldOrientation.to_euler().z > rres:
                obj.applyRotation((0, 0, rres), Local)
            # print ("delta z ",rz-obj.worldOrientation.to_euler().z)