python

#! /usr/bin/env python
# -*- coding:utf-8 -*-
# 
__author__ = 'Patrick'


import matplotlib.pyplot as plt
import matplotlib.animation as animation
import time
import datetime
import thread
import threading

plt.ion()
#plt.plot([0,1],[0,1],marker='o',linestyle='--',color='r')
plt.show()


def update_line(num, data, line):
    line.set_data(data[...,:num])
    return line,

class motor:
	def __init__(self,J=0.01,b=0.1,K=0.01,R=1,L=0.5):
		self.J=J;
		self.b=b;
		self.K=K;
		self.R=R;
		self.L=L;
		self.i=[0,0,0];
		self.ve=[0,0,0];
		self.interact=0;

		self.u=0;

		self.I=[];
		self.V=[];
		self.T=[];

		self.lasttime=0;

	def run(self):
		thread.start_new_thread( self.apply,("Apply",))
		thread.start_new_thread( self.show,("Show",))
		
	def apply(self,Name):
		while(1):
			time1=time.clock()
			temp1=0;
			temp2=0;

			if(self.lasttime==0):
				self.lasttime=time.clock()

			if(self.interact>0):
				temp1=self.i;
				temp2=self.ve;

			timeclock=time.clock()
			self.i[1]=((self.u-self.R*self.i[0]-self.K*self.ve[0])/self.L)
			self.i[0]=(self.i[1]+self.i[2])*(timeclock-self.lasttime)/2+self.i[0]
			self.i[2]=self.i[1];

			self.ve[1]=((self.K*self.i[0]-self.b*self.ve[0])/self.J)
			self.ve[0]=(self.ve[1]+self.ve[2])*(timeclock-self.lasttime)/2+self.ve[0]
			self.ve[2]=self.ve[1];

			self.lasttime=time.clock();

			self.I.append(self.i[0]);
			self.V.append(self.ve[0]);
			self.T.append(timeclock);

			self.interact+=1;

#			while((time.clock()-time1)<0.0100000):
#				pass

			
			#print "i = " + str(self.i) + " : " + "Speed = " + str(self.ve) + " : Time = " +  str(self.lasttime)
			time.sleep(0.001)

	def show(self,Name):
		while(1):
			plt.plot(self.T,self.V)
			plt.draw()
			time.sleep(1);

	def input(self,u):
		self.u=u;

	def output(self):
		temp=[self.ve[0],self.i[0]]
		return temp


class PID:
	def __init__(self,P=0,I=0,D=0):
		self.P=P;
		self.I=I;
		self.D=D;
		self.u=0;
		self.ref=0;
		self.val=0;

		self.lasttime=0;
		self.error=[0,0,0,0]

	def run(self):
		thread.start_new_thread( self.calc,("Calc",))

	def put(self,ref,val):
		self.ref=ref
		self.val=val

	def take(self):
		return self.u;

	def calc(self,Name):
		while(1):
			timeclock=time.clock();
			if(self.lasttime==0):
				self.lasttime=timeclock

			self.error[3]=self.ref-self.val;
			if(timeclock!=self.lasttime):
				self.error[1]=(self.error[3]+self.error[2])*(timeclock-self.lasttime)/2 + self.error[1]
				self.error[0]=(self.error[3]-self.error[2])/(timeclock-self.lasttime)
			self.lasttime=timeclock
			self.u=self.P*(self.error[3])+self.I*(self.error[1])+self.D*(self.error[0])

			self.error[2]=self.error[3];

#			while((time.clock()-timeclock)<0.020000):
#				pass
			time.sleep(0.01)
			print self.error




if __name__ == '__main__':
	motor=motor()
	pid=PID(200,20,0)
	pid.put(1,0)

	
	motor.run()
	time.sleep(2)

	pid.run()
	while 1:
		pid.put(1,motor.output()[0])
		motor.input(pid.take())
		time.sleep(0.001)