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@3:7bd1afb46094, 2018-09-11 (annotated)

Committer:
aoikoizumi
Date:
Tue Sep 11 04:48:43 2018 +0000
Revision:
3:7bd1afb46094
Parent:
2:3176040a4777
Child:
4:e3d739bf09b4
9/11?????

; ?????????

; ?????????

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

;

; ??????

; printf ??

; ???????

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 3:7bd1afb46094 298 }
aoikoizumi 3:7bd1afb46094 299 if(i==4) {
aoikoizumi 3:7bd1afb46094 300 susumu_ang(1,1/3,0);
aoikoizumi 3:7bd1afb46094 301 }
aoikoizumi 3:7bd1afb46094 302 if(i==5) {
aoikoizumi 3:7bd1afb46094 303 susumu_y(1,1,1700);
aoikoizumi 3:7bd1afb46094 304 }
aoikoizumi 3:7bd1afb46094 305 if(i==6) {
aoikoizumi 3:7bd1afb46094 306 motorR.stop();
aoikoizumi 3:7bd1afb46094 307 motorL.stop();
aoikoizumi 3:7bd1afb46094 308 t.start();
aoikoizumi 3:7bd1afb46094 309 if(t<1) {
aoikoizumi 3:7bd1afb46094 310 motor_f=0.82;
aoikoizumi 3:7bd1afb46094 311 motor_b=0;
aoikoizumi 3:7bd1afb46094 312 } else {
aoikoizumi 3:7bd1afb46094 313 motor_f=0;
aoikoizumi 3:7bd1afb46094 314 motor_b=0;
aoikoizumi 3:7bd1afb46094 315 printf("finish");
aoikoizumi 3:7bd1afb46094 316 t.reset();
aoikoizumi 3:7bd1afb46094 317 i++;
aoikoizumi 3:7bd1afb46094 318 }
aoikoizumi 3:7bd1afb46094 319 }//gatiasari
aoikoizumi 3:7bd1afb46094 320
aoikoizumi 3:7bd1afb46094 321
aoikoizumi 3:7bd1afb46094 322 if(i==7) {
aoikoizumi 0:29c024d6882f 323 t.start();
aoikoizumi 0:29c024d6882f 324 if(t<1) {
aoikoizumi 1:1817e9243a6e 325 motor_f=0;
aoikoizumi 1:1817e9243a6e 326 motor_b=0.82;
aoikoizumi 0:29c024d6882f 327 } else {
aoikoizumi 0:29c024d6882f 328 motor_f=0;
aoikoizumi 0:29c024d6882f 329 motor_b=0;
aoikoizumi 0:29c024d6882f 330 printf("finish");
aoikoizumi 0:29c024d6882f 331 t.reset();
aoikoizumi 1:1817e9243a6e 332 }
aoikoizumi 3:7bd1afb46094 333 if(angle>=-89) {
aoikoizumi 3:7bd1afb46094 334 target_R=BASIC_SPEED/5;
aoikoizumi 3:7bd1afb46094 335 target_L=BASIC_SPEED/(-5);
aoikoizumi 0:29c024d6882f 336 }
aoikoizumi 3:7bd1afb46094 337 if(angle<=-91) {
aoikoizumi 3:7bd1afb46094 338 target_R=BASIC_SPEED/(-5);
aoikoizumi 3:7bd1afb46094 339 target_L=BASIC_SPEED/-5;
aoikoizumi 1:1817e9243a6e 340 }
aoikoizumi 3:7bd1afb46094 341 if(angle>-91&angle<-89) {
aoikoizumi 1:1817e9243a6e 342 motorR.stop();
aoikoizumi 1:1817e9243a6e 343 motorL.stop();
aoikoizumi 1:1817e9243a6e 344 wait(0.5);
aoikoizumi 3:7bd1afb46094 345 if(angle>-91&angle<-89) {
aoikoizumi 3:7bd1afb46094 346 i++;
aoikoizumi 3:7bd1afb46094 347 }
aoikoizumi 1:1817e9243a6e 348 }
aoikoizumi 3:7bd1afb46094 349 }//kakudo tyousei
aoikoizumi 3:7bd1afb46094 350 if(i==8) {
aoikoizumi 3:7bd1afb46094 351 susumu_xr(1,1,700);
aoikoizumi 3:7bd1afb46094 352 }
aoikoizumi 3:7bd1afb46094 353 if(i==9) {
aoikoizumi 3:7bd1afb46094 354 susumu_ang(1/3,1,0);
aoikoizumi 3:7bd1afb46094 355 }
aoikoizumi 3:7bd1afb46094 356 if(i==10) {
aoikoizumi 3:7bd1afb46094 357 susumu_y(1,1,2200);
aoikoizumi 3:7bd1afb46094 358 }
aoikoizumi 3:7bd1afb46094 359 if(i==11) {
aoikoizumi 3:7bd1afb46094 360 susumu_ang(1/3,1,90);
aoikoizumi 3:7bd1afb46094 361 }
aoikoizumi 3:7bd1afb46094 362 if(i==12) {
aoikoizumi 3:7bd1afb46094 363 motorR.stop();
aoikoizumi 3:7bd1afb46094 364 motorL.stop();
aoikoizumi 3:7bd1afb46094 365 servo.pulsewidth_us(1000);
aoikoizumi 3:7bd1afb46094 366 wait(0.5);
aoikoizumi 3:7bd1afb46094 367 i++;
aoikoizumi 3:7bd1afb46094 368 }
aoikoizumi 3:7bd1afb46094 369 if(i==13) {
aoikoizumi 3:7bd1afb46094 370 if(angle>=91) {
aoikoizumi 3:7bd1afb46094 371 target_R=BASIC_SPEED/5;
aoikoizumi 3:7bd1afb46094 372 target_L=BASIC_SPEED/(-5);
aoikoizumi 1:1817e9243a6e 373 }
aoikoizumi 3:7bd1afb46094 374 if(angle<=89) {
aoikoizumi 3:7bd1afb46094 375 target_R=BASIC_SPEED/(-5);
aoikoizumi 3:7bd1afb46094 376 target_L=BASIC_SPEED/5;
aoikoizumi 1:1817e9243a6e 377 }
aoikoizumi 3:7bd1afb46094 378 if(angle>89&angle<91) {
aoikoizumi 1:1817e9243a6e 379 motorR.stop();
aoikoizumi 1:1817e9243a6e 380 motorL.stop();
aoikoizumi 3:7bd1afb46094 381 wait(0.1);
aoikoizumi 3:7bd1afb46094 382 if(angle>89&angle<91) {
aoikoizumi 3:7bd1afb46094 383 i++;
aoikoizumi 3:7bd1afb46094 384 }
aoikoizumi 3:7bd1afb46094 385 }
aoikoizumi 3:7bd1afb46094 386 }
aoikoizumi 3:7bd1afb46094 387 if(i==14) {
aoikoizumi 3:7bd1afb46094 388 susumu_xl(1,1,1000);
aoikoizumi 3:7bd1afb46094 389 }
aoikoizumi 3:7bd1afb46094 390 if(i==15) {
aoikoizumi 3:7bd1afb46094 391 motorR.stop();
aoikoizumi 3:7bd1afb46094 392 motorL.stop();
aoikoizumi 3:7bd1afb46094 393 wait(0.5);
aoikoizumi 3:7bd1afb46094 394 denjiben=1;
aoikoizumi 3:7bd1afb46094 395 wait(0.5);
aoikoizumi 3:7bd1afb46094 396 servo.pulsewidth_us(1500);
aoikoizumi 3:7bd1afb46094 397 wait(0.5);
aoikoizumi 3:7bd1afb46094 398 i++;
aoikoizumi 3:7bd1afb46094 399 }
aoikoizumi 3:7bd1afb46094 400 if(i==16) {
aoikoizumi 3:7bd1afb46094 401 susumu_xr(-1,-1,700);
aoikoizumi 3:7bd1afb46094 402 }
aoikoizumi 3:7bd1afb46094 403 if(i==17) {
aoikoizumi 3:7bd1afb46094 404 susumu_ang(-1/3,-1,0);
aoikoizumi 3:7bd1afb46094 405 }
aoikoizumi 3:7bd1afb46094 406 if(i==18) {
aoikoizumi 3:7bd1afb46094 407 susumu_y(-1,-1,2000);
aoikoizumi 3:7bd1afb46094 408 }
aoikoizumi 3:7bd1afb46094 409 if(i==19) {
aoikoizumi 3:7bd1afb46094 410 susumu_ang(-1/3,-1,-90);
aoikoizumi 3:7bd1afb46094 411 }
aoikoizumi 3:7bd1afb46094 412 if(i==20) {
aoikoizumi 3:7bd1afb46094 413 susumu_xl(-1,-1,1100);
aoikoizumi 3:7bd1afb46094 414 }
aoikoizumi 3:7bd1afb46094 415 if(i==21) {
aoikoizumi 3:7bd1afb46094 416 motorR.stop();
aoikoizumi 3:7bd1afb46094 417 motorL.stop();
aoikoizumi 3:7bd1afb46094 418 servo.pulsewidth_us(1500);
aoikoizumi 3:7bd1afb46094 419 wait(0.5);
aoikoizumi 3:7bd1afb46094 420 denjiben=0;
aoikoizumi 3:7bd1afb46094 421 wait(0.5);
aoikoizumi 0:29c024d6882f 422
aoikoizumi 3:7bd1afb46094 423 break;
aoikoizumi 3:7bd1afb46094 424 }
aoikoizumi 0:29c024d6882f 425
aoikoizumi 0:29c024d6882f 426
aoikoizumi 3:7bd1afb46094 427 }//while
aoikoizumi 3:7bd1afb46094 428 tgt(0,0);
aoikoizumi 3:7bd1afb46094 429 return 0;
aoikoizumi 3:7bd1afb46094 430
aoikoizumi 0:29c024d6882f 431 }//intmain
aoikoizumi 0:29c024d6882f 432