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@0:29c024d6882f, 2018-09-07 (annotated)

Committer:
aoikoizumi
Date:
Fri Sep 07 06:32:23 2018 +0000
Revision:
0:29c024d6882f
Child:
1:1817e9243a6e
jk

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 0:29c024d6882f 26 DigitalIn button(USER_BUTTON);
aoikoizumi 0:29c024d6882f 27
aoikoizumi 0:29c024d6882f 28 PwmOut servo(PB_14);//servo
aoikoizumi 0:29c024d6882f 29 PwmOut motor_f(PC_9);
aoikoizumi 0:29c024d6882f 30 PwmOut motor_b(PB_9);//arm
aoikoizumi 0:29c024d6882f 31 DigitalOut denjiben(PC_0);//dennjibenn
aoikoizumi 0:29c024d6882f 32
aoikoizumi 0:29c024d6882f 33
aoikoizumi 0:29c024d6882f 34 double new_dist=0;
aoikoizumi 0:29c024d6882f 35 double old_dist=0;
aoikoizumi 0:29c024d6882f 36 double d_dist=0;
aoikoizumi 0:29c024d6882f 37 double x=250;
aoikoizumi 0:29c024d6882f 38 double y=150;//start point
aoikoizumi 0:29c024d6882f 39 Timer t;
aoikoizumi 0:29c024d6882f 40 int i=0;
aoikoizumi 0:29c024d6882f 41
aoikoizumi 0:29c024d6882f 42 int kai=0;//printf kansuu
aoikoizumi 0:29c024d6882f 43 double target_R=0,target_L=0;
aoikoizumi 0:29c024d6882f 44
aoikoizumi 0:29c024d6882f 45
aoikoizumi 0:29c024d6882f 46 double angle; //変数宣言
aoikoizumi 0:29c024d6882f 47
aoikoizumi 0:29c024d6882f 48
aoikoizumi 0:29c024d6882f 49 void tgt(double r,double l)
aoikoizumi 0:29c024d6882f 50 {
aoikoizumi 0:29c024d6882f 51 target_R=BASIC_SPEED*r;
aoikoizumi 0:29c024d6882f 52 target_L=BASIC_SPEED*l;
aoikoizumi 0:29c024d6882f 53 }
aoikoizumi 0:29c024d6882f 54
aoikoizumi 0:29c024d6882f 55
aoikoizumi 0:29c024d6882f 56 void susumu_y(double ay,double by,double goaly)
aoikoizumi 0:29c024d6882f 57 {
aoikoizumi 0:29c024d6882f 58
aoikoizumi 0:29c024d6882f 59 if(y<goaly-50&&ay>=0) {
aoikoizumi 0:29c024d6882f 60 t.start();
aoikoizumi 0:29c024d6882f 61 pc.printf("R=%f L=%f\r\n",target_R,target_L);
aoikoizumi 0:29c024d6882f 62 if(t.read_ms()<100) {
aoikoizumi 0:29c024d6882f 63 pc.printf("t=%f",t.read());
aoikoizumi 0:29c024d6882f 64 tgt(ay/3,by/3);
aoikoizumi 0:29c024d6882f 65 }
aoikoizumi 0:29c024d6882f 66 if(t.read_ms()>=100&&t.read_ms()<200) {
aoikoizumi 0:29c024d6882f 67 pc.printf("t=%f",t.read());
aoikoizumi 0:29c024d6882f 68 tgt(ay*2/3,by*2/3);
aoikoizumi 0:29c024d6882f 69 } else {
aoikoizumi 0:29c024d6882f 70 tgt(ay,by);
aoikoizumi 0:29c024d6882f 71 t.stop();
aoikoizumi 0:29c024d6882f 72 pc.printf("R=%f L=%f",target_R,target_L);
aoikoizumi 0:29c024d6882f 73 }
aoikoizumi 0:29c024d6882f 74 }
aoikoizumi 0:29c024d6882f 75 if(y<goaly&&y>=goaly-50&&ay>=0) {
aoikoizumi 0:29c024d6882f 76 tgt(ay/3,by/3);
aoikoizumi 0:29c024d6882f 77
aoikoizumi 0:29c024d6882f 78
aoikoizumi 0:29c024d6882f 79 }
aoikoizumi 0:29c024d6882f 80 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 0:29c024d6882f 86 }
aoikoizumi 0:29c024d6882f 87 if(t.read_ms()>=100&&t.read_ms()<200) {
aoikoizumi 0:29c024d6882f 88 pc.printf("t=%f",t.read());
aoikoizumi 0:29c024d6882f 89 tgt(ay*2/3,by*2/3);
aoikoizumi 0:29c024d6882f 90 } else {
aoikoizumi 0:29c024d6882f 91 tgt(ay,by);
aoikoizumi 0:29c024d6882f 92 t.stop();
aoikoizumi 0:29c024d6882f 93 pc.printf("R=%f L=%f",target_R,target_L);
aoikoizumi 0:29c024d6882f 94 }
aoikoizumi 0:29c024d6882f 95 }
aoikoizumi 0:29c024d6882f 96 if(y>goaly&&y<=goaly+50&&ay<0) {
aoikoizumi 0:29c024d6882f 97 tgt(ay/3,by/3);
aoikoizumi 0:29c024d6882f 98 } else {
aoikoizumi 0:29c024d6882f 99 i++;
aoikoizumi 0:29c024d6882f 100 t.reset();
aoikoizumi 0:29c024d6882f 101 pc.printf("owari\r\n");
aoikoizumi 0:29c024d6882f 102 }
aoikoizumi 0:29c024d6882f 103 }
aoikoizumi 0:29c024d6882f 104
aoikoizumi 0:29c024d6882f 105
aoikoizumi 0:29c024d6882f 106
aoikoizumi 0:29c024d6882f 107 void susumu_x(double ax,double bx,double goalx)
aoikoizumi 0:29c024d6882f 108 {
aoikoizumi 0:29c024d6882f 109
aoikoizumi 0:29c024d6882f 110 if(x<goalx-50&&ax>=0) {
aoikoizumi 0:29c024d6882f 111 t.start();
aoikoizumi 0:29c024d6882f 112 pc.printf("R=%f L=%f\r\n",target_R,target_L);
aoikoizumi 0:29c024d6882f 113 if(t.read_ms()<100) {
aoikoizumi 0:29c024d6882f 114 pc.printf("t=%f",t.read());
aoikoizumi 0:29c024d6882f 115 tgt(ax/3,bx/3);
aoikoizumi 0:29c024d6882f 116 }
aoikoizumi 0:29c024d6882f 117 if(t.read_ms()>=100&&t.read_ms()<200) {
aoikoizumi 0:29c024d6882f 118 pc.printf("t=%f",t.read());
aoikoizumi 0:29c024d6882f 119 tgt(ax*2/3,bx*2/3);
aoikoizumi 0:29c024d6882f 120 } else {
aoikoizumi 0:29c024d6882f 121 tgt(ax,bx);
aoikoizumi 0:29c024d6882f 122 t.stop();
aoikoizumi 0:29c024d6882f 123 pc.printf("R=%f L=%f",target_R,target_L);
aoikoizumi 0:29c024d6882f 124 }
aoikoizumi 0:29c024d6882f 125 }
aoikoizumi 0:29c024d6882f 126 if(x<goalx&&x>=goalx-50&&ax>=0) {
aoikoizumi 0:29c024d6882f 127 tgt(ax/3,bx/3);
aoikoizumi 0:29c024d6882f 128
aoikoizumi 0:29c024d6882f 129 }
aoikoizumi 0:29c024d6882f 130 if(x>goalx+50&&ax<0) {
aoikoizumi 0:29c024d6882f 131 t.start();
aoikoizumi 0:29c024d6882f 132 pc.printf("R=%f L=%f\r\n",target_R,target_L);
aoikoizumi 0:29c024d6882f 133 if(t.read_ms()<100) {
aoikoizumi 0:29c024d6882f 134 pc.printf("t=%f",t.read());
aoikoizumi 0:29c024d6882f 135 tgt(ax/3,bx/3);
aoikoizumi 0:29c024d6882f 136 }
aoikoizumi 0:29c024d6882f 137 if(t.read_ms()>=100&&t.read_ms()<200) {
aoikoizumi 0:29c024d6882f 138 pc.printf("t=%f",t.read());
aoikoizumi 0:29c024d6882f 139 tgt(ax*2/3,bx*2/3);
aoikoizumi 0:29c024d6882f 140 } else {
aoikoizumi 0:29c024d6882f 141 tgt(ax,bx);
aoikoizumi 0:29c024d6882f 142 t.stop();
aoikoizumi 0:29c024d6882f 143 pc.printf("R=%f L=%f",target_R,target_L);
aoikoizumi 0:29c024d6882f 144 }
aoikoizumi 0:29c024d6882f 145 }
aoikoizumi 0:29c024d6882f 146 if(x>goalx&&x<=goalx+50&&ax<0) {
aoikoizumi 0:29c024d6882f 147 tgt(ax/3,bx/3);
aoikoizumi 0:29c024d6882f 148
aoikoizumi 0:29c024d6882f 149 } else {
aoikoizumi 0:29c024d6882f 150 i++;
aoikoizumi 0:29c024d6882f 151 t.reset();
aoikoizumi 0:29c024d6882f 152 pc.printf("owari\r\n");
aoikoizumi 0:29c024d6882f 153 }
aoikoizumi 0:29c024d6882f 154 }
aoikoizumi 0:29c024d6882f 155
aoikoizumi 0:29c024d6882f 156
aoikoizumi 0:29c024d6882f 157 void susumu_ang(double a,double b,double goal)
aoikoizumi 0:29c024d6882f 158 {
aoikoizumi 0:29c024d6882f 159 if(x<angle-5&&a>b) {//usetsu
aoikoizumi 0:29c024d6882f 160 t.start();
aoikoizumi 0:29c024d6882f 161 pc.printf("R=%f L=%f\r\n",target_R,target_L);
aoikoizumi 0:29c024d6882f 162 if(t.read_ms()<100) {
aoikoizumi 0:29c024d6882f 163 pc.printf("t=%f",t.read());
aoikoizumi 0:29c024d6882f 164 tgt(a/3,b/3);
aoikoizumi 0:29c024d6882f 165 }
aoikoizumi 0:29c024d6882f 166 if(t.read_ms()>=100&&t.read_ms()<200) {
aoikoizumi 0:29c024d6882f 167 pc.printf("t=%f",t.read());
aoikoizumi 0:29c024d6882f 168 tgt(a*2/3,b*2/3);
aoikoizumi 0:29c024d6882f 169 } else {
aoikoizumi 0:29c024d6882f 170 tgt(a,b);
aoikoizumi 0:29c024d6882f 171 t.stop();
aoikoizumi 0:29c024d6882f 172 pc.printf("R=%f L=%f",target_R,target_L);
aoikoizumi 0:29c024d6882f 173 }
aoikoizumi 0:29c024d6882f 174 }
aoikoizumi 0:29c024d6882f 175 if(angle<goal&&angle>=goal-5&&a>b) {
aoikoizumi 0:29c024d6882f 176 tgt(a/3,b/3);
aoikoizumi 0:29c024d6882f 177
aoikoizumi 0:29c024d6882f 178 }
aoikoizumi 0:29c024d6882f 179 if(angle>goal+5&&a<b) {//sasetsu
aoikoizumi 0:29c024d6882f 180 t.start();
aoikoizumi 0:29c024d6882f 181 pc.printf("R=%f L=%f\r\n",target_R,target_L);
aoikoizumi 0:29c024d6882f 182 if(t.read_ms()<100) {
aoikoizumi 0:29c024d6882f 183 pc.printf("t=%f",t.read());
aoikoizumi 0:29c024d6882f 184 tgt(a/3,b/3);
aoikoizumi 0:29c024d6882f 185 }
aoikoizumi 0:29c024d6882f 186 if(t.read_ms()>=100&&t.read_ms()<200) {
aoikoizumi 0:29c024d6882f 187 pc.printf("t=%f",t.read());
aoikoizumi 0:29c024d6882f 188 tgt(a*2/3,b*2/3);
aoikoizumi 0:29c024d6882f 189 } else {
aoikoizumi 0:29c024d6882f 190 tgt(a,b);
aoikoizumi 0:29c024d6882f 191 t.stop();
aoikoizumi 0:29c024d6882f 192 pc.printf("R=%f L=%f",target_R,target_L);
aoikoizumi 0:29c024d6882f 193 }
aoikoizumi 0:29c024d6882f 194 }
aoikoizumi 0:29c024d6882f 195 if(angle>goal&&angle<=goal+5&&a<b) {
aoikoizumi 0:29c024d6882f 196 tgt(a/3,b/3);
aoikoizumi 0:29c024d6882f 197
aoikoizumi 0:29c024d6882f 198 } else {
aoikoizumi 0:29c024d6882f 199 i++;
aoikoizumi 0:29c024d6882f 200 t.reset();
aoikoizumi 0:29c024d6882f 201 pc.printf("reset\r\n");
aoikoizumi 0:29c024d6882f 202 }
aoikoizumi 0:29c024d6882f 203 }
aoikoizumi 0:29c024d6882f 204
aoikoizumi 0:29c024d6882f 205
aoikoizumi 0:29c024d6882f 206
aoikoizumi 0:29c024d6882f 207
aoikoizumi 0:29c024d6882f 208
aoikoizumi 0:29c024d6882f 209 int main(void)
aoikoizumi 0:29c024d6882f 210 {
aoikoizumi 0:29c024d6882f 211 printf("start\r\n");
aoikoizumi 0:29c024d6882f 212 motor_tick.attach(&calOmega,0.05);
aoikoizumi 0:29c024d6882f 213 motorR.setDOconstant(34.1);
aoikoizumi 0:29c024d6882f 214 motorL.setDOconstant(30);//c
aoikoizumi 0:29c024d6882f 215 motorR.setPDparam(0,0);
aoikoizumi 0:29c024d6882f 216 motorR.setPDparam(0,0);//pd
aoikoizumi 0:29c024d6882f 217
aoikoizumi 0:29c024d6882f 218
aoikoizumi 0:29c024d6882f 219 gyro.initialize(); //main関数の最初に一度だけ実行
aoikoizumi 0:29c024d6882f 220 EC1.setDiameter_mm(50);//sokuteirinnhannkei
aoikoizumi 0:29c024d6882f 221 double getDistance_mm();
aoikoizumi 0:29c024d6882f 222 //int EC1.getCount()=0;
aoikoizumi 0:29c024d6882f 223 EC1.reset();
aoikoizumi 0:29c024d6882f 224 void reset ();
aoikoizumi 0:29c024d6882f 225
aoikoizumi 0:29c024d6882f 226
aoikoizumi 0:29c024d6882f 227 // servo.period_ms(60);
aoikoizumi 0:29c024d6882f 228
aoikoizumi 0:29c024d6882f 229 motor_f.period_ms(30);
aoikoizumi 0:29c024d6882f 230 motor_b.period_ms(30);//arm
aoikoizumi 0:29c024d6882f 231
aoikoizumi 0:29c024d6882f 232 while(1) {
aoikoizumi 0:29c024d6882f 233 // motorR.turnF(0.3);
aoikoizumi 0:29c024d6882f 234 //motorL.turnF(0.3);//for debug
aoikoizumi 0:29c024d6882f 235 motorR.Sc(target_R);
aoikoizumi 0:29c024d6882f 236 motorL.Sc(target_L);//target_R,target_Lの値を変えることで直進・後退・右回転・左回転を行う
aoikoizumi 0:29c024d6882f 237 angle=gyro.getAngle(); //角度の値を受け取る
aoikoizumi 0:29c024d6882f 238
aoikoizumi 0:29c024d6882f 239 EC1.getDistance_mm();
aoikoizumi 0:29c024d6882f 240 // EC1.CalOmega();
aoikoizumi 0:29c024d6882f 241
aoikoizumi 0:29c024d6882f 242
aoikoizumi 0:29c024d6882f 243 if(target_R==0) motorR.stop();
aoikoizumi 0:29c024d6882f 244 else motorR.Sc(target_R);
aoikoizumi 0:29c024d6882f 245 if(target_L==0) motorL.stop();
aoikoizumi 0:29c024d6882f 246 else motorL.Sc(target_L);
aoikoizumi 0:29c024d6882f 247
aoikoizumi 0:29c024d6882f 248 double d_x=d_dist*sin(angle);
aoikoizumi 0:29c024d6882f 249 double d_y=d_dist*cos(angle);
aoikoizumi 0:29c024d6882f 250 x=x+d_x;
aoikoizumi 0:29c024d6882f 251 y=y+d_y;
aoikoizumi 0:29c024d6882f 252
aoikoizumi 0:29c024d6882f 253
aoikoizumi 0:29c024d6882f 254
aoikoizumi 0:29c024d6882f 255 if(kai>=3) {
aoikoizumi 0:29c024d6882f 256 new_dist=EC1.getDistance_mm();
aoikoizumi 0:29c024d6882f 257 d_dist=new_dist-old_dist;
aoikoizumi 0:29c024d6882f 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 pc.printf("R=%f L=%f",target_R,target_L);
aoikoizumi 0:29c024d6882f 266 // pc.printf("omg_R=%f omg_L=%f \r\n",motorR.getOmega(),motorL.getOmega());
aoikoizumi 0:29c024d6882f 267 pc.printf("i=%d\r\n",i);
aoikoizumi 0:29c024d6882f 268 //pc.printf("EC1=%f",EC1.getDistance_mm(),EC1.getCount());
aoikoizumi 0:29c024d6882f 269 pc.printf("x=%f y=%f",x,y);
aoikoizumi 0:29c024d6882f 270 //pc.printf("angle=%f\r\n",angle);
aoikoizumi 0:29c024d6882f 271 //機体の進む方向、右モーターの角速度、左モーターの角速度を表示
aoikoizumi 0:29c024d6882f 272 kai=0;
aoikoizumi 0:29c024d6882f 273 }
aoikoizumi 0:29c024d6882f 274 kai++;
aoikoizumi 0:29c024d6882f 275 if(i==0) {
aoikoizumi 0:29c024d6882f 276 t.start();
aoikoizumi 0:29c024d6882f 277 if(t<1) {
aoikoizumi 0:29c024d6882f 278 motor_f=0.8;
aoikoizumi 0:29c024d6882f 279 motor_b=0;
aoikoizumi 0:29c024d6882f 280 } else {
aoikoizumi 0:29c024d6882f 281 motor_f=0;
aoikoizumi 0:29c024d6882f 282 motor_b=0;
aoikoizumi 0:29c024d6882f 283 printf("finish");
aoikoizumi 0:29c024d6882f 284 t.reset();
aoikoizumi 0:29c024d6882f 285 i++;
aoikoizumi 0:29c024d6882f 286 }
aoikoizumi 0:29c024d6882f 287 }//motor test
aoikoizumi 0:29c024d6882f 288
aoikoizumi 0:29c024d6882f 289 //kokomade kihonnsettei
aoikoizumi 0:29c024d6882f 290
aoikoizumi 0:29c024d6882f 291
aoikoizumi 0:29c024d6882f 292 /* if(i==0) {
aoikoizumi 0:29c024d6882f 293 printf("a\n\r");
aoikoizumi 0:29c024d6882f 294 target_R=0;
aoikoizumi 0:29c024d6882f 295 target_L=0;
aoikoizumi 0:29c024d6882f 296 i++;
aoikoizumi 0:29c024d6882f 297 pc.printf("i=0\r\n");
aoikoizumi 0:29c024d6882f 298 pc.printf("R=%f L=%f",target_R,target_L);
aoikoizumi 0:29c024d6882f 299 }*/
aoikoizumi 0:29c024d6882f 300 if(i==1) {
aoikoizumi 0:29c024d6882f 301 susumu_y(1,1,472);
aoikoizumi 0:29c024d6882f 302 /* if(y<472) {
aoikoizumi 0:29c024d6882f 303 t.start();
aoikoizumi 0:29c024d6882f 304 pc.printf("R=%f L=%f\r\n",target_R,target_L);
aoikoizumi 0:29c024d6882f 305 if(t.read_ms()<200) {
aoikoizumi 0:29c024d6882f 306 pc.printf("t=%f",t.read());
aoikoizumi 0:29c024d6882f 307 target_R=BASIC_SPEED/3;
aoikoizumi 0:29c024d6882f 308 target_L=BASIC_SPEED/3;
aoikoizumi 0:29c024d6882f 309 } else {
aoikoizumi 0:29c024d6882f 310 target_R=BASIC_SPEED;
aoikoizumi 0:29c024d6882f 311 target_L=BASIC_SPEED;
aoikoizumi 0:29c024d6882f 312 t.stop();
aoikoizumi 0:29c024d6882f 313 pc.printf("R=%f L=%f",target_R,target_L);
aoikoizumi 0:29c024d6882f 314 }//250150 250472
aoikoizumi 0:29c024d6882f 315 } else {
aoikoizumi 0:29c024d6882f 316 i++;
aoikoizumi 0:29c024d6882f 317 t.reset();
aoikoizumi 0:29c024d6882f 318 pc.printf("reset\r\n");
aoikoizumi 0:29c024d6882f 319 */ //}
aoikoizumi 0:29c024d6882f 320 }
aoikoizumi 0:29c024d6882f 321 if(i==2) {
aoikoizumi 0:29c024d6882f 322 susumu_ang(1/3,1,45);
aoikoizumi 0:29c024d6882f 323 /* t.start();
aoikoizumi 0:29c024d6882f 324 if(angle<45) {
aoikoizumi 0:29c024d6882f 325 if(t.read_ms()<200) {
aoikoizumi 0:29c024d6882f 326 target_R=BASIC_SPEED/6;
aoikoizumi 0:29c024d6882f 327 target_L=BASIC_SPEED/2;
aoikoizumi 0:29c024d6882f 328 } else {
aoikoizumi 0:29c024d6882f 329 target_R=BASIC_SPEED/3;
aoikoizumi 0:29c024d6882f 330 target_L=BASIC_SPEED;
aoikoizumi 0:29c024d6882f 331 }//250472 341691
aoikoizumi 0:29c024d6882f 332 } else i++;
aoikoizumi 0:29c024d6882f 333 t.stop();
aoikoizumi 0:29c024d6882f 334 t.reset();
aoikoizumi 0:29c024d6882f 335 */
aoikoizumi 0:29c024d6882f 336 }//kairyoutyuu
aoikoizumi 0:29c024d6882f 337 if(i==3) {
aoikoizumi 0:29c024d6882f 338 susumu_x(1,1,709);
aoikoizumi 0:29c024d6882f 339 // if(x<709) {
aoikoizumi 0:29c024d6882f 340 // target_R=BASIC_SPEED;
aoikoizumi 0:29c024d6882f 341 // target_L=BASIC_SPEED;
aoikoizumi 0:29c024d6882f 342 // //341691 7091059
aoikoizumi 0:29c024d6882f 343 // } else i++;
aoikoizumi 0:29c024d6882f 344 }
aoikoizumi 0:29c024d6882f 345 if(i==4) {
aoikoizumi 0:29c024d6882f 346 susumu_ang(1,1/3,0);
aoikoizumi 0:29c024d6882f 347 /* if(angle>0) {
aoikoizumi 0:29c024d6882f 348 target_R=BASIC_SPEED;
aoikoizumi 0:29c024d6882f 349 target_L=BASIC_SPEED/3;
aoikoizumi 0:29c024d6882f 350 //7091059 8001278
aoikoizumi 0:29c024d6882f 351 } else i++;
aoikoizumi 0:29c024d6882f 352 */
aoikoizumi 0:29c024d6882f 353 }
aoikoizumi 0:29c024d6882f 354 if(i==5) {
aoikoizumi 0:29c024d6882f 355 susumu_y(1,1,1700);
aoikoizumi 0:29c024d6882f 356 // if(y<1700) {
aoikoizumi 0:29c024d6882f 357 // target_R=BASIC_SPEED;
aoikoizumi 0:29c024d6882f 358 // target_L=BASIC_SPEED;
aoikoizumi 0:29c024d6882f 359 // //8001278 8001700
aoikoizumi 0:29c024d6882f 360 // } else i++;
aoikoizumi 0:29c024d6882f 361 motorR.stop();
aoikoizumi 0:29c024d6882f 362 motorL.stop();
aoikoizumi 0:29c024d6882f 363 }
aoikoizumi 0:29c024d6882f 364
aoikoizumi 0:29c024d6882f 365 if(i==6) {
aoikoizumi 0:29c024d6882f 366
aoikoizumi 0:29c024d6882f 367 i++;
aoikoizumi 0:29c024d6882f 368 }//gatiasari
aoikoizumi 0:29c024d6882f 369
aoikoizumi 0:29c024d6882f 370
aoikoizumi 0:29c024d6882f 371 /* if(i==7) {
aoikoizumi 0:29c024d6882f 372 if(angle>=-89) {
aoikoizumi 0:29c024d6882f 373 target_R=BASIC_SPEED/4;
aoikoizumi 0:29c024d6882f 374 target_L=BASIC_SPEED*(-4);
aoikoizumi 0:29c024d6882f 375 }
aoikoizumi 0:29c024d6882f 376 if(angle<=-91) {
aoikoizumi 0:29c024d6882f 377 target_R=BASIC_SPEED/4;
aoikoizumi 0:29c024d6882f 378 target_L=BASIC_SPEED*(-4
aoikoizumi 0:29c024d6882f 379 );
aoikoizumi 0:29c024d6882f 380 }
aoikoizumi 0:29c024d6882f 381 if(angle>-91&angle<89){
aoikoizumi 0:29c024d6882f 382 motorR.stop();
aoikoizumi 0:29c024d6882f 383 motorL.stop();
aoikoizumi 0:29c024d6882f 384 wait(0.5);
aoikoizumi 0:29c024d6882f 385 if(angle>-91&angle<89){
aoikoizumi 0:29c024d6882f 386 i++;}
aoikoizumi 0:29c024d6882f 387 }
aoikoizumi 0:29c024d6882f 388 }
aoikoizumi 0:29c024d6882f 389 if(i==8) {
aoikoizumi 0:29c024d6882f 390 if(x>700) {
aoikoizumi 0:29c024d6882f 391 target_R=BASIC_SPEED;
aoikoizumi 0:29c024d6882f 392 target_L=BASIC_SPEED;
aoikoizumi 0:29c024d6882f 393 //8001700 7001700
aoikoizumi 0:29c024d6882f 394 } else i++;
aoikoizumi 0:29c024d6882f 395 }
aoikoizumi 0:29c024d6882f 396 if(i==9) {
aoikoizumi 0:29c024d6882f 397 if(angle<0) {
aoikoizumi 0:29c024d6882f 398 target_R=BASIC_SPEED/3;
aoikoizumi 0:29c024d6882f 399 target_L=BASIC_SPEED;
aoikoizumi 0:29c024d6882f 400 //7001700 4002000
aoikoizumi 0:29c024d6882f 401 } else i++;
aoikoizumi 0:29c024d6882f 402 }
aoikoizumi 0:29c024d6882f 403 if(i==10) {
aoikoizumi 0:29c024d6882f 404 if(y<2200) {
aoikoizumi 0:29c024d6882f 405 target_R=BASIC_SPEED;
aoikoizumi 0:29c024d6882f 406 target_L=BASIC_SPEED;
aoikoizumi 0:29c024d6882f 407 //4002000 4002200
aoikoizumi 0:29c024d6882f 408 } else i++;
aoikoizumi 0:29c024d6882f 409 }
aoikoizumi 0:29c024d6882f 410 if(i==11) {
aoikoizumi 0:29c024d6882f 411 if(angle<90) {
aoikoizumi 0:29c024d6882f 412 target_R=BASIC_SPEED/3;
aoikoizumi 0:29c024d6882f 413 target_L=BASIC_SPEED;
aoikoizumi 0:29c024d6882f 414 //4002200 7002500
aoikoizumi 0:29c024d6882f 415 } else i++;
aoikoizumi 0:29c024d6882f 416 }
aoikoizumi 0:29c024d6882f 417 if(i==12) {
aoikoizumi 0:29c024d6882f 418 if(x<1000) {
aoikoizumi 0:29c024d6882f 419 target_R=BASIC_SPEED;
aoikoizumi 0:29c024d6882f 420 target_L=BASIC_SPEED;
aoikoizumi 0:29c024d6882f 421 //7002500 10002500
aoikoizumi 0:29c024d6882f 422 } else i++;
aoikoizumi 0:29c024d6882f 423 motorR.stop();
aoikoizumi 0:29c024d6882f 424 motorL.stop();
aoikoizumi 0:29c024d6882f 425 }
aoikoizumi 0:29c024d6882f 426
aoikoizumi 0:29c024d6882f 427 if(i==13){
aoikoizumi 0:29c024d6882f 428 i++;
aoikoizumi 0:29c024d6882f 429 }
aoikoizumi 0:29c024d6882f 430
aoikoizumi 0:29c024d6882f 431
aoikoizumi 0:29c024d6882f 432
aoikoizumi 0:29c024d6882f 433
aoikoizumi 0:29c024d6882f 434 if(i==14) {
aoikoizumi 0:29c024d6882f 435 if(x>700) {
aoikoizumi 0:29c024d6882f 436 target_R=BASIC_SPEED*(-1);
aoikoizumi 0:29c024d6882f 437 target_L=BASIC_SPEED*(-1);
aoikoizumi 0:29c024d6882f 438 //10002500 7002500
aoikoizumi 0:29c024d6882f 439 } else i++;
aoikoizumi 0:29c024d6882f 440 }
aoikoizumi 0:29c024d6882f 441 if(i==15) {
aoikoizumi 0:29c024d6882f 442 if(angle>0) {
aoikoizumi 0:29c024d6882f 443 target_R=BASIC_SPEED/(-3);
aoikoizumi 0:29c024d6882f 444 target_L=BASIC_SPEED/(-1);
aoikoizumi 0:29c024d6882f 445 //7002500 4002200
aoikoizumi 0:29c024d6882f 446 } else i++;
aoikoizumi 0:29c024d6882f 447 }
aoikoizumi 0:29c024d6882f 448 if(i==16) {
aoikoizumi 0:29c024d6882f 449 if(y>2000) {
aoikoizumi 0:29c024d6882f 450 target_R=BASIC_SPEED*(-1);
aoikoizumi 0:29c024d6882f 451 target_L=BASIC_SPEED*(-1);
aoikoizumi 0:29c024d6882f 452 //4002200 4002000
aoikoizumi 0:29c024d6882f 453 } else i++;
aoikoizumi 0:29c024d6882f 454 }
aoikoizumi 0:29c024d6882f 455 if(i==17) {
aoikoizumi 0:29c024d6882f 456 if(angle>-90) {
aoikoizumi 0:29c024d6882f 457 target_R=BASIC_SPEED/(-3);
aoikoizumi 0:29c024d6882f 458 target_L=BASIC_SPEED/(-1);
aoikoizumi 0:29c024d6882f 459 //4002000 7001700
aoikoizumi 0:29c024d6882f 460 } else i++;
aoikoizumi 0:29c024d6882f 461 }
aoikoizumi 0:29c024d6882f 462 if(i==16) {
aoikoizumi 0:29c024d6882f 463 if(x<1100) {
aoikoizumi 0:29c024d6882f 464 target_R=BASIC_SPEED*(-1);
aoikoizumi 0:29c024d6882f 465 target_L=BASIC_SPEED*(-1);
aoikoizumi 0:29c024d6882f 466 //7001700 4002000
aoikoizumi 0:29c024d6882f 467 } else i++;
aoikoizumi 0:29c024d6882f 468 }
aoikoizumi 0:29c024d6882f 469 */
aoikoizumi 0:29c024d6882f 470 //kairyoutyuu kokomade
aoikoizumi 0:29c024d6882f 471
aoikoizumi 0:29c024d6882f 472 }//while
aoikoizumi 0:29c024d6882f 473 motorR.stop();
aoikoizumi 0:29c024d6882f 474 motorL.stop();
aoikoizumi 0:29c024d6882f 475 //LR(0,0);
aoikoizumi 0:29c024d6882f 476 return 0;
aoikoizumi 0:29c024d6882f 477
aoikoizumi 0:29c024d6882f 478 }//intmain
aoikoizumi 0:29c024d6882f 479