File content as of revision 0:5252dc3a058e:

/*
  Rotary Encoder Test
  Demonstrates operation of Rotary Encoder
  Displays results on Serial Monitor
*/

#include "mbed.h"

#define tick 32.727272727272727
#define pi 3.14159265358979323846264338

InterruptIn encoder (p22);
PwmOut motor (p21);
Serial pc (USBTX, USBRX);
DigitalOut led(LED1);
DigitalOut blinker(LED2);
Timer t;
Timer t2;

volatile double rev = 0, old_rev = 0, raw_rev = 0, print_rev = 0;
volatile double elapsed_time = 99999999999, cur_elapsed_time = 0;
volatile double angle = 0, raw_angle = 0, target = 0, target_diff = 0;
volatile double cur_angle = 0, print_angle = 0, num = 0, err = 0;

volatile int p = 0, i = 0, d = 0, spd = 0, up_lim = 255;
volatile int low_lim = 20, cntr = 0, print_spd = 0;
char buf[5];
volatile bool bypass = false, bypass2 = false;

void PID(){
    // PID
    err = target-rev;
    p = err*(110*target+500);
    double temp = err*elapsed_time*(target*330+110);
    if (temp < 20 && temp > -20)
        i += temp;
    else if (temp >= 20)
        i += 20;
    else
        i -= 20;
    if (i < -5)
        i = -5;
    else if (i > 120+target*80)
        i = 300;
    d = (old_rev-rev)/elapsed_time*7;
    spd = p + i + d;
  
    if (target == 0)
        spd = 0;
    
    else{
        up_lim = target * 25 + 160;
    
        if (spd < low_lim)
            spd = low_lim;
        else{
            if (spd > up_lim)
                spd = up_lim;
            if (spd > 255)
                spd = 255;
        }
    }
    
    motor = spd / 255.0;
}

void control() {
    t.stop();
    elapsed_time = t.read();
    t.reset();
    t.start();
    
    blinker = !blinker;
  
    if (elapsed_time == 0)
        raw_rev = 7;
    else
        raw_rev = tick/360.0/elapsed_time;
    rev = raw_rev;
  
    // Discard unrealistic revs
    if (rev - 1.3*print_rev > 1 || rev > 6){
        rev = print_rev;
        cntr++;
    }
    else{
        old_rev = rev;
        raw_angle += tick;
        angle = (raw_angle + 2*cur_angle) / 3.;
    }
    if (rev<6.5)
        print_rev = (print_rev * 39. + rev) / 40.;
  
    PID();
}

int main() {
    encoder.rise(&control);
    pc.baud(38400);
    pc.printf("Hello\n");
    motor.period_us(800);
  
    while (true){
  
        if (pc.readable()){
            pc.scanf("%s", &buf);
            num = atof(buf);
            if (num > 6){
                angle -= cur_angle;
                raw_angle -= cur_angle;
                cur_angle = 0;
                elapsed_time = 99999999999;
                rev = 0;
            }
            else{
                target_diff = abs(target-num);
                target = num * 1.005;
                motor = (target*40.0+50.0) / 255.0;
                bypass = true;
                i = 0;
                low_lim = 0;
            }
        }

        cur_elapsed_time = t.read();
        if (target<2 && cur_elapsed_time>elapsed_time){
            rev = tick/360.0/cur_elapsed_time;
            if (rev - 1.2*print_rev > 0.2 || rev > 5)
                rev = print_rev;
            PID();
        }
       
        cur_angle = angle + print_rev*360*cur_elapsed_time;
        if (cur_angle > angle + tick)
            cur_angle = angle + tick;
        if (cur_angle > 360.){
            raw_angle -= 360.;
            angle -= 360.;
            cur_angle -= 360.;
        }
        
    
        if (rev < 6.5){
            print_rev = (print_rev * 39. + rev) / 40.;
            print_angle = cur_angle;
            if (print_angle < 10 || print_angle > 350)
                led = 1;
            else
                led = 0;
            pc.printf("%5.1f,%4.2f,%d,%3.1f\n", print_angle, print_rev, i, target);
        }
    }
}