serbo4soku

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

Revision:
9:905f93247688
Child:
10:7a340c52e270
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ToePosi/ToePosi.cpp	Mon Feb 11 01:56:49 2019 +0000
@@ -0,0 +1,43 @@
+#include "ToePosi.h"
+#include "math.h"
+const float M_PI = 3.141592;
+//足先の軌道を決める関数。要調整。取り敢えず楕円軌道で実装
+float ToePosi::GetX_m()
+{
+    float phase = GetPhase();
+    float x_m = cos(phase) * stride_m_;
+    return x_m;
+}
+float ToePosi::GetY_m()
+{
+    float phase = GetPhase();
+    float y_m;
+    if (phase < M_PI) //最初の半周期は空中
+        y_m = sin(phase) * height_m_;
+    else //後の半周期は着地。直線移動
+        y_m = 0;
+    return y_m;
+}
+
+ToePosi::ToePosi(float stride_m, float height_m)
+{
+    phase_ = 0;
+    stride_m_ = stride_m;
+    height_m_ = height_m;
+}
+float ToePosi::GetPhase()
+{
+    return phase_;
+}
+void ToePosi::SetPhase(float phase)
+{
+    //値域を[0,2pi]にする
+    while (phase > 2 * M_PI)
+        phase -= 2 * M_PI;
+    while (phase < 0)
+        phase += 2 * M_PI;
+
+    phase_ = phase;
+}
+
+