Diff: move4wheel.cpp

Revision:

0:ac911845484d

Child:

1:6a2b95e78d25

--- /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