float kp=2,kd=3,ki=4,prop=0,deriv=0,integ=0,control=0;

int difflast=0,diff=0,rate=0,pos=0;
int con=0;
con=porta&00000111;
switch(con)
{
case 100: pos=-2;
break;
case 110: pos=-1;
break;
case 10: pos=0; break;
case 11: pos=1; break;
case 1: pos=2; break;
case 0: pos=(pos>0)?(3):(-3);pos=(pos==0)?(0):(pos); break;
}
diff=pos;
rate=diff-difflast;
difflast=diff;
prop=kp*diff;
deriv=kd*rate;
integ+=ki*diff;
out<<"Last control ="<<control;
control=prop+deriv+integ;
out<<"New control ="<<control;
use control value