ROBOSTEP_5期 / Mbed 2 deprecated George_Master_BOTHMOVE

Dependencies:   mbed robot

Committer:
shimizuta
Date:
Thu May 02 09:39:31 2019 +0000
Revision:
18:d0a6771fa38d
Parent:
17:2b3fa9b1a05b
Child:
19:b162e7f4cc06
a

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 "pin.h"
eri 0:c1476d342c13 4 #include "microinfinity.h"
shimizuta 17:2b3fa9b1a05b 5 #include "robot.h"
shimizuta 16:1e91753f0a01 6 /*
yuto17320508 4:81f01f93e502 7 //#define DEBUG_ON
yuto17320508 4:81f01f93e502 8
yuto17320508 4:81f01f93e502 9 #ifdef DEBUG_ON
yuto17320508 4:81f01f93e502 10 #define DEBUG(...) printf("" __VA_ARGS__);
yuto17320508 4:81f01f93e502 11 #else
yuto17320508 4:81f01f93e502 12 #define DEBUG(...)
yuto17320508 4:81f01f93e502 13 #endif
shimizuta 16:1e91753f0a01 14 */
yuto17320508 4:81f01f93e502 15 #define Pi 3.14159265359 //円周率π
yuto17320508 4:81f01f93e502 16
shimizuta 18:d0a6771fa38d 17 //PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う
shimizuta 18:d0a6771fa38d 18 PIDcontroller pid_lo(0.01, 0.000, 0.000);
shimizuta 18:d0a6771fa38d 19 PIDcontroller pid_li(0.01, 0.000, 0.000); //Kp.Ki,Kd
shimizuta 18:d0a6771fa38d 20 Motor motor_lo(&motor_lo_f, &motor_lo_b),
shimizuta 18:d0a6771fa38d 21 motor_li(&motor_li_f, &motor_li_b); //forward,backのピンを代入
shimizuta 18:d0a6771fa38d 22 OneLeg leg_lo, leg_li;
shimizuta 18:d0a6771fa38d 23 Robot robot;
shimizuta 18:d0a6771fa38d 24
yuto17320508 14:1a3a673d85e3 25 enum WalkMode {
yuto17320508 12:9a5de6adae9c 26 BACK,
yuto17320508 12:9a5de6adae9c 27 STOP,
yuto17320508 12:9a5de6adae9c 28 FORWARD,
yuto17320508 12:9a5de6adae9c 29 };
yuto17320508 14:1a3a673d85e3 30 enum EVENT {
yuto17320508 12:9a5de6adae9c 31 NORMAL,
yuto17320508 12:9a5de6adae9c 32 GEREGE,
yuto17320508 14:1a3a673d85e3 33 GOAL,
yuto17320508 12:9a5de6adae9c 34 };
yuto17320508 4:81f01f93e502 35 float accel_max = 0.01; //これグローバルにしたのはごめん。set関数多すぎてめんどくなった。
shimizuta 18:d0a6771fa38d 36 int hand_mode=NORMAL;
shimizuta 18:d0a6771fa38d 37 int step_num_l, step_num_r;
yuto17320508 4:81f01f93e502 38
yuto17320508 4:81f01f93e502 39 void reset();
eri 0:c1476d342c13 40 void setup();
yuto17320508 10:a335588b9ef0 41 void set_gyro();
yuto17320508 10:a335588b9ef0 42 double get_dist_forward();
yuto17320508 10:a335588b9ef0 43 double get_dist_back();
yuto17320508 12:9a5de6adae9c 44 void can_send(int mode, float duty);
yuto17320508 12:9a5de6adae9c 45 void straight();
yuto17320508 12:9a5de6adae9c 46 void turnLeft();
yuto17320508 12:9a5de6adae9c 47 void curveLeft();
yuto17320508 12:9a5de6adae9c 48 void turnRight();
yuto17320508 12:9a5de6adae9c 49 void curveRight();
yuto17320508 13:29e71a2fd11e 50 void turn(float target_degree);//回転角, 収束許容誤差
yuto17320508 10:a335588b9ef0 51 void straightAndInfinity(float target_degree, float tolerance_degree);//角度を補正するのと前進
yuto17320508 12:9a5de6adae9c 52 void wait_gerege();
yuto17320508 12:9a5de6adae9c 53
kageyuta 1:86c4c38abe40 54 int main()
kageyuta 1:86c4c38abe40 55 {
eri 0:c1476d342c13 56 setup();
yuto17320508 12:9a5de6adae9c 57 printf("reset standby\n\r");
kageyuta 2:55c616d2e0fe 58 reset();
yuto17320508 4:81f01f93e502 59 printf("bus standby\n\r");
yuto17320508 5:63462d696256 60 while(1) {
yuto17320508 5:63462d696256 61 if(bus_in.read() == 1) break;
yuto17320508 3:7a608fbd3bcd 62 }
yuto17320508 4:81f01f93e502 63 printf("bus is %d\n\r", bus_in.read());
yuto17320508 12:9a5de6adae9c 64 wait(0.5);
yuto17320508 12:9a5de6adae9c 65 straight();
yuto17320508 12:9a5de6adae9c 66 wait_gerege();
yuto17320508 12:9a5de6adae9c 67 hand_mode = GEREGE;
yuto17320508 10:a335588b9ef0 68 set_gyro();
yuto17320508 10:a335588b9ef0 69 printf("dist:%.3f\r\n", get_dist_forward());
shimizuta 18:d0a6771fa38d 70 //直進スタート
yuto17320508 13:29e71a2fd11e 71 for(int i=0;i<3;++i){
yuto17320508 13:29e71a2fd11e 72 while(get_dist_back() < 280)
yuto17320508 13:29e71a2fd11e 73 {
yuto17320508 13:29e71a2fd11e 74 straightAndInfinity(0, 5);
yuto17320508 13:29e71a2fd11e 75 //wait(0.01);
yuto17320508 14:1a3a673d85e3 76 printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
yuto17320508 13:29e71a2fd11e 77 }
yuto17320508 3:7a608fbd3bcd 78 }
yuto17320508 12:9a5de6adae9c 79 //printf("back dist:%.3f\r\n", get_dist_back());
yuto17320508 10:a335588b9ef0 80 //段差前旋回
yuto17320508 12:9a5de6adae9c 81 motor_lo.setDutyLimit(0.4);//0.5
yuto17320508 12:9a5de6adae9c 82 motor_li.setDutyLimit(0.4);
yuto17320508 13:29e71a2fd11e 83 turn(40.0);
yuto17320508 10:a335588b9ef0 84 //段差乗り越え
yuto17320508 12:9a5de6adae9c 85 for(int i= 0;i<5;++i) straight();
yuto17320508 12:9a5de6adae9c 86 while(get_dist_back() > 40) straight();
yuto17320508 10:a335588b9ef0 87 //段差後旋回
yuto17320508 12:9a5de6adae9c 88 printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
yuto17320508 13:29e71a2fd11e 89 turn(90.0);
yuto17320508 12:9a5de6adae9c 90 printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
yuto17320508 10:a335588b9ef0 91 //前進
yuto17320508 12:9a5de6adae9c 92 motor_lo.setDutyLimit(0.5);//0.5
yuto17320508 12:9a5de6adae9c 93 motor_li.setDutyLimit(0.5);
yuto17320508 13:29e71a2fd11e 94 for(int i=0;i<3;++i)
yuto17320508 12:9a5de6adae9c 95 {
yuto17320508 14:1a3a673d85e3 96 while(get_dist_forward() > 65)
yuto17320508 13:29e71a2fd11e 97 {
yuto17320508 13:29e71a2fd11e 98 straightAndInfinity(90.0, 5.0);
yuto17320508 13:29e71a2fd11e 99 printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
yuto17320508 13:29e71a2fd11e 100 }
yuto17320508 12:9a5de6adae9c 101 }
yuto17320508 10:a335588b9ef0 102 //ロープ前旋回
yuto17320508 13:29e71a2fd11e 103 printf("before roop deg:%.3f\n\r",degree0);
yuto17320508 13:29e71a2fd11e 104 turn(0);
yuto17320508 14:1a3a673d85e3 105
yuto17320508 10:a335588b9ef0 106 //ロープ
yuto17320508 13:29e71a2fd11e 107 while(mode4.read() == 0)
yuto17320508 13:29e71a2fd11e 108 {
yuto17320508 14:1a3a673d85e3 109
yuto17320508 13:29e71a2fd11e 110 straightAndInfinity(0, 5);
yuto17320508 13:29e71a2fd11e 111 }
yuto17320508 14:1a3a673d85e3 112
yuto17320508 14:1a3a673d85e3 113 printf("go to uhai deg:%.3f forward dist:%.3f \n\r",degree0,get_dist_forward());
yuto17320508 13:29e71a2fd11e 114 for(int i=0;i<3;++i)
kageyuta 11:a6fadff7cc78 115 {
yuto17320508 14:1a3a673d85e3 116 while(get_dist_forward() > 65)//65
yuto17320508 13:29e71a2fd11e 117 {
yuto17320508 13:29e71a2fd11e 118 straightAndInfinity(0, 5);
yuto17320508 13:29e71a2fd11e 119 printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
yuto17320508 13:29e71a2fd11e 120 }
kageyuta 11:a6fadff7cc78 121 }
yuto17320508 14:1a3a673d85e3 122 printf("turn");
yuto17320508 14:1a3a673d85e3 123 turn(-90);
yuto17320508 14:1a3a673d85e3 124
yuto17320508 15:1098bf926b5b 125 while(get_dist_back() > 10.0){}
yuto17320508 15:1098bf926b5b 126 while(get_dist_back() < 30.0){}
yuto17320508 15:1098bf926b5b 127
yuto17320508 15:1098bf926b5b 128 printf("last spart!!!!!!!!");
yuto17320508 12:9a5de6adae9c 129
yuto17320508 14:1a3a673d85e3 130 motor_lo.setDutyLimit(0.2);//0.5
yuto17320508 14:1a3a673d85e3 131 motor_li.setDutyLimit(0.2);
yuto17320508 14:1a3a673d85e3 132
yuto17320508 14:1a3a673d85e3 133 for(int i=0; i<15; ++i)straightAndInfinity(0, 5);
yuto17320508 14:1a3a673d85e3 134 printf("wall standby");
yuto17320508 14:1a3a673d85e3 135 while(get_dist_back() < 250) {
yuto17320508 14:1a3a673d85e3 136 straightAndInfinity(0, 5);
yuto17320508 14:1a3a673d85e3 137 printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
yuto17320508 14:1a3a673d85e3 138 }
yuto17320508 14:1a3a673d85e3 139 for(int i=0; i<2; ++i) {
yuto17320508 14:1a3a673d85e3 140 while(get_dist_forward() > 65) {
yuto17320508 14:1a3a673d85e3 141 straightAndInfinity(0, 5);
yuto17320508 14:1a3a673d85e3 142 printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
yuto17320508 14:1a3a673d85e3 143 }
yuto17320508 14:1a3a673d85e3 144 }
yuto17320508 12:9a5de6adae9c 145 hand_mode = GOAL;
yuto17320508 12:9a5de6adae9c 146 straight();
yuto17320508 13:29e71a2fd11e 147 printf("uhai!!!!!!!!!!!!!!!");
yuto17320508 10:a335588b9ef0 148 }
yuto17320508 13:29e71a2fd11e 149 void turn(float target_degree)//turn_degreeを超えるまで旋回し続ける関数
yuto17320508 10:a335588b9ef0 150 {
yuto17320508 14:1a3a673d85e3 151 if(target_degree - degree0 > 0) {
yuto17320508 10:a335588b9ef0 152 while(target_degree - degree0 > 0)
yuto17320508 14:1a3a673d85e3 153 turnLeft();
yuto17320508 14:1a3a673d85e3 154 } else {
yuto17320508 10:a335588b9ef0 155 while(target_degree - degree0 < 0)
yuto17320508 14:1a3a673d85e3 156 turnRight();
yuto17320508 10:a335588b9ef0 157 }
yuto17320508 13:29e71a2fd11e 158 printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
yuto17320508 10:a335588b9ef0 159 }
yuto17320508 10:a335588b9ef0 160 void straightAndInfinity(float target_degree, float tolerance_degree)
yuto17320508 10:a335588b9ef0 161 {
yuto17320508 12:9a5de6adae9c 162 if(target_degree - degree0 > tolerance_degree) curveLeft();
yuto17320508 12:9a5de6adae9c 163 else if(degree0 - target_degree > tolerance_degree) curveRight();
yuto17320508 12:9a5de6adae9c 164 else straight();
yuto17320508 5:63462d696256 165 }
yuto17320508 12:9a5de6adae9c 166 void straight()
yuto17320508 5:63462d696256 167 {
yuto17320508 12:9a5de6adae9c 168 can_send(FORWARD, motor_lo.getDutyLimit());
yuto17320508 5:63462d696256 169 leg_lo.setTargetPose(360+step_num_l*180);
yuto17320508 5:63462d696256 170 leg_li.setTargetPose(180+step_num_l*180);
yuto17320508 5:63462d696256 171 robot.run();
yuto17320508 4:81f01f93e502 172 motor_lo_f.write(0);
yuto17320508 4:81f01f93e502 173 motor_lo_b.write(0);
yuto17320508 4:81f01f93e502 174 motor_li_f.write(0);
yuto17320508 4:81f01f93e502 175 motor_li_b.write(0);
yuto17320508 5:63462d696256 176 while(1) {
yuto17320508 5:63462d696256 177 if(bus_in.read() == 1) break;
yuto17320508 5:63462d696256 178 }
yuto17320508 7:cff25545558f 179 step_num_l++;
yuto17320508 7:cff25545558f 180 step_num_r++;
yuto17320508 5:63462d696256 181 }
yuto17320508 12:9a5de6adae9c 182 void turnLeft()
yuto17320508 5:63462d696256 183 {
yuto17320508 12:9a5de6adae9c 184 can_send(FORWARD, motor_lo.getDutyLimit());
yuto17320508 5:63462d696256 185 leg_lo.setTargetPose(360+(step_num_l-2)*180);
yuto17320508 5:63462d696256 186 leg_li.setTargetPose(180+(step_num_l-2)*180);
yuto17320508 5:63462d696256 187 robot.run();
yuto17320508 5:63462d696256 188 motor_lo_f.write(0);
yuto17320508 5:63462d696256 189 motor_lo_b.write(0);
yuto17320508 5:63462d696256 190 motor_li_f.write(0);
yuto17320508 5:63462d696256 191 motor_li_b.write(0);
yuto17320508 5:63462d696256 192 while(1) {
yuto17320508 5:63462d696256 193 if(bus_in.read() == 1) break;
yuto17320508 5:63462d696256 194 }
yuto17320508 5:63462d696256 195 step_num_r++;
yuto17320508 5:63462d696256 196 step_num_l--;
yuto17320508 5:63462d696256 197 }
yuto17320508 12:9a5de6adae9c 198 void curveLeft()
yuto17320508 10:a335588b9ef0 199 {
yuto17320508 12:9a5de6adae9c 200 can_send(FORWARD, motor_lo.getDutyLimit());
yuto17320508 10:a335588b9ef0 201 leg_lo.setTargetPose(360+(step_num_l-1)*180);
yuto17320508 10:a335588b9ef0 202 leg_li.setTargetPose(180+(step_num_l-1)*180);
yuto17320508 10:a335588b9ef0 203 robot.run();
yuto17320508 10:a335588b9ef0 204 motor_lo_f.write(0);
yuto17320508 10:a335588b9ef0 205 motor_lo_b.write(0);
yuto17320508 10:a335588b9ef0 206 motor_li_f.write(0);
yuto17320508 10:a335588b9ef0 207 motor_li_b.write(0);
yuto17320508 10:a335588b9ef0 208 while(1) {
yuto17320508 10:a335588b9ef0 209 if(bus_in.read() == 1) break;
yuto17320508 10:a335588b9ef0 210 }
yuto17320508 10:a335588b9ef0 211 step_num_r++;
yuto17320508 10:a335588b9ef0 212 }
yuto17320508 12:9a5de6adae9c 213 void turnRight()
yuto17320508 5:63462d696256 214 {
yuto17320508 12:9a5de6adae9c 215 can_send(BACK, motor_lo.getDutyLimit());
yuto17320508 8:ded0354412ae 216 leg_lo.setTargetPose(360+step_num_l*180);
yuto17320508 8:ded0354412ae 217 leg_li.setTargetPose(180+step_num_l*180);
yuto17320508 5:63462d696256 218 robot.run();
yuto17320508 5:63462d696256 219 motor_lo_f.write(0);
yuto17320508 5:63462d696256 220 motor_lo_b.write(0);
yuto17320508 5:63462d696256 221 motor_li_f.write(0);
yuto17320508 5:63462d696256 222 motor_li_b.write(0);
yuto17320508 5:63462d696256 223 while(1) {
yuto17320508 5:63462d696256 224 if(bus_in.read() == 1) break;
yuto17320508 5:63462d696256 225 }
yuto17320508 8:ded0354412ae 226 step_num_r--;
yuto17320508 8:ded0354412ae 227 step_num_l++;
eri 0:c1476d342c13 228 }
yuto17320508 12:9a5de6adae9c 229 void curveRight()
yuto17320508 10:a335588b9ef0 230 {
yuto17320508 12:9a5de6adae9c 231 can_send(STOP, motor_lo.getDutyLimit());
yuto17320508 10:a335588b9ef0 232 leg_lo.setTargetPose(360+step_num_l*180);
yuto17320508 10:a335588b9ef0 233 leg_li.setTargetPose(180+step_num_l*180);
yuto17320508 10:a335588b9ef0 234 robot.run();
yuto17320508 10:a335588b9ef0 235 motor_lo_f.write(0);
yuto17320508 10:a335588b9ef0 236 motor_lo_b.write(0);
yuto17320508 10:a335588b9ef0 237 motor_li_f.write(0);
yuto17320508 10:a335588b9ef0 238 motor_li_b.write(0);
yuto17320508 10:a335588b9ef0 239 while(1) {
yuto17320508 10:a335588b9ef0 240 if(bus_in.read() == 1) break;
yuto17320508 10:a335588b9ef0 241 }
yuto17320508 10:a335588b9ef0 242 step_num_l++;
yuto17320508 10:a335588b9ef0 243 }
eri 0:c1476d342c13 244
eri 0:c1476d342c13 245
kageyuta 1:86c4c38abe40 246 void setup()
kageyuta 1:86c4c38abe40 247 {
eri 0:c1476d342c13 248 can1.frequency(1000000);
eri 0:c1476d342c13 249 motor_lo_f.period_us(100);
eri 0:c1476d342c13 250 motor_lo_b.period_us(100);
eri 0:c1476d342c13 251 motor_li_f.period_us(100);
eri 0:c1476d342c13 252 motor_li_b.period_us(100);
kageyuta 1:86c4c38abe40 253
eri 0:c1476d342c13 254 hand.mode(PullUp);
kageyuta 2:55c616d2e0fe 255 switch_lo.mode(PullUp);
kageyuta 2:55c616d2e0fe 256 switch_li.mode(PullUp);
yuto17320508 13:29e71a2fd11e 257 mode4.mode(PullUp);
shimizuta 18:d0a6771fa38d 258
shimizuta 18:d0a6771fa38d 259 pid_lo.setTolerance(10);
shimizuta 18:d0a6771fa38d 260 pid_li.setTolerance(10);
shimizuta 18:d0a6771fa38d 261 motor_lo.setEncoder(&ec_lo);
shimizuta 18:d0a6771fa38d 262 motor_lo.setResolution(1000);
shimizuta 18:d0a6771fa38d 263 motor_li.setEncoder(&ec_li);
shimizuta 18:d0a6771fa38d 264 motor_li.setResolution(600);
shimizuta 18:d0a6771fa38d 265 leg_lo.setMotor(&motor_lo);
shimizuta 18:d0a6771fa38d 266 leg_lo.setPIDcontroller(&pid_lo);
shimizuta 18:d0a6771fa38d 267 leg_li.setMotor(&motor_li);
shimizuta 18:d0a6771fa38d 268 leg_li.setPIDcontroller(&pid_li);
shimizuta 18:d0a6771fa38d 269 robot.setLeg(&leg_lo, &leg_li);
shimizuta 18:d0a6771fa38d 270 robot.setTickerTime(0.01); //モータ出力間隔 0.01
shimizuta 18:d0a6771fa38d 271 motor_lo.setDutyLimit(0.5);//0.5
shimizuta 18:d0a6771fa38d 272 motor_li.setDutyLimit(0.5);
kageyuta 1:86c4c38abe40 273
yuto17320508 10:a335588b9ef0 274 }
eri 0:c1476d342c13 275
yuto17320508 10:a335588b9ef0 276 void set_gyro()
yuto17320508 10:a335588b9ef0 277 {
eri 0:c1476d342c13 278 device.baud(115200);
eri 0:c1476d342c13 279 device.format(8,Serial::None,1);
eri 0:c1476d342c13 280 device.attach(dev_rx, Serial::RxIrq);
eri 0:c1476d342c13 281 wait(0.05);
eri 0:c1476d342c13 282 theta0=degree0;
eri 0:c1476d342c13 283 check_gyro();
eri 0:c1476d342c13 284 }
eri 0:c1476d342c13 285
eri 0:c1476d342c13 286
eri 0:c1476d342c13 287 //////////////////////////////////////can
yuto17320508 12:9a5de6adae9c 288 void can_send(int mode, float duty)
eri 0:c1476d342c13 289 {
yuto17320508 12:9a5de6adae9c 290 char data[2]= {0};
yuto17320508 12:9a5de6adae9c 291 int send=mode * 10 + (int)(duty * 10.0);
yuto17320508 12:9a5de6adae9c 292 //printf("duty is %.3f\n\r",duty);
yuto17320508 12:9a5de6adae9c 293 data[0]=send & 0b11111111;
yuto17320508 12:9a5de6adae9c 294 data[1]=hand_mode;
kageyuta 1:86c4c38abe40 295
yuto17320508 12:9a5de6adae9c 296 if(can1.write(CANMessage(0,data,2)))led4=1;
eri 0:c1476d342c13 297 else led4=0;
kageyuta 1:86c4c38abe40 298 }
kageyuta 2:55c616d2e0fe 299 void reset()
kageyuta 2:55c616d2e0fe 300 {
kageyuta 2:55c616d2e0fe 301 while(switch_lo.read()) {
kageyuta 6:fe9fa8c2e6f9 302 motor_lo.output(0.3);
kageyuta 2:55c616d2e0fe 303 }
yuto17320508 5:63462d696256 304 ec_lo.reset();
yuto17320508 5:63462d696256 305 motor_lo.output(0.0);
kageyuta 2:55c616d2e0fe 306 while(switch_li.read()) {
kageyuta 6:fe9fa8c2e6f9 307 motor_li.output(0.3);
kageyuta 2:55c616d2e0fe 308 }
yuto17320508 5:63462d696256 309 ec_li.reset();
yuto17320508 5:63462d696256 310 motor_li.output(0.0);
kageyuta 1:86c4c38abe40 311 }
yuto17320508 10:a335588b9ef0 312 double get_dist_back()
yuto17320508 10:a335588b9ef0 313 {
yuto17320508 10:a335588b9ef0 314 sensor_back.start();
yuto17320508 14:1a3a673d85e3 315 wait_ms(10);//10 //sensor.start()で信号を送ったあと反射波が帰ってきてから距離データが更新されるため、少し待つ必要がある。
yuto17320508 10:a335588b9ef0 316 //何ループも回す場合は不要。また、時間は適当だから調整が必要。
yuto17320508 14:1a3a673d85e3 317 float dist = sensor_back.get_dist_cm();
yuto17320508 14:1a3a673d85e3 318 if(dist < 0.1) dist = 2000.0;
yuto17320508 14:1a3a673d85e3 319 return dist;
yuto17320508 10:a335588b9ef0 320 }
yuto17320508 10:a335588b9ef0 321 double get_dist_forward()
yuto17320508 10:a335588b9ef0 322 {
kageyuta 11:a6fadff7cc78 323 sensor_forward.start();
yuto17320508 14:1a3a673d85e3 324 wait_ms(10);//10
yuto17320508 14:1a3a673d85e3 325 float dist = sensor_forward.get_dist_cm();
yuto17320508 14:1a3a673d85e3 326 if(dist < 0.1) dist = 2000.0;
yuto17320508 14:1a3a673d85e3 327 return dist;
yuto17320508 10:a335588b9ef0 328 }
yuto17320508 12:9a5de6adae9c 329
yuto17320508 12:9a5de6adae9c 330 void wait_gerege()
yuto17320508 12:9a5de6adae9c 331 {
yuto17320508 12:9a5de6adae9c 332 int i = 0;
yuto17320508 14:1a3a673d85e3 333 while(i!=100) {
yuto17320508 14:1a3a673d85e3 334 if(hand.read()==0)i++;
yuto17320508 14:1a3a673d85e3 335 }
yuto17320508 12:9a5de6adae9c 336 }