File content as of revision 5:61a0a21134f7:

#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;

float avg_speed_list [100];
float small_avg_speed_list [10];

Timer t;
//constant *(PWM-expected)
const float TUNING_CONSTANT_20 = 3.697;
const float TUNING_CONSTANT_30 = 5.607;
const float TUNING_CONSTANT_50 = 6.880;
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;
    
}

float get_avg_speed() {
    
    float avg_avg_speed = 0;
    
    for (int c = 0; c < 10; c++) {
        small_avg_speed_list[c] = get_speed();
        wait(.05);
        }
                
        for (int c = 0; c < 10; c++) {
            avg_avg_speed += small_avg_speed_list[c];
        }
        
    return avg_avg_speed/10.0f;
}

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){
        
        float avg_speed = 0;
        
        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);       
                float tuning_val = 1;
                pc.printf("speed: %f\n\rtuning val: %f\n\r", get_avg_speed());
                
                while(1){
                    
                    avg_speed = get_avg_speed();
                    
                    if((avg_speed -  TUNING_CONSTANT_20) > 0.5f){
                        tuning_val -= .1;
                    } else if (avg_speed - TUNING_CONSTANT_20 < -0.5f){
                        tuning_val += .1;
                    } else {
                        tuning_val = 1;
                    }
                    motor.pulsewidth(.0025 * .2 * tuning_val);
                    pc.printf("speed: %f\n\rtuning val: %f\n\r", avg_speed, tuning_val);
                    wait(.5);
                }
                
                //break;
                
            case '3':
                motor.pulsewidth(.0025*.3);
                pc.printf("30% \n\r");
                wait(.5);
                pc.printf("speed: %f",get_avg_speed());

                while(1){
                    
                    avg_speed = get_avg_speed();
                    
                    if((avg_speed -  TUNING_CONSTANT_30) > 0.5f){
                        tuning_val -= .1;
                    } else if (avg_speed - TUNING_CONSTANT_30 < -0.5f){
                        tuning_val += .1;
                    } else {
                        tuning_val = 1;
                    }
                    motor.pulsewidth(.0025 * .3 * tuning_val);
                    pc.printf("speed: %f\n\rtuning val: %f\n\r", avg_speed, tuning_val);
                    wait(.5);
                }
                
                //break;
            case '5':
                motor.pulsewidth(.0025*.5);
                pc.printf("50% \n\r");
                wait(.5);
                pc.printf("speed: %f",get_avg_speed());

                while(1){
                    
                    avg_speed = get_avg_speed();
                    
                    if((avg_speed -  TUNING_CONSTANT_50) > 0.5f){
                        tuning_val -= .1;
                    } else if (avg_speed - TUNING_CONSTANT_50 < -0.5f){
                        tuning_val += .1;
                    } else {
                        tuning_val = 1;
                    }
                    motor.pulsewidth(.0025 * .5 * tuning_val);
                    pc.printf("speed: %f\n\rtuning val: %f\n\r", avg_speed, tuning_val);
                    wait(.5);
                }

                //break;
            default:
                motor.pulsewidth(.0025*.3);
                pc.printf("default\n\r");
                break;
        }
    
       //servo_sweep();
    }
}