File content as of revision 1:d6e30aa7bc5b:

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

Omuni::Omuni(I2C *i, TIM_TypeDef *timer_type0, TIM_TypeDef *timer_type1, TIM_TypeDef *timer_type2, size_t pulse_per_revol, float _cy_ms, const int a[3], float _R, float _d)
{
    i2c = i;
    o[0] = new speed_control(timer_type0, pulse_per_revol, _cy_ms, false);
    o[1] = new speed_control(timer_type1, pulse_per_revol, _cy_ms, false);
    o[2] = new speed_control(timer_type2, pulse_per_revol, _cy_ms, false);
    R = _R; d = _d;
    vx = 0.0f; vy = 0.0f; omega = 0.0f;
    for(int i = 0; i < 3; i++)
    {
        addr[i] = a[i];
        v[i] = 0.0f;
    }
    time.start(); t.start();
    f = false;
}
void Omuni::set_pid(int n, float _kp, float _ki, float _kd)
{
    if(n >= 0 && n <= 2)
    {
        o[n]->set_pid(_kp, _ki, _kd);
    }
}


void Omuni::set_speed(float _vx, float _vy)
{
    tvx = _vx; tvy = _vy;
}

void Omuni::set_speed(float _vx, float _vy, float _omega)
{
    set_speed(_vx, _vy);
    omega = _omega;
}

void Omuni::set_speed(float _vx, float _vy, float _omega, bool _f)
{
    set_speed(_vx, _vy, _omega);
    f = _f;
}

void Omuni::renew_theta()
{
    static float now_time, pre_time;
    now_time = time.read();
    
    if(now_time - pre_time < 0.1f)
    {
        if(f == true)
        {
            theta += (omega * (now_time - pre_time)) * THETA_ADDJUST;
        }
        else
        {
            theta = 0.0f;
        }
    }
    if(now_time > 1800)
    {
        time.reset();
        pre_time = 0.0f;
    }
    pre_time = now_time;
}

bool Omuni::renew_speed()
{
    const double a = 10;
    static float now_time, pre_time;
    now_time = time.read();
    if(now_time - pre_time > 0.01f)
    {
        pre_time = now_time;
        
        if(tvx - vx <= a * 0.01 && tvx - vx >= -1 * a * 0.01 && tvy - vy <= a * 0.01 && tvy - vy >= -1 * a * 0.01)
        {
            vx = tvx; vy = tvy;
        }
        else
        {
            float x, y;
            x = tvx - vx;
            y = tvy - vy;
            vx += a * 0.01 * x / sqrt(x * x + y * y);
            vy += a * 0.01 * y / sqrt(x * x + y * y);
        }
        return true;
    }
    return false;
}

void Omuni::_set_speed()
{
    //if(renew_speed())
    {
        float _vx, _vy;
        _vx =      tvx * cos(theta) + tvy * sin(theta);
        _vy = -1 * tvx * sin(theta) + tvy * cos(theta);
        v[0] = ( 1.0f * _vx              ) / (3.1416f * d);
        v[1] = (-0.5f * _vx + 0.866f * _vy) / (3.1416f * d);
        v[2] = (-0.5f * _vx - 0.866f * _vy) / (3.1416f * d);
        v[0] += (R * omega) / (3.1416f * d);
        v[1] += (R * omega) / (3.1416f * d);
        v[2] += (R * omega) / (3.1416f * d);
        
        
        for(int i = 0; i < 3; i++)
        {
            o[i]->set_speed(v[i]);
        }
    }
}

void Omuni::drive()
{
    renew_theta();
    
    _set_speed();
    
    for(int i = 0; i < 3; i++)
    {
        char duty = o[i]->get_duty();
        i2c->write(addr[i], &duty, 1);
    }
}

void Omuni::reset_theta()
{
    theta = 0.0f;
}

float Omuni::get_theta()
{
    return theta;
}

float Omuni::get_vx()
{
    return vx;
}

float Omuni::get_vy()
{
    return vy;
}