from porcou_lib import * # Bibliothèque utilisateur du portail coulissant

###############################################################################
# porcou_cmd.py
# @title: Commandes du portail coulissant
###############################################################################

###############################################################################
# Instructions élémentaires pour le portail coulissant
#
# Actions (ordre = True ou False) :
# - Gyrophare : gyr(True | False)
# - Ouvrir le portail (moteur sens trigo) : mot_o(True | False)
# - Fermer le portail (moteur sens horaire) : mot_f(True | False)
# - Emetteur pour le capteur barrage IR  : ir_emet(True | False)
#
# Capteurs (valeur retournée = True ou False) :
# - Capteur fin de course portail ouvert : fdc_o()
# - Capteur fin de course portail fermé : fdc_f()
# - Capteur barrage IR  (absence d'obstacle) : ir_recep()
#
# Consignes du pupitre (valeur retournée = True ou False) :
# - Bouton poussoir coté rue : bp_ext()
# - Bouton poussoir coté cour : bp_int()
#
# Gestion du temps :
# - Temporisation en seconde : tempo(duree)
#
###############################################################################

###############################################################################
# Fonctions
###############################################################################

###############################################################################
# Commandes
###############################################################################

def commandes():

    # Init -> Ouverture
    while fdc_o() ==False :
        gyr(True)
        mot_f(False)
        mot_o(True)
    mot_o(False)
    gyr(False)
    print ("")

    # daq(['mot_angle', 'mot_vitesse', 'portail_x', 'portail_vitesse'])
    daq(['bp_ext', 'bp_ext_r', 'bp_int', 'bp_int_r', 'fdc_o', 'fdc_o_r', 'fdc_f', 'fdc_f_r', 'mot_o', 'mot_f', 'gyr', 'mot_angle', 'mot_vitesse', 'portail_x', 'portail_vitesse', 'ir_emet', 'ir_recep', 'ir_recep_r'])
    # daq(['bp_ext', 'bp_ext_r', 'bp_int', 'bp_int_r', 'fdc_o', 'fdc_o_r', 'fdc_f', 'fdc_f_r', 'mot_o', 'mot_f', 'gyr'])
    reset_t()
    
    # Fermeture
    mot_digitset (1256)  # Vitesse par défaut 125,6 rad /s ( 20 tr / s )
    t0,x0, a0= get('t'), get('portail_x'), get('mot_angle')
    # print ("Début fermeture :  portail_x : "+str(round(x0, 3)) + " - mot_angle : " + str(round(a0, 3)))
    while fdc_f() ==False :
        gyr(True)
        mot_o(False)
        mot_f(True)
        mot_pas, mot_vitesse, portail_pas, portail_vitesse= get('mot_pas'), get('mot_vitesse'), get('portail_pas'), get('portail_vitesse')
    mot_f(False)
    gyr(False)
    t1,x1, a1= get('t'), get('portail_x'), get('mot_angle')
    # print ("Fin fermeture :  portail_x : "+str(round(x1, 3)) + " - mot_angle : " + str(round(a1, 3)))
    # print ("")
    # print ("Fermeture : "+str(round(t1-t0, 3)) +" s - distance : " +str(round(x1-x0, 3))+" mm - angle : " +str(round(a1-a0, 3))+
    #        " rad - portail_vitesse : " +str(round(portail_vitesse, 3))+" mm/s - moteur_vitesse : " +str(round(mot_vitesse, 3))+
    #        " rad/s - portail_pas : " +str(round(portail_pas, 3))+" mm/impulsion - moteur_pas : " +str(round(mot_pas, 3))+" rad/impulsion")
    # print ("")
        
    # Ouverture
    mot_digitset ()  # Vitesse par défaut 125,6 rad /s ( 20 tr / s )
    mot_digitset (2000)  # Vitesse par défaut 125,6 rad /s ( 20 tr / s )
    t0,x0, a0= get('t'), get('portail_x'), get('mot_angle')
    # print ("Début ouverture :  portail_x : "+str(round(x0, 3)) + " - mot_angle : " + str(round(a0, 3)))
    while fdc_o() ==False :
        gyr(True)
        mot_f(False)
        mot_o(True)
        mot_pas, mot_vitesse, portail_pas, portail_vitesse= get('mot_pas'), get('mot_vitesse'), get('portail_pas'), get('portail_vitesse')
    mot_o(False)
    gyr(False)
    t1,x1, a1= get('t'), get('portail_x'), get('mot_angle')
    # print ("Fin ouverture :  portail_x : "+str(round(x1, 3)) + " - mot_angle : " + str(round(a1, 3)))
    # print ("")
    # print ("Ouverture : "+str(round(t1-t0, 3)) +" s - distance : " +str(round(x1-x0, 3))+" mm - angle : " +str(round(a1-a0, 3))+
    #        " rad - portail_vitesse : " +str(round(portail_vitesse, 3))+" mm/s - moteur_vitesse : " +str(round(mot_vitesse, 3))+
    #        " rad/s - portail_pas : " +str(round(portail_pas, 3))+" mm/impulsion - moteur_pas : " +str(round(mot_pas, 3))+" rad/impulsion")

    plot([['bp_ext', 'bp_ext_r'], ['bp_int', 'bp_int_r'], ['fdc_o', 'fdc_o_r'], ['fdc_f', 'fdc_f_r'], 'mot_o', 'mot_f', 'gyr', ['mot_angle', 'mot_vitesse', 'portail_x', 'portail_vitesse']])
    fin() # A garder

    ###############################################################################
    # En: External call << DONT CHANGE THIS SECTION >>
    # Fr: Appel externe << NE PAS MODIFIER CETTE SECTION >>
    ###############################################################################
        
if __name__=='start':
    thread_cmd_start(commandes)
if __name__=='stop':
    stop()