ケンタ ミヤザキ
Dependencies: PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter
Fork of
nhk_2018_undercarry_test09
by 
ケンタ ミヤザキ
Diff: main.cpp
- Revision:
- 8:91a72648f312
- Parent:
- 7:f1a924244b76
- Child:
- 9:0c1fce6d8094
--- a/main.cpp Fri Apr 06 08:22:03 2018 +0000
+++ b/main.cpp Sat Apr 07 05:10:02 2018 +0000
@@ -66,6 +66,8 @@
void touteki(float armpower, int pos, bool thro_mode);
void yellow_throwing();
+bool set_pos_flag = false;
+bool redthrowing = false;
bool yellow_throw_flag = false;
bool taiki_flag = false;
int crosscount = 0;
@@ -164,7 +166,14 @@
directmode = LEFT;
}else if(kbtread == START){
state = START;
+ }else if(kbtread == STAND_BY_1){
+ state = STAND_BY_1;
+ }else if(kbtread == YELLOW_RECEIVE){
+ state = YELLOW_RECEIVE;
+ crosscount = 5;
+ air = 0.0;
}
+
//--------------手動--------------------------------------
if(mode == true){
led2 = 1.0;
@@ -211,16 +220,23 @@
crosscount = 0;
directmode = STOP;
if(distance == 1.0){
- if(redIn == 1.0){
- buzzer = 1.0;
- redcount++;
- }else if(blueIn == 1.0){
- buzzer = 1.0;
- bluecount++;
- }else{
- buzzer = 0.0;
+ if(redthrowing == false){
+ if(redIn == 1.0){
+ buzzer = 1.0;
+ redcount++;
+ }else {
+ buzzer = 0.0;
+ }
+ }else if(redthrowing == true){
+ if(redIn == 1.0){
+ buzzer = 1.0;
+ bluecount++;
+ }else {
+ buzzer = 0.0;
+ }
}
if(redcount >= 100 ){
+ buzzer = 0;
servo_hand.move(hand_tsukami_degree);
servo1.move(hiji_tsukami_degree);
servo1.move(hiji_tsukami_degree);
@@ -230,15 +246,15 @@
servo1.move(hiji_tsukami_degree);
}
}else if(distance == 0.0 && redcount >= 100){
- servo2.move(koshi_nage_degree);
- servo2.move(koshi_nage_degree);
+ servo2.move(koshi_red_nage_degree);
+ servo2.move(koshi_red_nage_degree);
state = RED_RECEIVE;
directmode = STOP;
redcount = 0;
crosscount = 0;
}else if(distance == 0.0 && bluecount >= 100){
- servo2.move(koshi_nage_degree);
- servo2.move(koshi_nage_degree);
+ servo2.move(koshi_blue_nage_degree);
+ servo2.move(koshi_blue_nage_degree);
state = BLUE_RECEIVE_1;
directmode = STOP;
bluecount = 0;
@@ -252,11 +268,11 @@
}else if(crosscount == 1){
directmode = STOP;
prelinedata = 0;
- touteki(0.8,190,0);
+ touteki(1.0,195,0);
//syokikanomai2();
if(set_flag == false){
directmode = RIGHT;
- Linecount();
+ Linecount();
}
}else if(crosscount == 2){
directmode = STOP;
@@ -265,6 +281,7 @@
state = STAND_BY_1;
//taiki_flag = false;
crosscount = 0;
+ redthrowing = true;
}
}
break;
@@ -289,7 +306,7 @@
crosscount = 0;
directmode = STOP;
if(distance == 1.0){
- if(blueIn == 1.0){
+ if(redIn == 1.0){
buzzer = 1.0;
bluecount++;
}else if(micro_switch == 0.0){
@@ -306,14 +323,15 @@
air = 0.0;
}
}else if(distance == 0.0 && bluecount >= 100){
- servo2.move(koshi_nage_degree);
- servo2.move(koshi_nage_degree);
+ servo2.move(koshi_blue_nage_degree);
+ servo2.move(koshi_blue_nage_degree);
state = BLUE_RECEIVE_2;
directmode = STOP;
bluecount = 0;
crosscount = 0;
}else if(distance == 0.0 && yellowcount >= 500){
state = YELLOW_RECEIVE;
+ buzzer = 0.0;
directmode = STOP;
yellowcount = 0;
crosscount = 0;
@@ -326,7 +344,7 @@
}else if(crosscount == 1){
directmode = STOP;
prelinedata = 0;
- touteki(1.0,190,1);
+ touteki(1.0,200,1);
//syokikanomai2();
if(set_flag == false){
directmode = RIGHT;
@@ -406,11 +424,15 @@
timer.start();
}
int times = timer.read();
- if(times >= 0.5 && times < 1.5){
+ if(times >= 2.0 && times < 3.0){
+ servo_hand.move(hand_uke_degree);
+ }else if(times >= 3.0 && times < 4.0){
+ servo1.move(hiji_uke_degree);
+ }else if(times >= 4.0 && times < 5.0){
servo_hand.move(hand_tsukami_degree);
- }else if(times >=1.5 && times <2.5){
+ }else if(times >=5.0 && times <5.5){
servo2.move(koshi_nage_degree);
- }else if(times >= 2.5){
+ }else if(times >= 5.5){
yellow_throw_flag = true;
timer.stop();
timer.reset();
@@ -428,7 +450,7 @@
servo2.move(koshi_uke_degree);
motor_dir1=0;
motor_dir2=1;
- motor_pwm=0.15;
+ motor_pwm=0.12;
if(Enc_Arm.read_z() == 1){
syokika_flag=true;
buzzer = 1.0;
@@ -450,30 +472,37 @@
void syokikanomai2(void){
if(set_flag==false) {
encoder_count= Enc_Arm.read_rotate();
- if(yellow_throw_flag == true){
- servo_hand.move(hand_tsukami_degree);
- }else{
- servo_hand.move(hand_uke_degree);
+ servo_hand.move(hand_uke_degree);
+ servo1.move(hiji_nage_degree);//退避
+ servo1.move(hiji_nage_degree);//退避
+ if(yellow_throw_flag == false){
+ servo2.move(koshi_uke_degree);
+ servo2.move(koshi_uke_degree);
}
- servo1.move(hiji_nage_degree);//退避
- servo2.move(koshi_uke_degree);
- servo1.move(hiji_nage_degree);//退避
- servo2.move(koshi_uke_degree);
motor_dir1=0;
motor_dir2=1;
- motor_pwm=0.15;
+ motor_pwm=0.12;
if(Enc_Arm.read_z() == 1) {
syokika_flag = true;
buzzer = 1.0;
}
if (syokika_flag==true) {
- if(encoder_count == 170){
+ int set_pos;
+ if(yellow_throw_flag == true) set_pos = 164;
+ else set_pos = 170;
+ if(encoder_count == set_pos){
motor_pwm=0;
motor_dir1=1;
motor_dir2=1;
- servo_hand.move(hand_uke_degree);
- servo1.move(hiji_uke_degree);
- servo1.move(hiji_uke_degree);
+ if(yellow_throw_flag == false){
+ servo_hand.move(hand_uke_degree);
+ servo1.move(hiji_uke_degree);
+ servo1.move(hiji_uke_degree);
+ }else{
+ servo_hand.move(hand_tsukami_degree);
+ servo2.move(koshi_uke_degree);
+ servo2.move(koshi_uke_degree);
+ }
set_flag = true; //投てき後，falseにする.
taiki_flag = true;
yellow_throw_flag = false;
@@ -498,15 +527,19 @@
}
if(times >= 4.0 && times < 6.0){
lamp = 0.0;
- while(Enc_Arm.read_rotate() < pos) {
- motor_dir1 = 0;
- motor_dir2 = 1;
- motor_pwm = 0.15;
+ if(set_pos_flag == false){
+ if(Enc_Arm.read_rotate() < pos) {
+ motor_dir1 = 0;
+ motor_dir2 = 1;
+ motor_pwm = 0.15;
+ }else{
+ motor_dir1=1;
+ motor_dir2=1;
+ motor_pwm=0;
+ set_pos_flag = true;
+ }
}
}
- motor_dir1=1;
- motor_dir2=1;
- motor_pwm=0;
//----投てき-------------------------
if(throwing_mode == 0.0){
@@ -516,19 +549,26 @@
motor_pwm = armpower;
}
}else if(throwing_mode == 1.0){
- if(times >= 6.0 && times < 6.5){
+ if(times >= 6.0 && times < 6.1){
throwing_relay = 1.0;
}else{
- throwing_relay = 0.0;
+ throwing_relay = 0.0;
}
}
if(times >= 7.0 && times <8.0){
motor_pwm = 0.0;
- motor_dir1 = 0;
- motor_dir2 = 0;
+ motor_dir1 = 1;
+ motor_dir2 = 1;
buzzer = 0;
lamp = 1.0;
+ }else if(times >= 8.0 && yellow_throwing == false){
set_flag = false;
+ set_pos_flag = false;
+ timer.reset();
+ timer.stop();
+ }else if(times >= 10.0 && yellow_throw_flag == true){
+ set_flag = false;
+ set_pos_flag = false;
timer.reset();
timer.stop();
}