File content as of revision 4:263bddc51c0f:

#include "mbed.h"

DigitalOut myled(LED1);
PwmOut servo(PTA5);
PwmOut motor(PTA4);
Serial pc(USBTX, USBRX); // tx, rx

// encoder setup and variables
InterruptIn interrupt(PTA13);

int interval1=0;
int interval2=0;
int interval3=0;
int avg_interval=0;
int lastchange1 = 0;
int lastchange2 = 0;
int lastchange3 = 0;
int lastchange4 = 0;
Timer t;
//constant *(PWM-expected)
const float TUNING_CONSTANT = .5;
const float PI = 3.14159;
const float WHEEL_CIRCUMFERENCE = .05*PI;

float get_speed(){
    float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f;
    float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE;
    return linearSpeed;
    
}
void servo_sweep(){
    for(float p = 0.001; p<0.002; p+=0.0001){
            servo.pulsewidth(p);
            wait(0.5);
    }
}
void fallInterrupt(){
    
    int time = t.read_us();
    interval1 = time - lastchange2;
    interval2 = lastchange1-lastchange3;
    interval3 = lastchange2 - lastchange4;
    avg_interval = (interval1 + interval2 + interval3)/3;
    
    lastchange4 = lastchange3;
    lastchange3 = lastchange2;
    lastchange2 = lastchange1;
    lastchange1 = time;
    //pc.printf("dark to light time : %d\n\r", interval);
    //pc.printf("fall");
}
void riseInterrupt(){
    int time = t.read_us();
    interval1 = time - lastchange2;
    interval2 = lastchange1-lastchange3;
    interval3 = lastchange2 - lastchange4;
    avg_interval = (interval1 + interval2 + interval3)/3;
    
    lastchange4 = lastchange3;
    lastchange3 = lastchange2;
    lastchange2 = lastchange1;
    lastchange1 = time;
    //pc.printf("light to dark time : %d\n\r", interval);
    //pc.printf("rise");
}
    
int main() {
    servo.period(0.005);
    motor.period(.0025);
    interrupt.fall(&fallInterrupt);
    interrupt.rise(&riseInterrupt);
    
    
    t.start();
    while(1){
        
        char choice = pc.getc();
        pc.putc(choice);
        switch(choice){
            case '0':
                motor.pulsewidth(0.0);
                pc.printf("0% \n\r");
                
                break;
            case '1':
                motor.pulsewidth(.0025);
                pc.printf("100% \n\r");
                wait(.5);
                pc.printf("speed: %f",get_speed());
                break;
            case '2':
                motor.pulsewidth(.0025*.2);
                pc.printf("20% \n\r");
                wait(.5);
                pc.printf("speed: %f",get_speed());
                break;
            case '3':
                motor.pulsewidth(.0025*.3);
                pc.printf("30% \n\r");
                wait(.5);
                pc.printf("speed: %f",get_speed());
                break;
            case '5':
                motor.pulsewidth(.0025*.5);
                pc.printf("50% \n\r");
                wait(.5);
                pc.printf("speed: %f",get_speed());
                break;
            default:
                motor.pulsewidth(.0025*.3);
                pc.printf("default\n\r");
                break;
        }
    
       //servo_sweep();
    }
}