4輪オムニベクトル計算

readme.md

omni4Vector

4輪オムニのベクトル変換をする関数

convertXY

コントローラーのアナログスティックの値から角度とパワーに変換する関数

main.c

// ----------
// created: FUKUMOTO Yuki
// date: 2021/12/09 ~ 2021/12/25
// ----------
#include <stdio.h>
#include <math.h>

typedef struct{
    double m1power;
    double m2power;
    double m3power;
    double m4power;
} mPw_t;
mPw_t omni4Vector(double angle, double power) {
    mPw_t mPw;
    mPw.m1power = cos((45+angle)*(M_PI/180)) * power;
    mPw.m2power = cos((135+angle)*(M_PI/180)) * power;
    mPw.m3power = -mPw.m1power;
    mPw.m4power = -mPw.m2power;
    return mPw;
}

void convertXY(double x, double y, double *angle, double *power) {
    *power = sqrt(x*x + y*y);
    if(*power > 64) {
    *power = 64;
    }
    *power = *power/64*100;
    if(x == 0 && y == 0) {
      *angle = 0;
      return;
    }
    *angle = atan2(y,x);
    *angle = -((*angle)*180/M_PI)+90;
}

int main(void) {
    mPw_t test;
    double x, y, power, angle;

    printf("X: ");
    scanf("%lf", &x);
    printf("Y: ");
    scanf("%lf", &y);

    convertXY(x, y, &angle, &power);
    test = omni4Vector(angle, power);
    printf("m1: %f\nm2: %f\nm3: %f\nm4: %f\n", test.m1power, test.m2power, test.m3power, test.m4power);
    return 0;
}

ひとまずこんなもんかな

一応メモ。
Xは左が+で右が-、Yは上が+で下が-とかいう状況。

更新。
・Xを右が+にしたこと
・範囲的な問題でバグってたのでatanをatan2に変更。