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

Committer:
shimizuta
Date:
Fri May 03 01:46:30 2019 +0000
Revision:
20:e30e6e175991
Parent:
19:b162e7f4cc06
Child:
21:14133581387b
filter;

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