riko-ten / Mbed 2 deprecated F3RC

f3rc

Dependencies:   CruizCore_R1370P EC2 delta enc_1multi mbed

Fork of F3RC by ROBOSTEP_LIBRARY

F3rc.h@9:b172328cc975, 2018-11-03 (annotated)

Committer:
aoikoizumi
Date:
Sat Nov 03 23:10:12 2018 +0000
Revision:
9:b172328cc975
F3RC

Who changed what in which revision?

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