ROBOSTEP_5期 / Mbed 2 deprecated George_Master_BOTHMOVE

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@19:b162e7f4cc06, 2019-05-03 (annotated)

Committer:
yuto17320508
Date:
Fri May 03 00:30:11 2019 +0000
Revision:
19:b162e7f4cc06
Parent:
18:d0a6771fa38d
Child:
20:e30e6e175991
l

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