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) - \
            self.b * delta_t * self.w / self.M + \
            delta_t * _input / self.M
        self.theta = self.theta + delta_t * self.w
        self.w = w_temp

        return

        if self.theta > math.pi:
            self.theta = self.theta - 2*math.pi
        if self.theta < -math.pi:
            self.theta = 2*math.pi + self.theta



class PIDSat:

    def __init__(self, kp, ki, kd, sat):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.saturation = sat
        self.integral = 0
        self.prev_error = 0
        self.saturation_flag = False

    def evaluate(self, target, current, delta_t):
        error = target - current
        if not(self.saturation_flag):
            self.integral = self.integral + error * delta_t
        deriv = (error - self.prev_error) / delta_t
        self.prev_error = error
        output = self.kp * error + self.ki * self.integral + self.kd * deriv
        if output > self.saturation:
            output = self.saturation
            self.saturation_flag = True
        elif output < -self.saturation:
            output = -self.saturation
            self.saturation_flag = True
        else:
            self.saturation_flag = False
        return output






delta_t = 1e-3 # 1 ms

braccio = Pendolo(6.0, 4.0)

speed_controller = PIDSat(10000, 20000, 0, 100)

t = 0.0

# tariamo il controllore sulla rampa
w_target = 0
w_max = 0.5
acc = 0.05

vettore_theta = [ ]
vettore_w = [ ]
vettore_wt = [ ]
vettore_tempi = [ ]
vettore_output = [ ]

while t < 100:

    output = speed_controller.evaluate(w_target, braccio.w, delta_t)
    braccio.evaluate(output, delta_t)

    t = t + delta_t

    vettore_w.append(braccio.w)
    vettore_wt.append(w_target)
    vettore_theta.append(math.degrees(braccio.theta))
    vettore_output.append(output)
    vettore_tempi.append(t)

    w_target += acc * delta_t
    if w_target >= w_max:
        w_target = w_max


pylab.figure(1)
pylab.plot(vettore_tempi, vettore_wt, 'b-+', label='target, w(t)')
pylab.plot(vettore_tempi, vettore_w, 'r-+', label='vel, w(t)')
pylab.xlabel('time')
pylab.legend()

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

pylab.figure(3)
pylab.plot(vettore_tempi, vettore_output, 'b-+',
		label='force')
pylab.xlabel('time')
pylab.legend()

pylab.show()