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@8:7b368096fed8, 2018-09-18 (annotated)

Committer:
aoikoizumi
Date:
Tue Sep 18 04:49:40 2018 +0000
Revision:
8:7b368096fed8
Parent:
7:3a7e49ed1162 
F3RC?4?????

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