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@21:14133581387b, 2019-05-03 (annotated)

Committer:
shimizuta
Date:
Fri May 03 02:21:47 2019 +0000
Revision:
21:14133581387b
Parent:
20:e30e6e175991
Child:
22:0ed9de464f40
filter weight change

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