from microbit import *

import xgo

def executar():    # funció que s'executarà des de main.py

    NEUTRE = 0x80

    BAIX   = 0x60

    ALT    = 0xA0

    SERVO_POTA_ID = 11  # servo amb el que farem la prova: pota davantera dreta

    pos_pota = NEUTRE

    pos_brac = NEUTRE

    pinca_oberta = True

    temps_ab = 0

    while True:

        # Sortir amb A+B mantingut

        if button_a.is_pressed() and button_b.is_pressed():

            temps_ab += 50

            if temps_ab >= 1000:

                return

        else:

            temps_ab = 0

        # A: toggle pota, mou l'articulació inferior

        if button_a.was_pressed():

            pos_pota = BAIX if pos_pota == NEUTRE else NEUTRE

            xgo.pota_posar_servo_id(SERVO_POTA_ID, pos_pota)

        # B: toggle braç (avantbraç)

        if button_b.was_pressed():

            pos_brac = ALT if pos_brac == NEUTRE else NEUTRE

            xgo.avant_brac(pos_brac)

        # Logo: toggle pinça

        if pin_logo.is_touched():

            sleep(200)  # debounce

            pinca_oberta = not pinca_oberta

            if pinca_oberta:

                xgo.pinca_obrir()

            else:

                xgo.pinca_tancar()

        sleep(30)