Revision 50:36741e8ab197, committed 2019-03-11

Comitter:

shimizuta

Date:

Mon Mar 11 10:38:07 2019 +0000

Parent:

49:198030e84936

Commit message:

a

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HCSR04/hcsr04.cpp	Mon Mar 11 10:38:07 2019 +0000
@@ -0,0 +1,98 @@
+/* Copyright (c) 2013 Prabhu Desai
+ * pdtechworld@gmail.com
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+ * and associated documentation files (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge, publish, distribute,
+ * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all copies or
+ * substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+ * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+
+#include "hcsr04.h"
+
+
+HCSR04::HCSR04(PinName TrigPin,PinName EchoPin):
+    trigger(TrigPin), echo(EchoPin)
+{
+    pulsetime.stop();
+    pulsetime.reset();
+    echo.rise(this,&HCSR04::isr_rise);
+    echo.fall(this,&HCSR04::isr_fall);
+    trigger=0;
+}
+
+HCSR04::~HCSR04()
+{
+}
+
+void HCSR04::isr_rise(void)
+{
+    pulsetime.start();
+}
+void HCSR04::start(void)
+{
+    trigger=1;
+    wait_us(10);
+    trigger=0;
+}
+
+void HCSR04::isr_fall(void)
+{
+    pulsetime.stop();
+    pulsedur = pulsetime.read_us();
+    distance= (pulsedur*343)/20000;
+    pulsetime.reset();
+}
+
+void HCSR04::rise (void (*fptr)(void))
+{
+    echo.rise(fptr);
+}
+void HCSR04::fall (void (*fptr)(void))
+{
+    echo.fall(fptr);
+}
+
+float HCSR04::get_dist_cm()
+{
+    return distance;
+}
+float HCSR04::get_pulse_us()
+{
+    return pulsedur;
+}
+
+
+
+/*******************************************************
+   Here is a sample code usage
+********************************************************* 
+#include "hcsr04.h"
+HCSR04  usensor(p25,p6);
+int main()
+{
+    unsigned char count=0;
+    while(1) {
+        usensor.start();
+        wait_ms(500); 
+        dist=usensor.get_dist_cm();
+        lcd.cls();
+        lcd.locate(0,0);
+        lcd.printf("cm:%ld",dist );
+ 
+        count++;
+        lcd.locate(0,1);
+        lcd.printf("Distance =%d",count);
+        
+    }
+*/
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HCSR04/hcsr04.h	Mon Mar 11 10:38:07 2019 +0000
@@ -0,0 +1,68 @@
+/* Copyright (c) 2013 Prabhu Desai
+ * pdtechworld@gmail.com
+ *
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+ * and associated documentation files (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge, publish, distribute,
+ * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all copies or
+ * substantial portions of the Software.
+ *
+ * For more details on the sensor :
+ * http://www.elecfreaks.com/store/hcsr04-ultrasonic-sensor-distance-measuring-module-p-91.html?zenid=pgm8pgnvaodbe36dibq5s1soi3
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+ * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef MBED_HCSR04_H
+#define MBED_HCSR04_H
+
+#include "mbed.h"
+
+/** HCSR04 Class(es)
+ */
+
+class HCSR04
+{
+public:
+    /** Create a HCSR04 object connected to the specified pin
+    * @param pin i/o pin to connect to
+    */
+    HCSR04(PinName TrigPin,PinName EchoPin);
+    ~HCSR04();
+
+    /** Return the distance from obstacle in cm
+    * @param distance in cms and returns -1, in case of failure
+    */
+    float get_dist_cm(void);
+    /** Return the pulse duration equal to sonic waves travelling to obstacle and back to receiver.
+    * @param pulse duration in microseconds.
+    */
+    float get_pulse_us(void);
+    /** Generates the trigger pulse of 10us on the trigger PIN.
+    */
+    void start(void );
+    void isr_rise(void);
+    void isr_fall(void);
+    void fall (void (*fptr)(void));
+    void rise (void (*fptr)(void));
+
+
+
+private:
+
+    Timer pulsetime;
+    DigitalOut  trigger;
+    InterruptIn echo;
+    float pulsedur;
+    float distance;
+};
+
+#endif
\ No newline at end of file

--- a/Walk/OverCome/OverCome.cpp	Mon Mar 11 06:36:56 2019 +0000
+++ b/Walk/OverCome/OverCome.cpp	Mon Mar 11 10:38:07 2019 +0000
@@ -3,7 +3,7 @@
 #include "math.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], float raise_offset_x_m[4]) : walk(legs)
+                   OneLeg legs[4], float raise_offset_x_m[4], float d_time, float d_time_slow) : walk(legs)
 {
     for (int i = 0; i < 4; i++)
     {
@@ -16,8 +16,8 @@
     }
     d_x_m_ = d_x_m; //目標地点までのx
 
-    d_time_ = 0.2; //各動きの時間
-    d_time_slow_ = 0.3;
+    d_time_ = d_time;//0 .2; //各動きの時間
+    d_time_slow_ = d_time_slow;// 0.3;
 
     next_point_ = 0; //次のparamのindex
     GetLine();

--- a/Walk/OverCome/OverCome.h	Mon Mar 11 06:36:56 2019 +0000
+++ b/Walk/OverCome/OverCome.h	Mon Mar 11 10:38:07 2019 +0000
@@ -18,9 +18,9 @@
   float gravity_dist_[4]; //重心ずらし距離
   float d_time_;          //各動きの時間
   float d_time_slow_;
-  float raise_offset_x_m_[4];//raiseするときにずらすx_m
-  int next_point_;        //次のparamのindex
-  LineParam legs_[4][30]; //多めに
+  float raise_offset_x_m_[4]; //raiseするときにずらすx_m
+  int next_point_;            //次のparamのindex
+  LineParam legs_[4][30];     //多めに
   void Rise(int legnum);
   void Land(int legnum);
   void Forward(int legnum);
@@ -33,7 +33,7 @@
 public:
   Walk walk;
   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], float raise_offset_x_m[4]);
+           float d_x_m, float goal_y_m[4], float height_m[4], float gravity_dist[4],
+           OneLeg legs[4], float raise_offset_x_m[4], float d_time, float d_time_slow);
 };
 #endif
\ No newline at end of file

--- a/main.cpp	Mon Mar 11 06:36:56 2019 +0000
+++ b/main.cpp	Mon Mar 11 10:38:07 2019 +0000
@@ -31,11 +31,8 @@
     STOP,
     AREA1_LRFWALK_STATE,
     SANDDUNE,
-    AREA2_LRFWALK_STATE,
     ROPE_STATE,
     SLOPE_STATE,
-    TURNRIGHT_STATE,
-    TRUNLEFT_STATE,
 };
 ROS_STATE state_from_ros = STOP; //ここを変えると動く方法が変わる
 #ifdef USE_ROS
@@ -65,9 +62,9 @@
     BACKLEG_ON_SANDDUNE,  //後ろ脚を段差に載せる
     OVERCOME2,            //後ろ脚が乗った状態で進む
     AFTER_OVERCOME,       //段差を終了したら座って指示まち
+    WALK_BEFORE_ROPE,     //紐前で歩く
     ROPE,                 //rope前足超える
     ROPE_BACK,            //rope後ろ足超える
-    ROPE_DOWN,            //rope機体高さをLRF用に落とす
     SLOPE,
     TRUNRIGHT,
     TRUNLEFT,
@@ -91,10 +88,12 @@
     float overcome_start_y_m[] = {0.38, 0.38, 0.38, 0.38};
     float flontleg_sand_goal_y_m[4] = {0.38, 0.27, 0.38, 0.27};
     float backleg_sand_goal_y_m[4] = {0.27, 0.38, 0.27, 0.38};
+    float d_time = 0.2, d_time_slow = 0.3;
     //旋回
     float turn_start_x_m = 0, turn_start_y_m = 0.275,
-          turn_stride_m = 0.05, turn_height_m = 0.05,
-          turn_stridetime_s = 0.6, turn_risetime_s = 0.3;
+          turn_stride_m = 0.075, turn_height_m = 0.05,
+          turn_stridetime_s = 0.5, turn_risetime_s = 0.2;
+    float d_time_rope = 0.6, d_time_slow_rope = 0.3;
     switch (way)
     {
     case STANDUP:
@@ -141,7 +140,8 @@
         float overcome_height_m[] = {0.05, 0.15, 0.05, 0.15};
         float gravity_dist[] = {0.05, 0, 0.05, -0.05};
         OverCome overcome(offset_x_m, overcome_start_y_m, d_x_m, flontleg_sand_goal_y_m,
-                          overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m);
+                          overcome_height_m, gravity_dist, walk.leg,
+                          raise_offset_x_m, d_time, d_time_slow);
         walk.Copy(overcome.walk);
         break;
     }
@@ -163,7 +163,8 @@
         float overcome_height_m[] = {0.05, 0.05, 0.05, 0.05};
         float gravity_dist[] = {0.1, 0, 0.1, 0};
         OverCome overcome(start_x_m, flontleg_sand_goal_y_m, d_x_m, overcome_start_y_m,
-                          overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m);
+                          overcome_height_m, gravity_dist, walk.leg,
+                          raise_offset_x_m, d_time, d_time_slow);
         walk.Copy(overcome.walk);
         break;
     }
@@ -177,13 +178,14 @@
         float overcome_height_m[] = {0.2, 0.1, 0.2, 0.1};
         float gravity_dist[] = {0.1, 0, 0.1, 0};
         OverCome overcome(start_x_m, start_y_m, d_x_m, backleg_sand_goal_y_m,
-                          overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m);
+                          overcome_height_m, gravity_dist, walk.leg,
+                          raise_offset_x_m, d_time, d_time_slow);
         walk.Copy(overcome.walk);
         break;
     }
     case OVERCOME2:
     { //後ろ脚が乗った状態で進む
-        float offset_x_m[4] = {0.1,0,0.1,0};
+        float offset_x_m[4] = {0.1, 0, 0.1, 0};
         float offset_y_m[4] = {0.27, 0.33, 0.27, 0.33};
         float stride_m[4] = {0.2, 0.2, 0.2, 0.2},
               height_m[4] = {0.1, 0.1, 0.1, 0.1}, buffer_height_m = 0.05,
@@ -202,47 +204,47 @@
         walk.SetOffsetTime(0, 0, 0, 0);
         break;
     }
+    case WALK_BEFORE_ROPE:
+    {
+        // float stride_m[] = {0.2, 0.2, 0.125, 0.125};
+        float stride_m[] = {0.075, 0.075, 0.2, 0.2};
+        for (int i = 0; i < 4; i++)
+            SetOneLegFourPointParam(walk, i, offset_x_m[i], area1_offset_y_m[i],
+                                    stride_m[i], height_m[i], buffer_height_m,
+                                    stridetime_s, toptime_s, buffer_time_s);
+        walk.SetOffsetTime(0, 0.5, 0.5, 0);
+        break;
+    }
     case ROPE:
     {
         float offset_x_m[4] = {-0.05, 0, -0.05, 0};
         float d_x_m = 0.2;
         float start_y_m[4] = {0.41, 0.41, 0.41, 0.41};
         float goal_y_m[4] = {0.41, 0.41, 0.41, 0.41};
-        float raise_offset_x_m[] = {0, -0.08, 0, -0.08};
+        float raise_offset_x_m[] = {0, -0.15, 0, -0.15};
         float overcome_height_m[] = {0.05, 0.2, 0.05, 0.2};
         float gravity_dist[] = {0.05, -0.05, 0.05, -0.05};
         OverCome overcome(offset_x_m, start_y_m, d_x_m, goal_y_m,
-                          overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m);
+                          overcome_height_m, gravity_dist, walk.leg,
+                          raise_offset_x_m, d_time_rope, d_time_slow_rope);
         walk.Copy(overcome.walk);
         break;
     }
     case ROPE_BACK:
     {
         float offset_x_m[4] = {};
-        float d_x_m = 0.15;
-        float start_y_m[4] = {0.41, 0.41, 0.41, 0.41};
-        float goal_y_m[4] = {0.41, 0.41, 0.41, 0.41};
-        float raise_offset_x_m[] = {-0.03, 0, -0.03, 0};
+        float d_x_m = 0.2;
+        float start_y_m[4] = {0.41, 0.38, 0.41, 0.38};
+        float goal_y_m[4] = {0.41, 0.38, 0.41, 0.38};
+        float raise_offset_x_m[] = {-0.1, 0, -0.1, 0};
         float overcome_height_m[] = {0.2, 0.05, 0.2, 0.05};
-        float gravity_dist[] = {0.08, 0, 0.08, 0};
+        float gravity_dist[] = {0.25, -0.05, 0.05, -0.05};
         OverCome overcome(offset_x_m, start_y_m, d_x_m, goal_y_m,
-                          overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m);
+                          overcome_height_m, gravity_dist, walk.leg,
+                          raise_offset_x_m, d_time_rope, d_time_slow_rope);
         walk.Copy(overcome.walk);
         break;
     }
-    case ROPE_DOWN:
-    {
-        for (int i = 0; i < 4; i++)
-        {
-            LineParam lines[] = {
-                {.time_s = 0, .x_m = walk.leg[i].GetX_m(), .y_m = walk.leg[i].GetY_m(), .is_point_to_point = 0},
-                {.time_s = 0.5, .x_m = offset_x_m[i], .y_m = area1_offset_y_m[i], .is_point_to_point = 0},
-            };
-            SetOneLegFreeLinesParam(walk, i, lines, sizeof(lines) / sizeof(lines[0]));
-        }
-        walk.SetOffsetTime(0, 0, 0, 0);
-        break;
-    }
     case SLOPE:
     {
         float offset_x_m[4] = {-0.15, 0.15, -0.15, 0.15},
@@ -306,63 +308,51 @@
     nh_mbed.advertise(pub_vel);
     nh_mbed.spinOnce(); //一度受信
 #endif
-    ROS_STATE old_state = STOP;
+
     /////立つ
-#ifndef USE_ROS
-    state_from_ros = SANDDUNE; //遠してやらないならやりたいSTATEに変更
-#endif
+    state_from_ros = ROPE_STATE;
+    switch (state_from_ros)
+    {
+    case SANDDUNE:
+        SetWalk(walk, STANDUP_SANDDUNE);
+        break;
+    default:
+        SetWalk(walk, STANDUP);
+        break;
+    }
+    printf("Stand up?\r\n");
+    WaitStdin('y'); // キーボード入力されるまでまで待つ
+    MoveOneCycle(walk, leg);
+    printf("Move?\r\n");
+    WaitStdin('y');
     //プログラムの最初のほうにあるstate_from_rosで動かすものを切り替える.caseはenum ROS_STATEで分ける
-    // while (1)
-    // {
     switch (state_from_ros)
     {
     case STOP:
         break;
-    case AREA1_LRFWALK_STATE:
-        printf("Stand up?\r\n");
-        WaitStdin('y'); // キーボード入力されるまでまで待つ
-        SetWalk(walk, STANDUP);
-        MoveOneCycle(walk, leg);
-        printf("Move?\r\n");
-        WaitStdin('y');                              //キーボード入力されるまでまで待つ
+    case AREA1_LRFWALK_STATE:                        //キーボード入力されるまでまで待つ
         SmoothChange(walk, leg, AREA1_LRFWALK, 0.5); //切り替え
         for (int i = 0; i < 20; i++)
             MoveOneCycle(walk, leg);
-        state_from_ros = STOP;
-        break;
-    case AREA2_LRFWALK_STATE:
-        if (old_state != state_from_ros)
-            SmoothChange(walk, leg, AREA2_LRFWALK, 0.5); //切り替え
-        else
-            SetWalk(walk, AREA2_LRFWALK);
-        MoveOneCycle(walk, leg);
         break;
     case SANDDUNE:
-        //前足を段差にかける
-        printf("Stand up?\r\n");
-        WaitStdin('y'); // キーボード入力されるまでまで待つ
-        SetWalk(walk, STANDUP_SANDDUNE);
-        MoveOneCycle(walk, leg);
-        printf("Move?\r\n");
-        WaitStdin('y');                                     //キーボード入力されるまでまで待つ
+        //前足を段差にかける                               //キーボード入力されるまでまで待つ
         SmoothChange(walk, leg, FRONTLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ
         MoveOneCycle(walk, leg);
         //前足が段差に乗った状態で進む
         SmoothChange(walk, leg, OVERCOME, 0.5); //切り替えスムーズ
         for (int i = 0; i < 2; i++)
             MoveOneCycle(walk, leg);
-            //前足を降ろす
+        //前足を降ろす
         SmoothChange(walk, leg, FALL_FRONTLEG, 0.5);
         MoveOneCycle(walk, leg);
         //またいだ状態で歩く
         SmoothChange(walk, leg, AREA1_LRFWALK, 0.5);
         MoveOneCycle(walk, leg);
-
         //後ろ脚載せる
         SmoothChange(walk, leg, BACKLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ
         MoveOneCycle(walk, leg);
         //後ろ脚乗った状態で進む
-        // SmoothChange(walk, leg, OVERCOME2, 0.5); //切り替えスムーズ
         SetWalk(walk, OVERCOME2);
         for (int i = 0; i < 3; i++)
             MoveOneCycle(walk, leg);
@@ -370,46 +360,72 @@
         SetWalk(walk, AREA1_LRFWALK);
         for (int i = 0; i < 4; i++)
             MoveOneCycle(walk, leg);
-        /* //数歩左に曲がる
-        SmoothChange(walk, leg, TRUNLEFT, 0.5); //切り替えスムーズ
+        break;
+
+    case ROPE_STATE:
+        //紐前歩く
+        SetWalk(walk, WALK_BEFORE_ROPE);
+        for (int i = 0; i < 5; i++)
+            MoveOneCycle(walk, leg);
+        int is_arrived = 0;
+        while (is_arrived == 0)
+        {
+            MoveOneCycle(walk, leg);
+            unsigned int dist_cm = GetDist_cm();
+            if (dist_cm > 90 && dist_cm < 150)
+                ++is_arrived;
+            printf("%d\r\n", dist_cm);
+        }
+        /*
+        SetWalk(walk, AREA1_LRFWALK);
+        for (int i = 0; i < 5; i++)
+            MoveOneCycle(walk, leg);
+        SmoothChange(walk, leg, TRUNRIGHT, 0.5);
         for (int i = 0; i < 2; i++)
+            MoveOneCycle(walk, leg); 
+        */
+        /*曲がりながら歩く
+        SmoothChange(walk, leg, WALK_BEFORE_ROPE, 0.5);
+        for (int i = 0; i < 7; i++)
             MoveOneCycle(walk, leg);
         */
-        state_from_ros = AREA2_LRFWALK_STATE;
-        break;
-    case ROPE_STATE:
         //前足だけ紐越え
         SmoothChange(walk, leg, ROPE, 0.5);
         MoveOneCycle(walk, leg);
         //LRFが取れる高さで歩行
-        SmoothChange(walk, leg, AREA1_LRFWALK, 0.5);
+        SmoothChange(walk, leg, AREA1_LRFWALK, 1.5);
         for (int i = 0; i < 3; i++)
             MoveOneCycle(walk, leg);
         //後ろ足越え
         SmoothChange(walk, leg, ROPE_BACK, 0.5);
         MoveOneCycle(walk, leg);
-        state_from_ros = AREA2_LRFWALK_STATE;
+        //紐間歩く
+        SmoothChange(walk, leg, AREA1_LRFWALK, 0.5);
+        for (int i = 0; i < 1; i++)
+            MoveOneCycle(walk, leg);
+        //2本目
+        //前足だけ紐越え
+        SmoothChange(walk, leg, ROPE, 0.5);
+        MoveOneCycle(walk, leg);
+        //LRFが取れる高さで歩行
+        SmoothChange(walk, leg, AREA1_LRFWALK, 1.5);
+        for (int i = 0; i < 3; i++)
+            MoveOneCycle(walk, leg);
+        //後ろ足越え
+        SmoothChange(walk, leg, ROPE_BACK, 0.5);
+        MoveOneCycle(walk, leg);
+        //紐後歩く
+        SmoothChange(walk, leg, AREA1_LRFWALK, 1.5);
+        for (int i = 0; i < 10; i++)
+            MoveOneCycle(walk, leg);
         break;
+
     case SLOPE_STATE:
         SmoothChange(walk, leg, SLOPE, 0.5);
         for (int i = 0; i < 30; i++)
             MoveOneCycle(walk, leg);
-        state_from_ros = STOP;
-        break;
-    case TURNRIGHT_STATE:
-        SmoothChange(walk, leg, TRUNRIGHT, 0.5);
-        for (int i = 0; i < 5; i++)
-            MoveOneCycle(walk, leg);
-        state_from_ros = AREA2_LRFWALK_STATE;
-        break;
-    case TRUNLEFT_STATE:
-        SmoothChange(walk, leg, TRUNLEFT, 0.5);
-        for (int i = 0; i < 5; i++)
-            MoveOneCycle(walk, leg);
-        state_from_ros = AREA2_LRFWALK_STATE;
         break;
     }
-    // old_state = state_from_ros;
 #ifdef USE_ROS
     nh_mbed.spinOnce();
 #endif

--- a/pinnames/pinnames.cpp	Mon Mar 11 06:36:56 2019 +0000
+++ b/pinnames/pinnames.cpp	Mon Mar 11 10:38:07 2019 +0000
@@ -6,8 +6,8 @@
 };
 PinName pin_can_rd = p30,
         pin_can_td = p29;
-PinName pin_serial_servo_tx[] = {p9, p13, p28};
-PinName pin_serial_servo_rx[] = {p10, p14, p27};
+PinName pin_serial_servo_tx[] = {p28, p13, p9};
+PinName pin_serial_servo_rx[] = {p27, p14, p10};
 PinName pin_touch_sensor[] = {p8};
 PinName pin_button[] = {p18, p17, p16, p15,p19};
 PinName pin_spi_mosi = p5,

--- a/servo_and_movefunc.cpp	Mon Mar 11 06:36:56 2019 +0000
+++ b/servo_and_movefunc.cpp	Mon Mar 11 10:38:07 2019 +0000
@@ -6,6 +6,7 @@
 #include "KondoServo.h"
 #include "pinnames.h"
 #include "can.h"
+#include "hcsr04.h"
 #define USE_CAN //can通信するならdefine.しないなら切らないとエラー出る
 #endif
 FILE *fp;
@@ -33,6 +34,7 @@
     KondoServo(pin_serial_servo_tx[1], pin_serial_servo_rx[1]),
 };
 DigitalOut led[4] = {DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4)};
+HCSR04 usensor(p25, p24); // Trigger(DO), Echo(PWMIN)
 #endif
 //一サイクル分進む.return 1:異常終了
 int MoveOneCycle(Walk walkway, OneLeg leg[4])
@@ -114,3 +116,8 @@
     fprintf(fp, "time[s],x[0],y[0],x[1],y[1],x[2],y[2],x[3],y[3]\r\n");
     return 0;
 }
+unsigned int GetDist_cm()
+{
+    usensor.start();
+    return usensor.get_dist_cm();
+}
\ No newline at end of file

--- a/servo_and_movefunc.h	Mon Mar 11 06:36:56 2019 +0000
+++ b/servo_and_movefunc.h	Mon Mar 11 10:38:07 2019 +0000
@@ -10,4 +10,5 @@
 void MoveServo(OneLeg leg, int legnum, int servo_id);
 void WaitStdin(char startchar);
 int FileOpen();
+unsigned int GetDist_cm();
 #endif
\ No newline at end of file