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
diff -r f1a924244b76 -r 91a72648f312 main.cpp --- 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(); }