File content as of revision 9:aecffea74d88:

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

//Intervals used during encoder data collection to measure velocity
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;

//Variables used to for velocity control
float avg_speed = 0;
float stall_check = 0;
float tuning_val = 1;

Timer t;

//Observed average speeds for each duty cycle
const float TUNING_CONSTANT_20 = 3.00;
const float TUNING_CONSTANT_30 = 4.30;
const float TUNING_CONSTANT_50 = 6.880;
const float PI = 3.14159;
const float WHEEL_CIRCUMFERENCE = .05*PI;

//Velocity Control Tuning Constants
const float TUNE_THRESH = 0.5f;
const float TUNE_AMT = 0.1f;

//Parameters specifying sample sizes and delays for small and large average speed samples
float num_samples_small = 10.0f;
float delay_small = 0.05f;
float num_samples_large = 100.0f;
float delay_large = 0.1f;

// Large and small arrays used to get average velocity values
float large_avg_speed_list [100];
float small_avg_speed_list [10];

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Functions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
float get_speed(){
    float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f;
    float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE;
    return linearSpeed;
}

float get_avg_speed(float num_samples, float delay) {
    
    float avg_avg_speed = 0;
    
    for (int c = 0; c < num_samples; c++) {
        if (num_samples == num_samples_small){
            small_avg_speed_list[c] = get_speed();
        } else if (num_samples == num_samples_large){
            large_avg_speed_list[c] = get_speed();
            //pc.printf("\n\rworking: %f", large_avg_speed_list[c]);
        }
        wait(delay);
        }
                
        for (int c = 1; c < num_samples; c++) {
            if (num_samples == num_samples_small){
                avg_avg_speed += small_avg_speed_list[c];
            } else if (num_samples == num_samples_large){
                avg_avg_speed += large_avg_speed_list[c];
                //pc.printf("\n\rworking: %f", large_avg_speed_list[c]);
            }
        }
    return avg_avg_speed/num_samples;
}

void velocity_control(float duty_cyc, float tuning_const) {
    
    avg_speed = get_avg_speed(num_samples_small, delay_small);
    
    if (avg_speed == stall_check) {
        avg_speed = 0;
        tuning_val += TUNE_AMT;
    } else if((avg_speed -  tuning_const) > TUNE_THRESH){
        tuning_val -= TUNE_AMT;
        stall_check = avg_speed;
    } else if (avg_speed - tuning_const < -1*TUNE_THRESH){
        tuning_val += TUNE_AMT;
        stall_check = avg_speed;
    } else {
        tuning_val = 1;
        stall_check = avg_speed;
    }
    motor.pulsewidth(.0025 * duty_cyc * tuning_val);
    
    pc.printf("speed: %f\n\rtuning val: %f\n\r", avg_speed, tuning_val);
    wait(.2);
}
    
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);      
        
        wait(5);
        char choice = '3';
        
        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_avg_speed(num_samples_small, delay_small));

                break;
            case '2':
                motor.pulsewidth(.0025*.2);
                pc.printf("\n\r20% \n\r");
                wait(.5);       
                pc.printf("speed: %f\n\rtuning val: %f\n\r", get_avg_speed(num_samples_small, delay_small));
                
                while(1){
                    velocity_control(0.2f, TUNING_CONSTANT_20);
                }
                
                //break; 
            case '3':
                motor.pulsewidth(.0025*.3);
                pc.printf("\n\r30% \n\r");
                wait(.5);
                pc.printf("speed: %f",get_avg_speed(num_samples_small, delay_small));

                while(1){
                    velocity_control(0.3f, TUNING_CONSTANT_30);
                }
                
                //break;
            case '5':
                motor.pulsewidth(.0025*.5);
                pc.printf("\n\r50% \n\r");
                wait(.5);
                pc.printf("speed: %f",get_avg_speed(num_samples_small, delay_small));

                while(1){
                    velocity_control(0.5f, TUNING_CONSTANT_50);
                }

                //break;
            case 'a':
                pc.printf("\n\rGet average velocity of which duty cycle?\n\r");
                choice = pc.getc();
                pc.putc(choice);
                
                switch(choice){
                    
                    case '2':
                        motor.pulsewidth(.0025*0.2f);
                        pc.printf("\n\rLongterm average speed at 20 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large));
                        break;
                        
                    case '3':
                        motor.pulsewidth(.0025*0.3f);
                        pc.printf("\n\rLongterm average speed at 30 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large));
                        break;
                        
                    case '5':
                        motor.pulsewidth(.0025*0.5f);
                        pc.printf("\n\rLongterm average speed at 50 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large));
                        break;
                        
                    default:
                        break;
                }
                break;
                
            default:
                motor.pulsewidth(.0025*0);
                pc.printf("default\n\r");
                break;
        }
    
       //servo_sweep();
    }
}