import pylab

class Massa:

    def __init__(self, _M, _b):
        self.M = _M
        self.b = _b
        # variabili di stato, p = posizione, v = velocita'
        self.p = 0
        self.v = 0

    def evaluate(self, _input, dt):
        self.p = self.p + delta_t * self.v
        self.v = (1 - self.b * dt/self.M) * self.v + dt / self.M * _input

    def get_position(self):
        return self.p

    def get_speed(self):
        return self.v


delta_t = 1e-3 # 1 ms

robot = Massa(6.0, 25.0)


target_pos = 5  # 5 metri

t = 0.0
vettore_pos = [ ]
vettore_vel = [ ]
vettore_tempi = [ ]

while t < 20:
    current_pos = robot.get_position()

    error = target_pos - current_pos
    if (error > 0):
        f = 20
    elif (error < 0):
        f = -20
    else:
        f = 0

    robot.evaluate(f, delta_t)

    t = t + delta_t
    vettore_vel.append(robot.get_speed())
    vettore_pos.append(robot.get_position())
    vettore_tempi.append(t)


pylab.figure(1)
pylab.plot(vettore_tempi, vettore_vel, 'r-+', label='vel, v(t)')
pylab.xlabel('time')
pylab.legend()

pylab.figure(2)
pylab.plot(vettore_tempi, vettore_pos, 'b-+', label='position, p(t)')
pylab.xlabel('time')
pylab.legend()

pylab.show()