File content as of revision 4:d4c2e7addc77:

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

mecanum2017::mecanum2017(I2C& i2c_, short para[], char addr[], int phase) //paraはモーターの回転方向を±1で表すよ いつもは全部+1だよ
    : m1(i2c_, addr[0]), m2(i2c_, addr[1]), m3(i2c_, addr[2]), m4(i2c_, addr[3])
{
    para1 = para[0];
    para2 = para[1];
    para3 = para[2];
    para4 = para[3];
    oldangle = 0.0;
    alpha = PI / 4;
    beta = 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)) + beta; //入力の角度
    if(angle == oldangle); //前回と角度が同じだったらそのまま
    else
    {
        m1.control(float(para1 * sin(angle - alpha)));
        m2.control(float(para2 * sin(angle + alpha)));
        m3.control(float(para3 * sin(angle - alpha)));
        m4.control(float(para4 * sin(angle + alpha)));
    }
    oldangle = angle; // 今回の角度を記憶
}

/*かくどー*/
void mecanum2017::anglemove(double angle)
{
    angle += beta;
    if(angle == oldangle); //前回と角度が同じだったらそのまま
    else
    {
        m1.control(float(para1 * sin(angle - alpha)));
        m2.control(float(para2 * sin(angle + alpha)));
        m3.control(float(para3 * sin(angle - alpha)));
        m4.control(float(para4 * sin(angle + alpha)));
    }
    oldangle = angle; // 今回の角度を記憶
}

/* まわれー */
void mecanum2017::rotation(int rxdate)
{
    m1.control(para1 * -rxdate * 0.015625); // 1 / 64 = 0.015625
    m2.control(para2 *  rxdate * 0.015625);
    m3.control(para3 *  rxdate * 0.015625);
    m4.control(para4 * -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();
}