全向轮底盘

// 三轮全向轮逆运动学
void omnidirectional_ik(float vx, float vy, float omega, 
                        float& w1, float& w2, float& w3) {
    const float R = 0.05f;  // 轮半径
    const float L = 0.15f;  // 轮距中心距离
    
    // 三个轮子的方向角：90°, 210°, 330°
    w1 = (vy + L * omega) / R;
    w2 = (-0.866f * vx - 0.5f * vy + L * omega) / R;
    w3 = (0.866f * vx - 0.5f * vy + L * omega) / R;
}