yuto kawamura / Mbed 2 deprecated MR2_n_1_class

l

Dependencies:   mbed

main.cpp@0:111abd91b0cb, 2018-12-14 (annotated)

Committer:
eri
Date:
Fri Dec 14 10:49:59 2018 +0000
Revision:
0:111abd91b0cb
Child:
1:3ae63be5592b
a;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eri 0:111abd91b0cb 1 #include "mbed.h"
eri 0:111abd91b0cb 2 #include "EC.h"
eri 0:111abd91b0cb 3 #include "KondoServo.h"
eri 0:111abd91b0cb 4 #include "hcsr04.h"
eri 0:111abd91b0cb 5
eri 0:111abd91b0cb 6 Serial pc(USBTX,USBRX);
eri 0:111abd91b0cb 7 Timer timer;
eri 0:111abd91b0cb 8
eri 0:111abd91b0cb 9 //パラメタ
eri 0:111abd91b0cb 10 double dutylimit = 0.6; // duty出力の上限値
eri 0:111abd91b0cb 11 int resolution = 1000; //エンコーダの分解能×逓倍
eri 0:111abd91b0cb 12 int ecgear = 35; //エンコーダのギアの歯数
eri 0:111abd91b0cb 13 double Kp = 0.342000; //P値
eri 0:111abd91b0cb 14 double Ki = 0.013062; //I値
eri 0:111abd91b0cb 15 double Kd = 0.003266; //D値
eri 0:111abd91b0cb 16 double leg_wait_time = 0.3; //脚の伸縮にかかる時間
eri 0:111abd91b0cb 17 double turn_wait_time = 1.5; //旋回にかかる時間
eri 0:111abd91b0cb 18 double target_max = 690; //直動機構の目標座標の上限（オーバーシュートを考慮して短めに設定）
eri 0:111abd91b0cb 19 double target_min = 20; //直動機構の目標座標の下限（オーバーシュートを考慮して短めに設定）
eri 0:111abd91b0cb 20 double rail_len = 710;
eri 0:111abd91b0cb 21 double turn_error=10; //旋回の際のY脚のずれの許容誤差
eri 0:111abd91b0cb 22 double servo_iniposi=105; //サーボ初期角度
eri 0:111abd91b0cb 23
eri 0:111abd91b0cb 24 double target_plot[][2]={ //(mode,target) mode 0→standby 1→straight 2→turn 3→sand dune 4→tussock 5→wait_shagai 6→mountain~finish
eri 0:111abd91b0cb 25 {0,0},
eri 0:111abd91b0cb 26 {1,3450},
eri 0:111abd91b0cb 27 {2,-45},
eri 0:111abd91b0cb 28 {3,0},
eri 0:111abd91b0cb 29 {2,-45},
eri 0:111abd91b0cb 30 {1,1030},
eri 0:111abd91b0cb 31 {2,90},
eri 0:111abd91b0cb 32 {1,2390},
eri 0:111abd91b0cb 33 {2,90},
eri 0:111abd91b0cb 34 {5,0},
eri 0:111abd91b0cb 35 {6,0}
eri 0:111abd91b0cb 36 };
eri 0:111abd91b0cb 37
eri 0:111abd91b0cb 38
eri 0:111abd91b0cb 39
eri 0:111abd91b0cb 40 //定数
eri 0:111abd91b0cb 41 #define Pi 3.14159265359 //円周率π
eri 0:111abd91b0cb 42
eri 0:111abd91b0cb 43 //変数
eri 0:111abd91b0cb 44 double linear_posi=0; //直線移動での距離 １プロットごとにリセットする?
eri 0:111abd91b0cb 45 int target_num=0; //現在の目標プロット
eri 0:111abd91b0cb 46
eri 0:111abd91b0cb 47
eri 0:111abd91b0cb 48
eri 0:111abd91b0cb 49 //ピン宣言
eri 0:111abd91b0cb 50 PwmOut motor_f(p21); //モータ正転
eri 0:111abd91b0cb 51 PwmOut motor_b(p22); //モータ逆転
eri 0:111abd91b0cb 52 DigitalIn switch_max(p9); //直動機構の上限のリミットスイッチ
eri 0:111abd91b0cb 53 DigitalIn switch_min(p8); //直動機構の下限のリミットスイッチ
eri 0:111abd91b0cb 54 DigitalIn switch_x(p7); //X脚の接地を判定するスイッチ（今後実装予定）
eri 0:111abd91b0cb 55 DigitalIn switch_y(p5); //Y脚の接地を判定するスイッチ（今後実装予定）
eri 0:111abd91b0cb 56 DigitalIn switch_hand(p6); //ゲルゲ回収アームのスイッチ
eri 0:111abd91b0cb 57 DigitalOut air_x(p18); //X脚の伸縮
eri 0:111abd91b0cb 58 DigitalOut air_y(p17); //Y脚の伸縮
eri 0:111abd91b0cb 59 DigitalOut air_x_sub(p14); //X脚の先端の伸縮（今後実装予定）
eri 0:111abd91b0cb 60 DigitalOut air_y_sub(p16); //Y脚の先端の伸縮（今後実装予定）
eri 0:111abd91b0cb 61 DigitalOut air_hand(p19); //ゲルゲ回収アームのハンド部開閉
eri 0:111abd91b0cb 62 DigitalOut air_trig(p20); //ゲルゲ持ち上げ機構のロック解除
eri 0:111abd91b0cb 63 Ec ec(p24,p23,NC,resolution,0.01); //直動機構のエンコーダ
eri 0:111abd91b0cb 64 KondoServo servo(p28,p27,1,115200); //旋回用のシリアルサーボ
eri 0:111abd91b0cb 65 HCSR04 sensor(p30, p29);
eri 0:111abd91b0cb 66
eri 0:111abd91b0cb 67
eri 0:111abd91b0cb 68
eri 0:111abd91b0cb 69 void setup();
eri 0:111abd91b0cb 70 void out(double);//モーターの出力
eri 0:111abd91b0cb 71 void move(double,double); //PID (移動先の座標(機体内)[mm],許容誤差の絶対値[mm])
eri 0:111abd91b0cb 72 void up_x();
eri 0:111abd91b0cb 73 void down_x();
eri 0:111abd91b0cb 74 void up_y();
eri 0:111abd91b0cb 75 void down_y();
eri 0:111abd91b0cb 76 void move_x (double,double);//X脚を移動
eri 0:111abd91b0cb 77 void move_y (double,double);//Y脚を移動
eri 0:111abd91b0cb 78 void reset(); //脚位置の初期化
eri 0:111abd91b0cb 79 void turn(double); //旋回動作
eri 0:111abd91b0cb 80 void straight(double); //目的地まで直進
eri 0:111abd91b0cb 81 void wait_MR1();
eri 0:111abd91b0cb 82
eri 0:111abd91b0cb 83
eri 0:111abd91b0cb 84 int main() {
eri 0:111abd91b0cb 85 setup();
eri 0:111abd91b0cb 86 while(switch_x==1){
eri 0:111abd91b0cb 87 }
eri 0:111abd91b0cb 88
eri 0:111abd91b0cb 89 reset();
eri 0:111abd91b0cb 90
eri 0:111abd91b0cb 91 /*while(1){
eri 0:111abd91b0cb 92 move(680,10);
eri 0:111abd91b0cb 93 pc.printf("%f , ",1.0*ecgear*Pi*ec.getCount()/resolution);
eri 0:111abd91b0cb 94 wait(0.5);
eri 0:111abd91b0cb 95 move(20,10);
eri 0:111abd91b0cb 96 pc.printf("%f\r\n",1.0*ecgear*Pi*ec.getCount()/resolution);
eri 0:111abd91b0cb 97 wait(0.5);
eri 0:111abd91b0cb 98 }*/
eri 0:111abd91b0cb 99
eri 0:111abd91b0cb 100 //up_x();
eri 0:111abd91b0cb 101 /*while(1){
eri 0:111abd91b0cb 102 servo.set_degree(0,105);
eri 0:111abd91b0cb 103 wait(1);
eri 0:111abd91b0cb 104 servo.set_degree(0,135);
eri 0:111abd91b0cb 105 wait(1);
eri 0:111abd91b0cb 106 servo.set_degree(0,75);
eri 0:111abd91b0cb 107 wait(1);
eri 0:111abd91b0cb 108 }*/
eri 0:111abd91b0cb 109 //down_x();
eri 0:111abd91b0cb 110
eri 0:111abd91b0cb 111 //reset();
eri 0:111abd91b0cb 112
eri 0:111abd91b0cb 113 /*while(1){
eri 0:111abd91b0cb 114 out(0.2);
eri 0:111abd91b0cb 115 }*/
eri 0:111abd91b0cb 116
eri 0:111abd91b0cb 117 //move_x(360,10);
eri 0:111abd91b0cb 118 //turn(90);
eri 0:111abd91b0cb 119 //turn(-45);
eri 0:111abd91b0cb 120
eri 0:111abd91b0cb 121
eri 0:111abd91b0cb 122
eri 0:111abd91b0cb 123
eri 0:111abd91b0cb 124 while(1) {
eri 0:111abd91b0cb 125 switch((int)(target_plot[target_num][0])){
eri 0:111abd91b0cb 126 case 0:
eri 0:111abd91b0cb 127 wait_MR1();
eri 0:111abd91b0cb 128 break;
eri 0:111abd91b0cb 129
eri 0:111abd91b0cb 130 case 1:
eri 0:111abd91b0cb 131 straight(target_plot[target_num][1]);
eri 0:111abd91b0cb 132 pc.printf("%f\r\n",linear_posi);
eri 0:111abd91b0cb 133 //while(1);
eri 0:111abd91b0cb 134 break;
eri 0:111abd91b0cb 135
eri 0:111abd91b0cb 136 case 2:
eri 0:111abd91b0cb 137 turn(target_plot[target_num][1]);
eri 0:111abd91b0cb 138 break;
eri 0:111abd91b0cb 139
eri 0:111abd91b0cb 140 case 3:
eri 0:111abd91b0cb 141 move_x(target_max,10);
eri 0:111abd91b0cb 142 move_y(target_min,10);
eri 0:111abd91b0cb 143 move_x(target_max,10);
eri 0:111abd91b0cb 144 move_y(target_min,10);
eri 0:111abd91b0cb 145 move_x(rail_len*0.5,10);
eri 0:111abd91b0cb 146
eri 0:111abd91b0cb 147 default:
eri 0:111abd91b0cb 148 break;
eri 0:111abd91b0cb 149 }
eri 0:111abd91b0cb 150 target_num++;
eri 0:111abd91b0cb 151 linear_posi=0;
eri 0:111abd91b0cb 152 }
eri 0:111abd91b0cb 153 }
eri 0:111abd91b0cb 154
eri 0:111abd91b0cb 155 double get_dist(){
eri 0:111abd91b0cb 156 sensor.start();
eri 0:111abd91b0cb 157 wait_ms(50); //sensor.start()で信号を送ったあと反射波が帰ってきてから距離データが更新されるため、少し待つ必要がある。
eri 0:111abd91b0cb 158 //何ループも回す場合は不要。また、時間は適当だから調整が必要。
eri 0:111abd91b0cb 159 return sensor.get_dist_cm();
eri 0:111abd91b0cb 160 }
eri 0:111abd91b0cb 161
eri 0:111abd91b0cb 162
eri 0:111abd91b0cb 163
eri 0:111abd91b0cb 164 //初期設定
eri 0:111abd91b0cb 165 void setup()
eri 0:111abd91b0cb 166 {
eri 0:111abd91b0cb 167 motor_f.period_us(100);
eri 0:111abd91b0cb 168 motor_b.period_us(100);
eri 0:111abd91b0cb 169 switch_max.mode(PullUp);
eri 0:111abd91b0cb 170 switch_min.mode(PullUp);
eri 0:111abd91b0cb 171 switch_x.mode(PullUp);
eri 0:111abd91b0cb 172 switch_y.mode(PullUp);
eri 0:111abd91b0cb 173 switch_hand.mode(PullUp);
eri 0:111abd91b0cb 174 air_x=0;
eri 0:111abd91b0cb 175 air_y=0;
eri 0:111abd91b0cb 176 air_x_sub=0;
eri 0:111abd91b0cb 177 air_y_sub=0;
eri 0:111abd91b0cb 178 air_hand=0;
eri 0:111abd91b0cb 179 air_trig=0;
eri 0:111abd91b0cb 180 servo.setSpeed(0,127);
eri 0:111abd91b0cb 181 wait_us(1000);
eri 0:111abd91b0cb 182 servo.init();
eri 0:111abd91b0cb 183 //servo.set_degree(0,servo_iniposi);
eri 0:111abd91b0cb 184 }
eri 0:111abd91b0cb 185 //モーターの出力
eri 0:111abd91b0cb 186 void out (double duty)
eri 0:111abd91b0cb 187 {
eri 0:111abd91b0cb 188 if(duty > 0) { //入力duty比が正の場合
eri 0:111abd91b0cb 189 if(switch_max == 0){ //上限のリミットスイッチが押されていない場合
eri 0:111abd91b0cb 190 if( fabs( duty ) < dutylimit ) { //制限値内
eri 0:111abd91b0cb 191 motor_f = fabs(duty);
eri 0:111abd91b0cb 192 motor_b = 0;
eri 0:111abd91b0cb 193 } else { //制限値超
eri 0:111abd91b0cb 194 motor_f = dutylimit;
eri 0:111abd91b0cb 195 motor_b = 0;
eri 0:111abd91b0cb 196 }
eri 0:111abd91b0cb 197 } else { //上限のリミットスイッチが押されている場合
eri 0:111abd91b0cb 198 motor_f = 0;
eri 0:111abd91b0cb 199 motor_b = 0;
eri 0:111abd91b0cb 200 //pc.printf("%f\r\n",1.0*ecgear*Pi*ec.getCount()/resolution);
eri 0:111abd91b0cb 201 }
eri 0:111abd91b0cb 202
eri 0:111abd91b0cb 203 } else {//入力duty比が負の場合
eri 0:111abd91b0cb 204 if(switch_min == 0){ //下限のリミットスイッチが押されていない場合
eri 0:111abd91b0cb 205 if( fabs(duty) < dutylimit) { //制限値内
eri 0:111abd91b0cb 206 motor_f = 0;
eri 0:111abd91b0cb 207 motor_b = fabs(duty);
eri 0:111abd91b0cb 208 } else { //制限値超
eri 0:111abd91b0cb 209 motor_f = 0;
eri 0:111abd91b0cb 210 motor_b = dutylimit;
eri 0:111abd91b0cb 211 }
eri 0:111abd91b0cb 212 }else { //下限のリミットスイッチが押されている場合
eri 0:111abd91b0cb 213 motor_f = 0;
eri 0:111abd91b0cb 214 motor_b = 0;
eri 0:111abd91b0cb 215 }
eri 0:111abd91b0cb 216 }
eri 0:111abd91b0cb 217 }
eri 0:111abd91b0cb 218
eri 0:111abd91b0cb 219 //PID制御
eri 0:111abd91b0cb 220 void move(double target,double error) //(移動先の座標(機体内)[mm],許容誤差の絶対値[mm])
eri 0:111abd91b0cb 221 {
eri 0:111abd91b0cb 222 double pile=0,deviation,distance,distance_old=0;
eri 0:111abd91b0cb 223 int i=0;
eri 0:111abd91b0cb 224 timer.start();
eri 0:111abd91b0cb 225 while(1) {
eri 0:111abd91b0cb 226 distance = 1.0*ecgear*Pi*ec.getCount()/resolution;
eri 0:111abd91b0cb 227 deviation = target - distance;
eri 0:111abd91b0cb 228 pile += deviation;
eri 0:111abd91b0cb 229 double period = timer.read();
eri 0:111abd91b0cb 230 out(deviation * Kp - (distance - distance_old) / period * Kd + pile * Ki * period);
eri 0:111abd91b0cb 231 timer.reset();
eri 0:111abd91b0cb 232 distance_old = distance;
eri 0:111abd91b0cb 233 if (fabs(deviation) < error) {
eri 0:111abd91b0cb 234 i++;
eri 0:111abd91b0cb 235 if (i==3000) {
eri 0:111abd91b0cb 236 out(0);
eri 0:111abd91b0cb 237 timer.stop();
eri 0:111abd91b0cb 238 //pc.printf("%f\r\n",1.0*ecgear*Pi*ec.getCount()/resolution);
eri 0:111abd91b0cb 239 break;
eri 0:111abd91b0cb 240 }
eri 0:111abd91b0cb 241 } else {
eri 0:111abd91b0cb 242 i=0;
eri 0:111abd91b0cb 243 }
eri 0:111abd91b0cb 244 }
eri 0:111abd91b0cb 245 }
eri 0:111abd91b0cb 246
eri 0:111abd91b0cb 247 //X脚を上げる
eri 0:111abd91b0cb 248 void up_x()
eri 0:111abd91b0cb 249 {
eri 0:111abd91b0cb 250 air_x =1;
eri 0:111abd91b0cb 251 wait(leg_wait_time);
eri 0:111abd91b0cb 252 }
eri 0:111abd91b0cb 253
eri 0:111abd91b0cb 254 //X脚を降ろす
eri 0:111abd91b0cb 255 void down_x()
eri 0:111abd91b0cb 256 {
eri 0:111abd91b0cb 257 air_x =0;
eri 0:111abd91b0cb 258 wait(leg_wait_time);
eri 0:111abd91b0cb 259 }
eri 0:111abd91b0cb 260
eri 0:111abd91b0cb 261 //Y脚を上げる
eri 0:111abd91b0cb 262 void up_y()
eri 0:111abd91b0cb 263 {
eri 0:111abd91b0cb 264 air_y =1;
eri 0:111abd91b0cb 265 wait(leg_wait_time);
eri 0:111abd91b0cb 266 }
eri 0:111abd91b0cb 267
eri 0:111abd91b0cb 268 //Y脚を降ろす
eri 0:111abd91b0cb 269 void down_y()
eri 0:111abd91b0cb 270 {
eri 0:111abd91b0cb 271 air_y =0;
eri 0:111abd91b0cb 272 wait(leg_wait_time);
eri 0:111abd91b0cb 273 }
eri 0:111abd91b0cb 274 //X脚を移動
eri 0:111abd91b0cb 275 void move_x (double x,double error)
eri 0:111abd91b0cb 276 {
eri 0:111abd91b0cb 277 //pc.printf("x target=%f , posi=%f\r\n",x,linear_posi);
eri 0:111abd91b0cb 278 double pre_posi=1.0*ecgear*Pi*ec.getCount()/resolution;
eri 0:111abd91b0cb 279 up_x();
eri 0:111abd91b0cb 280 if(x>target_max)x=target_max;
eri 0:111abd91b0cb 281 else if(x<target_min)x=target_min;
eri 0:111abd91b0cb 282 move(x,error);
eri 0:111abd91b0cb 283 down_x();
eri 0:111abd91b0cb 284 linear_posi+=1.0*ecgear*Pi*ec.getCount()/resolution-pre_posi;
eri 0:111abd91b0cb 285 }
eri 0:111abd91b0cb 286
eri 0:111abd91b0cb 287 //Y脚を移動
eri 0:111abd91b0cb 288 void move_y (double y,double error)
eri 0:111abd91b0cb 289 {
eri 0:111abd91b0cb 290 //pc.printf("y target=%f , posi=%f\r\n",y,linear_posi);
eri 0:111abd91b0cb 291 up_y();
eri 0:111abd91b0cb 292 if(y>target_max)y=target_max;
eri 0:111abd91b0cb 293 else if(y<target_min)y=target_min;
eri 0:111abd91b0cb 294 move(y,error);
eri 0:111abd91b0cb 295 down_y();
eri 0:111abd91b0cb 296 }
eri 0:111abd91b0cb 297
eri 0:111abd91b0cb 298 int k=0;
eri 0:111abd91b0cb 299 //脚位置の初期化
eri 0:111abd91b0cb 300 void reset()
eri 0:111abd91b0cb 301 {
eri 0:111abd91b0cb 302 //servo.set_degree(0,servo_iniposi);
eri 0:111abd91b0cb 303 up_x();
eri 0:111abd91b0cb 304 while(switch_min == 0) {
eri 0:111abd91b0cb 305 out(-0.2);
eri 0:111abd91b0cb 306 k++;
eri 0:111abd91b0cb 307 if(k>5000){
eri 0:111abd91b0cb 308 servo.set_degree(0,servo_iniposi);
eri 0:111abd91b0cb 309 k=0;
eri 0:111abd91b0cb 310 }
eri 0:111abd91b0cb 311 }
eri 0:111abd91b0cb 312 //pc.printf("%d\r\n",ec.getCount());
eri 0:111abd91b0cb 313 ec.reset();
eri 0:111abd91b0cb 314 out(0);
eri 0:111abd91b0cb 315 down_x();
eri 0:111abd91b0cb 316 //pc.printf("%d\r\n",ec.getCount());
eri 0:111abd91b0cb 317 }
eri 0:111abd91b0cb 318
eri 0:111abd91b0cb 319 //旋回動作
eri 0:111abd91b0cb 320 void turn(double degree)
eri 0:111abd91b0cb 321 {
eri 0:111abd91b0cb 322 if(fabs(1.0*ecgear*Pi*ec.getCount()/resolution-rail_len*0.5)>turn_error){
eri 0:111abd91b0cb 323 move_y(rail_len*0.5,turn_error);
eri 0:111abd91b0cb 324 }
eri 0:111abd91b0cb 325 bool turn_finish=0;
eri 0:111abd91b0cb 326 while(turn_finish==0){
eri 0:111abd91b0cb 327 double degree_out=0;
eri 0:111abd91b0cb 328 if(fabs(degree)<=45){
eri 0:111abd91b0cb 329 degree_out=degree;
eri 0:111abd91b0cb 330 turn_finish=1;
eri 0:111abd91b0cb 331 }else if(degree>45){
eri 0:111abd91b0cb 332 degree_out=45;
eri 0:111abd91b0cb 333 degree-=45;
eri 0:111abd91b0cb 334 }else{
eri 0:111abd91b0cb 335 degree_out=-45;
eri 0:111abd91b0cb 336 degree+=45;
eri 0:111abd91b0cb 337 }
eri 0:111abd91b0cb 338 up_y();
eri 0:111abd91b0cb 339 servo.set_degree(0,servo_iniposi+degree_out);
eri 0:111abd91b0cb 340 wait(turn_wait_time);
eri 0:111abd91b0cb 341 down_y();
eri 0:111abd91b0cb 342 up_x();
eri 0:111abd91b0cb 343 servo.set_degree(0,servo_iniposi);
eri 0:111abd91b0cb 344 wait(turn_wait_time);
eri 0:111abd91b0cb 345 down_x();
eri 0:111abd91b0cb 346 }
eri 0:111abd91b0cb 347 }
eri 0:111abd91b0cb 348
eri 0:111abd91b0cb 349
eri 0:111abd91b0cb 350
eri 0:111abd91b0cb 351 void straight(double target){
eri 0:111abd91b0cb 352 while(1){
eri 0:111abd91b0cb 353 double xleg_posi=1.0*ecgear*Pi*ec.getCount()/resolution; //機体内のX脚位置
eri 0:111abd91b0cb 354 double dist=target-linear_posi;
eri 0:111abd91b0cb 355 pc.printf("leg=%f,dist=%f,linear_posi=%f\r\n",xleg_posi,dist,linear_posi);
eri 0:111abd91b0cb 356 if(fabs(dist)<10){
eri 0:111abd91b0cb 357 break;
eri 0:111abd91b0cb 358 }else if((dist>0 && dist<=(target_max-xleg_posi)) || (dist<0 && dist>=(target_min-xleg_posi))){
eri 0:111abd91b0cb 359 move_x(xleg_posi+dist,10);
eri 0:111abd91b0cb 360 }else if(dist<0){
eri 0:111abd91b0cb 361 pc.printf("distance < 0");
eri 0:111abd91b0cb 362 break;
eri 0:111abd91b0cb 363 }else if(dist<=(target_max-target_min-10)){
eri 0:111abd91b0cb 364 if(dist<=(rail_len*0.5-target_min-10)){
eri 0:111abd91b0cb 365 move_y(dist+xleg_posi-rail_len*0.5,10);
eri 0:111abd91b0cb 366 }else{
eri 0:111abd91b0cb 367 move_y(target_min,10);
eri 0:111abd91b0cb 368 }
eri 0:111abd91b0cb 369 }else{
eri 0:111abd91b0cb 370 if(xleg_posi<(rail_len*0.5))move_x(target_max,10);
eri 0:111abd91b0cb 371 else move_y(target_min,10);
eri 0:111abd91b0cb 372 }
eri 0:111abd91b0cb 373 }
eri 0:111abd91b0cb 374 }
eri 0:111abd91b0cb 375
eri 0:111abd91b0cb 376 void wait_MR1(){
eri 0:111abd91b0cb 377 /*while(switch_x==1){
eri 0:111abd91b0cb 378
eri 0:111abd91b0cb 379 }*/
eri 0:111abd91b0cb 380 while(switch_hand==0);
eri 0:111abd91b0cb 381 air_hand=1;
eri 0:111abd91b0cb 382 wait(1);
eri 0:111abd91b0cb 383
eri 0:111abd91b0cb 384 }