olol
Dependencies: CruizCore_R1370P EC delta enc_1multi mbed
Diff: main.cpp
- Revision:
- 1:1817e9243a6e
- Parent:
- 0:29c024d6882f
- Child:
- 2:3176040a4777
--- a/main.cpp Fri Sep 07 06:32:23 2018 +0000 +++ b/main.cpp Tue Sep 11 04:18:28 2018 +0000 @@ -23,7 +23,9 @@ EC1.CalOmega(); } -DigitalIn button(USER_BUTTON); +//DigitalIn button(USER_BUTTON); +DigitalIn reset_f(PC_1,PullUp); +DigitalIn reset_a(PA_4,PullUp); PwmOut servo(PB_14);//servo PwmOut motor_f(PC_9); @@ -35,7 +37,7 @@ double old_dist=0; double d_dist=0; double x=250; -double y=150;//start point +double y=150;//start point//keisann wo sinaosu hitsuyouga arimasu Timer t; int i=0; @@ -46,24 +48,24 @@ double angle; //変数宣言 -void tgt(double r,double l) +void tgt(double r,double l)//void de sika tsukawanaino ha mottainai kimosuru { target_R=BASIC_SPEED*r; target_L=BASIC_SPEED*l; } -void susumu_y(double ay,double by,double goaly) +//printf wo dousa kakunin sidai kesou toha omotte imasu +void susumu_y(double ay,double by,double goaly)//y zahyou wo motiita susumikata subete { - +//pc.printf("ay=%f by=%f y=%f goaly=%f\r\n",ay,by,y,goaly); if(y<goaly-50&&ay>=0) { t.start(); pc.printf("R=%f L=%f\r\n",target_R,target_L); if(t.read_ms()<100) { pc.printf("t=%f",t.read()); tgt(ay/3,by/3); - } - if(t.read_ms()>=100&&t.read_ms()<200) { + } else if(t.read_ms()>=100&&t.read_ms()<200) { pc.printf("t=%f",t.read()); tgt(ay*2/3,by*2/3); } else { @@ -71,20 +73,17 @@ t.stop(); pc.printf("R=%f L=%f",target_R,target_L); } - } - if(y<goaly&&y>=goaly-50&&ay>=0) { + } else if(y<goaly&&y>=goaly-50&&ay>=0) { tgt(ay/3,by/3); - } - if(y>goaly+50&&ay<0) { + } else if(y>goaly+50&&ay<0) { t.start(); pc.printf("R=%f L=%f\r\n",target_R,target_L); if(t.read_ms()<100) { pc.printf("t=%f",t.read()); tgt(ay/3,by/3); - } - if(t.read_ms()>=100&&t.read_ms()<200) { + } else if(t.read_ms()>=100&&t.read_ms()<200) { pc.printf("t=%f",t.read()); tgt(ay*2/3,by*2/3); } else { @@ -92,69 +91,72 @@ t.stop(); pc.printf("R=%f L=%f",target_R,target_L); } - } - if(y>goaly&&y<=goaly+50&&ay<0) { + } else if(y>goaly&&y<=goaly+50&&ay<0) { tgt(ay/3,by/3); } else { i++; t.reset(); - pc.printf("owari\r\n"); + pc.printf("owari%d\r\n",i); } } -void susumu_x(double ax,double bx,double goalx) +void susumu_xl(double axl,double bxl,double goalxl)//hidarikara mokuhyoutenn ni toutatsu surutameno dousa { - if(x<goalx-50&&ax>=0) { + if(x<goalxl-50) { t.start(); pc.printf("R=%f L=%f\r\n",target_R,target_L); if(t.read_ms()<100) { pc.printf("t=%f",t.read()); - tgt(ax/3,bx/3); - } - if(t.read_ms()>=100&&t.read_ms()<200) { + tgt(axl/3,bxl/3); + } else if(t.read_ms()>=100&&t.read_ms()<200) { pc.printf("t=%f",t.read()); - tgt(ax*2/3,bx*2/3); + tgt(axl*2/3,bxl*2/3); } else { - tgt(ax,bx); + tgt(axl,bxl); t.stop(); pc.printf("R=%f L=%f",target_R,target_L); } + } else if(x<goalxl&&x>=goalxl-50) { + tgt(axl/3,bxl/3); + + } else { + t.reset(); + i++; + pc.printf("owari\r\n"); } - if(x<goalx&&x>=goalx-50&&ax>=0) { - tgt(ax/3,bx/3); +} - } - if(x>goalx+50&&ax<0) { +void susumu_xr(double axr,double bxr,double goalxr)//migikara mokuhyoutenn ni toutatsu surutameno dousa +{ + + if(x>goalxr+50) { t.start(); pc.printf("R=%f L=%f\r\n",target_R,target_L); if(t.read_ms()<100) { pc.printf("t=%f",t.read()); - tgt(ax/3,bx/3); - } - if(t.read_ms()>=100&&t.read_ms()<200) { + tgt(axr/3,bxr/3); + } else if(t.read_ms()>=100&&t.read_ms()<200) { pc.printf("t=%f",t.read()); - tgt(ax*2/3,bx*2/3); + tgt(axr*2/3,bxr*2/3); } else { - tgt(ax,bx); + tgt(axr,bxr); t.stop(); pc.printf("R=%f L=%f",target_R,target_L); } - } - if(x>goalx&&x<=goalx+50&&ax<0) { - tgt(ax/3,bx/3); + } else if(x>goalxr&&x<=goalxr+50) { + tgt(axr/3,bxr/3); } else { + t.reset(); i++; - t.reset(); pc.printf("owari\r\n"); } } - -void susumu_ang(double a,double b,double goal) +void susumu_ang(double a,double b,double goal)//kakudo { if(x<angle-5&&a>b) {//usetsu t.start(); @@ -162,8 +164,7 @@ if(t.read_ms()<100) { pc.printf("t=%f",t.read()); tgt(a/3,b/3); - } - if(t.read_ms()>=100&&t.read_ms()<200) { + } else if(t.read_ms()>=100&&t.read_ms()<200) { pc.printf("t=%f",t.read()); tgt(a*2/3,b*2/3); } else { @@ -171,19 +172,16 @@ t.stop(); pc.printf("R=%f L=%f",target_R,target_L); } - } - if(angle<goal&&angle>=goal-5&&a>b) { + } else if(angle<goal&&angle>=goal-5&&a>b) { tgt(a/3,b/3); - } - if(angle>goal+5&&a<b) {//sasetsu + } else if(angle>goal+5&&a<b) { //sasetsu t.start(); pc.printf("R=%f L=%f\r\n",target_R,target_L); if(t.read_ms()<100) { pc.printf("t=%f",t.read()); tgt(a/3,b/3); - } - if(t.read_ms()>=100&&t.read_ms()<200) { + } else if(t.read_ms()>=100&&t.read_ms()<200) { pc.printf("t=%f",t.read()); tgt(a*2/3,b*2/3); } else { @@ -191,8 +189,7 @@ t.stop(); pc.printf("R=%f L=%f",target_R,target_L); } - } - if(angle>goal&&angle<=goal+5&&a<b) { + } else if(angle>goal&&angle<=goal+5&&a<b) { tgt(a/3,b/3); } else { @@ -213,7 +210,7 @@ motorR.setDOconstant(34.1); motorL.setDOconstant(30);//c motorR.setPDparam(0,0); - motorR.setPDparam(0,0);//pd + motorR.setPDparam(0,0);//pd//akirameta gyro.initialize(); //main関数の最初に一度だけ実行 @@ -224,7 +221,7 @@ void reset (); - // servo.period_ms(60); + servo.period_ms(20); motor_f.period_ms(30); motor_b.period_ms(30);//arm @@ -273,207 +270,153 @@ } kai++; if(i==0) { + if(reset_f.read()==1) { + wait(0.05); + if(reset_f.read()==1) { + denjiben=0; + i++; + } + } + } + if(i==1) { + susumu_y(1,1,472); + } + if(i==2) { + susumu_ang(1/3,1,45); + } + if(i==3) { + susumu_xl(1,1,709); t.start(); if(t<1) { - motor_f=0.8; - motor_b=0; + motor_f=0; + motor_b=0.82; } else { motor_f=0; motor_b=0; printf("finish"); t.reset(); - i++; + } + if(i==4) { + susumu_ang(1,1/3,0); + } + if(i==5) { + susumu_y(1,1,1700); } - }//motor test - -//kokomade kihonnsettei - - - /* if(i==0) { - printf("a\n\r"); - target_R=0; - target_L=0; - i++; - pc.printf("i=0\r\n"); - pc.printf("R=%f L=%f",target_R,target_L); - }*/ - if(i==1) { - susumu_y(1,1,472); - /* if(y<472) { - t.start(); - pc.printf("R=%f L=%f\r\n",target_R,target_L); - if(t.read_ms()<200) { - pc.printf("t=%f",t.read()); - target_R=BASIC_SPEED/3; - target_L=BASIC_SPEED/3; - } else { - target_R=BASIC_SPEED; - target_L=BASIC_SPEED; - t.stop(); - pc.printf("R=%f L=%f",target_R,target_L); - }//250150 250472 - } else { - i++; - t.reset(); - pc.printf("reset\r\n"); - */ //} - } - if(i==2) { - susumu_ang(1/3,1,45); - /* t.start(); - if(angle<45) { - if(t.read_ms()<200) { - target_R=BASIC_SPEED/6; - target_L=BASIC_SPEED/2; - } else { - target_R=BASIC_SPEED/3; - target_L=BASIC_SPEED; - }//250472 341691 - } else i++; - t.stop(); - t.reset(); - */ - }//kairyoutyuu - if(i==3) { - susumu_x(1,1,709); - // if(x<709) { - // target_R=BASIC_SPEED; - // target_L=BASIC_SPEED; - // //341691 7091059 - // } else i++; - } - if(i==4) { - susumu_ang(1,1/3,0); - /* if(angle>0) { - target_R=BASIC_SPEED; - target_L=BASIC_SPEED/3; - //7091059 8001278 - } else i++; - */ - } - if(i==5) { - susumu_y(1,1,1700); - // if(y<1700) { - // target_R=BASIC_SPEED; - // target_L=BASIC_SPEED; - // //8001278 8001700 - // } else i++; - motorR.stop(); - motorL.stop(); - } - - if(i==6) { - - i++; - }//gatiasari + if(i==6) { + motorR.stop(); + motorL.stop(); + t.start(); + if(t<1) { + motor_f=0.82; + motor_b=0; + } else { + motor_f=0; + motor_b=0; + printf("finish"); + t.reset(); + i++; + } + }//gatiasari - /* if(i==7) { - if(angle>=-89) { - target_R=BASIC_SPEED/4; - target_L=BASIC_SPEED*(-4); - } - if(angle<=-91) { - target_R=BASIC_SPEED/4; - target_L=BASIC_SPEED*(-4 - ); - } - if(angle>-91&angle<89){ - motorR.stop(); - motorL.stop(); - wait(0.5); - if(angle>-91&angle<89){ - i++;} - } - } - if(i==8) { - if(x>700) { - target_R=BASIC_SPEED; - target_L=BASIC_SPEED; - //8001700 7001700 - } else i++; + if(i==7) { + if(angle>=-89) { + target_R=BASIC_SPEED/5; + target_L=BASIC_SPEED/(-5); + } + if(angle<=-91) { + target_R=BASIC_SPEED/(-5); + target_L=BASIC_SPEED/-5; + } + if(angle>-91&angle<-89) { + motorR.stop(); + motorL.stop(); + wait(0.5); + if(angle>-91&angle<-89) { + i++; } - if(i==9) { - if(angle<0) { - target_R=BASIC_SPEED/3; - target_L=BASIC_SPEED; - //7001700 4002000 - } else i++; - } - if(i==10) { - if(y<2200) { - target_R=BASIC_SPEED; - target_L=BASIC_SPEED; - //4002000 4002200 - } else i++; + } + }//kakudo tyousei + if(i==8) { + susumu_xr(1,1,700); + } + if(i==9) { + susumu_ang(1/3,1,0); + } + if(i==10) { + susumu_y(1,1,2200); + } + if(i==11) { + susumu_y(1/3,1,2500); + } + if(i==12) { + motorR.stop(); + motorL.stop(); + servo.pulsewidth_us(1000); + wait(0.5); + i++; + } + if(i==13) { + if(angle>=91) { + target_R=BASIC_SPEED/5; + target_L=BASIC_SPEED/(-5); + } + if(angle<=89) { + target_R=BASIC_SPEED/(-5); + target_L=BASIC_SPEED/5; + } + if(angle>89&angle<91) { + motorR.stop(); + motorL.stop(); + wait(0.1); + if(angle>89&angle<91) { + i++; } - if(i==11) { - if(angle<90) { - target_R=BASIC_SPEED/3; - target_L=BASIC_SPEED; - //4002200 7002500 - } else i++; - } - if(i==12) { - if(x<1000) { - target_R=BASIC_SPEED; - target_L=BASIC_SPEED; - //7002500 10002500 - } else i++; - motorR.stop(); - motorL.stop(); - } + } + } + if(i==14) { + susumu_xl(1,1,1000); + } + if(i==15) { + motorR.stop(); + motorL.stop(); + wait(0.5); + denjiben=1; + wait(0.5); + servo.pulsewidth_us(1500); + wait(0.5); + i++; + } + if(i==16) { + susumu_xr(-1,-1,700); + } + if(i==17) { + susumu_ang(-1/3,-1,0); + } + if(i==18) { + susumu_y(-1,-1,2000); + } + if(i==19) { + susumu_ang(-1/3,-1,-90); + } + if(i==20) { + susumu_xl(-1,-1,1100); + } + if(i==21) { + motorR.stop(); + motorL.stop(); + servo.pulsewidth_us(1500); + wait(0.5); + denjiben=0; + wait(0.5); - if(i==13){ - i++; - } + break; + } - - - if(i==14) { - if(x>700) { - target_R=BASIC_SPEED*(-1); - target_L=BASIC_SPEED*(-1); - //10002500 7002500 - } else i++; - } - if(i==15) { - if(angle>0) { - target_R=BASIC_SPEED/(-3); - target_L=BASIC_SPEED/(-1); - //7002500 4002200 - } else i++; - } - if(i==16) { - if(y>2000) { - target_R=BASIC_SPEED*(-1); - target_L=BASIC_SPEED*(-1); - //4002200 4002000 - } else i++; - } - if(i==17) { - if(angle>-90) { - target_R=BASIC_SPEED/(-3); - target_L=BASIC_SPEED/(-1); - //4002000 7001700 - } else i++; - } - if(i==16) { - if(x<1100) { - target_R=BASIC_SPEED*(-1); - target_L=BASIC_SPEED*(-1); - //7001700 4002000 - } else i++; - } - */ - //kairyoutyuu kokomade - - }//while - motorR.stop(); - motorL.stop(); -//LR(0,0); - return 0; - + }//while + tgt(0,0); + return 0; + } }//intmain