File content as of revision 1:d6e30aa7bc5b:

#include "mbed.h"
#include "speed_control.hpp"
#include "rotary_encoder_ab_phase.hpp"

speed_control::speed_control(TIM_TypeDef *timer_type, float _pulse_per_revol, float _cy_ms, bool rev)
{
    e = new rotary_encoder_ab_phase(timer_type, _pulse_per_revol);
    pulse_per_revol = _pulse_per_revol;
    cy_ms = _cy_ms;
    f1 = f2 = 0;
    target_value = 0.0f;
    
    _rev = rev;
    
    e->start();
    t.start();
}

void speed_control::set_speed(float _target_value)
{
    target_value = _target_value;
}

void speed_control::set_pid(float _kp, float _ki, float _kd)
{
    kp = _kp; ki = _ki; kd = _kd;
    f1 = kp + ki / 2.0f + kd;
    f2 = -1.0f * kp + ki / 2.0f - 2.0f * kd;
    
    e->reset();
    t.reset();
    time_counter = 0;
    pre_counts = 0;
}

bool speed_control::check_get_speed()
{
    if(t.read_ms() > time_counter)
    {
        time_counter += cy_ms;
        
        for(int i = 9; i > 0; i--)
        {
            e_value[i] = e_value[i-1];
        }
        int32_t ec = e->get_counts();
        e_value[0] = ec - pre_counts;
        if(ec > 20000 || ec < -20000)
        {
            e->reset();
            pre_counts = pre_counts - ec;
        }
        else
        {
            pre_counts = ec;
        }
        int32_t esum = 0;
        for(int i = 0; i < 10; i++)
        {
            esum += e_value[i];
        }
        sensor_value = esum / pulse_per_revol / (10.0f * cy_ms) * 1000;
        
        return true;
    }
    else
    {
        return false;
    }
}

char speed_control::get_duty()
{
    if(t.read() > 1800)
    {
        t.reset();
        time_counter = 0;
    }
    if(check_get_speed())
    {
        diff[0] = sensor_value - target_value;
        dm = f1 * diff[0] + j;
        m = m + dm;
        
        int m_int = (int)m * -1;
        if(m_int <= -126)   m_int = -126;
        if(m_int >=  126)   m_int =  126;
        duty = m_int;
        
        j = f2 * diff[0] + kd * diff[1];
        diff[1] = diff[0];
    }
    
    if(_rev)duty *= -1;
    
    return duty;
}

float speed_control::get_speed()
{
    return sensor_value;
}