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:
- 3:7bd1afb46094
- Parent:
- 2:3176040a4777
- Child:
- 4:e3d739bf09b4
--- a/main.cpp Tue Sep 11 04:39:47 2018 +0000
+++ b/main.cpp Tue Sep 11 04:48:43 2018 +0000
@@ -295,6 +295,31 @@
}
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++;
+ }
+ }//gatiasari
+
+
+ if(i==7) {
t.start();
if(t<1) {
motor_f=0;
@@ -305,127 +330,103 @@
printf("finish");
t.reset();
}
- if(i==4) {
- susumu_ang(1,1/3,0);
- }
- if(i==5) {
- susumu_y(1,1,1700);
+ if(angle>=-89) {
+ target_R=BASIC_SPEED/5;
+ target_L=BASIC_SPEED/(-5);
}
- 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/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(angle<=-91) {
+ target_R=BASIC_SPEED/(-5);
+ target_L=BASIC_SPEED/-5;
}
- 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(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==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