yuto kawamura / Mbed 2 deprecated MR2_n_1_class

l

Dependencies:   mbed

main.cpp@3:df0c9883e403, 2019-03-30 (annotated)

Committer:
eri
Date:
Sat Mar 30 01:47:42 2019 +0000
Revision:
3:df0c9883e403
Parent:
2:90bdca5d5b60
Child:
4:3f59e4f5ca30
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 3:df0c9883e403 5 #include "pin.h"
eri 3:df0c9883e403 6 #include "microinfinity.h"
eri 0:111abd91b0cb 7
eri 3:df0c9883e403 8
eri 0:111abd91b0cb 9 Timer timer;
eri 0:111abd91b0cb 10
eri 0:111abd91b0cb 11 //パラメタ
eri 3:df0c9883e403 12 double dutylimit_nomal = 0.2; // duty出力の上限値(通常時)
eri 3:df0c9883e403 13 double dutylimit_turn = 0.4; // duty出力の上限値（回転時）
eri 3:df0c9883e403 14 int ecgear = 25; //エンコーダのギアの歯数
eri 3:df0c9883e403 15 /*double Kp_r = 0.342000; //P値
eri 3:df0c9883e403 16 double Ki_r = 0.013062; //I値
eri 3:df0c9883e403 17 double Kd_r = 0.003266; //D値
eri 3:df0c9883e403 18 double Kp_l = 0.342000; //P値
eri 3:df0c9883e403 19 double Ki_l= 0.013062; //I値
eri 3:df0c9883e403 20 double Kd_l = 0.003266; //D値*/
eri 3:df0c9883e403 21 double Kp_r_n = 0.0015; //P値 通常時右
eri 3:df0c9883e403 22 double Ki_r_n = 0.001; //I値
eri 3:df0c9883e403 23 double Kd_r_n = 0.0001; //D値
eri 3:df0c9883e403 24 double Kp_l_n = 0.0015; //P値 回転時右
eri 3:df0c9883e403 25 double Ki_l_n = 0.001; //I値
eri 3:df0c9883e403 26 double Kd_l_n = 0.0001; //D値
eri 3:df0c9883e403 27 double Kp_r_t = 0.004; //P値
eri 3:df0c9883e403 28 double Ki_r_t = 0.001; //I値
eri 3:df0c9883e403 29 double Kd_r_t = 0.0001; //D値
eri 3:df0c9883e403 30 double Kp_l_t = 0.004; //P値
eri 3:df0c9883e403 31 double Ki_l_t = 0.001; //I値
eri 3:df0c9883e403 32 double Kd_l_t = 0.0001; //D値
eri 3:df0c9883e403 33 double leg_wait_time = 0.2; //脚の伸縮にかかる時間
eri 3:df0c9883e403 34 double leg_wait_time_l = 1; //長い足にかかる時間
eri 3:df0c9883e403 35 double target_max = 310; //直動機構の目標座標の上限（オーバーシュートを考慮して短めに設定）
eri 0:111abd91b0cb 36 double target_min = 20; //直動機構の目標座標の下限（オーバーシュートを考慮して短めに設定）
eri 0:111abd91b0cb 37 double servo_iniposi=105; //サーボ初期角度
eri 3:df0c9883e403 38 double error_target=10;//PIDでの許容誤差
eri 0:111abd91b0cb 39
eri 3:df0c9883e403 40 double pid_time=0.01;//PIDの割り込み時間
eri 3:df0c9883e403 41 double accel_max=0.01;//加速制限
eri 0:111abd91b0cb 42
eri 0:111abd91b0cb 43
eri 0:111abd91b0cb 44
eri 0:111abd91b0cb 45 //定数
eri 0:111abd91b0cb 46 #define Pi 3.14159265359 //円周率π
eri 0:111abd91b0cb 47
eri 0:111abd91b0cb 48 //変数
eri 0:111abd91b0cb 49 int target_num=0; //現在の目標プロット
eri 3:df0c9883e403 50 bool move_r=0,move_l=0; //
eri 3:df0c9883e403 51 double target_r_out=0,target_l_out=0;
eri 3:df0c9883e403 52 bool reach_target_r=0,reach_target_l=0;
eri 3:df0c9883e403 53 double posi_r=0,posi_l=0;
eri 3:df0c9883e403 54 double pre_out_r=0,pre_out_l=0;
eri 3:df0c9883e403 55 bool turn_mode=0;
eri 0:111abd91b0cb 56
eri 0:111abd91b0cb 57
eri 0:111abd91b0cb 58
eri 0:111abd91b0cb 59
eri 0:111abd91b0cb 60
eri 0:111abd91b0cb 61
eri 0:111abd91b0cb 62 void setup();
eri 3:df0c9883e403 63 void out_r(double);//モーターの出力
eri 3:df0c9883e403 64 void out_l(double);//モーターの出力
eri 3:df0c9883e403 65 void pid_r(double,double); //PID (移動先の座標(機体内)[mm],許容誤差の絶対値[mm])
eri 3:df0c9883e403 66 void pid_l(double,double); //PID (移動先の座標(機体内)[mm],許容誤差の絶対値[mm])
eri 3:df0c9883e403 67 void move(double,double); //(target_r,target_l)
eri 3:df0c9883e403 68 void up(int);
eri 3:df0c9883e403 69 void down(int);
eri 3:df0c9883e403 70 //void reset(); //脚位置の初期化
eri 0:111abd91b0cb 71 void turn(double); //旋回動作
eri 3:df0c9883e403 72 void turn_2(double);
eri 0:111abd91b0cb 73 void straight(double); //目的地まで直進
eri 0:111abd91b0cb 74 void wait_MR1();
eri 2:90bdca5d5b60 75 double get_dist();
eri 3:df0c9883e403 76 void output();
eri 3:df0c9883e403 77 void sanddune();
eri 3:df0c9883e403 78 void tussock();
eri 3:df0c9883e403 79
eri 3:df0c9883e403 80
eri 3:df0c9883e403 81 Ticker ticker;
eri 0:111abd91b0cb 82
eri 0:111abd91b0cb 83
eri 3:df0c9883e403 84 int main()
eri 3:df0c9883e403 85 {
eri 3:df0c9883e403 86 setup();
eri 3:df0c9883e403 87
eri 3:df0c9883e403 88 ticker.attach(&output,pid_time);
eri 3:df0c9883e403 89
eri 3:df0c9883e403 90 /*while(1){
eri 3:df0c9883e403 91 printf("%f\r\n",degree0);
eri 3:df0c9883e403 92 }*/
eri 3:df0c9883e403 93
eri 3:df0c9883e403 94 wait(3);
eri 3:df0c9883e403 95 /*down(long_r);
eri 3:df0c9883e403 96 down(long_l);
eri 3:df0c9883e403 97 wait(1);
eri 3:df0c9883e403 98 up(long_r);*/
eri 3:df0c9883e403 99
eri 3:df0c9883e403 100 //straight(2800);
eri 3:df0c9883e403 101 //turn_2(45);
eri 3:df0c9883e403 102 //move(50,50);
eri 3:df0c9883e403 103 //move(190,190);
eri 3:df0c9883e403 104 sanddune();
eri 3:df0c9883e403 105
eri 3:df0c9883e403 106 /*for(int i=0;i<15;i++){
eri 3:df0c9883e403 107 move(220,220);
eri 3:df0c9883e403 108 move(80,80);
eri 3:df0c9883e403 109 }*/
eri 3:df0c9883e403 110
eri 3:df0c9883e403 111 /*for(int i=0;i<5;i++){
eri 3:df0c9883e403 112 move(300,300);
eri 3:df0c9883e403 113 move(30,30);
eri 3:df0c9883e403 114 }
eri 3:df0c9883e403 115 turn_2(45);
eri 3:df0c9883e403 116 for(int i=0;i<2;i++){
eri 3:df0c9883e403 117 move(300,300);
eri 3:df0c9883e403 118 move(30,30);
eri 3:df0c9883e403 119 }
eri 3:df0c9883e403 120 turn_2(0);
eri 3:df0c9883e403 121
eri 3:df0c9883e403 122 for(int i=0;i<4;i++){
eri 3:df0c9883e403 123 move(300,300);
eri 3:df0c9883e403 124 move(30,30);
eri 3:df0c9883e403 125 }*/
eri 3:df0c9883e403 126
eri 3:df0c9883e403 127
eri 3:df0c9883e403 128
eri 3:df0c9883e403 129 //sanddune();
eri 3:df0c9883e403 130
eri 3:df0c9883e403 131 //move(300,300);
eri 3:df0c9883e403 132 //move(-300,-300);
eri 3:df0c9883e403 133
eri 3:df0c9883e403 134 //wait(5);
eri 3:df0c9883e403 135 /*move(300,300);
eri 3:df0c9883e403 136 move(30,30);
eri 3:df0c9883e403 137 move(300,300);
eri 3:df0c9883e403 138 move(30,30);
eri 3:df0c9883e403 139 move(300,300);
eri 3:df0c9883e403 140 move(30,30);
eri 3:df0c9883e403 141 move(300,300);
eri 3:df0c9883e403 142 move(30,30);
eri 3:df0c9883e403 143 move(300,300);
eri 3:df0c9883e403 144 move(30,30);*/
eri 3:df0c9883e403 145
eri 3:df0c9883e403 146
eri 3:df0c9883e403 147 /*
eri 3:df0c9883e403 148 wait_MR1();
eri 3:df0c9883e403 149
eri 3:df0c9883e403 150 for(int i=0; i<4; i++) {
eri 3:df0c9883e403 151 move(360,360);
eri 3:df0c9883e403 152 move(10,10);
eri 0:111abd91b0cb 153 }
eri 3:df0c9883e403 154 turn(700);
eri 3:df0c9883e403 155 sanddune();
eri 3:df0c9883e403 156
eri 3:df0c9883e403 157 move(360,360);
eri 3:df0c9883e403 158 move(10,10);
eri 3:df0c9883e403 159 move(10,10);
eri 3:df0c9883e403 160
eri 3:df0c9883e403 161 turn(-700);
eri 3:df0c9883e403 162
eri 3:df0c9883e403 163 tussock();*/
eri 0:111abd91b0cb 164 }
eri 0:111abd91b0cb 165
eri 3:df0c9883e403 166 double get_dist()
eri 3:df0c9883e403 167 {
eri 3:df0c9883e403 168 sensor.start();
eri 2:90bdca5d5b60 169 //wait_ms(50); //sensor.start()で信号を送ったあと反射波が帰ってきてから距離データが更新されるため、少し待つ必要がある。
eri 3:df0c9883e403 170 //何ループも回す場合は不要。また、時間は適当だから調整が必要。
eri 0:111abd91b0cb 171 return sensor.get_dist_cm();
eri 0:111abd91b0cb 172 }
eri 0:111abd91b0cb 173
eri 0:111abd91b0cb 174
eri 0:111abd91b0cb 175
eri 0:111abd91b0cb 176 //初期設定
eri 0:111abd91b0cb 177 void setup()
eri 0:111abd91b0cb 178 {
eri 3:df0c9883e403 179 motor_r_f.period_us(100);
eri 3:df0c9883e403 180 motor_r_b.period_us(100);
eri 3:df0c9883e403 181 motor_l_f.period_us(100);
eri 3:df0c9883e403 182 motor_l_b.period_us(100);
eri 0:111abd91b0cb 183 switch_max.mode(PullUp);
eri 0:111abd91b0cb 184 switch_min.mode(PullUp);
eri 0:111abd91b0cb 185 switch_x.mode(PullUp);
eri 0:111abd91b0cb 186 switch_y.mode(PullUp);
eri 0:111abd91b0cb 187 switch_hand.mode(PullUp);
eri 3:df0c9883e403 188 for(int i=0; i<4; i++) {
eri 3:df0c9883e403 189 air[i]=1;
eri 3:df0c9883e403 190 }
eri 3:df0c9883e403 191 air[4]=0;
eri 3:df0c9883e403 192 air[5]=0;
eri 0:111abd91b0cb 193 servo.init();
eri 2:90bdca5d5b60 194 wait_us(1000);
eri 2:90bdca5d5b60 195 servo.set_degree(0,servo_iniposi);
eri 3:df0c9883e403 196
eri 3:df0c9883e403 197 device.baud(115200);
eri 3:df0c9883e403 198 device.format(8,Serial::None,1);
eri 3:df0c9883e403 199 device.attach(dev_rx, Serial::RxIrq);
eri 3:df0c9883e403 200
eri 3:df0c9883e403 201 wait(0.05);
eri 3:df0c9883e403 202 theta0=degree0;
eri 0:111abd91b0cb 203 }
eri 3:df0c9883e403 204 int k=0;
eri 3:df0c9883e403 205 void output()
eri 3:df0c9883e403 206 {
eri 3:df0c9883e403 207 posi_r=1.0*ecgear*Pi*ec_r.getCount()/resolution;
eri 3:df0c9883e403 208 posi_l=1.0*ecgear*Pi*ec_l.getCount()/resolution;
eri 3:df0c9883e403 209
eri 3:df0c9883e403 210 if(move_r) pid_r(target_r_out,error_target);
eri 3:df0c9883e403 211 else out_r(0);
eri 3:df0c9883e403 212 if(move_l) pid_l(target_l_out,error_target);
eri 3:df0c9883e403 213 else out_l(0);
eri 3:df0c9883e403 214
eri 3:df0c9883e403 215 k++;
eri 3:df0c9883e403 216 if(k>50){
eri 3:df0c9883e403 217 printf("%f, %f, %f\r\n",posi_r,posi_l,degree0);
eri 3:df0c9883e403 218 k=0;
eri 3:df0c9883e403 219 }
eri 3:df0c9883e403 220 }
eri 3:df0c9883e403 221
eri 3:df0c9883e403 222
eri 0:111abd91b0cb 223 //モーターの出力
eri 3:df0c9883e403 224 void out_r (double duty)
eri 0:111abd91b0cb 225 {
eri 3:df0c9883e403 226 double dutylimit=0;
eri 3:df0c9883e403 227 if(turn_mode)dutylimit=dutylimit_turn;
eri 3:df0c9883e403 228 else dutylimit=dutylimit_nomal;
eri 3:df0c9883e403 229
eri 0:111abd91b0cb 230 if(duty > 0) { //入力duty比が正の場合
eri 3:df0c9883e403 231 if(duty-pre_out_r >accel_max)duty=pre_out_r+accel_max;
eri 3:df0c9883e403 232 //if(switch_max == 0) { //上限のリミットスイッチが押されていない場合
eri 0:111abd91b0cb 233 if( fabs( duty ) < dutylimit ) { //制限値内
eri 3:df0c9883e403 234 motor_r_f = fabs(duty);
eri 3:df0c9883e403 235 motor_r_b = 0;
eri 0:111abd91b0cb 236 } else { //制限値超
eri 3:df0c9883e403 237 motor_r_f = dutylimit;
eri 3:df0c9883e403 238 motor_r_b = 0;
eri 3:df0c9883e403 239 }
eri 3:df0c9883e403 240 /*} else { //上限のリミットスイッチが押されている場合
eri 3:df0c9883e403 241 motor_r_f = 0;
eri 3:df0c9883e403 242 motor_r_b = 0;
eri 3:df0c9883e403 243 }*/
eri 3:df0c9883e403 244
eri 3:df0c9883e403 245 } else {//入力duty比が負の場合
eri 3:df0c9883e403 246 if(pre_out_r-duty >accel_max)duty=pre_out_r-accel_max;
eri 3:df0c9883e403 247 //if(switch_min == 0) { //下限のリミットスイッチが押されていない場合
eri 3:df0c9883e403 248 if( fabs(duty) < dutylimit) { //制限値内
eri 3:df0c9883e403 249 motor_r_f = 0;
eri 3:df0c9883e403 250 motor_r_b = fabs(duty);
eri 3:df0c9883e403 251 } else { //制限値超
eri 3:df0c9883e403 252 motor_r_f = 0;
eri 3:df0c9883e403 253 motor_r_b = dutylimit;
eri 0:111abd91b0cb 254 }
eri 3:df0c9883e403 255 /*} else { //下限のリミットスイッチが押されている場合
eri 3:df0c9883e403 256 motor_r_f = 0;
eri 3:df0c9883e403 257 motor_r_b = 0;
eri 3:df0c9883e403 258 }*/
eri 3:df0c9883e403 259 }
eri 3:df0c9883e403 260 pre_out_r=duty;
eri 3:df0c9883e403 261 //if(k>49)printf("%f\r\n",dutylimit);
eri 3:df0c9883e403 262 }
eri 3:df0c9883e403 263
eri 3:df0c9883e403 264 void out_l (double duty)
eri 3:df0c9883e403 265 {
eri 3:df0c9883e403 266 double dutylimit=0;
eri 3:df0c9883e403 267 if(turn_mode)dutylimit=dutylimit_turn;
eri 3:df0c9883e403 268 else dutylimit=dutylimit_nomal;
eri 3:df0c9883e403 269
eri 3:df0c9883e403 270 if(duty > 0) { //入力duty比が正の場合
eri 3:df0c9883e403 271 if(duty-pre_out_l >accel_max)duty=pre_out_l+accel_max;
eri 3:df0c9883e403 272 //if(switch_max == 0) { //上限のリミットスイッチが押されていない場合
eri 3:df0c9883e403 273 if( fabs( duty ) < dutylimit ) { //制限値内
eri 3:df0c9883e403 274 motor_l_f = fabs(duty);
eri 3:df0c9883e403 275 motor_l_b = 0;
eri 3:df0c9883e403 276 } else { //制限値超
eri 3:df0c9883e403 277 motor_l_f = dutylimit;
eri 3:df0c9883e403 278 motor_l_b = 0;
eri 3:df0c9883e403 279 }
eri 3:df0c9883e403 280 /*} else { //上限のリミットスイッチが押されている場合
eri 3:df0c9883e403 281 motor_l_f = 0;
eri 3:df0c9883e403 282 motor_l_b = 0;
eri 3:df0c9883e403 283 }*/
eri 0:111abd91b0cb 284
eri 0:111abd91b0cb 285 } else {//入力duty比が負の場合
eri 3:df0c9883e403 286 if(pre_out_l-duty >accel_max)duty=pre_out_l-accel_max;
eri 3:df0c9883e403 287 //if(switch_min == 0) { //下限のリミットスイッチが押されていない場合
eri 3:df0c9883e403 288 if( fabs(duty) < dutylimit) { //制限値内
eri 3:df0c9883e403 289 motor_l_f = 0;
eri 3:df0c9883e403 290 motor_l_b = fabs(duty);
eri 3:df0c9883e403 291 } else { //制限値超
eri 3:df0c9883e403 292 motor_l_f = 0;
eri 3:df0c9883e403 293 motor_l_b = dutylimit;
eri 3:df0c9883e403 294 }
eri 3:df0c9883e403 295 /*} else { //下限のリミットスイッチが押されている場合
eri 3:df0c9883e403 296 motor_l_f = 0;
eri 3:df0c9883e403 297 motor_l_b = 0;
eri 3:df0c9883e403 298 }*/
eri 3:df0c9883e403 299 }
eri 3:df0c9883e403 300 pre_out_l=duty;
eri 3:df0c9883e403 301 }
eri 3:df0c9883e403 302
eri 3:df0c9883e403 303
eri 3:df0c9883e403 304 double pile_r=0,distance_r_old=0,pile_l=0,distance_l_old=0;
eri 3:df0c9883e403 305
eri 3:df0c9883e403 306 //PID制御
eri 3:df0c9883e403 307 void pid_r(double target,double error) //(移動先の座標(機体内)[mm],許容誤差の絶対値[mm])
eri 3:df0c9883e403 308 {
eri 3:df0c9883e403 309 double Kp_r,Ki_r,Kd_r;
eri 3:df0c9883e403 310 if(turn_mode){
eri 3:df0c9883e403 311 Kp_r=Kp_r_t;
eri 3:df0c9883e403 312 Ki_r=Ki_r_t;
eri 3:df0c9883e403 313 Kd_r=Kd_r_t;
eri 3:df0c9883e403 314 }else{
eri 3:df0c9883e403 315 Kp_r=Kp_r_n;
eri 3:df0c9883e403 316 Ki_r=Ki_r_n;
eri 3:df0c9883e403 317 Kd_r=Kd_r_n;
eri 3:df0c9883e403 318 }
eri 3:df0c9883e403 319 move_r=1;
eri 3:df0c9883e403 320 double deviation = target - posi_r;
eri 3:df0c9883e403 321 //printf("%f,%f\r\n",posi_r,deviation);
eri 3:df0c9883e403 322 pile_r += deviation;
eri 3:df0c9883e403 323 out_r(deviation * Kp_r - (posi_r - distance_r_old) / pid_time * Kd_r + pile_r * Ki_r * pid_time);
eri 3:df0c9883e403 324 distance_r_old = posi_r;
eri 3:df0c9883e403 325 if (fabs(deviation) < error) {
eri 3:df0c9883e403 326 out_r(0);
eri 3:df0c9883e403 327 pile_r=0;
eri 3:df0c9883e403 328 move_r=0;
eri 3:df0c9883e403 329 reach_target_r=1;
eri 3:df0c9883e403 330 }
eri 3:df0c9883e403 331 }
eri 3:df0c9883e403 332
eri 3:df0c9883e403 333 void pid_l(double target,double error) //(移動先の座標(機体内)[mm],許容誤差の絶対値[mm])
eri 3:df0c9883e403 334 {
eri 3:df0c9883e403 335 double Kp_l,Ki_l,Kd_l;
eri 3:df0c9883e403 336 if(turn_mode){
eri 3:df0c9883e403 337 Kp_l=Kp_l_t;
eri 3:df0c9883e403 338 Ki_l=Ki_l_t;
eri 3:df0c9883e403 339 Kd_l=Kd_l_t;
eri 3:df0c9883e403 340 }else{
eri 3:df0c9883e403 341 Kp_l=Kp_l_n;
eri 3:df0c9883e403 342 Ki_l=Ki_l_n;
eri 3:df0c9883e403 343 Kd_l=Kd_l_n;
eri 3:df0c9883e403 344 }
eri 3:df0c9883e403 345 move_l=1;
eri 3:df0c9883e403 346 double deviation = target - posi_l;
eri 3:df0c9883e403 347 pile_l += deviation;
eri 3:df0c9883e403 348 out_l(deviation * Kp_l - (posi_l - distance_l_old) / pid_time * Kd_l + pile_l * Ki_l * pid_time);
eri 3:df0c9883e403 349 distance_l_old = posi_l;
eri 3:df0c9883e403 350 if (fabs(deviation) < error) {
eri 3:df0c9883e403 351 out_l(0);
eri 3:df0c9883e403 352 pile_l=0;
eri 3:df0c9883e403 353 move_l=0;
eri 3:df0c9883e403 354 reach_target_l=1;
eri 3:df0c9883e403 355 }
eri 3:df0c9883e403 356 }
eri 3:df0c9883e403 357
eri 3:df0c9883e403 358
eri 3:df0c9883e403 359 //脚を上げる
eri 3:df0c9883e403 360 void up(int i) // j=1 待ち時間あり
eri 3:df0c9883e403 361 {
eri 3:df0c9883e403 362 air[i]=0;
eri 3:df0c9883e403 363 //if(j)wait(leg_wait_time);
eri 3:df0c9883e403 364 }
eri 3:df0c9883e403 365
eri 3:df0c9883e403 366 //脚を降ろす
eri 3:df0c9883e403 367 void down(int i)
eri 3:df0c9883e403 368 {
eri 3:df0c9883e403 369 air[i]=1;
eri 3:df0c9883e403 370 //if(j)wait(leg_wait_time);
eri 3:df0c9883e403 371 }
eri 3:df0c9883e403 372
eri 3:df0c9883e403 373
eri 3:df0c9883e403 374
eri 3:df0c9883e403 375 //移動
eri 3:df0c9883e403 376 void move (double target_r,double target_l)
eri 3:df0c9883e403 377 {
eri 3:df0c9883e403 378 //pc.printf("x target=%f , posi=%f\r\n",x,linear_posi);
eri 3:df0c9883e403 379 int upleg_r=0,upleg_l=0;
eri 3:df0c9883e403 380 if(fabs(target_r)>target_max)target_r=target_max;
eri 3:df0c9883e403 381 else if(fabs(target_r)<target_min)target_r=target_min;
eri 3:df0c9883e403 382 if(target_r>0) {
eri 3:df0c9883e403 383 if(posi_r<target_r)upleg_r=r_o_s;
eri 3:df0c9883e403 384 else upleg_r=r_i_s;
eri 3:df0c9883e403 385 } else {
eri 3:df0c9883e403 386 if(posi_r>-target_r)upleg_r=r_o_s;
eri 3:df0c9883e403 387 else upleg_r=r_i_s;
eri 3:df0c9883e403 388 }
eri 3:df0c9883e403 389
eri 3:df0c9883e403 390 if(fabs(target_l)>target_max)target_l=target_max;
eri 3:df0c9883e403 391 else if(fabs(target_l)<target_min)target_l=target_min;
eri 3:df0c9883e403 392 if(target_l>0) {
eri 3:df0c9883e403 393 if(posi_l>target_l)upleg_l=l_o_s;
eri 3:df0c9883e403 394 else upleg_l=l_i_s;
eri 3:df0c9883e403 395 } else {
eri 3:df0c9883e403 396 if(posi_l<-target_l)upleg_l=l_o_s;
eri 3:df0c9883e403 397 else upleg_l=l_i_s;
eri 3:df0c9883e403 398 }
eri 3:df0c9883e403 399 up(upleg_r);
eri 3:df0c9883e403 400 up(upleg_l);
eri 3:df0c9883e403 401
eri 3:df0c9883e403 402 wait(leg_wait_time);
eri 3:df0c9883e403 403 target_r_out=fabs(target_r);
eri 3:df0c9883e403 404 target_l_out=fabs(target_l);
eri 3:df0c9883e403 405 reach_target_r=0;
eri 3:df0c9883e403 406 reach_target_l=0;
eri 3:df0c9883e403 407 move_r=1;
eri 3:df0c9883e403 408 move_l=1;
eri 3:df0c9883e403 409 //printf("%d\r\n",reach_target_r);
eri 3:df0c9883e403 410 /*while(reach_target_r==0 || reach_target_l==0){
eri 3:df0c9883e403 411 printf("%d\r\n",reach_target_r);
eri 3:df0c9883e403 412 };*/
eri 3:df0c9883e403 413 while(reach_target_r==0 || reach_target_l==0){
eri 3:df0c9883e403 414 wait(0.01);
eri 3:df0c9883e403 415 //printf("%d\r\n",reach_target_r);
eri 3:df0c9883e403 416 }
eri 3:df0c9883e403 417 down(upleg_r);
eri 3:df0c9883e403 418 down(upleg_l);
eri 3:df0c9883e403 419 wait(leg_wait_time);
eri 3:df0c9883e403 420 reach_target_r=0;
eri 3:df0c9883e403 421 reach_target_l=0;
eri 3:df0c9883e403 422 }
eri 3:df0c9883e403 423
eri 3:df0c9883e403 424
eri 3:df0c9883e403 425
eri 3:df0c9883e403 426 void straight(double distance) //とりあえず左右で位置をずらさない前提
eri 3:df0c9883e403 427 {
eri 3:df0c9883e403 428 bool reach=0;
eri 3:df0c9883e403 429 while(1) {
eri 3:df0c9883e403 430 if(distance>0) {
eri 3:df0c9883e403 431 if(target_max-posi_r > posi_r-target_min) {
eri 3:df0c9883e403 432 if(target_max-posi_r > distance) {
eri 3:df0c9883e403 433 move(posi_r+distance,posi_r+distance);
eri 3:df0c9883e403 434 reach=1;
eri 3:df0c9883e403 435 } else {
eri 3:df0c9883e403 436 distance-=target_max-posi_r;
eri 3:df0c9883e403 437 move(target_max,target_max);
eri 3:df0c9883e403 438 }
eri 3:df0c9883e403 439 } else {
eri 3:df0c9883e403 440 if(posi_r-target_min > distance) {
eri 3:df0c9883e403 441 move(posi_r-distance,posi_r-distance);
eri 3:df0c9883e403 442 reach=1;
eri 3:df0c9883e403 443 } else {
eri 3:df0c9883e403 444 distance-=posi_r-target_min;
eri 3:df0c9883e403 445 move(target_min,target_min);
eri 3:df0c9883e403 446 }
eri 0:111abd91b0cb 447 }
eri 3:df0c9883e403 448 } else {
eri 3:df0c9883e403 449 if(target_max-posi_r > posi_r-target_min) {
eri 3:df0c9883e403 450 if(target_max-posi_r > -distance) {
eri 3:df0c9883e403 451 move(-posi_r+distance,-posi_r+distance);
eri 3:df0c9883e403 452 reach=1;
eri 3:df0c9883e403 453 } else {
eri 3:df0c9883e403 454 distance+=target_max-posi_r;
eri 3:df0c9883e403 455 move(-target_max,-target_max);
eri 3:df0c9883e403 456 }
eri 3:df0c9883e403 457 } else {
eri 3:df0c9883e403 458 if(posi_r-target_min> -distance) {
eri 3:df0c9883e403 459 move(-posi_r-distance,-posi_r-distance);
eri 3:df0c9883e403 460 reach=1;
eri 3:df0c9883e403 461 } else {
eri 3:df0c9883e403 462 distance+=posi_r-target_min;
eri 3:df0c9883e403 463 move(-target_min,-target_min);
eri 3:df0c9883e403 464 }
eri 3:df0c9883e403 465 }
eri 3:df0c9883e403 466 }
eri 3:df0c9883e403 467 if(reach==1)break;
eri 3:df0c9883e403 468 }
eri 3:df0c9883e403 469 }
eri 3:df0c9883e403 470
eri 3:df0c9883e403 471 void turn(double distance) //理論計算の元気が出ないから脚の動かす量のみ distance>0:左回転
eri 3:df0c9883e403 472 {
eri 3:df0c9883e403 473 while(1) {
eri 3:df0c9883e403 474 if(distance>0) {
eri 3:df0c9883e403 475 if(target_max-posi_r > posi_r-target_min) {
eri 3:df0c9883e403 476 if(target_max-posi_r > distance) {
eri 3:df0c9883e403 477 move(posi_r+distance,-(posi_r+distance));
eri 3:df0c9883e403 478 break;
eri 3:df0c9883e403 479 } else {
eri 3:df0c9883e403 480 distance-=target_max-posi_r;
eri 3:df0c9883e403 481 move(target_max,-target_max);
eri 3:df0c9883e403 482 }
eri 3:df0c9883e403 483 } else {
eri 3:df0c9883e403 484 if(posi_r-target_min > distance) {
eri 3:df0c9883e403 485 move(-(posi_r-distance),posi_r-distance);
eri 3:df0c9883e403 486 break;
eri 3:df0c9883e403 487 } else {
eri 3:df0c9883e403 488 distance-=posi_r-target_min;
eri 3:df0c9883e403 489 move(-target_min,target_min);
eri 3:df0c9883e403 490 }
eri 3:df0c9883e403 491 }
eri 3:df0c9883e403 492 } else {
eri 3:df0c9883e403 493 if(target_max-posi_r > posi_r-target_min) {
eri 3:df0c9883e403 494 if(target_max-posi_r > -distance) {
eri 3:df0c9883e403 495 move(-(-posi_r+distance),-posi_r+distance);
eri 3:df0c9883e403 496 break;
eri 3:df0c9883e403 497 } else {
eri 3:df0c9883e403 498 distance+=target_max-posi_r;
eri 3:df0c9883e403 499 move(target_max,-target_max);
eri 3:df0c9883e403 500 }
eri 3:df0c9883e403 501 } else {
eri 3:df0c9883e403 502 if(posi_r-target_min> -distance) {
eri 3:df0c9883e403 503 move(-posi_r-distance,-(-posi_r-distance));
eri 3:df0c9883e403 504 break;
eri 3:df0c9883e403 505 } else {
eri 3:df0c9883e403 506 distance+=posi_r-target_min;
eri 3:df0c9883e403 507 move(-target_min,target_min);
eri 3:df0c9883e403 508 }
eri 3:df0c9883e403 509 }
eri 0:111abd91b0cb 510 }
eri 0:111abd91b0cb 511 }
eri 0:111abd91b0cb 512 }
eri 0:111abd91b0cb 513
eri 3:df0c9883e403 514
eri 3:df0c9883e403 515 double a=140,b=70;
eri 3:df0c9883e403 516 void turn_2(double target){
eri 3:df0c9883e403 517 turn_mode=1;
eri 3:df0c9883e403 518 if(target-degree0>0){
eri 3:df0c9883e403 519 move(a,a);
eri 3:df0c9883e403 520 while(1){
eri 3:df0c9883e403 521 move(a+b,-(a+b));
eri 3:df0c9883e403 522 if(fabs(target-degree0)<5)break;
eri 3:df0c9883e403 523 move(a,-a);
eri 3:df0c9883e403 524 if(fabs(target-degree0)<5)break;
eri 3:df0c9883e403 525 //printf("%f\r\n",degree0);
eri 3:df0c9883e403 526 }
eri 3:df0c9883e403 527 }else{
eri 3:df0c9883e403 528 move(a,a);
eri 3:df0c9883e403 529 while(1){
eri 3:df0c9883e403 530 move(-(a+b),a+b);
eri 3:df0c9883e403 531 if(fabs(target-degree0)<5)break;
eri 3:df0c9883e403 532 move(-a,a);
eri 3:df0c9883e403 533 if(fabs(target-degree0)<5)break;
eri 3:df0c9883e403 534 //printf("%f\r\n",degree0);
eri 3:df0c9883e403 535 }
eri 3:df0c9883e403 536 }
eri 3:df0c9883e403 537 turn_mode=0;
eri 3:df0c9883e403 538 }
eri 3:df0c9883e403 539
eri 3:df0c9883e403 540 void sanddune()
eri 0:111abd91b0cb 541 {
eri 3:df0c9883e403 542 move(160,160);
eri 3:df0c9883e403 543 down(long_r);
eri 3:df0c9883e403 544 down(long_l);
eri 3:df0c9883e403 545 wait(1);
eri 3:df0c9883e403 546
eri 3:df0c9883e403 547 up(long_l);
eri 3:df0c9883e403 548 wait(leg_wait_time_l);
eri 3:df0c9883e403 549 target_r_out=50;
eri 3:df0c9883e403 550 //target_l_out=50;
eri 3:df0c9883e403 551 target_l_out=270;
eri 3:df0c9883e403 552 reach_target_r=0;
eri 3:df0c9883e403 553 reach_target_l=0;
eri 3:df0c9883e403 554 move_r=1;
eri 3:df0c9883e403 555 move_l=1;
eri 3:df0c9883e403 556 while(reach_target_r==0 || reach_target_l==0){
eri 3:df0c9883e403 557 wait(0.01);
eri 3:df0c9883e403 558 }
eri 3:df0c9883e403 559 down(long_l);
eri 3:df0c9883e403 560 //down(l_o_s);
eri 3:df0c9883e403 561 //down(r_i_s);
eri 3:df0c9883e403 562 wait(leg_wait_time_l);
eri 3:df0c9883e403 563 reach_target_r=0;
eri 3:df0c9883e403 564 reach_target_l=0;
eri 3:df0c9883e403 565
eri 3:df0c9883e403 566 up(long_r);
eri 3:df0c9883e403 567 wait(leg_wait_time_l);
eri 3:df0c9883e403 568 target_r_out=250;
eri 3:df0c9883e403 569 //target_l_out=250;
eri 3:df0c9883e403 570 target_l_out=70;
eri 3:df0c9883e403 571 reach_target_r=0;
eri 3:df0c9883e403 572 reach_target_l=0;
eri 3:df0c9883e403 573 move_r=1;
eri 3:df0c9883e403 574 move_l=1;
eri 3:df0c9883e403 575 while(reach_target_r==0 || reach_target_l==0){
eri 3:df0c9883e403 576 wait(0.01);
eri 3:df0c9883e403 577 }
eri 3:df0c9883e403 578 down(long_r);
eri 3:df0c9883e403 579 wait(leg_wait_time_l);
eri 3:df0c9883e403 580 reach_target_r=0;
eri 3:df0c9883e403 581 reach_target_l=0;
eri 3:df0c9883e403 582
eri 3:df0c9883e403 583 up(long_l);
eri 3:df0c9883e403 584 //up(l_o_s);
eri 3:df0c9883e403 585 //up(r_i_s);
eri 3:df0c9883e403 586 wait(leg_wait_time_l);
eri 3:df0c9883e403 587 target_r_out=160;
eri 3:df0c9883e403 588 target_l_out=160;
eri 3:df0c9883e403 589 reach_target_r=0;
eri 3:df0c9883e403 590 reach_target_l=0;
eri 3:df0c9883e403 591 move_r=1;
eri 3:df0c9883e403 592 move_l=1;
eri 3:df0c9883e403 593 while(reach_target_r==0 || reach_target_l==0){
eri 3:df0c9883e403 594 wait(0.01);
eri 3:df0c9883e403 595 }
eri 3:df0c9883e403 596 down(long_l);
eri 3:df0c9883e403 597 wait(leg_wait_time_l);
eri 3:df0c9883e403 598 reach_target_r=0;
eri 3:df0c9883e403 599 reach_target_l=0;
eri 3:df0c9883e403 600
eri 3:df0c9883e403 601 up(long_r);
eri 3:df0c9883e403 602 wait(leg_wait_time_l);
eri 3:df0c9883e403 603 target_r_out=220;
eri 3:df0c9883e403 604 //target_l_out=220;
eri 3:df0c9883e403 605 target_l_out=100;
eri 3:df0c9883e403 606 reach_target_r=0;
eri 3:df0c9883e403 607 reach_target_l=0;
eri 3:df0c9883e403 608 move_r=1;
eri 3:df0c9883e403 609 move_l=1;
eri 3:df0c9883e403 610 while(reach_target_r==0 || reach_target_l==0){
eri 3:df0c9883e403 611 wait(0.01);
eri 3:df0c9883e403 612 }
eri 3:df0c9883e403 613 down(long_r);
eri 3:df0c9883e403 614 //down(r_o_s);
eri 3:df0c9883e403 615 //down(l_i_s);
eri 3:df0c9883e403 616 wait(leg_wait_time_l);
eri 3:df0c9883e403 617 reach_target_r=0;
eri 3:df0c9883e403 618 reach_target_l=0;
eri 3:df0c9883e403 619
eri 3:df0c9883e403 620 up(long_l);
eri 3:df0c9883e403 621 wait(leg_wait_time_l);
eri 3:df0c9883e403 622 target_r_out=80;
eri 3:df0c9883e403 623 //target_l_out=80;
eri 3:df0c9883e403 624 target_l_out=240;
eri 3:df0c9883e403 625 reach_target_r=0;
eri 3:df0c9883e403 626 reach_target_l=0;
eri 3:df0c9883e403 627 move_r=1;
eri 3:df0c9883e403 628 move_l=1;
eri 3:df0c9883e403 629 while(reach_target_r==0 || reach_target_l==0){
eri 3:df0c9883e403 630 wait(0.01);
eri 3:df0c9883e403 631 }
eri 3:df0c9883e403 632 down(long_l);
eri 3:df0c9883e403 633 wait(leg_wait_time_l);
eri 3:df0c9883e403 634 reach_target_r=0;
eri 3:df0c9883e403 635 reach_target_l=0;
eri 3:df0c9883e403 636
eri 3:df0c9883e403 637 //up(r_o_s);
eri 3:df0c9883e403 638 //up(l_i_s);
eri 3:df0c9883e403 639 up(long_r);
eri 3:df0c9883e403 640 wait(leg_wait_time_l);
eri 3:df0c9883e403 641 target_r_out=0;
eri 3:df0c9883e403 642 target_l_out=0;
eri 3:df0c9883e403 643 reach_target_r=160;
eri 3:df0c9883e403 644 reach_target_l=160;
eri 3:df0c9883e403 645 move_r=1;
eri 3:df0c9883e403 646 move_l=1;
eri 3:df0c9883e403 647 while(reach_target_r==0 || reach_target_l==0){
eri 3:df0c9883e403 648 wait(0.01);
eri 3:df0c9883e403 649 }
eri 3:df0c9883e403 650 down(long_r);
eri 3:df0c9883e403 651 wait(leg_wait_time_l);
eri 3:df0c9883e403 652 reach_target_r=0;
eri 3:df0c9883e403 653 reach_target_l=0;
eri 3:df0c9883e403 654 }
eri 3:df0c9883e403 655
eri 3:df0c9883e403 656 void tussock()
eri 3:df0c9883e403 657 {
eri 3:df0c9883e403 658 for(int i=0; i<3; i++) {
eri 3:df0c9883e403 659 up(long_l);
eri 3:df0c9883e403 660 wait(leg_wait_time);
eri 3:df0c9883e403 661 target_r_out=360;
eri 3:df0c9883e403 662 target_l_out=360;
eri 3:df0c9883e403 663 while(reach_target_r==0 || reach_target_l==0);
eri 3:df0c9883e403 664 down(long_l);
eri 3:df0c9883e403 665 wait(leg_wait_time);
eri 3:df0c9883e403 666 reach_target_r=0;
eri 3:df0c9883e403 667 reach_target_l=0;
eri 3:df0c9883e403 668
eri 3:df0c9883e403 669 up(long_r);
eri 3:df0c9883e403 670 wait(leg_wait_time);
eri 3:df0c9883e403 671 target_r_out=10;
eri 3:df0c9883e403 672 target_l_out=10;
eri 3:df0c9883e403 673 while(reach_target_r==0 || reach_target_l==0);
eri 3:df0c9883e403 674 down(long_r);
eri 3:df0c9883e403 675 wait(leg_wait_time);
eri 3:df0c9883e403 676 reach_target_r=0;
eri 3:df0c9883e403 677 reach_target_l=0;
eri 0:111abd91b0cb 678 }
eri 0:111abd91b0cb 679 }
eri 0:111abd91b0cb 680
eri 3:df0c9883e403 681 /*int k=0;
eri 0:111abd91b0cb 682 //脚位置の初期化
eri 0:111abd91b0cb 683 void reset()
eri 0:111abd91b0cb 684 {
eri 0:111abd91b0cb 685 up_x();
eri 0:111abd91b0cb 686 while(switch_min == 0) {
eri 0:111abd91b0cb 687 out(-0.2);
eri 0:111abd91b0cb 688 k++;
eri 3:df0c9883e403 689 if(k>5000) {
eri 0:111abd91b0cb 690 servo.set_degree(0,servo_iniposi);
eri 0:111abd91b0cb 691 k=0;
eri 0:111abd91b0cb 692 }
eri 0:111abd91b0cb 693 }
eri 0:111abd91b0cb 694 //pc.printf("%d\r\n",ec.getCount());
eri 0:111abd91b0cb 695 ec.reset();
eri 0:111abd91b0cb 696 out(0);
eri 0:111abd91b0cb 697 down_x();
eri 0:111abd91b0cb 698 //pc.printf("%d\r\n",ec.getCount());
eri 3:df0c9883e403 699 }*/
eri 0:111abd91b0cb 700
eri 3:df0c9883e403 701
eri 0:111abd91b0cb 702
eri 0:111abd91b0cb 703
eri 0:111abd91b0cb 704
eri 3:df0c9883e403 705 /*void straight(double target)
eri 3:df0c9883e403 706 {
eri 3:df0c9883e403 707 while(1) {
eri 0:111abd91b0cb 708 double xleg_posi=1.0*ecgear*Pi*ec.getCount()/resolution; //機体内のX脚位置
eri 0:111abd91b0cb 709 double dist=target-linear_posi;
eri 0:111abd91b0cb 710 pc.printf("leg=%f,dist=%f,linear_posi=%f\r\n",xleg_posi,dist,linear_posi);
eri 3:df0c9883e403 711 if(fabs(dist)<10) {
eri 3:df0c9883e403 712 break;
eri 3:df0c9883e403 713 } else if((dist>0 && dist<=(target_max-xleg_posi)) || (dist<0 && dist>=(target_min-xleg_posi))) {
eri 0:111abd91b0cb 714 move_x(xleg_posi+dist,10);
eri 3:df0c9883e403 715 } else if(dist<0) {
eri 0:111abd91b0cb 716 pc.printf("distance < 0");
eri 0:111abd91b0cb 717 break;
eri 3:df0c9883e403 718 } else if(dist<=(target_max-target_min-10)) {
eri 3:df0c9883e403 719 if(dist<=(rail_len*0.5-target_min-10)) {
eri 0:111abd91b0cb 720 move_y(dist+xleg_posi-rail_len*0.5,10);
eri 3:df0c9883e403 721 } else {
eri 0:111abd91b0cb 722 move_y(target_min,10);
eri 0:111abd91b0cb 723 }
eri 3:df0c9883e403 724 } else {
eri 0:111abd91b0cb 725 if(xleg_posi<(rail_len*0.5))move_x(target_max,10);
eri 0:111abd91b0cb 726 else move_y(target_min,10);
eri 0:111abd91b0cb 727 }
eri 0:111abd91b0cb 728 }
eri 3:df0c9883e403 729 }*/
eri 0:111abd91b0cb 730
eri 3:df0c9883e403 731 void wait_MR1()
eri 3:df0c9883e403 732 {
eri 3:df0c9883e403 733 while(switch_x==1) {
eri 3:df0c9883e403 734
kageyuta 1:3ae63be5592b 735 }
kageyuta 1:3ae63be5592b 736 //while(switch_hand==0);
eri 3:df0c9883e403 737 //air_hand=1;
eri 0:111abd91b0cb 738 wait(1);
eri 3:df0c9883e403 739
eri 0:111abd91b0cb 740 }