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:
- 4:e3d739bf09b4
- Parent:
- 3:7bd1afb46094
- Child:
- 5:0160b73f3d9e
--- a/main.cpp Tue Sep 11 04:48:43 2018 +0000
+++ b/main.cpp Tue Sep 11 06:42:51 2018 +0000
@@ -7,7 +7,7 @@
#define BASIC_SPEED 30 //モーターはこの角速度で駆動させる
SpeedControl motorR(PB_13,PA_10,NC,500,0.05,PB_10,PB_1); //right enc migi ue
-SpeedControl motorL(PB_3,PB_5,NC,500,0.05,PA_5,PA_7); //left enc hidari sita //ok
+SpeedControl motorL(PB_5,PB_3,NC,500,0.05,PA_5,PA_7); //left enc hidari sita //ok
Ec EC1(PB_4,PA_8,NC,300,0.05); //center enc
Ticker motor_tick; //角速度計算用ticker
Ticker ticker;//for enc
@@ -39,7 +39,7 @@
double x=185;
double y=150;//start point//keisann wo sinaosu hitsuyouga arimasu
Timer t;
-int i=0;
+int i=1;
int kai=0;//printf kansuu
double target_R=0,target_L=0;
@@ -264,169 +264,169 @@
pc.printf("i=%d\r\n",i);
//pc.printf("EC1=%f",EC1.getDistance_mm(),EC1.getCount());
pc.printf("x=%f y=%f",x,y);
- //pc.printf("angle=%f\r\n",angle);
+ pc.printf("angle=%f\r\n",angle);
//機体の進む方向、右モーターの角速度、左モーターの角速度を表示
kai=0;
}
kai++;
if(i==0) {
- if(reset_f.read()==1) {
+ /*if(reset_f.read()==1) {
wait(0.05);
if(reset_f.read()==1) {
- denjiben=0;
- i++;
- }
- }
+ */ denjiben=0;
+ i++;
+ // }
+ //}
+ if(reset_a.read()==1) {
+ wait(0.05);
if(reset_a.read()==1) {
- wait(0.05);
- if(reset_a.read()==1) {
- denjiben=0;
- x=70;
- y=2500;
- i=14;
- }
+ denjiben=0;
+ x=70;
+ y=2500;
+ i=14;
}
}
- if(i==1) {
- susumu_y(1,1,407);
- }
- if(i==2) {
- susumu_ang(1/3,1,45);
- }
- if(i==3) {
- susumu_xl(1,1,709);
- }
- if(i==4) {
- susumu_ang(1,1/3,0);
- }
- if(i==5) {
- susumu_y(1,1,1700);
+ }
+ if(i==1) {
+ susumu_y(1,1,407);
+ }
+ if(i==2) {
+ susumu_ang(1/3,1,45);
+ }
+ if(i==3) {
+ susumu_xl(1,1,709);
+ }
+ if(i==4) {
+ susumu_ang(1,1/3,0);
+ }
+ if(i==5) {
+ susumu_y(1,1,1700);
+ }
+ 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++;
}
- 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
+ }//gatiasari
- if(i==7) {
- t.start();
- if(t<1) {
- motor_f=0;
- motor_b=0.82;
- } else {
- motor_f=0;
- motor_b=0;
- printf("finish");
- t.reset();
- }
- 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++;
- }
- }
- }//kakudo tyousei
- if(i==8) {
- susumu_xr(1,1,700);
- }
- if(i==9) {
- susumu_ang(1/3,1,0);
+ if(i==7) {
+ t.start();
+ if(t<1) {
+ motor_f=0;
+ motor_b=0.82;
+ } else {
+ motor_f=0;
+ motor_b=0;
+ printf("finish");
+ t.reset();
}
- if(i==10) {
- susumu_y(1,1,2200);
- }
- if(i==11) {
- susumu_ang(1/3,1,90);
- }
- if(i==12) {
- motorR.stop();
- motorL.stop();
- servo.pulsewidth_us(1000);
- wait(0.5);
- i++;
+ if(angle>=-89) {
+ target_R=BASIC_SPEED/5;
+ target_L=BASIC_SPEED/(-5);
}
- 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(angle<=-91) {
+ target_R=BASIC_SPEED/(-5);
+ target_L=BASIC_SPEED/-5;
}
- if(i==14) {
- susumu_xl(1,1,1000);
- }
- if(i==15) {
+ if(angle>-91&angle<-89) {
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(angle>-91&angle<-89) {
+ i++;
+ }
}
- if(i==17) {
- susumu_ang(-1/3,-1,0);
- }
- if(i==18) {
- susumu_y(-1,-1,2000);
+ }//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_ang(1/3,1,90);
+ }
+ 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(i==19) {
- susumu_ang(-1/3,-1,-90);
+ if(angle<=89) {
+ target_R=BASIC_SPEED/(-5);
+ target_L=BASIC_SPEED/5;
}
- if(i==20) {
- susumu_xl(-1,-1,1100);
- }
- if(i==21) {
+ if(angle>89&angle<91) {
motorR.stop();
motorL.stop();
- servo.pulsewidth_us(1500);
- wait(0.5);
- denjiben=0;
- wait(0.5);
+ wait(0.1);
+ if(angle>89&angle<91) {
+ i++;
+ }
+ }
+ }
+ 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);
- break;
- }
+ break;
+ }
- }//while
- tgt(0,0);
- return 0;
+}//while
+tgt(0,0);
+return 0;
}//intmain