#
#
#
import pylab
import time

from motor_driver import *
from simple_kinematics import *
from controllers import *

driver = MotorDriver()
driver.open()

kinematics = Kinematics(driver)

controller = PI_Sat_AntiW_Controller(10, 50, 4200)

target_speed = 80

delta_t = 0.005 # 5ms
t_sim = 2

n_iter = int(t_sim / delta_t)

speed_array = []
time_array = []

for i in range(0, n_iter):
	time.sleep(delta_t)
	kinematics.run(delta_t)
	pwm_out = controller.evaluate(target_speed, 
				      kinematics.speed,
				      delta_t)
	driver.pwm(pwm_out)
	time_array.append(i*delta_t)
	speed_array.append(kinematics.speed)

driver.pwm(0)
pylab.plot(time_array, speed_array)
pylab.show()