File content as of revision 0:7d6d030c6a1b:

#include "mbed.h"
#include "T_motor.h"
#include "mecanum2017_2.h"

double mecanum2017_2::get_angle(short xdata, short ydata)
{
    if(xdata || ydata)  return atan2((double)ydata, (double)xdata);
    else  return 0;
}

short mecanum2017_2::get_magnitude(short xdata, short ydata, short max)
{
    short magnitude = (short)(sqrt((double)xdata * xdata + ydata * ydata));
    if(magnitude > max)  return max;
    else  return magnitude;
}

short mecanum2017_2::map(short value, short fromLow, short fromHigh, short toLow, short toHigh)
{
    if(value < fromLow) return fromLow;
    else if(value > fromHigh) return fromHigh;
    return (value - fromLow) * (toHigh - toLow) / (fromHigh - fromLow) + toLow;
}

mecanum2017_2::mecanum2017_2(I2C* i2c_, float* para_, char addr[], float rate_, int phase)//paraはモーターの回転方向を±1で表すよ いつもは全部+1だよ
    : m1(i2c_, addr[0]), m2(i2c_, addr[1]), m3(i2c_, addr[2]), m4(i2c_, addr[3])
{
    para = para_;
    rate = rate_;
    alpha = PI / 4;
    beta = phase * (PI / 2); //phaseは初期角度の指定 左に90°で-1, 右に90°で+1
    for(short i = 0; i < 4; i++)
        para[i] *= rate;
} //paraはモーターの回転方向を±1で表すよ いつもは全部+1だよ

/* うごけー */
void mecanum2017_2::move(short xdata_L, short ydata_L, short xdata_R)
{
    xdata_L = map(xdata_L, -64, 64, -100, 100);
    ydata_L = map(ydata_L, -64, 64, -100, 100);
    xdata_R = map(xdata_R, -64, 64, -100, 100);
    float magnitude = get_magnitude(xdata_L, ydata_L, 100) / 100.0f;
    double angle = get_angle(xdata_L, ydata_L);
    float move_component[2] = {magnitude * (float)sin(angle + beta - alpha),
                               magnitude * (float)sin(angle + beta + alpha)
                              };
    float roll_component[2] = {-xdata_R * 0.9f / 100.0f, xdata_R * 0.9f / 100.0f};

    if(magnitude || xdata_R)  kasoku();
    else timer.reset();

    m1 = speed * para[0] * (move_component[0] + roll_component[0]);
    m2 = speed * para[1] * (move_component[1] + roll_component[1]);
    m3 = speed * para[2] * (move_component[0] + roll_component[1]);
    m4 = speed * para[3] * (move_component[1] + roll_component[0]);
}

void mecanum2017_2::anglemove(double angle)
{
    float move_component[2] = {(float)sin(angle + beta - alpha),
                               (float)sin(angle + beta + alpha)
                              };
    kasoku();
    m1 = speed * para[0] * move_component[0];
    m2 = speed * para[1] * move_component[1];
    m3 = speed * para[2] * move_component[0];
    m4 = speed * para[3] * move_component[1];
}

/* とまれー */
void mecanum2017_2::stop()
{
    m1.stop();
    m2.stop();
    m3.stop();
    m4.stop();
    timer.reset();
}

void mecanum2017_2::kasoku()
{
    int time = timer.read_ms();
    if(time < 1000)
    {
        if(time == 0)
        {
            timer.start();
            time = 10;
        }
        if(time >= 1000) timer.stop();
        speed = time / 1000.0f; //ms * 1000 / 10;
    }
}