#############################################
#           Compilé par B MOREAU            #
#############################################
#importation des bibliotheques nécessaires
from microbit import *
from machine import time_pulse_us
import music
from math import floor
import time
import neopixel
import machine

#parametrages
LRGB = neopixel.NeoPixel(pin15, 4)
def initI2C():
        buf = bytearray(1)
        buf[0] = 0x32
        version=0
        while version==0:
            i2c.write(0x10, buf)
            version=(i2c.read(0x10, 1))[0]
        display.show("v"+ str(version))
        sleep(500)
        display.show(" ")
# Indique si le robot est sous tension
# au cas ou la micobit est alimentee separement
def power_is_on():
    return (16 in i2c.scan())
# test si le bus i2c est actif
while not power_is_on():
    pass
#############################################
#                Fonction OK                #
#############################################
#Leds rouges, 0 pour éteint, 1 pour on.
#ledD pour led rouge droite
#ledG pour led rouge gauche
def ledRouge(ledG, ledD):
    buf = bytearray(3)
    buf[0] = 0x0b
    buf[1] = ledG
    buf[2] = ledD
    i2c.write(0x10, buf)
#############################################
# Allume une led RGB du socle
# Parametres :
#  num = numero de la led (0 <-> 4)
#  si num = 4 on allume les 4 leds RGB
#  couleur au format Rouge Vert Bleu
#  les paramètres R, V et B sont compris entre 0 et 255
def ledRGB(num, R, V, B):
    laquelle = max(0, min(num,4))
    rouge = max(0, min(R,255))
    vert = max(0, min(V,255))
    bleu = max(0, min(B,255))
    if laquelle < 4:
        LRGB[laquelle] = (rouge, vert, bleu)
    else:
        for i in range(4):
           LRGB[i] = (rouge, vert, bleu)
    LRGB.show()
#############################################
# Eteint une led RGB
# Parametre :
#   num = numero de la led (0 <-> 4)
#   si num = 4 on eteint les 4 leds RGB
def eteintLedRGB(num):
    laquelle = max(0, min(num,4))
    if laquelle < 4:
        LRGB[laquelle] = (0, 0, 0)
    else:
        for i in range(4):
            LRGB[i] = (0, 0, 0)
    LRGB.show()
#############################################
#Donne la distance en cm
#pinTrig : pin13, pinEcho : pin14
def distance(pinTrig, pinEcho):
    pinTrig.write_digital(1)
    sleep(0.010)
    pinTrig.write_digital(0)
    pinEcho.read_digital()
    t = time_pulse_us(pinEcho, 1)
    d = 0.017315 * t
    return d
#############################################
# Fait tourner le moteur droit
# Parametre : vitesse (-255 <-> 255)
# Sens Avant si vitesse > 0, Arriere si vitesse < 0
def moteurD(vitesse):
    v= max(-255, min(vitesse,255))
    if v >= 0:
        i2c.write(0x10, bytearray([0x02, 0x0, v]))
    else:
        i2c.write(0x10, bytearray([0x02, 0x1, -v]))
#############################################

# Fait tourner le moteur gauche
# Parametre : vitesse (-255 <-> 255)
# Sens Avant si vitesse > 0, Arriere si vitesse < 0
def moteurG(vitesse):
    v= max(-255, min(vitesse,255))
    if v >= 0:
        i2c.write(0x10, bytearray([0x00, 0x0, v]))
    else:
        i2c.write(0x10, bytearray([0x00, 0x1, -v]))
#############################################
#Fonction qui permet d'aller tout droit
#et récupérer le différentiel de vitesse entre les 2 moteurs
# Parametre : n (-255 <-> 255)
# Sens Avant si n > 0, Arriere si n < 0
def avance(n):
    if n==0:
        f=0
    elif n>0:
        f=floor(0.86*n)+1
    else:
        f=-floor(-0.86*n)-1
    moteurG(f)
    moteurD(n)
#Stop des moteurs
def stop():
    moteurG(0)
    moteurD(0)
#############################################
#Capteurs de ligne
#Permet de détecter une ligne noire.
#indexCapteur : 1 à 5 ( 1: R2, 2:R1, 3:M, 4:L1, 5:L2).
#Renvoi 1 pour du noir, 0 pour du blanc
def ligne(indexCapteur):
    buf = bytearray(1)
    buf[0] = 0x1D
    i2c.write(0x10, buf)
    etatByte = (i2c.read(0x10, 1)[0])               #on récupère l'octet d'index 0 dans le tableau
    etatCapteur = etatByte & 2**(indexCapteur-1)    #masque
    if etatCapteur != 0:
        etatCapteur = 1
    return etatCapteur

def son_r2d2():
    tune=["A7:0", "G7:0", "E7:0","C7:0","D7:0","B7:0","F7:0","C8:0","A7:0","G7:0","E7:0","C7:0","D7:0","B7:0","F7:0","C8:0"]
    music.play(tune)
#############################################
#             Fonctions en test             #
#############################################

#############################################
#Pour accessoires, index de 1 à 3 pour les connexions P1 à P3
#Max pour angle: 170°
#Inutile pour l'instant
def servo(index, angle):
    buf =bytearray(2)
    if(index == 1):
        buf[0]=0x14
    if(index == 2):
        buf[0]=0x15
    if(index == 3):
        buf[0]=0x16
    buf[1]=angle
    i2c.write(0x10, buf)

#############################################
#              Votre Programme              #
#############################################
while True :
    ledRouge(0, 0)
    #compte a rebours
    for i in range(3,0,-1):
        display.show(i)
        sleep(1)
        display.clear()
    #Test led rouge
    for i in range(0,4):
        ledRouge(1, 0)
        sleep(0.25)
        ledRouge(0, 0)
        ledRouge(0,1)
        sleep(0.25)
        ledRouge(0, 0)
    #Test led RGB
    for j in range(0,4):
        ledRGB(j,258, 108, 158)
        sleep(1)
    #Test son
    son_r2d2()
    #Test déplacement
    avance(100)
    sleep(1)
    stop()
    moteurD(30)
    moteurG(-30)
    sleep(0.5)
    stop()
    avance(100)
    sleep(1)
    stop()
    break