勇紀 有岡 / move4wheel

あ

move4wheel.cpp

Committer:
la00noix
Date:
2018-10-27
Revision:
0:ac911845484d
Child:
1:6a2b95e78d25

File content as of revision 0:ac911845484d:

#include <move4wheel.h>
#include <math.h>

namespace
{
const int kMotorNum = 4;
double motorOut[kMotorNum] = {};
}//namespace

//x,y,回転rの出力を4輪に変換。x,y,rの取り方は数学と同じ
void CalMotorOut(double x_out, double y_out, double r_out)
{
    const double Pi = 3.1415926535897;
    //motorのついている角度
    const double kMotorRad[kMotorNum] = {(3.0/4.0)*Pi, (5.0/4.0)*Pi, (7.0/4.0)*Pi, (1.0/4.0)*Pi};

    //motorの回転方向。機体が反時計回りに回る向きなら1,逆なら-1
    const int rotation[kMotorNum] = {1,          1,          1,          1};
    for (int i = 0; i < kMotorNum; i++) {
        motorOut[i] = cos(kMotorRad[i]) * x_out + sin(kMotorRad[i]) * y_out + rotation[i] * r_out;

        // → x軸, ↑ Y軸, 反時計回り を正とする


    }
}

double GetMotorOut(int motor_num)
{
    return motorOut[motor_num];
}