// Chapter 13 6812 C programs
// Jonathan W. Valvano, 2/26/07
// This software accompanies the book,
// Embedded Microcomputer Systems: Real Time Interfacing, Second Edition
// published by Thomson Engineering, 2006



#define CW 0
#define CCW 1
#define RPM0   0  // means stop
#define RPM0_5 8  // 8*75ms=600ms => 0.5 RPM
#define RPM1   4  // 4*75ms=300ms => 1 RPM
#define RPM2   2  // 2*75ms=150ms => 2 RPM
#define RPM4   1  // 1*75ms=75ms  => 5 RPM
const struct Command{
  unsigned short Duration;      // time for the command, 0.5s units
  unsigned short YawTime;       // determines Yaw speed, 75ms units
  unsigned short YawDirection;  // 0 for CW, 1 for CCW
  unsigned short PitchTime;     // determines Pitch speed, 75ms units
  unsigned short PitchDirection; 
  unsigned short RollTime;      // determines Roll speed, 75ms units
  unsigned short RollDirection; // 0 for CW, 1 for CCW
  const struct Command *Next;   // circular link 
};
typedef const struct Command CommandType;
CommandType Machine[9]={
// Duration Yaw           Pitch      Roll
  {2,       RPM1,    CW,  RPM0,   0, RPM0,   0, &Machine[1]},
  {2,       RPM1,   CCW,  RPM0,   0, RPM1,  CW, &Machine[2]},
  {1,       RPM2,    CW,  RPM1,  CW, RPM1,  CW, &Machine[3]},
  {1,       RPM0_5, CCW,  RPM1,  CW, RPM1,  CW, &Machine[4]},
  {1,       RPM0_5, CCW,  RPM1,  CW, RPM4, CCW, &Machine[5]},
  {1,       RPM0_5, CCW,  RPM1,  CW, RPM0,   0, &Machine[6]},
  {1,       RPM0_5, CCW,  RPM2, CCW, RPM0,   0, &Machine[7]},
  {1,       RPM0,     0,  RPM2, CCW, RPM0,   0, &Machine[8]},
  {10,      RPM0,     0,  RPM0,   0, RPM0,   0, &Machine[0]}};
CommandType *CmdPt;  // Current command 
//Program 13.1.  Table structure used to define high-level commands.




const struct StepperState{
  unsigned char OutHigh;  // Output for motors with bits 7-4 
  unsigned char OutLow;   // Output for motors with bits 3-0
  const struct StepperState *Next[2]; // CW or CCW 
}; 
typedef const struct StepperState StepperStateType;
StepperStateType Stepper[4]={
  { 0x50,0x05,{&Stepper[1],&Stepper[3]}},
  { 0x60,0x06,{&Stepper[2],&Stepper[0]}},
  { 0xA0,0x0A,{&Stepper[3],&Stepper[1]}},
  { 0x90,0x09,{&Stepper[0],&Stepper[2]}}
};
StepperStateType *YawPt;    // Current Stepper state for Yaw motor
StepperStateType *PitchPt;  // Current Stepper state for Pitch motor
StepperStateType *RollPt;   // Current Stepper state for Roll motor
//Program 13.2. Linked list structure used to define low-level stepper outputs.





unsigned short HighCount,YawCount,PitchCount,RollCount;
interrupt 8 void TC0handler(void){
  TFLG1 = 0x01;              // acknowledge C0F
  TC0 = TC0+62500U;          // 0.5sec period 
  if(--HighCount==0){        // done?
    CmdPt = CmdPt->Next;     // next command
    HighCount = CmdPt->Duration;
  }
}
interrupt 9 void TC1handler(void){
  TFLG1 = 0x02;          // acknowledge C1F
  TC1 = TC1+9375;        // 75ms period
  if(CmdPt->YawTime){
    if(--YawCount == 0){
      YawCount = CmdPt->YawTime;
      YawPt = YawPt->Next[CmdPt->YawDirection];  
      PTT = (PTT&0x0F)+YawPt->OutHigh; //set Port T bits 7-4
    } 
  }
  if(CmdPt->PitchTime){
    if(--PitchCount == 0){
      PitchCount = CmdPt->PitchTime;
      PitchPt = PitchPt->Next[CmdPt->PitchDirection];  
      PTT = (PTT&0xF0)+PitchPt->OutLow; //set Port T bits 3-0
    } 
  }
  if(CmdPt->RollTime){
    if(--RollCount == 0){
      RollCount = CmdPt->RollTime;
      RollPt = RollPt->Next[CmdPt->RollDirection];  
      PTM = (PTM&0x30)+RollPt->OutLow; //set Port M bits 3-0
    } 
  }
}
void Motor_Init(void){ // assumes 4MHz E clock
  PTT = 0x55;       // start at 5, first output moves motor
  PTM = 0x05;
  DDRT = 0xFF;      // PT7-4 is yaw, PT3-0 is pitch
  DDRM |= 0x0F;     // PM3-0 is roll
  TIOS |= 0x03;     // activate TC0, TC1 as output compares
  TSCR1 = 0x80;     // Enable TCNT, 8us
  TSCR2 = 0x05;     // divide by 32 TCNT prescale, TOI disarm
  PACTL = 0;        // timer prescale used for TCNT
  CmdPt = &Machine[8]; // first command will be 0
  YawPt = PitchPt = RollPt = &Stepper[0];
  HighCount = YawCount = PitchCount = RollCount = 1;
  TIE  |= 0x03;     // arm OC1, OC0
  TC0   = TCNT+50;  // first interrupt right away
  TC1   = TC0+50;   // interrupts after TC0
  asm cli  
}
//Program 13.3.  C software to spin three stepper motors.




short Tlow,Thigh;     // controller set points, 0.5 C
// PTM bit 0 turns power on/off
interrupt 13 void TC5handler(void){ 
short T=SE(ADC_In(0)); // estimated temperature, 0.5 C 
  if(T < Tlow){ 
    PTM |= 0x01;     // too cold so on
  }
  else if (T > Thigh){
    PTM &= ~0x01;    // too hot so off
  }                  // leave as is if Tlow<T<Thigh
  TC5 = TC5+Period;  // periodic rate
  TFLG1 = 0x20;      // acknowledge C5F
}
//Program 13.4. Bang-bang temperature control software.



short X,Xstar,E; // position, fixed-point in 0.1mm
// PTT is connected to the 8-bit DAC
interrupt 13 void TC5handler(void){ short U;
   X = SE(ADC_In(0));   // estimated (0.1 mm)
   E = Xstar-X;         // error (0.1 mm)
   U = (short)PTT;      // promote to 16 bits
   if(E < -10)     U--; // decrease if less than -1mm
   else if(E > 10) U++; // increase if greater than +1mm
                        // leave as is if -1mm<E<1mm
   if(U<0)   U=0;       // underflow
   if(U>255) U=255;     // overflow
   PTT = U;             // output to actuator
   TC5 = TC5+Period;    // periodic rate
   TFLG1 = 0x20;        // acknowledge C5F
}
//Program 13.5. Incremental position control software.







unsigned short Time; // Time in 0.1 msec
short X;             // Estimated position in 0.1 cm, 0 to 1000
short Xstar;         // Desired pos in 0.1 cm, 0 to 1000
short E;             // Position error in 0.1 cm, -1000 to +1000 
short U,I,P;         // Actuator duty cycle, 100 to 19900 cycles
unsigned short Cnt;  // once a sec
unsigned short Told; // used to measure period
void interrupt 13 TC5handler(void){ 
  TFLG1 = 0x20;      // ack C5F
  TC5 = TC5+200;     // every 0.1 ms
  Time++;            // used to measure period
  if((Cnt++)==4000){ // every 0.4 sec 
    Cnt = 0;         // 0<X<100, 0<Xstar<100, 100<U<19900
    E = Xstar-X;  
    P = (105*E)/20;
    I = I+(101*E)/640;
    if(I < -500) I=-500;  // anti-reset windup
    if(I > 4000) I=4000;
    U = P+I;             // PI controller has two parts
    if(U < 100) U=100;   // Constrain actuator output
    if(U>19900) U=19900;
  } 
}
//Program 13.6. Integral position control software.








void interrupt 12 TC4handler(void){
  TFLG1 = 0x10;        // acknowledge C4F
  if(PTT&0x10){   
    TC4 = TC4+U;       // PT4 is 1, High for the next U cycles
  } 
  else{
    TC4 = TC4+20000-U; // PT4 is 1, Low for the next 20000-U cycles 
  }
}     
//Program 13.7. PWM actuator control software.





// Time is incremented every 0.1 ms, by OC5
// This handler is executed on rise of PT1
void interrupt 9 TC1Handler(void){unsigned short p;
  TFLG1 = 0x02;  // Acknowledge C1F
  p = Time-Told; // period in msec
  X = p-100;     // estimated position (0.1 cm)
  Told = Time;
} 
//Program 13.8.  Sensor measurement software.




void Init(void){ // PT5 output for debugging
  asm sei        // make atomic
  TIOS = 0x30;   // output compare OC5, OC4
  DDRT &= ~0x02; // PT1 is input
  DDRT |= 0x30;  // PT5, PT4 are output
  TSCR1 = 0x80;  // enable
  TSCR2 = 0x01;  // 500 ns clock
  TCTL1 = (TCTL1&0xF0)|0x05; // toggle PT5, PT4
  TCTL4 = 0x08;  // Input capture on rise of IC1 
  TIE = 0x32;    // Arm OC5F+OC4F+IC1F  
  U = 100;       // Initial U, low power
  Time = Told = 0;  Cnt = 0;
  TFLG1 = 0x32;  // clear flags 
  TC5 = TCNT+50; // First OC5 in 25us 
  asm cli 
}
//Program 13.9.  Initialization software.





char Subtract(unsigned char N, unsigned char M){   
/* returns N-M */
unsigned short N16,M16; 
short Result16;
  N16 = N;        /* Promote N,M */
  M16 = M;
  Result16 = N16-M16;   /* -255=Result16=+255 */
  if(Result16<-128) Result16 = -128;
  if(Result16>127)  Result16 = 127;
  return(Result16);
}
//Program 13.10. Subtraction with overflow/underflow checking.

unsigned char Ts;     /* Desired Speed in 3.9 rpm units */
unsigned char T;      /* Current Speed in 3.9 rpm units */
unsigned char Told;   /* Previous Speed in 3.9 rpm units */
char D;               /* Change in Speed in 3.9 rpm/time units */
char E;               /* Error in Speed in 3.9 rpm units */
//Program 13.11. Inputs and crisp inputs.


//The need for the special Subtract function can be demonstrated with the following example:
     E=Ts-T;  // if Ts=200 and T=50 then E will be -106!!

//This function can be used to calculate both E and D,
void CrispInput(void){
     E=Subtract(Ts,T);
     D=Subtract(T,Told);
     Told=T;}     /* Set up Told for next time */
//Program 13.12. Calculation of crisp inputs.



													 
#define TE 20
unsigned char Fast, OK, Slow, Down, Constant, Up;
#define TD 20
unsigned char Increase,Same,Decrease;
#define TN 20
void InputMembership(void){
      if(E <= -TE) {            /*     E=-TE */
        Fast=255;
        OK=0;
        Slow=0;}
      else 
        if (E < 0) {            /* -TE<E<0   */
          Fast=(255*(-E))/TE;
          OK=255-Fast;
          Slow=0;}
        else 
          if (E < TE) {         /*  0<E<TE   */
            Fast=0;
            Slow=(255*E)/TE;
            OK=255-Slow;}
          else {                /* +TE=E     */
            Fast=0;
            OK=0;
            Slow=255;}
      if(D <= -TD) {            /*    D=-TD  */
        Down=255;
        Constant=0;
        Up=0;}
      else 
        if (D < 0) {            /* -TD<D<0   */
          Down=(255*(-D))/TD;
          Constant=255-Down;
          Up=0;}
        else 
          if (D < TD) {         /*  0<D<TD   */
            Down=0;
            Up=(255*D)/TD;
            Constant=255-Up;}
          else {                /* +TD=D     */
            Down=0;
            Constant=0;
            Up=255;}}
//Program 13.13. Calculation of the fuzzy membership variables in C.




unsigned char min(unsigned char u1,unsigned char u2){
     if(u1>u2) return(u2);
     else return(u1);}
unsigned char max(unsigned char u1,unsigned char u2){
     if(u1<u2) return(u2);
     else return(u1);}
void OutputMembership(void){
     Same=min(OK,Constant);
     Decrease=min(OK,Up)
     Decrease=max(Decrease,min(Fast,Constant));
     Decrease=max(Decrease,min(Fast,Up));
     Increase=min(OK,Down)
     Increase=max(Increase,min(Slow,Constant));
     Increase=max(Increase,min(Slow,Down));}
//Program 13.14. Calculation of the output fuzzy membership variables in C.



char dN;
void CrispOutput(void){
         dN=(TN*(Increase-Decrease))/(Decrease+Same+Increase);
}
//Program 13.15. Calculation of the crisp output in C.

unsigned int Time;
#define rate 2000  /* Rate must be less than 32767 E clocks */
void Initialize(void){
     PORTB=0;
     N=0;           /* Initial Actuator */
     Told=0;
     Ts=128; }      /* 500 rpm */
#define CCF 0x80
void Main(void){ int dT;
     Initialize();             /* Turn on A/D initialize globals */
     Time=TCNT+rate;           /* TCNT value for first calculation */
     while(1){
        while((dT=Time-TCNT)>0){};
        Time=Time+rate;        /* TCNT value for next calculation */
        T=Sample(0);           /* Sample A/D and set T */
        CrispInput();          /* Calculate E,D and new Told */
        InputMembership();     /* Sets Fast,OK,Slow,Down,Constant,Up */
        OutputMembership();    /* Sets Increase,Same,Decrease */
        CrispOutput();         /* Sets dN */
        N=max(0,min(N+dN,255));
        PORTB=N;               /* Set Actuator */
//Program 13.16. Main program for fuzzy logic controller in C.




// Program 13.1. C implementation of a Traffic Light Controller.
/* Port B bits 5-0 outputs that control the traffic signal */
const struct State {
  unsigned char Out;         /* Output to Port B */
  unsigned short Time;       /* Time in sec to wait */
  const struct State *Next;  /* Next state */
};
typedef const struct State StateType;
#define NorthRed_EastGreen  &fsm[0]
#define NorthRed_EastYellow &fsm[1]
#define NorthGreen_EastRed  &fsm[2]
#define NorthYellow_EastRed &fsm[3]
StateType fsm[4]={
/* NorthRed_EastGreen, wait= 3 min, next state */
  {0x21, 180, NorthRed_EastYellow},  
/* NorthRed_EastYellow, wait= 15 sec, next state */
  {0x22, 15, NorthGreen_EastRed},  
/* NorthGreen_EastRed, wait= 3 min, next state */
  {0x0C, 180, NorthYellow_EastRed},   
/* NorthYellow_EastRed, wait= 15 sec, next state */
  {0x14, 15, NorthRed_EastGreen}};
void main(void){ StatePtr *Pt;  /* Current State */
  Pt=NorthRed_EastGreen; /* Initial State */
  DDRB=0xFF;             /* Make Port B outputs  */
  while(1){
    PORTB=Pt->Out;    /* Perform output for this state */
    Wait(Pt->Time);   /* Time to wait in this state */
    Pt=Pt->Next;      /* Move to next state */
  }
};

// Program 13.2 Circular list used to spin a stepper motor.
/* Port B bits 3-0 outputs that control the stepper motor */
const struct State {
  unsigned char Out;         /* Output to Port B */
  const struct State *Next;  /* Next state */
};
typedef const struct State StateType;
#define S6  &fsm[0]
#define S5  &fsm[1]
#define S9  &fsm[2]
#define S10 &fsm[3]
StateType fsm[4]={
  {0x06, S5},  
  {0x05, S9},  
  {0x09, S10},  
  {0x0A, S6};
StatePtr *Pt;  /* Current State */
unsigned short Speed;

// Program 13.3 C software to spin a stepper motor at a constant speed.
// MC68HC812A4, ICC12 compiler
#define OC5  0x20
#pragma interrupt_handler TC5handler()
void TC5handler(void){
   PORTB=Pt->Out; // output for this state 
   Pt=Pt->Next;   // Move to next state 
   TFLG1=OC5;       // ack C5F 
   TC5=TC5+Speed;}  // Executed every step
void ritual(void) { 
asm(" sei");     // make atomic
  TIOS|=OC5;     // enable OC5
  TSCR|=0x80;    // enable
  TMSK2=0x32;    // 500 ns clock
  TMSK1|=OC5;    // Arm output compare 5
  DDRB=0xFF;     // Make Port B outputs
  TFLG1=OC5;     // Initially clear C5F
  Speed=10000;   // initial speed
  Pt=S6;         // initial state
  TC5=TCNT+2000; // First one in 1 ms
asm(" cli"); } 

// Program 13.4 Port B is used to turn on the heater.
unsigned char Tlow,Thigh,T; int E;  // units in degrees C 
void Actuator(unsigned char relay){
  PORTB=relay;    // turns power on/off
}

// Program 13.5 Bang-bang temperature control software.
// MC68HC812A4, ICC12 compiler
#pragma interrupt_handler TC5handler()
void TC5handler(void){
     T=SE(A2D(channel)); // estimated T 
     E=Tstar-T;          // error
     if(T<Tlow) 
       Actuator(0);      // too cold so off
     else if (T>Thigh)
       Actuator(1);      // too hot so on
// leave as is if Tlow<T<Thigh
     TC5=TC5+rate;      // periodic rate
     TFLG1=0x20; }      // ack C5F

// Program 13.6 Incremental position control software.
// MC68HC812A4, ICC12 compiler
#pragma interrupt_handler TC5handler()
void TC5handler(void){ int new;
   X=SE(A2D(channel));  // estimated (mm)
   E=Xstar-X;     // error (mm)
   new=PORTB;     // promote to 16 bits
   if(E< -1)     new--; // decrease
   else if (E>1) new++; // increase
// leave as is if -1<E<1
   if(new<0)   new=0;    // underflow
   if(new>255) new=255;  // overflow
   PORTB=new;   // output to actuator
   TC5=TC5+rate;      // periodic rate
   TFLG1=0x20; }      // ack C5F

// Program 13.7 Integral position control software.
// MC68HC812A4, ICC12 compiler
unsigned int Time;  // Time in msec
unsigned int X;     // Est position in cm
unsigned int Xstar; // Desired pos in cm 
unsigned int U;     // Actuator duty cycle
unsigned int Cnt;   // once a sec
unsigned int Told;  // used to meas period
#pragma interrupt_handler TC5handler()
void TC5handler(void){ int NewU;
  TFLG1=0x20;        // ack C5F
  TC5=TC5+2000;      // every 1 ms
  Time++;     // used to measure period
  if((Cnt++)==1000){  // every 1 sec 
     Cnt=0;
// 0<X<100, 0<Xstar<100, 100<U<19900, so
// Minimum occurs when U=100,Xstar=0,X=100
//    Minimum NewU = -900
// Max occurs when U=19900,Xstar=100,X=0
//    Maximum NewU = 20900
// so NewU will be a valid signed int 
     NewU=U+10*(Xstar-X);
     if(NewU<100) NewU=100;  // Constrain
     if(NewU>19900) NewU=19900;
     U=NewU;  } }

// Program 13.8 PWM actuator control software.
// MC68HC812A4, ICC12 compiler
#pragma interrupt_handler TC4handler()
void TC4handler(void){
  TFLG1=0x10;        // ack C4F
  if (TCTL1&0x01)   /* OL4 bit */
/* OL4=1, High for the next U cycles */
     TC4=TC4+U; 
  else
/* OL4=0, Low for the next 20000-U cyc */
     TC4=TC4+20000-U;   
    TCTL1^=0x01; }     /* Toggle OL4 */ 

// Program 13.9 Sensor measurement software.
// MC68HC812A4, ICC12 compiler
// Time is incremented every 1 ms, by OC5
// This handler is executed on rise
#pragma interrupt_handler TC1handler()
void TC1Handler(void){unsigned int p;
  TFLG1 = 0x02;  // Ack C5F
  p = Time-Told; // period in msec
  X = p-10;    // estimated position (cm)
  Told=Time;
} 

// Program 13.10 Initialization software.
// MC68HC812A4, ICC12 compiler
void ritual(void) { // Input capture IC1
  asm(" sei");      /* atomic */
  TIOS=0x30;  // output compare OC5, OC4
  TSCR|=0x80; // enable TCNT
  TMSK2=0x32; // 500 ns clock
  TCTL1=0x02; // Clear OC4 
  TCTL4=0x08; // capture on rise of IC1 
  TMSK1=0x32; // Arm OC5F+OC4F+IC1F  
  U=10000;      // Initial U , half power
  Time = 0; Told=0;  Cnt=0;
  TFLG1=0x32; // clear flags 
  TC5=TCNT+2000; // First OC5 in 1 ms 
  TC4=TCNT+100;  // First OC4 in 50us 
  asm(" cli");}

Program 13.11. Subtraction with overflow/underflow checking.
char Subtract(unsigned char N, unsigned char M){   
/* returns N-M */
unsigned int N16,M16; 
int Result16;
     N16=N;        /* Promote N,M */
     M16=M;
     Result16=N16-M16;   /* -255ČResult16Č+255 */
     if(Result16<-128) Result16 = -128;
     if(Result16>127)  Result16 = 127;
     return(Result16);}

// Program 13.12. Inputs and crisp inputs.
unsigned char Ts;     /* Desired Speed in 3.9 rpm units */
unsigned char T;      /* Current Speed in 3.9 rpm units */
unsigned char Told;   /* Previous Speed in 3.9 rpm units */
char D;               /* Change in Speed in 3.9 rpm/time units */
char E;               /* Error in Speed in 3.9 rpm units */

// Program 13.13. Calculation of crisp inputs.
void CrispInput(void){
     E=Subtract(Ts,T);
     D=Subtract(T,Told);
     Told=T;}     /* Set up Told for next time */

// Program 13.14. Calculation of the fuzzy membership variables in C.
#define TE 20
unsigned char Fast, OK, Slow, Down, Constant, Up;
#define TD 20
unsigned char Increase,Same,Decrease;
#define TN 20
void InputMembership(void){
      if(E <= -TE) {            /*     EČ-TE */
        Fast=255;
        OK=0;
        Slow=0;}
      else 
        if (E < 0) {            /* -TE<E<0   */
          Fast=(255*(-E))/TE;
          OK=255-Fast;
          Slow=0;}
        else 
          if (E < TE) {         /*  0<E<TE   */
            Fast=0;
            Slow=(255*E)/TE;
            OK=255-Slow;}
          else {                /* +TEČE     */
            Fast=0;
            OK=0;
            Slow=255;}
      if(D <= -TD) {            /*    DČ-TD  */
        Down=255;
        Constant=0;
        Up=0;}
      else 
        if (D < 0) {            /* -TD<D<0   */
          Down=(255*(-D))/TD;
          Constant=255-Down;
          Up=0;}
        else 
          if (D < TD) {         /*  0<D<TD   */
            Down=0;
            Up=(255*D)/TD;
            Constant=255-Up;}
          else {                /* +TDČD     */
            Down=0;
            Constant=0;
            Up=255;}}

// Program 13.15. Calculation of the output fuzzy membership variables in C.
unsigned char min(unsigned char u1,unsigned char u2){
     if(u1>u2) return(u2);
     else return(u1);}
unsigned char max(unsigned char u1,unsigned char u2){
     if(u1<u2) return(u2);
     else return(u1);}
void OutputMembership(void){
     Same=min(OK,Constant);
     Decrease=min(OK,Up)
     Decrease=max(Decrease,min(Fast,Constant));
     Decrease=max(Decrease,min(Fast,Up));
     Increase=min(OK,Down)
     Increase=max(Increase,min(Slow,Constant));
     Increase=max(Increase,min(Slow,Down));}

// Program 13.16. Calculation of the crisp output in C.
char dN;
void CrispOutput(void){
         dN=(TN*(Increase-Decrease))/(Decrease+Same+Increase);
}

// Program 13.17. Main program for fuzzy logic controller in C.
unsigned int Time;
#define rate 2000  /* Rate must be less than 32767 E clocks */
void Initialize(void){
     TSCR|=0x80;    // enable
     TMSK2=0x32;    // 500 ns clock
     ATDCTL2 = 0x80;  // Activate A/D
     DDRB=0xFF; 
     PORTB=0;
     N=0;           /* Initial Actuator */
     Told=0;
     Ts=128; }      /* 500 rpm */
#define SCF 0x8000
unsigned char Sample(unsigned char channel){
    ATDCTL5=channel;    // Start A/D 
    while ((ATDSTAT & SCF) == 0){};
    return(ADR0H); }
void Main(void){ int dT;
     Initialize();             /* Turn on A/D initialize globals */
     Time=TCNT+rate;           /* TCNT value for first calculation */
     while(1){
        while((dT=Time-TCNT)>0){};
        Time=Time+rate;        /* TCNT value for next calculation */
        T=Sample(0);           /* Sample A/D and set T */
        CrispInput();          /* Calculate E,D and new Told */
        InputMembership();     /* Sets Fast,OK,Slow,Down,Constant,Up */
        OutputMembership();    /* Sets Increase,Same,Decrease */
        CrispOutput();         /* Sets dN */
        N=max(0,min(N+dN,255));
        PORTB=N;               /* Set Actuator */