ROBOSTEP_5期 / Mbed 2 deprecated George_Master_Param

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed robot

main.cpp@55:1b1e5839dde1, 2019-05-20 (annotated)

Committer:
yuto17320508
Date:
Mon May 20 11:51:19 2019 +0000
Revision:
55:1b1e5839dde1
Parent:
53:4673b85e1d2a
Child:
58:dc0faefeba39
printf life

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eri 0:c1476d342c13 1 #include "mbed.h"
shimizuta 17:2b3fa9b1a05b 2 #include "debug.h"
eri 0:c1476d342c13 3 #include "microinfinity.h"
shimizuta 21:14133581387b 4 #include "sensors.h"
shimizuta 22:0ed9de464f40 5 #include "moves.h"
yuto17320508 48:d9f3ce701aca 6 #include <stdlib.h>
yuto17320508 48:d9f3ce701aca 7
yuto17320508 48:d9f3ce701aca 8
yuto17320508 53:4673b85e1d2a 9 #define BADRULEMODE
shimizuta 21:14133581387b 10
yuto17320508 25:c740e6fd5dab 11 void StateFlow(int i);
yuto17320508 32:aee87dcaf7ca 12 void SetMode();
yuto17320508 32:aee87dcaf7ca 13 void Start();
yuto17320508 32:aee87dcaf7ca 14 void NoHandSignal();
shimizuta 50:3e73f68c3b37 15
yuto17320508 33:2dbbe198adaf 16
yuto17320508 32:aee87dcaf7ca 17 int start_state=0;
yuto17320508 48:d9f3ce701aca 18
shimizuta 50:3e73f68c3b37 19
shimizuta 26:5fb1aa9cb7f0 20
kageyuta 1:86c4c38abe40 21 int main()
kageyuta 1:86c4c38abe40 22 {
yuto17320508 48:d9f3ce701aca 23 LoadParameter();
shimizuta 26:5fb1aa9cb7f0 24 //setup関連
shimizuta 22:0ed9de464f40 25 can1.frequency(1000000);
shimizuta 22:0ed9de464f40 26 SetupMoves();
yuto17320508 25:c740e6fd5dab 27 set_gyro();
yuto17320508 32:aee87dcaf7ca 28 printf("hand read:%d\r\n",hand.read());
yuto17320508 53:4673b85e1d2a 29 /*while(1)
yuto17320508 53:4673b85e1d2a 30 {
yuto17320508 53:4673b85e1d2a 31 printf("forward:%.2f back:%.2f\r\n",get_dist_forward(),get_dist_back());
yuto17320508 53:4673b85e1d2a 32 wait(0.01);
yuto17320508 53:4673b85e1d2a 33 }*/
shimizuta 50:3e73f68c3b37 34
shimizuta 22:0ed9de464f40 35 printf("reset standby\r\n");
shimizuta 26:5fb1aa9cb7f0 36 reset();//足をリセット位置に移動
shimizuta 22:0ed9de464f40 37 printf("bus standby\r\n");
yuto17320508 5:63462d696256 38 while(1) {
yuto17320508 5:63462d696256 39 if(bus_in.read() == 1) break;
yuto17320508 3:7a608fbd3bcd 40 }
shimizuta 22:0ed9de464f40 41 printf("bus is %d\r\n", bus_in.read());
yuto17320508 12:9a5de6adae9c 42 wait(0.5);
shimizuta 26:5fb1aa9cb7f0 43 //一歩動かして初期位置へ
yuto17320508 25:c740e6fd5dab 44 motor_lo.setDutyLimit(0.1);
yuto17320508 25:c740e6fd5dab 45 motor_li.setDutyLimit(0.1);
yuto17320508 12:9a5de6adae9c 46 straight();
shimizuta 26:5fb1aa9cb7f0 47 // mode選択
yuto17320508 32:aee87dcaf7ca 48 Start();
yuto17320508 48:d9f3ce701aca 49 #ifndef BADRULEMODE
yuto17320508 38:89d2a9e6c96f 50 if(RightOrLeft == 0) {
yuto17320508 32:aee87dcaf7ca 51 if(start_state == 0) theta0 = -degree/100.0;
yuto17320508 32:aee87dcaf7ca 52 else if(start_state == 1) theta0 = -degree/100.0 - 45;
yuto17320508 32:aee87dcaf7ca 53 else if(start_state == 2) theta0 = -degree/100.0 - 90;
yuto17320508 32:aee87dcaf7ca 54 else if(start_state == 3) theta0 = -degree/100.0 + 90;
yuto17320508 32:aee87dcaf7ca 55 else printf("state_error");
yuto17320508 38:89d2a9e6c96f 56 } else {
yuto17320508 32:aee87dcaf7ca 57 if(start_state == 0) theta0 = -degree/100.0;
yuto17320508 32:aee87dcaf7ca 58 else if(start_state == 1) theta0 = -degree/100.0 + 45;
yuto17320508 32:aee87dcaf7ca 59 else if(start_state == 2) theta0 = -degree/100.0 + 90;
yuto17320508 32:aee87dcaf7ca 60 else if(start_state == 3) theta0 = -degree/100.0 - 90;
yuto17320508 32:aee87dcaf7ca 61 else printf("state_error");
yuto17320508 32:aee87dcaf7ca 62 }
yuto17320508 48:d9f3ce701aca 63 #endif
yuto17320508 48:d9f3ce701aca 64 #ifdef BADRULEMODE
yuto17320508 53:4673b85e1d2a 65 printf("BAD RULE!!!!!!!!!\r\n");
yuto17320508 48:d9f3ce701aca 66 if(RightOrLeft == 0) {
yuto17320508 48:d9f3ce701aca 67 if(start_state == 0) theta0 = -degree/100.0;
yuto17320508 48:d9f3ce701aca 68 else if(start_state == 1) {
yuto17320508 52:dc3d83eb31eb 69 theta0 = -degree/100.0 - 90;
yuto17320508 55:1b1e5839dde1 70 //ジャイロの初期化待ちなので切るな！！
yuto17320508 53:4673b85e1d2a 71 printf("degree is %.2f for 90\r\n", degree0);
yuto17320508 48:d9f3ce701aca 72 motor_lo.setDutyLimit(0.6);//0.5
yuto17320508 48:d9f3ce701aca 73 motor_li.setDutyLimit(0.6);
yuto17320508 52:dc3d83eb31eb 74 turn(55.0);
yuto17320508 48:d9f3ce701aca 75 } else if(start_state == 2) {
yuto17320508 48:d9f3ce701aca 76 theta0 = -degree/100.0 - 135;
yuto17320508 55:1b1e5839dde1 77 //ジャイロの初期化待ちなので切るな！！
yuto17320508 53:4673b85e1d2a 78 printf("degree is %.2f for 135\r\n", degree0);
yuto17320508 48:d9f3ce701aca 79 motor_lo.setDutyLimit(0.6);//0.5
yuto17320508 48:d9f3ce701aca 80 motor_li.setDutyLimit(0.6);
yuto17320508 48:d9f3ce701aca 81 turn(100.0);
yuto17320508 48:d9f3ce701aca 82 } else if(start_state == 3) theta0 = -degree/100.0 + 90;
yuto17320508 48:d9f3ce701aca 83 else printf("state_error");
yuto17320508 48:d9f3ce701aca 84 } else {
yuto17320508 48:d9f3ce701aca 85 if(start_state == 0) theta0 = -degree/100.0;
yuto17320508 48:d9f3ce701aca 86 else if(start_state == 1) {
yuto17320508 52:dc3d83eb31eb 87 theta0 = -degree/100.0 + 90;
yuto17320508 55:1b1e5839dde1 88 //ジャイロの初期化待ちなので切るな！！
yuto17320508 53:4673b85e1d2a 89 printf("degree is %.2f for -90\r\n", degree0);
yuto17320508 48:d9f3ce701aca 90 //段差前旋回
yuto17320508 48:d9f3ce701aca 91 motor_lo.setDutyLimit(0.6);//0.5
yuto17320508 48:d9f3ce701aca 92 motor_li.setDutyLimit(0.6);
yuto17320508 52:dc3d83eb31eb 93 turn(-55.0);
yuto17320508 48:d9f3ce701aca 94 } else if(start_state == 2) {
yuto17320508 48:d9f3ce701aca 95 theta0 = -degree/100.0 + 135;
yuto17320508 55:1b1e5839dde1 96 //ジャイロの初期化待ちなので切るな！！
yuto17320508 53:4673b85e1d2a 97 printf("degree is %.2f for -135\r\n", degree0);
yuto17320508 48:d9f3ce701aca 98 motor_lo.setDutyLimit(0.6);//0.5
yuto17320508 48:d9f3ce701aca 99 motor_li.setDutyLimit(0.6);
yuto17320508 48:d9f3ce701aca 100 turn(-100.0);
yuto17320508 48:d9f3ce701aca 101 } else if(start_state == 3) theta0 = -degree/100.0 - 90;
yuto17320508 48:d9f3ce701aca 102 else printf("state_error");
yuto17320508 48:d9f3ce701aca 103 }
yuto17320508 48:d9f3ce701aca 104 #endif
yuto17320508 33:2dbbe198adaf 105 FileOpen();
yuto17320508 32:aee87dcaf7ca 106 printf("LR:%d StateFlow:%d\r\n", RightOrLeft,start_state);
yuto17320508 25:c740e6fd5dab 107 StateFlow(start_state);
yuto17320508 33:2dbbe198adaf 108 FileClose();
yuto17320508 25:c740e6fd5dab 109 }
yuto17320508 25:c740e6fd5dab 110
yuto17320508 25:c740e6fd5dab 111 void StateFlow(int i)
yuto17320508 25:c740e6fd5dab 112 {
yuto17320508 25:c740e6fd5dab 113 switch(i) {
yuto17320508 25:c740e6fd5dab 114 case 0://最初の直線
yuto17320508 25:c740e6fd5dab 115 printf("go straight!!!!!!!!!!\r\n");
yuto17320508 49:6337717484fe 116 motor_lo.setDutyLimit(params[0].duty);
yuto17320508 49:6337717484fe 117 motor_li.setDutyLimit(params[0].duty);
shimizuta 50:3e73f68c3b37 118
shimizuta 50:3e73f68c3b37 119 while(checkUW(params[1].condition, degree0, ec_lo.getCount()) == 0){
yuto17320508 49:6337717484fe 120 straightAndInfinity(params[0].argument[0], params[0].argument[1]);
yuto17320508 25:c740e6fd5dab 121 }
yuto17320508 52:dc3d83eb31eb 122 printf("get distance mode\r\n");
yuto17320508 49:6337717484fe 123
yuto17320508 49:6337717484fe 124 motor_lo.setDutyLimit(params[1].duty);
yuto17320508 49:6337717484fe 125 motor_li.setDutyLimit(params[1].duty);
yuto17320508 25:c740e6fd5dab 126 for(int i=0; i<1; ++i) {
shimizuta 50:3e73f68c3b37 127 while(Hcsr04BackWithEc() < params[1].condition) {//300
yuto17320508 49:6337717484fe 128 straightAndInfinity(params[1].argument[0], params[1].argument[1]);
yuto17320508 25:c740e6fd5dab 129 }
yuto17320508 25:c740e6fd5dab 130 }
yuto17320508 25:c740e6fd5dab 131 //段差前旋回
yuto17320508 49:6337717484fe 132 motor_lo.setDutyLimit(params[2].duty);
yuto17320508 49:6337717484fe 133 motor_li.setDutyLimit(params[2].duty);
yuto17320508 49:6337717484fe 134 if(RightOrLeft == 0) turn(params[2].argument[0]);
yuto17320508 49:6337717484fe 135 else turn(-params[2].argument[0]);
yuto17320508 25:c740e6fd5dab 136 case 1://段差前
yuto17320508 25:c740e6fd5dab 137 printf("climb!!!!!!!!!!\r\n");
yuto17320508 49:6337717484fe 138
yuto17320508 49:6337717484fe 139 motor_lo.setDutyLimit(params[3].duty);
yuto17320508 49:6337717484fe 140 motor_li.setDutyLimit(params[3].duty);
yuto17320508 38:89d2a9e6c96f 141 //段差乗り越え
yuto17320508 53:4673b85e1d2a 142 while(get_dist_forward() > params[3].condition) {
yuto17320508 49:6337717484fe 143 if(RightOrLeft == 0) straightAndInfinity(params[3].argument[0], params[3].argument[1]);
yuto17320508 49:6337717484fe 144 else straightAndInfinity(-params[3].argument[0], params[3].argument[1]);
yuto17320508 25:c740e6fd5dab 145 }
yuto17320508 49:6337717484fe 146 motor_lo.setDutyLimit(params[4].duty);
yuto17320508 49:6337717484fe 147 motor_li.setDutyLimit(params[4].duty);
yuto17320508 49:6337717484fe 148 if(RightOrLeft == 0) phasing(params[4].argument[0]);
yuto17320508 49:6337717484fe 149 else phasing(-params[4].argument[0]);
yuto17320508 49:6337717484fe 150 motor_lo.setDutyLimit(params[5].duty);
yuto17320508 49:6337717484fe 151 motor_li.setDutyLimit(params[5].duty);
yuto17320508 49:6337717484fe 152 for(int i= 0; i<(int)params[5].condition; ++i) straight();
yuto17320508 49:6337717484fe 153
yuto17320508 49:6337717484fe 154 motor_lo.setDutyLimit(params[6].duty);
yuto17320508 49:6337717484fe 155 motor_li.setDutyLimit(params[6].duty);
yuto17320508 52:dc3d83eb31eb 156 printf("start finding back block\r\n");
yuto17320508 49:6337717484fe 157 while(get_dist_back() > params[6].condition) straight();
yuto17320508 55:1b1e5839dde1 158
yuto17320508 25:c740e6fd5dab 159 //段差後旋回
yuto17320508 49:6337717484fe 160 motor_lo.setDutyLimit(params[7].duty);
yuto17320508 49:6337717484fe 161 motor_li.setDutyLimit(params[7].duty);
yuto17320508 49:6337717484fe 162 if(RightOrLeft == 0) turn(params[7].argument[0]);
yuto17320508 49:6337717484fe 163 else turn(-params[7].argument[0]);
yuto17320508 38:89d2a9e6c96f 164
yuto17320508 25:c740e6fd5dab 165 case 2://段差直後
yuto17320508 25:c740e6fd5dab 166 printf("go lope!!!!!!!!!!\r\n");
yuto17320508 25:c740e6fd5dab 167 //前進
yuto17320508 49:6337717484fe 168 motor_lo.setDutyLimit(params[8].duty);
yuto17320508 49:6337717484fe 169 motor_li.setDutyLimit(params[8].duty);
yuto17320508 49:6337717484fe 170 for(int i=0; i<(int)params[8].condition; ++i) {
yuto17320508 49:6337717484fe 171 if(RightOrLeft == 0) straightAndInfinity(params[8].argument[0], params[8].argument[1]);
yuto17320508 49:6337717484fe 172 else straightAndInfinity(-params[8].argument[0], params[8].argument[1]);
yuto17320508 38:89d2a9e6c96f 173 }
yuto17320508 49:6337717484fe 174
yuto17320508 49:6337717484fe 175 motor_lo.setDutyLimit(params[9].duty);
yuto17320508 49:6337717484fe 176 motor_li.setDutyLimit(params[9].duty);
yuto17320508 25:c740e6fd5dab 177 for(int i=0; i<3; ++i) {
yuto17320508 49:6337717484fe 178 while(get_dist_forward() > params[9].condition) {
yuto17320508 49:6337717484fe 179 if(RightOrLeft == 0) straightAndInfinity(params[9].argument[0], params[9].argument[1]);
yuto17320508 49:6337717484fe 180 else straightAndInfinity(-params[9].argument[0], params[9].argument[1]);
yuto17320508 25:c740e6fd5dab 181 }
yuto17320508 25:c740e6fd5dab 182 }
yuto17320508 25:c740e6fd5dab 183 //ロープ前旋回
yuto17320508 25:c740e6fd5dab 184 printf("before roop deg:%.3f\r\n",degree0);
yuto17320508 49:6337717484fe 185 motor_lo.setDutyLimit(params[10].duty);
yuto17320508 49:6337717484fe 186 motor_li.setDutyLimit(params[10].duty);
yuto17320508 49:6337717484fe 187 if(RightOrLeft == 0) turn(params[10].argument[0]);
yuto17320508 49:6337717484fe 188 else turn(-params[10].argument[0]);
yuto17320508 48:d9f3ce701aca 189
yuto17320508 25:c740e6fd5dab 190 //ロープ位置ジャストまで前進
yuto17320508 49:6337717484fe 191 motor_lo.setDutyLimit(params[11].duty);
yuto17320508 49:6337717484fe 192 motor_li.setDutyLimit(params[11].duty);
yuto17320508 25:c740e6fd5dab 193 for(int i=0; i<3; ++i) {
yuto17320508 49:6337717484fe 194 straightAndInfinity(params[11].argument[0], params[11].argument[1]);
yuto17320508 25:c740e6fd5dab 195 }
yuto17320508 25:c740e6fd5dab 196 for(int i=0; i<2; ++i) {
yuto17320508 49:6337717484fe 197 while(get_dist_back() < params[11].condition) {
yuto17320508 49:6337717484fe 198 straightAndInfinity(params[11].argument[0], params[11].argument[1]);
yuto17320508 25:c740e6fd5dab 199 }
yuto17320508 25:c740e6fd5dab 200 }
yuto17320508 49:6337717484fe 201
yuto17320508 49:6337717484fe 202 motor_lo.setDutyLimit(params[12].duty);
yuto17320508 49:6337717484fe 203 motor_li.setDutyLimit(params[12].duty);
yuto17320508 49:6337717484fe 204 phasing(params[12].argument[0]);
yuto17320508 49:6337717484fe 205
yuto17320508 25:c740e6fd5dab 206 //ロープ
yuto17320508 49:6337717484fe 207 motor_lo.setDutyLimit(params[13].duty);
yuto17320508 49:6337717484fe 208 motor_li.setDutyLimit(params[13].duty);
yuto17320508 32:aee87dcaf7ca 209 while(switch_modes[2].read() == 0) {
yuto17320508 53:4673b85e1d2a 210 lopeAndInfinity(params[13].argument[0], params[13].argument[1]);
yuto17320508 53:4673b85e1d2a 211 //straight();
yuto17320508 25:c740e6fd5dab 212 }
yuto17320508 32:aee87dcaf7ca 213 printf("uhai stand by ok!!!!!!!!!!\r\n");
yuto17320508 32:aee87dcaf7ca 214 NoHandSignal();
yuto17320508 25:c740e6fd5dab 215 case 3://坂
yuto17320508 32:aee87dcaf7ca 216 printf("last spart!!!!!!!!");
yuto17320508 38:89d2a9e6c96f 217
yuto17320508 32:aee87dcaf7ca 218 if(RightOrLeft == 0) theta0 = -degree/100.0+90;
yuto17320508 32:aee87dcaf7ca 219 else theta0 = -degree/100.0-90;
yuto17320508 49:6337717484fe 220 motor_lo.setDutyLimit(params[14].duty);
yuto17320508 49:6337717484fe 221 motor_li.setDutyLimit(params[14].duty);
yuto17320508 49:6337717484fe 222 for(int i=0; i<(int)params[14].condition; ++i) {
yuto17320508 49:6337717484fe 223 if(RightOrLeft == 0) straightAndInfinity(params[14].argument[0], params[14].argument[1]);//straight();//climbAndInfinity(-90,5);
yuto17320508 49:6337717484fe 224 else straightAndInfinity(-params[14].argument[0], params[14].argument[1]);
yuto17320508 25:c740e6fd5dab 225 }
yuto17320508 25:c740e6fd5dab 226 printf("wall standby");
yuto17320508 49:6337717484fe 227 motor_lo.setDutyLimit(params[15].duty);
yuto17320508 49:6337717484fe 228 motor_li.setDutyLimit(params[15].duty);
yuto17320508 49:6337717484fe 229 while(get_dist_back() < params[15].condition) {
shimizuta 27:d392a95f4799 230 // straight();
yuto17320508 49:6337717484fe 231 if(RightOrLeft == 0) climbAndInfinity(params[15].argument[0], params[15].argument[1]);
yuto17320508 49:6337717484fe 232 else climbAndInfinity(-params[15].argument[0], params[15].argument[1]);
yuto17320508 25:c740e6fd5dab 233 }
yuto17320508 49:6337717484fe 234
yuto17320508 25:c740e6fd5dab 235 for(int i=0; i<1; ++i) {
yuto17320508 49:6337717484fe 236 while(get_dist_forward() > params[16].condition) {
yuto17320508 25:c740e6fd5dab 237 //straight();
yuto17320508 49:6337717484fe 238 if(RightOrLeft == 0) climbAndInfinity(params[16].argument[0], params[16].argument[1]);
yuto17320508 49:6337717484fe 239 else climbAndInfinity(-params[16].argument[0], params[16].argument[1]);
yuto17320508 25:c740e6fd5dab 240 }
yuto17320508 25:c740e6fd5dab 241 }
yuto17320508 25:c740e6fd5dab 242 hand_mode = GOAL;
shimizuta 30:231e6584afe9 243 stop();
yuto17320508 32:aee87dcaf7ca 244 printf("uhai!!!!!!!!!!!!!!!/r/n");
yuto17320508 32:aee87dcaf7ca 245 }
yuto17320508 32:aee87dcaf7ca 246 }
yuto17320508 32:aee87dcaf7ca 247 void Start()
yuto17320508 32:aee87dcaf7ca 248 {
yuto17320508 38:89d2a9e6c96f 249 if(hand.read()==0) { //開始時すでにスイッチが押されていた場合
yuto17320508 38:89d2a9e6c96f 250
yuto17320508 32:aee87dcaf7ca 251 SetMode();
yuto17320508 35:04699b49c463 252 hand_mode = G_CLOSE;
yuto17320508 32:aee87dcaf7ca 253 NoHandSignal();
yuto17320508 38:89d2a9e6c96f 254 } else {
yuto17320508 32:aee87dcaf7ca 255 SetMode();
yuto17320508 35:04699b49c463 256 hand_mode = G_OPEN;
yuto17320508 35:04699b49c463 257 stop();//ここで開くことをcanでslaveに送る
yuto17320508 48:d9f3ce701aca 258 wait(1);
yuto17320508 32:aee87dcaf7ca 259 wait_gerege();
yuto17320508 35:04699b49c463 260 hand_mode = G_CLOSE;
yuto17320508 32:aee87dcaf7ca 261 HandMove();
yuto17320508 32:aee87dcaf7ca 262 wait(0.5);
yuto17320508 38:89d2a9e6c96f 263 }
yuto17320508 32:aee87dcaf7ca 264 }
yuto17320508 32:aee87dcaf7ca 265 void SetMode()
yuto17320508 32:aee87dcaf7ca 266 {
yuto17320508 38:89d2a9e6c96f 267 while(get_dist_back() > 50.0) {
yuto17320508 32:aee87dcaf7ca 268 led1 = led2 = led3 = led4 = 0;
yuto17320508 32:aee87dcaf7ca 269 if(switch_modes[2].read()) start_state=3;
yuto17320508 32:aee87dcaf7ca 270 else if(switch_modes[1].read()) start_state=2;
yuto17320508 32:aee87dcaf7ca 271 else if(switch_modes[0].read()) start_state=1;
yuto17320508 32:aee87dcaf7ca 272 else start_state=0;
yuto17320508 38:89d2a9e6c96f 273
yuto17320508 38:89d2a9e6c96f 274 if(switch_LR.read()) RightOrLeft = 1;
yuto17320508 38:89d2a9e6c96f 275 else RightOrLeft = 0;
yuto17320508 38:89d2a9e6c96f 276
yuto17320508 32:aee87dcaf7ca 277 if(start_state==0)led4 = 1;
yuto17320508 32:aee87dcaf7ca 278 else if(start_state==1)led3 = 1;
yuto17320508 32:aee87dcaf7ca 279 else if(start_state==2)led2 = 1;
yuto17320508 32:aee87dcaf7ca 280 else if(start_state==3)led1 = 1;
yuto17320508 38:89d2a9e6c96f 281
yuto17320508 32:aee87dcaf7ca 282 }
yuto17320508 32:aee87dcaf7ca 283 printf("mode is %d RightOrLeft is %d \r\n", start_state, RightOrLeft);
yuto17320508 32:aee87dcaf7ca 284 led1 = led2 = led3 = led4 = 1;
yuto17320508 32:aee87dcaf7ca 285 }
yuto17320508 32:aee87dcaf7ca 286 void NoHandSignal()
yuto17320508 32:aee87dcaf7ca 287 {
yuto17320508 38:89d2a9e6c96f 288 while(get_dist_back() > 40.0) {
yuto17320508 32:aee87dcaf7ca 289 wait(0.01);
yuto17320508 32:aee87dcaf7ca 290 }
yuto17320508 38:89d2a9e6c96f 291 while(get_dist_back() < 40.0) {
yuto17320508 32:aee87dcaf7ca 292 wait(0.01);
shimizuta 22:0ed9de464f40 293 }
yuto17320508 48:d9f3ce701aca 294 }