File content as of revision 2:483f189d3508:

#include "P_motor.h"

#define R 16
#define L 4
#define C 0.04
#define PI 3.141592654


P_motor::P_motor(I2C *i2c_, char addr_)
    : motor(i2c_, addr_), pc(USBTX, USBRX)
{
    pc.baud(115200);
    RC = R * C;
    LC = L * C;
    timer_reset();
    output_reset();
}

P_motor& P_motor::operator=(double target_)
{
    target = target_;
    compute();
    return *this;
}

void P_motor::run()
{
    motor = output_m;
    //pc.printf("%+3f\n\r", output_m);
}

void P_motor::compute()
{
    stat_old = stat;
    double time_now = timer.read_ms();
    double delta = (time_now - time_old) / 1000.0f;
    if(target > 0.01)
        stat = FORWARD;
    else if(target < -0.01)
        stat = BACKWARD;
    else
        free();
    if(stat)
    {
        if(stat * stat_old < 0)
        {
            //output[1] = -target / 2;
            free();
            //output_reset(target / -2.0f);
        }
        else
        {
            output[2] = (delta * delta * target
                    + output[1] * (delta * RC + 2 * LC)
                    - output[0] * LC)
                    / (delta * RC + LC + delta * delta);
            output[0] = output[1];
            output[1] = output[2];
            time_old = time_now;
        }
        if(target * output[2] < 1)
            output_m = output[2];
        else
            output_m = target * 1;
    }
}

void P_motor::direct_controll(double target_)
{
    motor = target_;
    output_reset();
}

void P_motor::free()
{
    motor.free();
    timer_reset();
    output_reset();
    stat = NONE;
}

void P_motor::stop()
{
    motor.stop();
    timer_reset();
    output_reset();
    stat = NONE;
}

void P_motor::timer_reset()
{
    timer.stop();
    timer.reset();
    time_old = 0;
    timer.start();
}

void P_motor::output_reset(double value)
{
    output[0] = value;
    output[1] = value;
    output[2] = value;
    output_m = value;
}