from microbit import *
from  machine import *
import music
import radio
import math
import time

###############################################################################
# rp_maqueen-robot.py
# @title: Jumeau Maqueen : Programme de la carte microbit du robot
# @project: Ropy (Blender-EduTech)
# @lang: fr
# @authors: Philippe Roy <philippe.roy@ac-grenoble.fr>
# @copyright: Copyright (C) 2022 Philippe Roy
# @license: GNU GPL
###############################################################################

# ###############################################################################
# Initialisation
# ###############################################################################

# Image
attente_image = Image("00000:00000:00900:00000:00000")
display.show(attente_image) # Témoin de fonctionnement
balise_image = Image("33333:36663:36963:36663:33333")

# Radio
radio.config(group=1, queue=4, length=8)
radio.on()

# Paramétrage
vitesse = 50 # Vitesse 
distance = 50 # Distance d'un pas
angle = 90 # Angle lors des rotations

vmax_roue = 85.5 # Vitesse maxi des roues en tr/min
diam_roue = 43 # Diamètre des roues en mm
dist_essieu = 70 # Distance entre les roues en mm

# ###############################################################################
# Boucle principale
# ###############################################################################

while True:

    # A propos
    if button_a.is_pressed() or button_b.is_pressed():
        display.scroll ("Ropy : Robot")
        display.show(attente_image) # Témoin de fonctionnement

    # Lecture de l'ordre
    ordre=radio.receive()

    # Configuration
    if ordre=="CF":
        display.scroll("CF")
        i=1
        while True:
            conf=radio.receive()
            if conf=="FC": # Fin de la configuration
                display.scroll("Config : " +str(vitesse) + " - " +str(distance) + " - " +str(angle))
                break
            if  conf is not None:
                # display.scroll(conf) # Affichage du message complet
                if i == 1: # Configuration de la vitesse
                    vitesse = int(str(conf[2:-1]))
                if i == 2: # Configuration de la distance
                    distance = int(str(conf[2:-1]))
                if i == 3: # Configuration de l'angle
                    angle = int(str(conf[2:-1]))
                i+=1
   
    # Avancer d'un pas
    if ordre=="AV":

        display.show(Image.ARROW_N) # Afficher flèche avancer
        pin8.write_digital(1) # Led avant gauche
        pin12.write_digital(1) # Led avant gauche
        i2c.write(0x10, bytearray([0x00, 0x0, vitesse])) # Avance moteur gauche
        i2c.write(0x10, bytearray([0x02, 0x0, vitesse])) # Avance moteur droit

        v_roue=(vmax_roue/255)*vitesse
        v_lin = ((v_roue/60)*2*math.pi)*(diam_roue/2) # Vitesse linéaire
        time.sleep(distance/v_lin)

        i2c.write(0x10, bytearray([0x00, 0x0, 0])) # Stop moteur gauche
        i2c.write(0x10, bytearray([0x02, 0x0, 0])) # Stop moteur droit
        pin8.write_digital(0) # Led avant gauche
        pin12.write_digital(0) # Led avant gauche
        display.clear() # Effacer matrice de leds

    # Reculer
    if ordre=="RE":

        display.show(Image.ARROW_S) # Afficher flèche reculer
        pin8.write_digital(1) # Led avant gauche
        pin12.write_digital(1) # Led avant gauche
        i2c.write(0x10, bytearray([0x00, 0x1, vitesse])) # Avance moteur gauche
        i2c.write(0x10, bytearray([0x02, 0x1, vitesse])) # Avance moteur droit

        v_roue=(vmax_roue/255)*vitesse
        v_lin = ((v_roue/60)*2*math.pi)*(diam_roue/2) # Vitesse linéaire
        time.sleep(distance/v_lin)

        i2c.write(0x10, bytearray([0x00, 0x0, 0])) # Stop moteur gauche
        i2c.write(0x10, bytearray([0x02, 0x0, 0])) # Stop moteur droit
        pin8.write_digital(0) # Led avant gauche
        pin12.write_digital(0) # Led avant gauche
        display.clear() # Effacer matrice de leds

    # Gauche
    if ordre=="GA":
        display.show(Image.ARROW_E)
        pin8.write_digital(1) # Led avant gauche
        i2c.write(0x10, bytearray([0x00, 0x1, vitesse])) # Avance moteur gauche
        i2c.write(0x10, bytearray([0x02, 0x0, vitesse])) # Avance moteur droit

        v_roue=(vmax_roue/255)*vitesse
        v_lin = ((v_roue/60)*2*math.pi)*(diam_roue/2) # Vitesse linéaire
        angle2=(angle/4)*(2*math.pi/360)
        time.sleep((dist_essieu*angle2)/v_lin)

        i2c.write(0x10, bytearray([0x00, 0x0, 0])) # Stop moteur gauche
        i2c.write(0x10, bytearray([0x02, 0x0, 0])) # Stop moteur droit
        pin8.write_digital(0) # Led avant gauche
        display.clear() # Effacer matrice de leds

    # Droite
    if ordre=="DR":
        display.show(Image.ARROW_W)
        pin12.write_digital(1) # Led avant droit
        i2c.write(0x10, bytearray([0x00, 0x0, vitesse])) # Avance moteur gauche
        i2c.write(0x10, bytearray([0x02, 0x1, vitesse])) # Avance moteur droit

        v_roue=(vmax_roue/255)*vitesse
        v_lin = ((v_roue/60)*2*math.pi)*(diam_roue/2) # Vitesse linéaire
        angle2=(angle/4)*(2*math.pi/360)
        time.sleep((dist_essieu*angle2)/v_lin)

        i2c.write(0x10, bytearray([0x00, 0x0, 0])) # Stop moteur gauche
        i2c.write(0x10, bytearray([0x02, 0x0, 0])) # Stop moteur droit
        pin12.write_digital(0) # Led avant droit
        display.clear() # Effacer matrice de leds
    
    # Marquer
    if ordre=="MA":
        display.show(balise_image)
        music.play("A7:0")

    # Objectif
    if ordre=="OB":
        display.show(Image.HAPPY)
        music.play(music.ENTERTAINER)
        display.show(attente_image) # Témoin de fonctionnement

    # Forer
    if ordre=="FO":
        display.show(Image.DIAMOND)

    # Colision : CO
    if ordre=="CO":
        display.show(Image.SKULL)
        music.play(music.FUNERAL)

    # Fin
    if ordre=="FI":
        display.show(attente_image) # Témoin de fonctionnement

    # Cadencement
    # sleep(100)