File content as of revision 0:ae2c570afbe8:

#include "newMecanum.hpp"

void newMecanum::init(float length, float width, float _maxVal)
{
    float vectorAngle = atan2(length, width);

    // 理想的なメカナムホイールの配置（45deg）からのズレの度合い
    rotationCorrection = cos(vectorAngle - PI / 4); 
    
    maxSpeed = _maxSpeed;
}

void newMecanum::calculate(float vx, float vy, float angularVelocity)
{
    // 計算（ヤコビ行列の中身を展開した状態で記述）
    v[0] = vx + vy + rotationCorrection * angularVelocity;
    v[1] = vx - vy + rotationCorrection * angularVelocity;
    v[2] = -vx + vy + rotationCorrection * angularVelocity;
    v[3] = -vx - vy + rotationCorrection * angularVelocity;

    // ガード処理
    float maxVal = 0;
    for (int i = 0; i < 4; i++)
    {
        if (abs(v[i]) > maxVal)
        {
            maxVal = v[i];
        }
    }

    if (maxVal > maxSpeed) 
    {
        for (int i = 0; i < 4; i++)
        {
            v[i] = map(v[i], -maxVal, maxVal, -maxSpeed, maxSpeed);
        }
    }
}

float newMecanum::getWheelVelocity(short num)
{
    return v[num];
}