ROBOSTEP_LIBRARY
F3RC4班 自動機プログラム by巨泉 速度制御ユニット使用の2輪(活かせなかった)、測定輪エンコーダ、MicroInfinityを用いている XY座標を読むことには成功したが、まともに制御できなかったのでノーカン()
Dependencies: CruizCore_R1370P EC delta enc_1multi mbed
Fork of
F3RC915
by 
春ロボ1班(元F3RC4班+)
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