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@2:3176040a4777, 2018-09-11 (annotated)

Committer:
aoikoizumi
Date:
Tue Sep 11 04:39:47 2018 +0000
Revision:
2:3176040a4777
Parent:
1:1817e9243a6e
Child:
3:7bd1afb46094
9/11

; ???????????

; ?????????

; ??????????

;

; ???????

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