ROBOSTEP_LIBRARY / Mbed 2 deprecated F3RC

F3RC4班　自動機プログラム　by巨泉 速度制御ユニット使用の2輪(活かせなかった)、測定輪エンコーダ、MicroInfinityを用いている XY座標を読むことには成功したが、まともに制御できなかったのでノーカン()

Dependencies:   CruizCore_R1370P EC delta enc_1multi mbed

Fork of F3RC915 by 春ロボ1班(元F3RC4班+)

main.cpp@5:0160b73f3d9e, 2018-09-12 (annotated)

Committer:
aoikoizumi
Date:
Wed Sep 12 05:35:59 2018 +0000
Revision:
5:0160b73f3d9e
Parent:
4:e3d739bf09b4
Child:
6:7ec6c3d3a30a
9/12

; ??

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aoikoizumi 0:29c024d6882f 1
aoikoizumi 0:29c024d6882f 2 #include "mbed.h"
aoikoizumi 0:29c024d6882f 3 #include "SpeedController.h"
aoikoizumi 0:29c024d6882f 4 #include "EC.h"
aoikoizumi 0:29c024d6882f 5 #include "R1370P.h"
aoikoizumi 0:29c024d6882f 6 #include "enc_1multi.h"
aoikoizumi 0:29c024d6882f 7 #define BASIC_SPEED 30 //モーターはこの角速度で駆動させる
aoikoizumi 0:29c024d6882f 8
aoikoizumi 0:29c024d6882f 9 SpeedControl motorR(PB_13,PA_10,NC,500,0.05,PB_10,PB_1); //right enc migi ue
aoikoizumi 4:e3d739bf09b4 10 SpeedControl motorL(PB_5,PB_3,NC,500,0.05,PA_5,PA_7); //left enc hidari sita //ok
aoikoizumi 0:29c024d6882f 11 Ec EC1(PB_4,PA_8,NC,300,0.05); //center enc
aoikoizumi 0:29c024d6882f 12 Ticker motor_tick; //角速度計算用ticker
aoikoizumi 0:29c024d6882f 13 Ticker ticker;//for enc
aoikoizumi 0:29c024d6882f 14
aoikoizumi 0:29c024d6882f 15 Serial pc(USBTX, USBRX); // tx, rx //PC USB
aoikoizumi 0:29c024d6882f 16 R1370P gyro(PC_6,PC_7);
aoikoizumi 0:29c024d6882f 17
aoikoizumi 0:29c024d6882f 18
aoikoizumi 0:29c024d6882f 19 void calOmega() //角速度計算関数
aoikoizumi 0:29c024d6882f 20 {
aoikoizumi 0:29c024d6882f 21 motorR.CalOmega();
aoikoizumi 0:29c024d6882f 22 motorL.CalOmega();
aoikoizumi 0:29c024d6882f 23 EC1.CalOmega();
aoikoizumi 0:29c024d6882f 24 }
aoikoizumi 0:29c024d6882f 25
aoikoizumi 1:1817e9243a6e 26 //DigitalIn button(USER_BUTTON);
aoikoizumi 1:1817e9243a6e 27 DigitalIn reset_f(PC_1,PullUp);
aoikoizumi 1:1817e9243a6e 28 DigitalIn reset_a(PA_4,PullUp);
aoikoizumi 0:29c024d6882f 29
aoikoizumi 0:29c024d6882f 30 PwmOut servo(PB_14);//servo
aoikoizumi 0:29c024d6882f 31 PwmOut motor_f(PC_9);
aoikoizumi 0:29c024d6882f 32 PwmOut motor_b(PB_9);//arm
aoikoizumi 0:29c024d6882f 33 DigitalOut denjiben(PC_0);//dennjibenn
aoikoizumi 0:29c024d6882f 34
aoikoizumi 0:29c024d6882f 35
aoikoizumi 0:29c024d6882f 36 double new_dist=0;
aoikoizumi 0:29c024d6882f 37 double old_dist=0;
aoikoizumi 0:29c024d6882f 38 double d_dist=0;
aoikoizumi 5:0160b73f3d9e 39 double x;
aoikoizumi 5:0160b73f3d9e 40 double y;
aoikoizumi 5:0160b73f3d9e 41 double asari_x=900;
aoikoizumi 5:0160b73f3d9e 42 double asari_y=2500;//asariwo toru tekisetsuna zahyouwo kaeraremasu
aoikoizumi 5:0160b73f3d9e 43 double goal_x=1120;
aoikoizumi 5:0160b73f3d9e 44 double goal_y=1700;
aoikoizumi 5:0160b73f3d9e 45 double start_x=185;
aoikoizumi 5:0160b73f3d9e 46 double start_y=150;
aoikoizumi 0:29c024d6882f 47 Timer t;
aoikoizumi 4:e3d739bf09b4 48 int i=1;
aoikoizumi 0:29c024d6882f 49
aoikoizumi 0:29c024d6882f 50 int kai=0;//printf kansuu
aoikoizumi 0:29c024d6882f 51 double target_R=0,target_L=0;
aoikoizumi 0:29c024d6882f 52
aoikoizumi 0:29c024d6882f 53
aoikoizumi 0:29c024d6882f 54 double angle; //変数宣言
aoikoizumi 0:29c024d6882f 55
aoikoizumi 0:29c024d6882f 56
aoikoizumi 1:1817e9243a6e 57 void tgt(double r,double l)//void de sika tsukawanaino ha mottainai kimosuru
aoikoizumi 0:29c024d6882f 58 {
aoikoizumi 0:29c024d6882f 59 target_R=BASIC_SPEED*r;
aoikoizumi 0:29c024d6882f 60 target_L=BASIC_SPEED*l;
aoikoizumi 0:29c024d6882f 61 }
aoikoizumi 0:29c024d6882f 62
aoikoizumi 0:29c024d6882f 63
aoikoizumi 1:1817e9243a6e 64 //printf wo dousa kakunin sidai kesou toha omotte imasu
aoikoizumi 1:1817e9243a6e 65 void susumu_y(double ay,double by,double goaly)//y zahyou wo motiita susumikata subete
aoikoizumi 0:29c024d6882f 66 {
aoikoizumi 1:1817e9243a6e 67 //pc.printf("ay=%f by=%f y=%f goaly=%f\r\n",ay,by,y,goaly);
aoikoizumi 0:29c024d6882f 68 if(y<goaly-50&&ay>=0) {
aoikoizumi 0:29c024d6882f 69 t.start();
aoikoizumi 0:29c024d6882f 70 pc.printf("R=%f L=%f\r\n",target_R,target_L);
aoikoizumi 0:29c024d6882f 71 if(t.read_ms()<100) {
aoikoizumi 0:29c024d6882f 72 pc.printf("t=%f",t.read());
aoikoizumi 0:29c024d6882f 73 tgt(ay/3,by/3);
aoikoizumi 1:1817e9243a6e 74 } else if(t.read_ms()>=100&&t.read_ms()<200) {
aoikoizumi 0:29c024d6882f 75 pc.printf("t=%f",t.read());
aoikoizumi 0:29c024d6882f 76 tgt(ay*2/3,by*2/3);
aoikoizumi 0:29c024d6882f 77 } else {
aoikoizumi 0:29c024d6882f 78 tgt(ay,by);
aoikoizumi 0:29c024d6882f 79 t.stop();
aoikoizumi 0:29c024d6882f 80 pc.printf("R=%f L=%f",target_R,target_L);
aoikoizumi 0:29c024d6882f 81 }
aoikoizumi 1:1817e9243a6e 82 } else if(y<goaly&&y>=goaly-50&&ay>=0) {
aoikoizumi 0:29c024d6882f 83 tgt(ay/3,by/3);
aoikoizumi 0:29c024d6882f 84
aoikoizumi 0:29c024d6882f 85
aoikoizumi 1:1817e9243a6e 86 } else if(y>goaly+50&&ay<0) {
aoikoizumi 0:29c024d6882f 87 t.start();
aoikoizumi 0:29c024d6882f 88 pc.printf("R=%f L=%f\r\n",target_R,target_L);
aoikoizumi 0:29c024d6882f 89 if(t.read_ms()<100) {
aoikoizumi 0:29c024d6882f 90 pc.printf("t=%f",t.read());
aoikoizumi 0:29c024d6882f 91 tgt(ay/3,by/3);
aoikoizumi 1:1817e9243a6e 92 } else if(t.read_ms()>=100&&t.read_ms()<200) {
aoikoizumi 0:29c024d6882f 93 pc.printf("t=%f",t.read());
aoikoizumi 0:29c024d6882f 94 tgt(ay*2/3,by*2/3);
aoikoizumi 0:29c024d6882f 95 } else {
aoikoizumi 0:29c024d6882f 96 tgt(ay,by);
aoikoizumi 0:29c024d6882f 97 t.stop();
aoikoizumi 0:29c024d6882f 98 pc.printf("R=%f L=%f",target_R,target_L);
aoikoizumi 0:29c024d6882f 99 }
aoikoizumi 1:1817e9243a6e 100 } else if(y>goaly&&y<=goaly+50&&ay<0) {
aoikoizumi 0:29c024d6882f 101 tgt(ay/3,by/3);
aoikoizumi 0:29c024d6882f 102 } else {
aoikoizumi 0:29c024d6882f 103 i++;
aoikoizumi 0:29c024d6882f 104 t.reset();
aoikoizumi 1:1817e9243a6e 105 pc.printf("owari%d\r\n",i);
aoikoizumi 0:29c024d6882f 106 }
aoikoizumi 0:29c024d6882f 107 }
aoikoizumi 0:29c024d6882f 108
aoikoizumi 0:29c024d6882f 109
aoikoizumi 0:29c024d6882f 110
aoikoizumi 1:1817e9243a6e 111 void susumu_xl(double axl,double bxl,double goalxl)//hidarikara mokuhyoutenn ni toutatsu surutameno dousa
aoikoizumi 0:29c024d6882f 112 {
aoikoizumi 0:29c024d6882f 113
aoikoizumi 1:1817e9243a6e 114 if(x<goalxl-50) {
aoikoizumi 0:29c024d6882f 115 t.start();
aoikoizumi 0:29c024d6882f 116 pc.printf("R=%f L=%f\r\n",target_R,target_L);
aoikoizumi 0:29c024d6882f 117 if(t.read_ms()<100) {
aoikoizumi 0:29c024d6882f 118 pc.printf("t=%f",t.read());
aoikoizumi 1:1817e9243a6e 119 tgt(axl/3,bxl/3);
aoikoizumi 1:1817e9243a6e 120 } else if(t.read_ms()>=100&&t.read_ms()<200) {
aoikoizumi 0:29c024d6882f 121 pc.printf("t=%f",t.read());
aoikoizumi 1:1817e9243a6e 122 tgt(axl*2/3,bxl*2/3);
aoikoizumi 0:29c024d6882f 123 } else {
aoikoizumi 1:1817e9243a6e 124 tgt(axl,bxl);
aoikoizumi 0:29c024d6882f 125 t.stop();
aoikoizumi 0:29c024d6882f 126 pc.printf("R=%f L=%f",target_R,target_L);
aoikoizumi 0:29c024d6882f 127 }
aoikoizumi 1:1817e9243a6e 128 } else if(x<goalxl&&x>=goalxl-50) {
aoikoizumi 1:1817e9243a6e 129 tgt(axl/3,bxl/3);
aoikoizumi 1:1817e9243a6e 130
aoikoizumi 1:1817e9243a6e 131 } else {
aoikoizumi 1:1817e9243a6e 132 t.reset();
aoikoizumi 1:1817e9243a6e 133 i++;
aoikoizumi 1:1817e9243a6e 134 pc.printf("owari\r\n");
aoikoizumi 0:29c024d6882f 135 }
aoikoizumi 1:1817e9243a6e 136 }
aoikoizumi 0:29c024d6882f 137
aoikoizumi 1:1817e9243a6e 138 void susumu_xr(double axr,double bxr,double goalxr)//migikara mokuhyoutenn ni toutatsu surutameno dousa
aoikoizumi 1:1817e9243a6e 139 {
aoikoizumi 1:1817e9243a6e 140
aoikoizumi 1:1817e9243a6e 141 if(x>goalxr+50) {
aoikoizumi 0:29c024d6882f 142 t.start();
aoikoizumi 0:29c024d6882f 143 pc.printf("R=%f L=%f\r\n",target_R,target_L);
aoikoizumi 0:29c024d6882f 144 if(t.read_ms()<100) {
aoikoizumi 0:29c024d6882f 145 pc.printf("t=%f",t.read());
aoikoizumi 1:1817e9243a6e 146 tgt(axr/3,bxr/3);
aoikoizumi 1:1817e9243a6e 147 } else if(t.read_ms()>=100&&t.read_ms()<200) {
aoikoizumi 0:29c024d6882f 148 pc.printf("t=%f",t.read());
aoikoizumi 1:1817e9243a6e 149 tgt(axr*2/3,bxr*2/3);
aoikoizumi 0:29c024d6882f 150 } else {
aoikoizumi 1:1817e9243a6e 151 tgt(axr,bxr);
aoikoizumi 0:29c024d6882f 152 t.stop();
aoikoizumi 0:29c024d6882f 153 pc.printf("R=%f L=%f",target_R,target_L);
aoikoizumi 0:29c024d6882f 154 }
aoikoizumi 1:1817e9243a6e 155 } else if(x>goalxr&&x<=goalxr+50) {
aoikoizumi 1:1817e9243a6e 156 tgt(axr/3,bxr/3);
aoikoizumi 0:29c024d6882f 157
aoikoizumi 0:29c024d6882f 158 } else {
aoikoizumi 1:1817e9243a6e 159 t.reset();
aoikoizumi 0:29c024d6882f 160 i++;
aoikoizumi 0:29c024d6882f 161 pc.printf("owari\r\n");
aoikoizumi 0:29c024d6882f 162 }
aoikoizumi 0:29c024d6882f 163 }
aoikoizumi 0:29c024d6882f 164
aoikoizumi 1:1817e9243a6e 165 void susumu_ang(double a,double b,double goal)//kakudo
aoikoizumi 0:29c024d6882f 166 {
aoikoizumi 0:29c024d6882f 167 if(x<angle-5&&a>b) {//usetsu
aoikoizumi 0:29c024d6882f 168 t.start();
aoikoizumi 0:29c024d6882f 169 pc.printf("R=%f L=%f\r\n",target_R,target_L);
aoikoizumi 0:29c024d6882f 170 if(t.read_ms()<100) {
aoikoizumi 0:29c024d6882f 171 pc.printf("t=%f",t.read());
aoikoizumi 0:29c024d6882f 172 tgt(a/3,b/3);
aoikoizumi 1:1817e9243a6e 173 } else if(t.read_ms()>=100&&t.read_ms()<200) {
aoikoizumi 0:29c024d6882f 174 pc.printf("t=%f",t.read());
aoikoizumi 0:29c024d6882f 175 tgt(a*2/3,b*2/3);
aoikoizumi 0:29c024d6882f 176 } else {
aoikoizumi 0:29c024d6882f 177 tgt(a,b);
aoikoizumi 0:29c024d6882f 178 t.stop();
aoikoizumi 0:29c024d6882f 179 pc.printf("R=%f L=%f",target_R,target_L);
aoikoizumi 0:29c024d6882f 180 }
aoikoizumi 1:1817e9243a6e 181 } else if(angle<goal&&angle>=goal-5&&a>b) {
aoikoizumi 0:29c024d6882f 182 tgt(a/3,b/3);
aoikoizumi 0:29c024d6882f 183
aoikoizumi 1:1817e9243a6e 184 } else if(angle>goal+5&&a<b) { //sasetsu
aoikoizumi 0:29c024d6882f 185 t.start();
aoikoizumi 0:29c024d6882f 186 pc.printf("R=%f L=%f\r\n",target_R,target_L);
aoikoizumi 0:29c024d6882f 187 if(t.read_ms()<100) {
aoikoizumi 0:29c024d6882f 188 pc.printf("t=%f",t.read());
aoikoizumi 0:29c024d6882f 189 tgt(a/3,b/3);
aoikoizumi 1:1817e9243a6e 190 } else if(t.read_ms()>=100&&t.read_ms()<200) {
aoikoizumi 0:29c024d6882f 191 pc.printf("t=%f",t.read());
aoikoizumi 0:29c024d6882f 192 tgt(a*2/3,b*2/3);
aoikoizumi 0:29c024d6882f 193 } else {
aoikoizumi 0:29c024d6882f 194 tgt(a,b);
aoikoizumi 0:29c024d6882f 195 t.stop();
aoikoizumi 0:29c024d6882f 196 pc.printf("R=%f L=%f",target_R,target_L);
aoikoizumi 0:29c024d6882f 197 }
aoikoizumi 1:1817e9243a6e 198 } else if(angle>goal&&angle<=goal+5&&a<b) {
aoikoizumi 0:29c024d6882f 199 tgt(a/3,b/3);
aoikoizumi 0:29c024d6882f 200
aoikoizumi 0:29c024d6882f 201 } else {
aoikoizumi 0:29c024d6882f 202 i++;
aoikoizumi 0:29c024d6882f 203 t.reset();
aoikoizumi 0:29c024d6882f 204 pc.printf("reset\r\n");
aoikoizumi 0:29c024d6882f 205 }
aoikoizumi 0:29c024d6882f 206 }
aoikoizumi 0:29c024d6882f 207
aoikoizumi 0:29c024d6882f 208
aoikoizumi 0:29c024d6882f 209
aoikoizumi 0:29c024d6882f 210
aoikoizumi 0:29c024d6882f 211
aoikoizumi 0:29c024d6882f 212 int main(void)
aoikoizumi 0:29c024d6882f 213 {
aoikoizumi 5:0160b73f3d9e 214
aoikoizumi 5:0160b73f3d9e 215 gyro.initialize(); //main関数の最初に一度だけ実行
aoikoizumi 5:0160b73f3d9e 216 gyro.acc_offset(); //やってもやらなくてもいい
aoikoizumi 5:0160b73f3d9e 217
aoikoizumi 0:29c024d6882f 218 printf("start\r\n");
aoikoizumi 0:29c024d6882f 219 motor_tick.attach(&calOmega,0.05);
aoikoizumi 0:29c024d6882f 220 motorR.setDOconstant(34.1);
aoikoizumi 0:29c024d6882f 221 motorL.setDOconstant(30);//c
aoikoizumi 0:29c024d6882f 222 motorR.setPDparam(0,0);
aoikoizumi 1:1817e9243a6e 223 motorR.setPDparam(0,0);//pd//akirameta
aoikoizumi 0:29c024d6882f 224
aoikoizumi 0:29c024d6882f 225
aoikoizumi 0:29c024d6882f 226 EC1.setDiameter_mm(50);//sokuteirinnhannkei
aoikoizumi 0:29c024d6882f 227 double getDistance_mm();
aoikoizumi 0:29c024d6882f 228 //int EC1.getCount()=0;
aoikoizumi 5:0160b73f3d9e 229 void reset ();
aoikoizumi 0:29c024d6882f 230 EC1.reset();
aoikoizumi 0:29c024d6882f 231
aoikoizumi 0:29c024d6882f 232
aoikoizumi 1:1817e9243a6e 233 servo.period_ms(20);
aoikoizumi 0:29c024d6882f 234
aoikoizumi 0:29c024d6882f 235 motor_f.period_ms(30);
aoikoizumi 0:29c024d6882f 236 motor_b.period_ms(30);//arm
aoikoizumi 5:0160b73f3d9e 237 x=start_x;
aoikoizumi 5:0160b73f3d9e 238 y=start_y;//start point//keisann wo sinaosu hitsuyouga arimasu
aoikoizumi 0:29c024d6882f 239
aoikoizumi 0:29c024d6882f 240 while(1) {
aoikoizumi 0:29c024d6882f 241 // motorR.turnF(0.3);
aoikoizumi 0:29c024d6882f 242 //motorL.turnF(0.3);//for debug
aoikoizumi 0:29c024d6882f 243 motorR.Sc(target_R);
aoikoizumi 0:29c024d6882f 244 motorL.Sc(target_L);//target_R,target_Lの値を変えることで直進・後退・右回転・左回転を行う
aoikoizumi 5:0160b73f3d9e 245 angle=gyro.getAngle()*(-1); //角度の値を受け取る
aoikoizumi 0:29c024d6882f 246
aoikoizumi 0:29c024d6882f 247 EC1.getDistance_mm();
aoikoizumi 0:29c024d6882f 248 // EC1.CalOmega();
aoikoizumi 0:29c024d6882f 249
aoikoizumi 0:29c024d6882f 250
aoikoizumi 0:29c024d6882f 251 if(target_R==0) motorR.stop();
aoikoizumi 0:29c024d6882f 252 else motorR.Sc(target_R);
aoikoizumi 0:29c024d6882f 253 if(target_L==0) motorL.stop();
aoikoizumi 0:29c024d6882f 254 else motorL.Sc(target_L);
aoikoizumi 0:29c024d6882f 255
aoikoizumi 0:29c024d6882f 256 double d_x=d_dist*sin(angle);
aoikoizumi 0:29c024d6882f 257 double d_y=d_dist*cos(angle);
aoikoizumi 0:29c024d6882f 258 x=x+d_x;
aoikoizumi 0:29c024d6882f 259 y=y+d_y;
aoikoizumi 0:29c024d6882f 260
aoikoizumi 0:29c024d6882f 261
aoikoizumi 0:29c024d6882f 262
aoikoizumi 0:29c024d6882f 263 if(kai>=3) {
aoikoizumi 0:29c024d6882f 264 new_dist=EC1.getDistance_mm();
aoikoizumi 0:29c024d6882f 265 d_dist=new_dist-old_dist;
aoikoizumi 0:29c024d6882f 266 old_dist=new_dist;
aoikoizumi 0:29c024d6882f 267
aoikoizumi 0:29c024d6882f 268 //double d_x=d_dist*sin(angle);;
aoikoizumi 0:29c024d6882f 269 //double d_y=d_dist*cos(angle);;
aoikoizumi 0:29c024d6882f 270 //x=x+d_x;
aoikoizumi 0:29c024d6882f 271 //y=y+d_y;
aoikoizumi 0:29c024d6882f 272
aoikoizumi 0:29c024d6882f 273 pc.printf("R=%f L=%f",target_R,target_L);
aoikoizumi 0:29c024d6882f 274 // pc.printf("omg_R=%f omg_L=%f \r\n",motorR.getOmega(),motorL.getOmega());
aoikoizumi 0:29c024d6882f 275 pc.printf("i=%d\r\n",i);
aoikoizumi 0:29c024d6882f 276 //pc.printf("EC1=%f",EC1.getDistance_mm(),EC1.getCount());
aoikoizumi 0:29c024d6882f 277 pc.printf("x=%f y=%f",x,y);
aoikoizumi 4:e3d739bf09b4 278 pc.printf("angle=%f\r\n",angle);
aoikoizumi 0:29c024d6882f 279 //機体の進む方向、右モーターの角速度、左モーターの角速度を表示
aoikoizumi 0:29c024d6882f 280 kai=0;
aoikoizumi 0:29c024d6882f 281 }
aoikoizumi 0:29c024d6882f 282 kai++;
aoikoizumi 0:29c024d6882f 283 if(i==0) {
aoikoizumi 4:e3d739bf09b4 284 /*if(reset_f.read()==1) {
aoikoizumi 1:1817e9243a6e 285 wait(0.05);
aoikoizumi 1:1817e9243a6e 286 if(reset_f.read()==1) {
aoikoizumi 4:e3d739bf09b4 287 */ denjiben=0;
aoikoizumi 4:e3d739bf09b4 288 i++;
aoikoizumi 4:e3d739bf09b4 289 // }
aoikoizumi 4:e3d739bf09b4 290 //}
aoikoizumi 4:e3d739bf09b4 291 if(reset_a.read()==1) {
aoikoizumi 4:e3d739bf09b4 292 wait(0.05);
aoikoizumi 2:3176040a4777 293 if(reset_a.read()==1) {
aoikoizumi 4:e3d739bf09b4 294 denjiben=0;
aoikoizumi 4:e3d739bf09b4 295 x=70;
aoikoizumi 4:e3d739bf09b4 296 y=2500;
aoikoizumi 4:e3d739bf09b4 297 i=14;
aoikoizumi 2:3176040a4777 298 }
aoikoizumi 5:0160b73f3d9e 299 }//x,y
aoikoizumi 4:e3d739bf09b4 300 }
aoikoizumi 4:e3d739bf09b4 301 if(i==1) {
aoikoizumi 5:0160b73f3d9e 302 susumu_y(1,1,start_x+178);//x,x+178
aoikoizumi 4:e3d739bf09b4 303 }
aoikoizumi 4:e3d739bf09b4 304 if(i==2) {
aoikoizumi 5:0160b73f3d9e 305 susumu_ang(1/2,1,45);//x+121.5,x+471.5
aoikoizumi 4:e3d739bf09b4 306 }
aoikoizumi 4:e3d739bf09b4 307 if(i==3) {
aoikoizumi 5:0160b73f3d9e 308 susumu_xl(1,1,728.5);//728.5,1078.5
aoikoizumi 4:e3d739bf09b4 309 }
aoikoizumi 4:e3d739bf09b4 310 if(i==4) {
aoikoizumi 5:0160b73f3d9e 311 susumu_ang(1,1/2,0);//850,1372
aoikoizumi 4:e3d739bf09b4 312 }
aoikoizumi 4:e3d739bf09b4 313 if(i==5) {
aoikoizumi 5:0160b73f3d9e 314 susumu_y(1,1,1700);//850,1700
aoikoizumi 4:e3d739bf09b4 315 }
aoikoizumi 4:e3d739bf09b4 316 if(i==6) {
aoikoizumi 4:e3d739bf09b4 317 motorR.stop();
aoikoizumi 4:e3d739bf09b4 318 motorL.stop();
aoikoizumi 4:e3d739bf09b4 319 t.start();
aoikoizumi 4:e3d739bf09b4 320 if(t<1) {
aoikoizumi 4:e3d739bf09b4 321 motor_f=0.82;
aoikoizumi 4:e3d739bf09b4 322 motor_b=0;
aoikoizumi 4:e3d739bf09b4 323 } else {
aoikoizumi 4:e3d739bf09b4 324 motor_f=0;
aoikoizumi 4:e3d739bf09b4 325 motor_b=0;
aoikoizumi 4:e3d739bf09b4 326 printf("finish");
aoikoizumi 4:e3d739bf09b4 327 t.reset();
aoikoizumi 4:e3d739bf09b4 328 i++;
aoikoizumi 3:7bd1afb46094 329 }
aoikoizumi 4:e3d739bf09b4 330 }//gatiasari
aoikoizumi 3:7bd1afb46094 331
aoikoizumi 3:7bd1afb46094 332
aoikoizumi 4:e3d739bf09b4 333 if(i==7) {
aoikoizumi 4:e3d739bf09b4 334 t.start();
aoikoizumi 4:e3d739bf09b4 335 if(t<1) {
aoikoizumi 4:e3d739bf09b4 336 motor_f=0;
aoikoizumi 4:e3d739bf09b4 337 motor_b=0.82;
aoikoizumi 4:e3d739bf09b4 338 } else {
aoikoizumi 4:e3d739bf09b4 339 motor_f=0;
aoikoizumi 4:e3d739bf09b4 340 motor_b=0;
aoikoizumi 4:e3d739bf09b4 341 printf("finish");
aoikoizumi 4:e3d739bf09b4 342 t.reset();
aoikoizumi 3:7bd1afb46094 343 }
aoikoizumi 4:e3d739bf09b4 344 if(angle>=-89) {
aoikoizumi 4:e3d739bf09b4 345 target_R=BASIC_SPEED/5;
aoikoizumi 4:e3d739bf09b4 346 target_L=BASIC_SPEED/(-5);
aoikoizumi 3:7bd1afb46094 347 }
aoikoizumi 4:e3d739bf09b4 348 if(angle<=-91) {
aoikoizumi 4:e3d739bf09b4 349 target_R=BASIC_SPEED/(-5);
aoikoizumi 4:e3d739bf09b4 350 target_L=BASIC_SPEED/-5;
aoikoizumi 3:7bd1afb46094 351 }
aoikoizumi 4:e3d739bf09b4 352 if(angle>-91&angle<-89) {
aoikoizumi 3:7bd1afb46094 353 motorR.stop();
aoikoizumi 3:7bd1afb46094 354 motorL.stop();
aoikoizumi 3:7bd1afb46094 355 wait(0.5);
aoikoizumi 4:e3d739bf09b4 356 if(angle>-91&angle<-89) {
aoikoizumi 4:e3d739bf09b4 357 i++;
aoikoizumi 4:e3d739bf09b4 358 }
aoikoizumi 3:7bd1afb46094 359 }
aoikoizumi 4:e3d739bf09b4 360 }//kakudo tyousei
aoikoizumi 4:e3d739bf09b4 361 if(i==8) {
aoikoizumi 5:0160b73f3d9e 362 susumu_xr(1,1,700);//700,1700
aoikoizumi 4:e3d739bf09b4 363 }
aoikoizumi 4:e3d739bf09b4 364 if(i==9) {
aoikoizumi 5:0160b73f3d9e 365 susumu_ang(1/3,1,0);//390,2010
aoikoizumi 4:e3d739bf09b4 366 }
aoikoizumi 4:e3d739bf09b4 367 if(i==10) {
aoikoizumi 5:0160b73f3d9e 368 susumu_y(1,1,asari_y-310);//390,y-310
aoikoizumi 4:e3d739bf09b4 369 }
aoikoizumi 4:e3d739bf09b4 370 if(i==11) {
aoikoizumi 5:0160b73f3d9e 371 susumu_ang(1/3,1,90);//700,y
aoikoizumi 4:e3d739bf09b4 372 }
aoikoizumi 4:e3d739bf09b4 373 if(i==12) {
aoikoizumi 4:e3d739bf09b4 374 motorR.stop();
aoikoizumi 4:e3d739bf09b4 375 motorL.stop();
aoikoizumi 5:0160b73f3d9e 376 servo.pulsewidth_us(3000);
aoikoizumi 5:0160b73f3d9e 377 wait(1);
aoikoizumi 4:e3d739bf09b4 378 i++;
aoikoizumi 4:e3d739bf09b4 379 }
aoikoizumi 4:e3d739bf09b4 380 if(i==13) {
aoikoizumi 4:e3d739bf09b4 381 if(angle>=91) {
aoikoizumi 4:e3d739bf09b4 382 target_R=BASIC_SPEED/5;
aoikoizumi 4:e3d739bf09b4 383 target_L=BASIC_SPEED/(-5);
aoikoizumi 3:7bd1afb46094 384 }
aoikoizumi 4:e3d739bf09b4 385 if(angle<=89) {
aoikoizumi 4:e3d739bf09b4 386 target_R=BASIC_SPEED/(-5);
aoikoizumi 4:e3d739bf09b4 387 target_L=BASIC_SPEED/5;
aoikoizumi 3:7bd1afb46094 388 }
aoikoizumi 4:e3d739bf09b4 389 if(angle>89&angle<91) {
aoikoizumi 3:7bd1afb46094 390 motorR.stop();
aoikoizumi 3:7bd1afb46094 391 motorL.stop();
aoikoizumi 4:e3d739bf09b4 392 wait(0.1);
aoikoizumi 4:e3d739bf09b4 393 if(angle>89&angle<91) {
aoikoizumi 4:e3d739bf09b4 394 i++;
aoikoizumi 4:e3d739bf09b4 395 }
aoikoizumi 4:e3d739bf09b4 396 }
aoikoizumi 4:e3d739bf09b4 397 }
aoikoizumi 4:e3d739bf09b4 398 if(i==14) {
aoikoizumi 5:0160b73f3d9e 399 susumu_xl(1,1,asari_x);//x,y
aoikoizumi 4:e3d739bf09b4 400 }
aoikoizumi 4:e3d739bf09b4 401 if(i==15) {
aoikoizumi 4:e3d739bf09b4 402 motorR.stop();
aoikoizumi 4:e3d739bf09b4 403 motorL.stop();
aoikoizumi 4:e3d739bf09b4 404 wait(0.5);
aoikoizumi 4:e3d739bf09b4 405 denjiben=1;
aoikoizumi 4:e3d739bf09b4 406 wait(0.5);
aoikoizumi 5:0160b73f3d9e 407 servo.pulsewidth_us(900);
aoikoizumi 5:0160b73f3d9e 408 wait(1);
aoikoizumi 4:e3d739bf09b4 409 i++;
aoikoizumi 4:e3d739bf09b4 410 }
aoikoizumi 4:e3d739bf09b4 411 if(i==16) {
aoikoizumi 5:0160b73f3d9e 412 susumu_xr(-1,-1,700);//700,y
aoikoizumi 4:e3d739bf09b4 413 }
aoikoizumi 4:e3d739bf09b4 414 if(i==17) {
aoikoizumi 5:0160b73f3d9e 415 susumu_ang(-1/3,-1,0);//390,y-310
aoikoizumi 4:e3d739bf09b4 416 }
aoikoizumi 4:e3d739bf09b4 417 if(i==18) {
aoikoizumi 5:0160b73f3d9e 418 susumu_y(-1,-1,goal_y+310);//390,y+310
aoikoizumi 4:e3d739bf09b4 419 }
aoikoizumi 4:e3d739bf09b4 420 if(i==19) {
aoikoizumi 5:0160b73f3d9e 421 susumu_ang(-1/3,-1,-90);//700,y
aoikoizumi 4:e3d739bf09b4 422 }
aoikoizumi 4:e3d739bf09b4 423 if(i==20) {
aoikoizumi 5:0160b73f3d9e 424 susumu_xl(-1,-1,goal_x);//x,y
aoikoizumi 4:e3d739bf09b4 425 }
aoikoizumi 4:e3d739bf09b4 426 if(i==21) {
aoikoizumi 4:e3d739bf09b4 427 motorR.stop();
aoikoizumi 4:e3d739bf09b4 428 motorL.stop();
aoikoizumi 4:e3d739bf09b4 429 wait(0.5);
aoikoizumi 4:e3d739bf09b4 430 denjiben=0;
aoikoizumi 4:e3d739bf09b4 431 wait(0.5);
aoikoizumi 0:29c024d6882f 432
aoikoizumi 4:e3d739bf09b4 433 break;
aoikoizumi 4:e3d739bf09b4 434 }
aoikoizumi 0:29c024d6882f 435
aoikoizumi 0:29c024d6882f 436
aoikoizumi 4:e3d739bf09b4 437 }//while
aoikoizumi 4:e3d739bf09b4 438 tgt(0,0);
aoikoizumi 4:e3d739bf09b4 439 return 0;
aoikoizumi 3:7bd1afb46094 440
aoikoizumi 0:29c024d6882f 441 }//intmain
aoikoizumi 0:29c024d6882f 442