#!/usr/bin/python

## 2.1

from numpy import zeros

def pend_model():

    Km = 0.00767

    Kg = 3.7

    Rm = 2.6

    r = 0.00635

    M = 0.522

    m = 0.231

    g = 9.81

    L = 0.305

    J = 0

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

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

    beta1 = m*L/D1

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

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

    alpha1 = Km*Kg/Rm/r;

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

    A = zeros([4,4])

    A[0,2] = 1

    A[1,3] = 1

    A[2,1] = -gamma1

    A[2,2] = -alpha2*delta

    A[3,1] = alpha

    A[3,2] = alpha2*beta1

    B = zeros([4,1])

    B[2] = alpha1*delta

    B[3] = -alpha1*beta1

    return A, B

if __name__ == "__main__":

    A, B = pend_model()

    print "A", A

    print "B", B