1 #!/usr/bin/python

     2 ## 2.1

     3 

     4 from numpy import zeros

     5 

     6 def pend_model():

     7     Km = 0.00767

     8     Kg = 3.7

     9     Rm = 2.6

    10     r = 0.00635

    11     M = 0.522

    12     m = 0.231

    13     g = 9.81

    14     L = 0.305

    15     J = 0

    16 

    17     D1 = (J+m*L**2)*(M+m)-m**2*L**2

    18     alpha = m*g*L*(M+m)/D1

    19     beta1 = m*L/D1

    20     gamma1 = m**2*g*L**2/D1

    21     delta = (J+m*L**2)/D1

    22     

    23     alpha1 = Km*Kg/Rm/r;

    24     alpha2 = Km**2*Kg**2/Rm/r**2

    25     

    26     A = zeros([4,4])

    27     A[0,2] = 1

    28     A[1,3] = 1

    29     A[2,1] = -gamma1

    30     A[2,2] = -alpha2*delta

    31     A[3,1] = alpha

    32     A[3,2] = alpha2*beta1

    33     

    34     B = zeros([4,1])

    35     B[2] = alpha1*delta

    36     B[3] = -alpha1*beta1

    37 

    38     return A, B

    39 

    40 if __name__ == "__main__":

    41     A, B = pend_model()

    42     print "A", A

    43     print "B", B