ケンタ ミヤザキ
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
--- 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){