File content as of revision 0:3bd8aecadafc:

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

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

void Omuni::change_motor_direction(int n, bool _rev)
{
    if(n >= 0 && n <= 2)
    {
        o[n]->change_direction(_rev);
    }
}

void Omuni::change_coordinates_direction(bool _crevx, bool _crevy)
{
    crevx = _crevx;
    crevy = _crevy;
}

void Omuni::set_speed(double _vx, double _vy)
{
    tvx = _vx; tvy = _vy;
    if(crevx)       tvx *= -1;
    if(crevy)       tvy *= -1;
}

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

void Omuni::set_speed(double _vx, double _vy, double _omega, bool _f)
{
    set_speed(_vx, _vy, _omega);
    f = _f;
    if(!f)  gyro->reset();
}

void Omuni::renew_theta()
{
    double theta_temp[3];
    gyro->renew_angle();
    gyro->get_angle_rad(theta_temp);
    theta = theta_temp[2] * -1;
    /*
    static double 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 double now_time, pre_time;
    now_time = time.read();
    if(now_time - pre_time > 0.01)
    {
        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
        {
            double 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())
    {
        double _vx, _vy;
        _vx =      tvx * cos(theta) + tvy * sin(theta);
        _vy = -1 * tvx * sin(theta) + tvy * cos(theta);
        v[0] = (-1.0 * _vx              ) / (3.1416 * d);
        v[1] = ( 0.5 * _vx - 0.866 * _vy) / (3.1416 * d);
        v[2] = ( 0.5 * _vx + 0.866 * _vy) / (3.1416 * d);
        v[0] += (R * omega) / (3.1416 * d);
        v[1] += (R * omega) / (3.1416 * d);
        v[2] += (R * omega) / (3.1416 * 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.0;
}

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

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

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