File content as of revision 0:b6369e728067:

#include "mbed.h"

//------------------------------------
// Hyperterminal configuration
// 9600 bauds, 8-bit data, no parity
//------------------------------------

Serial pc(SERIAL_TX, SERIAL_RX);

InterruptIn sig(D4);
Timer sigTimer;
PwmOut ctrl(D9);
DigitalIn A(D7);
DigitalIn B(D2);

int sigPeriod = 0, refPeriod = 3000;

double dutyCycle = 0.5, stepSize = 0.01;

Timer displayTimer, controlTimer;

void onRisingEdge();
void displayRPM(int period);
void controlLoop(int period);

void readEncoder();
void encoderIncrement();
void encoderDecrement();


int main() {
  pc.printf("Hello world\n\r");
  displayTimer.start();
  controlTimer.start();
  sig.mode(PullNone);
  sig.rise(onRisingEdge);
  sigTimer.start();
  ctrl.period_us(1000);
  ctrl.write(0.5);
  while(1) { 
    displayRPM(sigPeriod);
    controlLoop(sigPeriod);
    readEncoder();
  }
}

void onRisingEdge()
{
    static const int queueLength = 1;
    static int queue[queueLength], queueStart = 0, queueEnd = 0;
    static uint64_t sum = 0;
    int time;
    
    time = sigTimer.read_us();
    if (time < 100)
      return;
    sigTimer.reset();
    if (queueStart == queueEnd && sum != 0) {
        sum -= queue[queueStart];
        queueStart = (queueStart + 1) % queueLength;
    }
    sum += time;
    queue[queueEnd] = time;
    queueEnd = (queueEnd + 1) % queueLength;
    
    if (queueStart == queueEnd)
        sigPeriod = sum / queueLength;
    else
        sigPeriod = sum / ((queueEnd + queueLength - queueStart) % queueLength);
}

void displayRPM(int period)
{        
    if (displayTimer.read_ms() < 100)
        return;
    displayTimer.reset();
    pc.printf("period is %d, refPeriod is %d, dutyCycle is %f\n\r", period, refPeriod, dutyCycle);
}

void controlLoop(int period)
{     
    if (controlTimer.read_ms() < 10)
        return;
    controlTimer.reset();

    double input = period - refPeriod;
    static const double intGain = 1.0e-7;
    static const double propGain = 1.0e-6;
    static double intState = 0.0;
    
    intState += intGain * input;
    if (intState > 0.9)
        intState = 0.9;
    else if (intState < 0.1)
        intState = 0.1;
    dutyCycle = propGain * input + intState;
    if (dutyCycle < 0.05)
        dutyCycle = 0.05;
    else if (dutyCycle > 0.95)
        dutyCycle = 0.95;
    ctrl.write(1.0 - dutyCycle);
}

void readEncoder()
{
    static int state = 0;
    int currentState;
    const int stateOrder[] = {0, 2, 3, 1};
    const int stateIndex[] = {0, 3, 1, 2};
    int ccwState, cwState;
    
    currentState = (A << 1) + B;
    ccwState = stateOrder[(stateIndex[state] + 3) & 3];
    cwState = stateOrder[(stateIndex[state] + 1) & 3];
    if (currentState == cwState)
        encoderIncrement();
    else if (currentState == ccwState)
        encoderDecrement();
    state = currentState;
}

void encoderIncrement()
{
    refPeriod -= 1000;
}

void encoderDecrement()
{
    refPeriod += 1000;
}