move4wheel.cpp

00001 #include <move4wheel.h>
00002 #include <math.h>
00003 
00004 namespace
00005 {
00006 const int kMotorNum = 4;
00007 double motorOut[kMotorNum] = {};
00008 }//namespace
00009 
00010 //x,y,回転rの出力を4輪に変換。x,y,rの取り方は数学と同じ
00011 void CalMotorOut(double x_out, double y_out, double r_out)
00012 {
00013     const double Pi = 3.1415926535897;
00014     //motorのついている角度
00015     const double kMotorRad[kMotorNum] = {(5.0/4.0)*Pi, (7.0/4.0)*Pi, (1.0/4.0)*Pi, (3.0/4.0)*Pi};
00016 
00017     //motorの回転方向。機体が反時計回りに回る向きなら1,逆なら-1
00018     const int rotation[kMotorNum] = {1,          1,          1,          1};
00019     for (int i = 0; i < kMotorNum; i++) {
00020         motorOut[i] = cos(kMotorRad[i]) * x_out + sin(kMotorRad[i]) * y_out + rotation[i] * r_out;
00021 
00022         // → x軸, ↑ Y軸, 反時計回り を正とする
00023 
00024 
00025     }
00026 }
00027 
00028 double GetMotorOut(int motor_num)
00029 {
00030     return motorOut[motor_num];
00031 }