Skip to content

Instantly share code, notes, and snippets.

@patrickelectric
Created February 25, 2015 21:03
Show Gist options
  • Save patrickelectric/adba9f4b420201210bff to your computer and use it in GitHub Desktop.
Save patrickelectric/adba9f4b420201210bff to your computer and use it in GitHub Desktop.
#! /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)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment