Diff: Walk/OverCome/OverCome.cpp

Revision:

35:b4e1b8f25cd7

Child:

43:2ed84f3558c1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Walk/OverCome/OverCome.cpp	Mon Mar 04 09:54:47 2019 +0000
@@ -0,0 +1,107 @@
+#include "OverCome.h"
+#include "change_walk.h"
+OverCome::OverCome(float start_x_m[4], float start_y_m[4],
+                   float d_x_m, float goal_y_m[4], float height_m[4], float gravity_dist[4],
+                   OneLeg legs[4]) :walk(legs)
+{
+    for (int i = 0; i < 4; i++)
+    {
+        start_x_m_[i] = start_x_m[i]; //足のスタートx
+        start_y_m_[i] = start_y_m[i]; //足のスタートy
+        goal_y_m_[i] = goal_y_m[i];   //目標地点までのy
+        gravity_dist_[i] = gravity_dist[i];
+        height_m_[i] = height_m[i];
+    }
+    d_x_m_ = d_x_m; //目標地点までのx
+
+    d_time_ = 0.5; //各動きの時間
+    d_time_slow_ = 0.5;
+    next_point_ = 0; //次のparamのindex
+    GetLine();
+    for (int i = 0; i < 4; i++)
+        SetOneLegFreeLinesParam(walk, i, legs_[i], next_point_);
+}
+
+void OverCome::Rise(int legnum)
+{
+    for (int i = 0; i < 4; i++)
+    {
+        legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
+        legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m;
+        legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
+    }
+    legs_[legnum][next_point_].y_m -= height_m_[legnum];
+    next_point_++;
+}
+void OverCome::Forward(int legnum)
+{
+    for (int i = 0; i < 4; i++)
+    {
+        legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
+        legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m;
+        legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
+    }
+    legs_[legnum][next_point_].x_m += d_x_m_;
+    next_point_++;
+}
+void OverCome::Land(int legnum)
+{
+    for (int i = 0; i < 4; i++)
+    {
+        legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_slow_;
+        legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m;
+        legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
+    }
+    legs_[legnum][next_point_].y_m = goal_y_m_[legnum]; //着地
+    next_point_++;
+}
+void OverCome::Step(int legnum)
+{
+    Rise(legnum);
+    Forward(legnum);
+    Land(legnum);
+}
+//重心移動、param direct:動く正負。+-1を入れる
+void OverCome::GravityMove(int legnum)
+{
+    for (int i = 0; i < 4; i++)
+    {
+        legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
+        legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m - gravity_dist_[legnum];
+        legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
+    }
+    ++next_point_;
+}
+void OverCome::StartPoint()
+{
+    for (int i = 0; i < 4; i++)
+    {
+        legs_[i][next_point_].time_s = 0;
+        legs_[i][next_point_].x_m = start_x_m_[i];
+        legs_[i][next_point_].y_m = start_y_m_[i];
+    }
+    ++next_point_;
+}
+void OverCome::GoalPoint()
+{
+    for (int i = 0; i < 4; i++)
+    {
+        legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
+        legs_[i][next_point_].x_m = start_x_m_[i];
+        legs_[i][next_point_].y_m = goal_y_m_[i];
+    }
+    ++next_point_;
+}
+void OverCome::GetLine()
+{
+    StartPoint(); //スタート時点
+    GravityMove(RIGHT_F);
+    Step(RIGHT_F);
+    GravityMove(LEFT_B);
+    Step(LEFT_B);
+    GravityMove(LEFT_F);
+    Step(LEFT_F);
+    GravityMove(RIGHT_B);
+    Step(RIGHT_B);
+    GoalPoint();
+}
\ No newline at end of file