import pylab
import math

GRAVITY = 9.81

class Pendolo:

    def __init__(self, _M, _b):
        self.w = 0
        self.theta = 0
        self.M = _M
        self.b = _b

    def evaluate(self, _input, delta_t):
        w_temp = self.w - GRAVITY * delta_t * math.sin(self.theta) + \
            delta_t * _input / self.M - self.b * delta_t * self.w / self.M
        self.theta = self.theta + delta_t * self.w
        self.w = w_temp

    def normalized_theta(self):
        t = self.theta
        while t > math.pi:
            t = t - 2*math.pi
        while t < -math.pi:
            t = 2*math.pi + t
        return t


delta_t = 1e-3 # 1 ms

braccio = Pendolo(6.0, 4.0)

t = 0.0

inp = 50

vettore_theta = [ ]
vettore_w = [ ]
vettore_tempi = [ ]

while t < 15:

    output = braccio.evaluate(inp, delta_t)

    t = t + delta_t

    vettore_w.append(braccio.w)
    vettore_theta.append(math.degrees(braccio.normalized_theta()))
    vettore_tempi.append(t)


pylab.figure(1)
pylab.plot(vettore_tempi, vettore_w, 'r-+', label='vel (rad/s)')
pylab.xlabel('time (s)')
pylab.legend()

pylab.figure(2)
pylab.plot(vettore_tempi, vettore_theta, 'b-+',
		label='pos (deg)')
pylab.xlabel('time (s)')
pylab.legend()

pylab.show()