#####################################################
#   Bras artificiel à structure anthropomorphique   #
#####################################################

import numpy as np
import matplotlib.pyplot as plt



## On définit ici la consigne mais pour un système asservi la consigne peut être mesurée

def Calcul_Cons1():         # Donne un échelon de consigne de 1 rad
    global Date,Cons        #   à partir de la date t = 6 s
    if Date<6 : Cons = 0
    else : Cons = 1

def Calcul_Cons2():         # Donne rampe de pente 0,2 rad/s
    global Date,Cons        #   entre les dates t = 6 s et t = 11 s
    if Date<6 : Cons = 0
    elif Date<11 : Cons=0.2*(Date-6)
    else : Cons = 1



#  Ces fonctions permettent de simuler la réponse à la consigne et à la perturbation
##  Pour un système asservi il ne s'agirait pas de calcul mais de mesure


def Calcul_Moment_pneum():
    global M,Mp,U,dt
    Mp=M
    M=(U*dt+0.06*Mp)/(0.06+dt)

def Calcul_Reponse():
    global Te,Tep,Tepp,dt,M,Cext,Date
    if Date>=0: Cpert=Cext  # Un échelon de perturbation -Pert à la date t = 0 s
    else: Cpert=0.0
    Calcul_Moment_pneum()
    Tepp=Tep
    Tep=Te
    Te=(0.2*(dt**2)*(M+Cpert)+Tep*(0.2+0.16*dt)-0.1*Tepp)/(0.1+0.16*dt+dt**2)

## Boucle d'asservissement


def asservissement(Calcul_Commande,Calcul_Consigne,Pert,T,N):
            # fcons et fpert : fonction temporelles de la
            #  consigne et de la perturbation (t en s)
            # Pert : Echelon de perturbation : Cext
            # T : Durée de la simulation (en s)
            # N : Nombre de points de calcul
    # Toutes les variables sont globales et sont initialisées à 0
    global Date,Datep,dt,Ic,Im,E,Ep,U1,U1p,U2,U2p,U,Up,M,Mp,Cext,Cons,Te,Tep,Tepp
    Cons,Te=0.0,0.0     # Consigne de l'asservissement (Cons) et réponse (Te)
    Tep,Tepp=0.0,0.0    # Theta(t-dt) et Theta(t-2.dt)
    Ic,Im=0,0           # Images de la consigne (Ic) et de la réponse (Im)
    E,Ep=0.0,0.0        # Ecart entre les images de la consigne et de la réponse
    U1,U1p=0.0,0.0      # Sortie du 1ier correcteur
    U2,U2p=0.0,0.0      # Sortie du 2nd correcteur
    U,Up=0.0,0.0        # Sortie du 3ième correcteur
    M,Mp=0.0,0.0        # Moment pneumatique
    Cext=-Pert          # Couple extérieur appliqué sur le bras
    Date,Datep=0.0,0.0  # Dates actuelle et précédentes

    LT=np.linspace(0,T,N+1)     # Dates prédéfinies dans une liste
    LTe=[Te]                      # Listes des réponses en fonction de la Date
    LC=[Cons]
    for i in range(1,len(LT)):    # Boucle d'asservissement
        Datep=Date
        Date=LT[i]              # Sur un carte de commande on utiliserait "Date=time()"
        dt=Date-Datep
        Calcul_Consigne()       # Pour calculer la consigne à la Date actuelle
        Ic=573*Cons             # Pour calculer l'image de la consigne à la Date actuelle
        Im=573*Te               # Pour mesurer l'image de la réponse à la Date actuelle
        Ep=E
        E=Ic-Im
        Calcul_Commande()       # Fonction à définir suivant le correcteur
        Calcul_Reponse()        # Calcul de la réponse
                                # Pour un asservissement on aurait rien ici
        # Pour un asservissement on introduirait ici une temporisation correspondant
        #   au temps d'échatillonage désiré
        LTe.append(Te)
        LC.append(Cons)
    plt.plot(LT,LC,label="Consigne")
    plt.plot(LT,LTe,label="Réponse")
    plt.axis([0,T,-0.25,1.25])


## Test avec les deux consignes

def test(Calcul_Commande,Pert,T,N):
    plt.close()
    asservissement(Calcul_Commande,Calcul_Cons1,Pert,T,N)
    asservissement(Calcul_Commande,Calcul_Cons2,Pert,T,N)
    plt.xlabel('Date en secondes')
    plt.ylabel('Position angulaire en radian')
    plt.legend(loc = 'upper left')
    plt.grid()
    plt.show()


## Fonctions de correction

def Calcul_Commande1():     # Correcteur proportionnel C(p) = 0,0063 V/inc
    global U,E


def Calcul_Commande2():     # Correcteur PI (T=0,343 ; K= 0,016)
                            #  + Avance de phase : T = 0,014 ; c=83,4
    global dt, E, Ep, U1, U1p, U, Up






def Calcul_Commande3():     # Correcteur PI (T=0,343 ; K= 0,034)
                            #  + 2 Avance de phase : T = 0,06 ; c=4.34
    global dt, E, Ep, U1, U1p, U2, U2p, U, Up