File content as of revision 23:d8cacd996cce:

#include <encoder.h>

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


Encoder::Encoder(Timer t1){
    
    //set timer
    t = t1;
    //Initialize Intervals used during encoder data collection to measure velocity
    interval1=0;
    interval2=0;
    interval3=0;
    avg_interval=0;
    lastchange1 = 0;
    lastchange2 = 0;
    lastchange3 = 0;
    lastchange4 = 0;
    
    //Initialize Variables used to for velocity control
    avg_speed = 0;
    stall_check = 0;
    tuning_val = 1;
    
    // Servo parameters
    lastTurnTime = 0.0f;
    servoLeft = true;
    
    //Parameters specifying sample sizes and delays for small and large average speed samples
    num_samples_small = 10.0f;
    delay_small = 0.05f;
    num_samples_large = 100.0f;
    delay_large = 0.1f;
    
}

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


float Encoder::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;
}

float Encoder::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;
    }
    return .0025 * duty_cyc * tuning_val;
    
}
void Encoder::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 Encoder::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");
}