ROBOSTEP_5期 / Mbed 2 deprecated MR2_2_2

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

main.cpp@1:3ae63be5592b, 2018-12-17 (annotated)

Committer:
kageyuta
Date:
Mon Dec 17 10:22:06 2018 +0000
Revision:
1:3ae63be5592b
Parent:
0:111abd91b0cb 
a

Who changed what in which revision?

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