serbo4soku

Dependencies:   mbed nhk19mr2_can_info splitData SerialHalfDuplex_HM

Files at this revision

API Documentation at this revision

Comitter:
shimizuta
Date:
Mon Feb 11 13:32:46 2019 +0000
Parent:
11:e81425872740
Child:
13:e7ecdb20665a
Commit message:
printf debug finish

Changed in this revision

Walk/Walk.cpp Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/Walk/Walk.cpp	Mon Feb 11 12:56:00 2019 +0000
+++ b/Walk/Walk.cpp	Mon Feb 11 13:32:46 2019 +0000
@@ -20,7 +20,7 @@
 //空中の動き.楕円軌道
 OneLeg Orbit::RiseEllipse_(OneLeg leg, float phasetime_s)
 {
-    float rad = M_PI * phasetime_s / risetime_s_ + M_PI;
+    float rad = M_PI * (phasetime_s -stridetime_s_)/ risetime_s_ + M_PI;
     float x_m = stride_m_ * 0.5 * cos(rad);
     float y_m = height_m_ * sin(rad) + ground_m_;
     leg.SetXY_m(x_m, y_m);
--- a/main.cpp	Mon Feb 11 12:56:00 2019 +0000
+++ b/main.cpp	Mon Feb 11 13:32:46 2019 +0000
@@ -6,11 +6,11 @@
 #include "Walk.h"   //歩き方に関するファイル
 
 ////////////調整すべきパラメータ.全てここに集めた。
-const float kCycleTime_s = 0.01f;              //計算周期
+const float kCycleTime_s = 0.02f;              //計算周期
 const float kBetweenServoHalf_m = 0.06f * 0.5; //サーボ間の距離の半分
 const float kLegLength1 = 0.1f;
 const float kLegLength2 = 0.2f;
-const float kServoSpan_ms = 5; //サーボの送信間隔
+const float kServoSpan_ms = 0; //サーボの送信間隔
 ///////////////
 Timer timer;
 KondoServo servo[2] = {
@@ -29,6 +29,7 @@
 const float kRadToDegree = 180.0 / M_PI;
 void Move(Walk WalkWay, OneLeg (&leg)[4], float dist_m);
 void MoveServo(OneLeg leg, int legnum, int servo_id);
+
 int main()
 {
     printf("When you push any key, this robot starts.\r\n");
@@ -67,8 +68,7 @@
     {
         float time_s = timer.read();
         //注:未実装。到着したかの判定.LRFからのデータが必要?
-        // is_arrived = IsArrived();
-
+         //is_arrived = IsArrived();
         //4本の足それぞれの足先サーボ角度更新
         WalkWay.Cal4LegsPosi(leg);
         //注:未実装。slave_mbed分の足の目標位置を送信
@@ -79,7 +79,6 @@
         wait_ms(kServoSpan_ms);
         MoveServo(leg[0], 0, 1);
         MoveServo(leg[1], 1, 1);
-        printf("%f,%f\r\n", leg[0].GetRad(0), leg[1].GetRad(0));
         //計算周期がWalkWay.cycletime_s_になるようwait
         float rest_time_s = WalkWay.cycletime_s_ - (timer.read() - time_s);
         if (rest_time_s > 0)
@@ -98,4 +97,4 @@
     else
         degree = 270 - degree;
     servo[legnum].set_degree(servo_id, degree);
-}
\ No newline at end of file
+}