File content as of revision 2:678b5128a4d6:

#include "mbed.h"
#include "T_motor.h"
#include "mecanum2017.h"

mecanum2017::mecanum2017(I2C& i2c_, char addr1, char addr2, char addr3, char addr4, int phase)
    : m1(i2c_, addr1), m2(i2c_, addr2), m3(i2c_, addr3), m4(i2c_, addr4)
{
    oldangle = 0.0;
    alpha = PI / 4 + phase * (PI / 2); //phaseは初期角度の指定 右に90°で-1, 左に90°で+1
}

/* うごけー */
void mecanum2017::move(int xdate, int ydate) //xdate, ydateは -64 ~ +64
{
    if(xdate == 0 && ydate == 0) free();
    double angle = atan2(double(ydate), double(xdate)); //入力の角度
    if(angle == oldangle); //前回と角度が同じだったらそのまま
    else {
        m1.control(float(sin(angle - alpha)));
        m2.control(float(sin(angle + alpha)));
        m3.control(float(sin(angle - alpha)));
        m4.control(float(sin(angle + alpha)));
    }
    oldangle = angle; // 今回の角度を記憶
}

/* まわれー */
void mecanum2017::rotation(int rxdate)
{
    m1.control(-rxdate * 0.015625); // 1 / 64 = 0.015625
    m2.control( rxdate * 0.015625);
    m3.control( rxdate * 0.015625);
    m4.control(-rxdate * 0.015625);
}

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

/*そのままー*/
void mecanum2017::free()
{
    m1.free();
    m2.free();
    m3.free();
    m4.free();
}