Dependencies: PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter
Fork of nhk_2018_undercarry_test09 by
Diff: main.cpp
- Revision:
- 9:0c1fce6d8094
- Parent:
- 8:91a72648f312
- Child:
- 10:01aa50804d85
diff -r 91a72648f312 -r 0c1fce6d8094 main.cpp --- a/main.cpp Sat Apr 07 05:10:02 2018 +0000 +++ b/main.cpp Sun Apr 08 09:56:29 2018 +0000 @@ -64,8 +64,10 @@ void syokikanomai(void); void syokikanomai2(void); void touteki(float armpower, int pos, bool thro_mode); -void yellow_throwing(); +void yellow_throwing(void); +void Linecount2(void); +bool set_pos_flag_2 = false; bool set_pos_flag = false; bool redthrowing = false; bool yellow_throw_flag = false; @@ -89,6 +91,7 @@ float y_dist = 0.0; float tar_x_dist = 0.0; float tar_x_dist_2 = 0.0; +float tar_x_dist_3 = 0.0; float tar_y_dist = 0.0; bool ontheline = false; int directmode = STRAIGHT; @@ -107,7 +110,7 @@ timer.stop(); throwing_relay=0; lamp=1; - air=1.0; + air=0.0; buzzer = 0.0; micro_switch.mode(PullUp); motor_pwm=0; @@ -171,7 +174,10 @@ }else if(kbtread == YELLOW_RECEIVE){ state = YELLOW_RECEIVE; crosscount = 5; - air = 0.0; + air = 1.0; + }else if(kbtread == STAND_BY_2){ + state = STAND_BY_2; + crosscount = 0;; } //--------------手動-------------------------------------- @@ -199,8 +205,11 @@ //------------------初期位置から待機位置1------------------------------------------ case START: if(crosscount == 0){ + directmode = LEFT; + Linecount2(); + }else if(crosscount == 1){ directmode = STRAIGHT; - }else if(crosscount == 1){ + }else if(crosscount == 2){ if(directmode == STRAIGHT){ directmode = LEFT; prelinedata = 0; @@ -208,7 +217,7 @@ directmode = LEFT; Linecount(); } - }else if(crosscount == 2){ + }else if(crosscount == 3){ directmode = STOP; state = STAND_BY_1; crosscount = 0; @@ -219,6 +228,7 @@ case STAND_BY_1: crosscount = 0; directmode = STOP; + if(distance == 1.0){ if(redthrowing == false){ if(redIn == 1.0){ @@ -235,24 +245,26 @@ buzzer = 0.0; } } - if(redcount >= 100 ){ + if(redcount >= 200 ){ buzzer = 0; servo_hand.move(hand_tsukami_degree); servo1.move(hiji_tsukami_degree); servo1.move(hiji_tsukami_degree); - }else if(bluecount >= 100){ + }else if(bluecount >= 200){ servo_hand.move(hand_tsukami_degree); servo1.move(hiji_tsukami_degree); servo1.move(hiji_tsukami_degree); - } - }else if(distance == 0.0 && redcount >= 100){ + } + } + if(distance == 0.0 && redcount >= 200){ + led2 = 1.0; 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){ + }else if(distance == 0.0 && bluecount >= 200){ servo2.move(koshi_blue_nage_degree); servo2.move(koshi_blue_nage_degree); state = BLUE_RECEIVE_1; @@ -263,12 +275,13 @@ break; //------------------待機位置1から投てき位置1---------------------------------------- case RED_RECEIVE: + led2 = 1.0; if(crosscount == 0){ directmode = LEFT; }else if(crosscount == 1){ directmode = STOP; prelinedata = 0; - touteki(1.0,195,0); + touteki(1.0,185,0); //syokikanomai2(); if(set_flag == false){ directmode = RIGHT; @@ -306,7 +319,7 @@ crosscount = 0; directmode = STOP; if(distance == 1.0){ - if(redIn == 1.0){ + if(redIn == 1.0 && micro_switch == 1){ buzzer = 1.0; bluecount++; }else if(micro_switch == 0.0){ @@ -315,21 +328,23 @@ }else{ buzzer = 0.0; } - if(bluecount >= 100){ + if(bluecount >= 200){ servo_hand.move(hand_tsukami_degree); servo1.move(hiji_tsukami_degree); servo1.move(hiji_tsukami_degree); }else if(yellowcount >= 500){ - air = 0.0; + air = 1.0; } - }else if(distance == 0.0 && bluecount >= 100){ + }else if(distance == 0.0 && bluecount >= 200){ 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){ + buzzer = 0.0; + } + if(yellowcount >= 1000){ state = YELLOW_RECEIVE; buzzer = 0.0; directmode = STOP; @@ -344,7 +359,7 @@ }else if(crosscount == 1){ directmode = STOP; prelinedata = 0; - touteki(1.0,200,1); + touteki(1.0,190,1); //syokikanomai2(); if(set_flag == false){ directmode = RIGHT; @@ -364,12 +379,14 @@ case YELLOW_RECEIVE: if(crosscount != 5){ directmode = LEFT; + servo2.move(koshi_uke_degree); + power = POWER + 0.1; }else{ directmode = STOP; if(yellow_throw_flag == false){ yellow_throwing(); }else{ - touteki(1.0,190,1.0); + touteki(1.0,184,1.0); syokikanomai2(); } } @@ -427,7 +444,7 @@ 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); + servo1.move(hiji_yellow_uke_degree); }else if(times >= 4.0 && times < 5.0){ servo_hand.move(hand_tsukami_degree); }else if(times >=5.0 && times <5.5){ @@ -457,7 +474,7 @@ } if (syokika_flag==true) { - if(encoder_count == 170){ + if(encoder_count == 165){ motor_pwm=0; motor_dir1=1; motor_dir2=1; @@ -486,11 +503,12 @@ syokika_flag = true; buzzer = 1.0; } - if (syokika_flag==true) { + if(syokika_flag==true) { int set_pos; - if(yellow_throw_flag == true) set_pos = 164; - else set_pos = 170; + if(yellow_throw_flag == true) set_pos = 165; + else set_pos = 165; if(encoder_count == set_pos){ + buzzer = 0.0; motor_pwm=0; motor_dir1=1; motor_dir2=1; @@ -498,6 +516,8 @@ servo_hand.move(hand_uke_degree); servo1.move(hiji_uke_degree); servo1.move(hiji_uke_degree); + servo1.move(hiji_uke_degree); + servo1.move(hiji_uke_degree); }else{ servo_hand.move(hand_tsukami_degree); servo2.move(koshi_uke_degree); @@ -512,20 +532,34 @@ } void touteki(float armpower ,int pos, bool throwing_mode){ - led2 = 1.0; if(set_flag == true){ timer.start(); } - int times = timer.read(); - if(times >= 2.0 && times < 3.0){ + int times = timer.read(); + if(times >= 1.0 && times < 4.0){ + if(yellow_throw_flag == true){ + if(set_pos_flag_2 == false){ + if(Enc_Arm.read_rotate() < 180) { + motor_dir1 = 0; + motor_dir2 = 1; + motor_pwm = 0.15; + }else{ + motor_dir1=1; + motor_dir2=1; + motor_pwm=0; + set_pos_flag_2 = true; + } + } + } + } + if(times >= 4.0 && times < 5.0){ servo_hand.move(hand_uke_degree); - led2 = 0.0; } - if(times >= 3.0 && times < 4.0){ + if(times >= 5.0 && times < 6.0){ servo1.move(hiji_nage_degree); servo1.move(hiji_nage_degree); } - if(times >= 4.0 && times < 6.0){ + if(times >= 6.0 && times < 9.0){ lamp = 0.0; if(set_pos_flag == false){ if(Enc_Arm.read_rotate() < pos) { @@ -543,32 +577,29 @@ //----投てき------------------------- if(throwing_mode == 0.0){ - if(times >= 6.0 && times < 7.0){ + if(times >= 9.0 && times < 10.0){ motor_dir1 = 0; motor_dir2 = 1; motor_pwm = armpower; } }else if(throwing_mode == 1.0){ - if(times >= 6.0 && times < 6.1){ + if(times >= 9.0 && times < 9.1){ throwing_relay = 1.0; }else{ throwing_relay = 0.0; } } - if(times >= 7.0 && times <8.0){ + if(times >= 10.0 && times <11.0){ motor_pwm = 0.0; motor_dir1 = 1; motor_dir2 = 1; buzzer = 0; lamp = 1.0; - }else if(times >= 8.0 && yellow_throwing == false){ + syokika_flag = false; + }else if(times >= 11.0){ 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; + set_pos_flag_2 = false; timer.reset(); timer.stop(); } @@ -598,6 +629,19 @@ } } +void Linecount2(void){ + if(directmode == LEFT){ + if(linecountcheck == false){ + tar_x_dist_3 = x_dist; + linecountcheck = true; + }else{ + if((tar_x_dist_3 - x_dist) >= X_STOP_DIST_4){ + crosscount++; + linecountcheck = false; + } + } + } +} void LineCheck(int dmode){ if(dmode == STRAIGHT){ if(linedata_1[1] == 1){