def run(self):
#--ref to: http://www.codeproject.com/Articles/36459/PID-process-control-a-Cruise-Control-example
lastTs=Time.currentTime();
previousError=0;
integral=0;
Kp=1.2;
Ki=0.003;
Kd=1;
pV=0.0;
noise=0.0;
while(not self.done_):
self.lock_.acquire();
tS=Time.currentTime();
dT=tS-lastTs;
error=self.pRef_-self.pResponse_;
integral=integral+error*dT;
derivative=(error-previousError)/dT;
output=Kp*error+Ki*integral+Kd*derivative;
previousError=error;
pV=pV+(output*0.20)-(pV*0.10)+noise;
dP=pV*dT;
self.pResponse_ = self.pResponse_+dP;
lastTs=tS;
# self.pResponse_ = self.pResponse_ + dP;
self.D_.append((Time.currentTime(),self.pResponse_));
self.lock_.release();
time.sleep(ControlLoopCycle);
def mainLoop():
plt.ion();
fig=plt.figure(1);
ax1=fig.add_subplot(511);
ax1.set_ylabel(loader_.name());
ax2=fig.add_subplot(512);
ax2.set_ylabel(shuttle_.name());
ax3=fig.add_subplot(513);
ax3.set_ylabel(flopTray_.name());
ax4=fig.add_subplot(514);
ax4.set_ylabel(rammer_.name());
ax5=fig.add_subplot(515);
ax5.set_ylabel("sequencer");
l1,=ax1.plot(100,100,'r-');
l2,=ax2.plot(100,100,'r-');
l3,=ax3.plot(100,100,'r-');
l4,=ax4.plot(100,100,'r-');
l5,=ax5.plot(100,100,'r-');
global donePlotting;
seqD=[];
step=0;
while (not donePlotting):
D1 = loader_.pList();
T1=[x[0] for x in D1];
L1=[x[1] for x in D1];
l1.set_xdata(T1);
l1.set_ydata(L1);
ax1.set_ylim([0,105]);
ax1.set_xlim([min(T1),max(T1)]);
D2 = shuttle_.pList();
T2=[x[0] for x in D2];
L2=[x[1] for x in D2];
l2.set_xdata(T2);
l2.set_ydata(L2);
ax2.set_ylim([0,95]);
ax2.set_xlim([min(T2),max(T2)]);
D3 = flopTray_.pList();
T3=[x[0] for x in D3];
L3=[x[1] for x in D3];
l3.set_xdata(T3);
l3.set_ydata(L3);
ax3.set_ylim([0,45]);
ax3.set_xlim([min(T3),max(T3)]);
D4 = rammer_.pList();
T4=[x[0] for x in D4];
L4=[x[1] for x in D4];
l4.set_xdata(T4);
l4.set_ydata(L4);
ax4.set_ylim([0,180]);
ax4.set_xlim([min(T4),max(T4)]);
seqD.append((Time.currentTime(),step));
T5=[x[0] for x in seqD];
L5=[x[1] for x in seqD];
l5.set_xdata(T5);
l5.set_ydata(L5);
ax5.set_ylim([0,15]);
ax5.set_xlim([min(T4),max(T4)]);
#--update sequencer
if (step==0):
#--move everything home
loader_.moveTo(15);
shuttle_.moveTo(0);
flopTray_.moveTo(0);
rammer_.moveTo(0);
if (loader_.position() < 16 and shuttle_.position() < 1 and flopTray_.position() < 1 and rammer_.position() < 1):
step=1;
elif(step==1):
#--move propellant to flopTray
shuttle_.moveTo(90);
if (shuttle_.position() >= 89):
step=2;
elif(step==2):
#--move flopTray to receive projectile & move shuttle to grab projectile
flopTray_.moveTo(30);
shuttle_.moveTo(0);
if(flopTray_.position() > 29 and shuttle_.position() < 1):
step=3;
elif(step==3):
#--move projectile to flopTray
shuttle_.moveTo(90);
if (shuttle_.position() >= 89):
step=4;
elif(step==4):
#--move loader to breech and home shuttle
loader_.moveTo(90);
shuttle_.moveTo(0);
if (loader_.position() >= 89):
step=5;
elif(step==5):
#--ram projectile
rammer_.moveTo(178);
if (rammer_.position() >= 177):
step=6;
elif(step==6):
#--retract rammer
rammer_.moveTo(0);
if (rammer_.position() <= 1):
step=7;
elif(step==7):
#--shift propellant on flopTray
flopTray_.moveTo(0);
if (flopTray_.position() <= 1):
step=8;
elif(step==8):
#--ram propellant
rammer_.moveTo(178);
if (rammer_.position() >= 177):
step=9;
elif(step==9):
loader_.moveTo(15);
if(loader_.position() <= 16):
step=10;
elif(step==10):
print "BANG!!!"
step=0;
plt.draw();
plt.pause(0.1);
