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@6:7ec6c3d3a30a, 2018-09-13 (annotated)

Committer:
aoikoizumi
Date:
Thu Sep 13 04:17:15 2018 +0000
Revision:
6:7ec6c3d3a30a
Parent:
5:0160b73f3d9e
Child:
7:3a7e49ed1162
9/13 1317

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 6:7ec6c3d3a30a 7 #define BASIC_SPEED 15 //モーターはこの角速度で駆動させる
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 6:7ec6c3d3a30a 167 if(goal-5>angle&&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 6:7ec6c3d3a30a 181 } else if(angle<goal&&angle>=goal-5&&a<b) {
aoikoizumi 0:29c024d6882f 182 tgt(a/3,b/3);
aoikoizumi 0:29c024d6882f 183
aoikoizumi 6:7ec6c3d3a30a 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 6:7ec6c3d3a30a 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 6:7ec6c3d3a30a 255
aoikoizumi 6:7ec6c3d3a30a 256 new_dist=EC1.getDistance_mm();
aoikoizumi 6:7ec6c3d3a30a 257 d_dist=new_dist-old_dist;
aoikoizumi 6:7ec6c3d3a30a 258 old_dist=new_dist;
aoikoizumi 0:29c024d6882f 259
aoikoizumi 0:29c024d6882f 260 double d_x=d_dist*sin(angle);
aoikoizumi 0:29c024d6882f 261 double d_y=d_dist*cos(angle);
aoikoizumi 0:29c024d6882f 262 x=x+d_x;
aoikoizumi 0:29c024d6882f 263 y=y+d_y;
aoikoizumi 0:29c024d6882f 264
aoikoizumi 0:29c024d6882f 265
aoikoizumi 0:29c024d6882f 266
aoikoizumi 0:29c024d6882f 267 if(kai>=3) {
aoikoizumi 6:7ec6c3d3a30a 268
aoikoizumi 0:29c024d6882f 269
aoikoizumi 0:29c024d6882f 270 //double d_x=d_dist*sin(angle);;
aoikoizumi 0:29c024d6882f 271 //double d_y=d_dist*cos(angle);;
aoikoizumi 0:29c024d6882f 272 //x=x+d_x;
aoikoizumi 0:29c024d6882f 273 //y=y+d_y;
aoikoizumi 0:29c024d6882f 274
aoikoizumi 0:29c024d6882f 275 pc.printf("R=%f L=%f",target_R,target_L);
aoikoizumi 0:29c024d6882f 276 // pc.printf("omg_R=%f omg_L=%f \r\n",motorR.getOmega(),motorL.getOmega());
aoikoizumi 0:29c024d6882f 277 pc.printf("i=%d\r\n",i);
aoikoizumi 0:29c024d6882f 278 //pc.printf("EC1=%f",EC1.getDistance_mm(),EC1.getCount());
aoikoizumi 0:29c024d6882f 279 pc.printf("x=%f y=%f",x,y);
aoikoizumi 4:e3d739bf09b4 280 pc.printf("angle=%f\r\n",angle);
aoikoizumi 0:29c024d6882f 281 //機体の進む方向、右モーターの角速度、左モーターの角速度を表示
aoikoizumi 0:29c024d6882f 282 kai=0;
aoikoizumi 0:29c024d6882f 283 }
aoikoizumi 0:29c024d6882f 284 kai++;
aoikoizumi 0:29c024d6882f 285 if(i==0) {
aoikoizumi 4:e3d739bf09b4 286 /*if(reset_f.read()==1) {
aoikoizumi 1:1817e9243a6e 287 wait(0.05);
aoikoizumi 1:1817e9243a6e 288 if(reset_f.read()==1) {
aoikoizumi 4:e3d739bf09b4 289 */ denjiben=0;
aoikoizumi 4:e3d739bf09b4 290 i++;
aoikoizumi 4:e3d739bf09b4 291 // }
aoikoizumi 4:e3d739bf09b4 292 //}
aoikoizumi 4:e3d739bf09b4 293 if(reset_a.read()==1) {
aoikoizumi 4:e3d739bf09b4 294 wait(0.05);
aoikoizumi 2:3176040a4777 295 if(reset_a.read()==1) {
aoikoizumi 4:e3d739bf09b4 296 denjiben=0;
aoikoizumi 4:e3d739bf09b4 297 x=70;
aoikoizumi 4:e3d739bf09b4 298 y=2500;
aoikoizumi 4:e3d739bf09b4 299 i=14;
aoikoizumi 2:3176040a4777 300 }
aoikoizumi 5:0160b73f3d9e 301 }//x,y
aoikoizumi 4:e3d739bf09b4 302 }
aoikoizumi 4:e3d739bf09b4 303 if(i==1) {
aoikoizumi 5:0160b73f3d9e 304 susumu_y(1,1,start_x+178);//x,x+178
aoikoizumi 4:e3d739bf09b4 305 }
aoikoizumi 4:e3d739bf09b4 306 if(i==2) {
aoikoizumi 6:7ec6c3d3a30a 307 susumu_ang(0.5,1,45);//x+121.5,x+471.5
aoikoizumi 4:e3d739bf09b4 308 }
aoikoizumi 4:e3d739bf09b4 309 if(i==3) {
aoikoizumi 5:0160b73f3d9e 310 susumu_xl(1,1,728.5);//728.5,1078.5
aoikoizumi 4:e3d739bf09b4 311 }
aoikoizumi 4:e3d739bf09b4 312 if(i==4) {
aoikoizumi 6:7ec6c3d3a30a 313 susumu_ang(1,0.5,0);//850,1372
aoikoizumi 4:e3d739bf09b4 314 }
aoikoizumi 4:e3d739bf09b4 315 if(i==5) {
aoikoizumi 5:0160b73f3d9e 316 susumu_y(1,1,1700);//850,1700
aoikoizumi 4:e3d739bf09b4 317 }
aoikoizumi 4:e3d739bf09b4 318 if(i==6) {
aoikoizumi 4:e3d739bf09b4 319 motorR.stop();
aoikoizumi 4:e3d739bf09b4 320 motorL.stop();
aoikoizumi 4:e3d739bf09b4 321 t.start();
aoikoizumi 4:e3d739bf09b4 322 if(t<1) {
aoikoizumi 4:e3d739bf09b4 323 motor_f=0.82;
aoikoizumi 4:e3d739bf09b4 324 motor_b=0;
aoikoizumi 4:e3d739bf09b4 325 } else {
aoikoizumi 4:e3d739bf09b4 326 motor_f=0;
aoikoizumi 4:e3d739bf09b4 327 motor_b=0;
aoikoizumi 4:e3d739bf09b4 328 printf("finish");
aoikoizumi 4:e3d739bf09b4 329 t.reset();
aoikoizumi 4:e3d739bf09b4 330 i++;
aoikoizumi 3:7bd1afb46094 331 }
aoikoizumi 4:e3d739bf09b4 332 }//gatiasari
aoikoizumi 3:7bd1afb46094 333
aoikoizumi 3:7bd1afb46094 334
aoikoizumi 4:e3d739bf09b4 335 if(i==7) {
aoikoizumi 4:e3d739bf09b4 336 t.start();
aoikoizumi 4:e3d739bf09b4 337 if(t<1) {
aoikoizumi 4:e3d739bf09b4 338 motor_f=0;
aoikoizumi 4:e3d739bf09b4 339 motor_b=0.82;
aoikoizumi 4:e3d739bf09b4 340 } else {
aoikoizumi 4:e3d739bf09b4 341 motor_f=0;
aoikoizumi 4:e3d739bf09b4 342 motor_b=0;
aoikoizumi 4:e3d739bf09b4 343 printf("finish");
aoikoizumi 4:e3d739bf09b4 344 t.reset();
aoikoizumi 3:7bd1afb46094 345 }
aoikoizumi 4:e3d739bf09b4 346 if(angle>=-89) {
aoikoizumi 4:e3d739bf09b4 347 target_R=BASIC_SPEED/5;
aoikoizumi 4:e3d739bf09b4 348 target_L=BASIC_SPEED/(-5);
aoikoizumi 3:7bd1afb46094 349 }
aoikoizumi 4:e3d739bf09b4 350 if(angle<=-91) {
aoikoizumi 4:e3d739bf09b4 351 target_R=BASIC_SPEED/(-5);
aoikoizumi 4:e3d739bf09b4 352 target_L=BASIC_SPEED/-5;
aoikoizumi 3:7bd1afb46094 353 }
aoikoizumi 4:e3d739bf09b4 354 if(angle>-91&angle<-89) {
aoikoizumi 3:7bd1afb46094 355 motorR.stop();
aoikoizumi 3:7bd1afb46094 356 motorL.stop();
aoikoizumi 3:7bd1afb46094 357 wait(0.5);
aoikoizumi 4:e3d739bf09b4 358 if(angle>-91&angle<-89) {
aoikoizumi 4:e3d739bf09b4 359 i++;
aoikoizumi 4:e3d739bf09b4 360 }
aoikoizumi 3:7bd1afb46094 361 }
aoikoizumi 4:e3d739bf09b4 362 }//kakudo tyousei
aoikoizumi 4:e3d739bf09b4 363 if(i==8) {
aoikoizumi 5:0160b73f3d9e 364 susumu_xr(1,1,700);//700,1700
aoikoizumi 4:e3d739bf09b4 365 }
aoikoizumi 4:e3d739bf09b4 366 if(i==9) {
aoikoizumi 5:0160b73f3d9e 367 susumu_ang(1/3,1,0);//390,2010
aoikoizumi 4:e3d739bf09b4 368 }
aoikoizumi 4:e3d739bf09b4 369 if(i==10) {
aoikoizumi 5:0160b73f3d9e 370 susumu_y(1,1,asari_y-310);//390,y-310
aoikoizumi 4:e3d739bf09b4 371 }
aoikoizumi 4:e3d739bf09b4 372 if(i==11) {
aoikoizumi 5:0160b73f3d9e 373 susumu_ang(1/3,1,90);//700,y
aoikoizumi 4:e3d739bf09b4 374 }
aoikoizumi 4:e3d739bf09b4 375 if(i==12) {
aoikoizumi 4:e3d739bf09b4 376 motorR.stop();
aoikoizumi 4:e3d739bf09b4 377 motorL.stop();
aoikoizumi 6:7ec6c3d3a30a 378 servo.pulsewidth_us(2100);
aoikoizumi 6:7ec6c3d3a30a 379 wait(1.5);
aoikoizumi 6:7ec6c3d3a30a 380 servo.pulsewidth_us(2400);
aoikoizumi 5:0160b73f3d9e 381 wait(1);
aoikoizumi 4:e3d739bf09b4 382 i++;
aoikoizumi 4:e3d739bf09b4 383 }
aoikoizumi 4:e3d739bf09b4 384 if(i==13) {
aoikoizumi 4:e3d739bf09b4 385 if(angle>=91) {
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) {
aoikoizumi 4:e3d739bf09b4 390 target_R=BASIC_SPEED/(-5);
aoikoizumi 4:e3d739bf09b4 391 target_L=BASIC_SPEED/5;
aoikoizumi 3:7bd1afb46094 392 }
aoikoizumi 4:e3d739bf09b4 393 if(angle>89&angle<91) {
aoikoizumi 3:7bd1afb46094 394 motorR.stop();
aoikoizumi 3:7bd1afb46094 395 motorL.stop();
aoikoizumi 4:e3d739bf09b4 396 wait(0.1);
aoikoizumi 4:e3d739bf09b4 397 if(angle>89&angle<91) {
aoikoizumi 4:e3d739bf09b4 398 i++;
aoikoizumi 4:e3d739bf09b4 399 }
aoikoizumi 4:e3d739bf09b4 400 }
aoikoizumi 4:e3d739bf09b4 401 }
aoikoizumi 4:e3d739bf09b4 402 if(i==14) {
aoikoizumi 5:0160b73f3d9e 403 susumu_xl(1,1,asari_x);//x,y
aoikoizumi 4:e3d739bf09b4 404 }
aoikoizumi 4:e3d739bf09b4 405 if(i==15) {
aoikoizumi 4:e3d739bf09b4 406 motorR.stop();
aoikoizumi 4:e3d739bf09b4 407 motorL.stop();
aoikoizumi 4:e3d739bf09b4 408 wait(0.5);
aoikoizumi 4:e3d739bf09b4 409 denjiben=1;
aoikoizumi 4:e3d739bf09b4 410 wait(0.5);
aoikoizumi 5:0160b73f3d9e 411 servo.pulsewidth_us(900);
aoikoizumi 6:7ec6c3d3a30a 412 wait(2);
aoikoizumi 4:e3d739bf09b4 413 i++;
aoikoizumi 4:e3d739bf09b4 414 }
aoikoizumi 4:e3d739bf09b4 415 if(i==16) {
aoikoizumi 5:0160b73f3d9e 416 susumu_xr(-1,-1,700);//700,y
aoikoizumi 4:e3d739bf09b4 417 }
aoikoizumi 4:e3d739bf09b4 418 if(i==17) {
aoikoizumi 5:0160b73f3d9e 419 susumu_ang(-1/3,-1,0);//390,y-310
aoikoizumi 4:e3d739bf09b4 420 }
aoikoizumi 4:e3d739bf09b4 421 if(i==18) {
aoikoizumi 5:0160b73f3d9e 422 susumu_y(-1,-1,goal_y+310);//390,y+310
aoikoizumi 4:e3d739bf09b4 423 }
aoikoizumi 4:e3d739bf09b4 424 if(i==19) {
aoikoizumi 5:0160b73f3d9e 425 susumu_ang(-1/3,-1,-90);//700,y
aoikoizumi 4:e3d739bf09b4 426 }
aoikoizumi 4:e3d739bf09b4 427 if(i==20) {
aoikoizumi 5:0160b73f3d9e 428 susumu_xl(-1,-1,goal_x);//x,y
aoikoizumi 4:e3d739bf09b4 429 }
aoikoizumi 4:e3d739bf09b4 430 if(i==21) {
aoikoizumi 4:e3d739bf09b4 431 motorR.stop();
aoikoizumi 4:e3d739bf09b4 432 motorL.stop();
aoikoizumi 4:e3d739bf09b4 433 wait(0.5);
aoikoizumi 4:e3d739bf09b4 434 denjiben=0;
aoikoizumi 4:e3d739bf09b4 435 wait(0.5);
aoikoizumi 0:29c024d6882f 436
aoikoizumi 4:e3d739bf09b4 437 break;
aoikoizumi 4:e3d739bf09b4 438 }
aoikoizumi 0:29c024d6882f 439
aoikoizumi 0:29c024d6882f 440
aoikoizumi 4:e3d739bf09b4 441 }//while
aoikoizumi 4:e3d739bf09b4 442 tgt(0,0);
aoikoizumi 4:e3d739bf09b4 443 return 0;
aoikoizumi 3:7bd1afb46094 444
aoikoizumi 0:29c024d6882f 445 }//intmain
aoikoizumi 0:29c024d6882f 446