test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
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
diff -r 198030e84936 -r 36741e8ab197 HCSR04/hcsr04.cpp --- /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
diff -r 198030e84936 -r 36741e8ab197 HCSR04/hcsr04.h --- /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
diff -r 198030e84936 -r 36741e8ab197 Walk/OverCome/OverCome.cpp --- 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();
diff -r 198030e84936 -r 36741e8ab197 Walk/OverCome/OverCome.h --- 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
diff -r 198030e84936 -r 36741e8ab197 main.cpp --- 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
diff -r 198030e84936 -r 36741e8ab197 pinnames/pinnames.cpp --- 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,
diff -r 198030e84936 -r 36741e8ab197 servo_and_movefunc.cpp --- 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
diff -r 198030e84936 -r 36741e8ab197 servo_and_movefunc.h --- 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