File content as of revision 3:8107bb13278b:

#include "PosVelFilter.hpp"
#include "StaticDefs.hpp"
#include "conversions.hpp"

PosVelFilter::PosVelFilter()
{
    x1 = 0;
    x2 = 0;
    //w_n is the natural frequency of the filter bigger increases frequency response
    
    //w_n = 5.0;  // larger number equals faster response
    
    w_n = 2.0;      //5.0 was way off

}

void PosVelFilter::update()
{
    //run the pos/vel estimate filter
    //this derives the timing from last run
    last_time = curr_time;
    curr_time = systemTime().read();
    dt = curr_time-last_time;

    // fetch the current distance reading from adc and convert to mm
    conv_distance = counts_to_dist(adc().ch0_filt);
    //conv_distance = counts_to_dist(300);  //testing

    x1_dot = x2*1.0;
    x2_dot = (-2.0*w_n*x2)-(w_n*w_n)*x1+(w_n*w_n)*conv_distance;

    position = x1;
    velocity = x2;
    
    //JUST A TEST (TROY)
    //position = dt;
    //velocity = 0;   //velocity needs to be zero, stabilize
    //conv_distance = 1234;
    

    x1 += x1_dot*dt;
    x2 += x2_dot*dt;

}

void PosVelFilter::update_la()
{
    //run the pos/vel estimate filter
    //this derives the timing from last run
    last_time = curr_time;
    curr_time = systemTime().read();
    dt = curr_time-last_time;

    // fetch the current distance reading from adc and convert to mm
    conv_distance = counts_to_dist(adc().ch1_filt);
    //conv_distance = counts_to_dist(300);  //testing

    x1_dot = x2*1.0;
    x2_dot = (-2.0*w_n*x2)-(w_n*w_n)*x1+(w_n*w_n)*conv_distance;

    position = x1;
    velocity = x2;
    
    //JUST A TEST (TROY)
    //position = dt;
    //velocity = 0;   //velocity needs to be zero, stabilize
    //conv_distance = 1234;
    

    x1 += x1_dot*dt;
    x2 += x2_dot*dt;

}
void PosVelFilter::init()
{
    // run filter for 2 seconds on startup
    float start = systemTime().read();
    float time = start;
    pc().printf("\n\rWarming Up \n\r");
 
    while ((time - start) < 1.0) {
        update();

        pc().printf("%5.3f \r", velocity);
        time = systemTime().read();
    }
    pc().printf("\n\r");
    while (1) {
        update();

        if (abs(velocity) > 0.005) {
            pc().printf("Waiting to stablize. velocity: %5.3f \r", velocity);

        } else {
            pc().printf("\n\rreading stabilized\n\r");
            break;
        }
    }

}

void PosVelFilter::init_la()
{
    // run filter for 2 seconds on startup
    float start = systemTime().read();
    float time = start;
    pc().printf("\n\rWarming Up \n\r");
 
    while ((time - start) < 1.0) {
        update_la();

        pc().printf("%5.3f \r", velocity);
        time = systemTime().read();
    }
    pc().printf("\n\r");
    while (1) {
        update();

        if (abs(velocity) > 0.005) {
            pc().printf("Waiting to stablize. velocity: %5.3f \r", velocity);

        } else {
            pc().printf("\n\rreading stabilized\n\r");
            break;
        }
    }

}

float PosVelFilter::getPosition()
{
    return position;
}

float PosVelFilter::getVelocity()
{
    return velocity;
}

float PosVelFilter::getDt()
{
    return dt;
}

void PosVelFilter::writeWn(float wn)
{
    w_n = wn;
    pc().printf("\n\rWn set to: %f", w_n);
}

float PosVelFilter::get_conv_distance()
{
    return conv_distance;
}

float PosVelFilter::get_curr_time()
{
    return curr_time;
}

float PosVelFilter::get_last_time()
{
    return last_time;
}

float PosVelFilter::get_x1_dot()
{
    return x1_dot;
}

float PosVelFilter::get_x2_dot()
{
    return x2_dot;
}

float PosVelFilter::get_x1()
{
    return x1;
}

float PosVelFilter::get_x2()
{
    return x2;
}