あ
Diff: move4wheel.cpp
- Revision:
- 0:ac911845484d
- Child:
- 1:6a2b95e78d25
diff -r 000000000000 -r ac911845484d move4wheel.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/move4wheel.cpp Sat Oct 27 11:24:45 2018 +0000 @@ -0,0 +1,31 @@ +#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]; +} \ No newline at end of file