ケンタ ミヤザキ
Dependencies: PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter
Fork of
nhk_2018_undercarry_test10
by 
ケンタ ミヤザキ
Revision 11:2c61a18f10c3, committed 2018-04-15
- Comitter:
- kenken0721
- Date:
- Sun Apr 15 01:33:59 2018 +0000
- Parent:
- 10:01aa50804d85
- Commit message:
Changed in this revision
| config.h | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 01aa50804d85 -r 2c61a18f10c3 config.h --- a/config.h Sun Apr 15 00:36:15 2018 +0000 +++ b/config.h Sun Apr 15 01:33:59 2018 +0000 @@ -43,7 +43,7 @@ #define hiji_uke_degree 225 #define hiji_tsukami_degree 230//赤、青を受け取る際の角度 #define hiji_yellow_uke_degree 237//ラックから受け取る際の角度 -#define hiji_nage_degree 270//投てき時退避位置 +#define hiji_nage_degree 265//投てき時退避位置 //アームの手 #define hand_uke_degree 1.0//受け取り前
diff -r 01aa50804d85 -r 2c61a18f10c3 main.cpp
--- a/main.cpp Sun Apr 15 00:36:15 2018 +0000
+++ b/main.cpp Sun Apr 15 01:33:59 2018 +0000
@@ -63,7 +63,9 @@
void Linecount(void);
void syokikanomai(void);
void syokikanomai2(void);
-void touteki(float armpower, int pos, bool thro_mode);
+//void touteki(float armpower, int pos, bool thro_mode);
+void touteki_1(float armpower, int pos, bool thro_mode);
+void touteki_2(float armpower, int pos);
void yellow_throwing(void);
void Linecount2(void);
@@ -283,7 +285,7 @@
}else if(crosscount == 1){
directmode = STOP;
prelinedata = 0;
- touteki(1.0,RED_TOUTEKI,0);
+ touteki_1(1.0,RED_TOUTEKI,0);
//syokikanomai2();
if(set_flag == false){
directmode = RIGHT;
@@ -361,7 +363,7 @@
}else if(crosscount == 1){
directmode = STOP;
prelinedata = 0;
- touteki(1.0,BLUE_TOUTEKI,1);
+ touteki_1(1.0,BLUE_TOUTEKI,1);
//syokikanomai2();
if(set_flag == false){
directmode = RIGHT;
@@ -388,7 +390,7 @@
if(yellow_throw_flag == false){
yellow_throwing();
}else{
- touteki(1.0,YELLOW_TOUTEKI,1.0);
+ touteki_2(1.0,YELLOW_TOUTEKI);
syokikanomai2();
}
}
@@ -443,15 +445,15 @@
timer.start();
}
int times = timer.read();
- if(times >= 2.0 && times < 3.0){
+ if(times >= 1.0 && times < 2.0){
servo_hand.move(hand_uke_degree);
- }else if(times >= 3.0 && times < 4.0){
+ }else if(times >= 2.0 && times < 3.0){
servo1.move(hiji_yellow_uke_degree);
- }else if(times >= 4.0 && times < 5.0){
+ }else if(times >= 3.0 && times < 4.0){
servo_hand.move(hand_tsukami_degree);
- }else if(times >=5.0 && times <5.5){
+ }else if(times >=4.0 && times <4.5){
servo2.move(koshi_nage_degree);
- }else if(times >= 5.5){
+ }else if(times >= 4.5){
yellow_throw_flag = true;
timer.stop();
timer.reset();
@@ -533,6 +535,128 @@
}
}
+//赤,青色投てき
+void touteki_1(float armpower ,int pos, bool throwing_mode){
+ if(set_flag == true){
+ timer.start();
+ }
+ int times = timer.read();
+ if(times >= 0.5 && times < 1.5){
+ servo_hand.move(hand_uke_degree);
+ }
+ if(times >= 1.5 && times < 2.5){
+ servo1.move(hiji_nage_degree);
+ servo1.move(hiji_nage_degree);
+ }
+ if(times >= 2.5 && times < 5.0){
+ lamp = 0.0;
+ 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;
+ }
+ }
+ }
+ if(throwing_mode == 0.0){
+ if(times >= 5.0 && times < 6.0){
+ motor_dir1 = 0;
+ motor_dir2 = 1;
+ motor_pwm = armpower;
+ }
+ }else if(throwing_mode == 1.0){
+ if(times >= 5.0 && times < 5.1){
+ throwing_relay = 1.0;
+ }else{
+ throwing_relay = 0.0;
+ }
+ }
+ if(times >= 6.0 && times <6.5){
+ motor_pwm = 0.0;
+ motor_dir1 = 1;
+ motor_dir2 = 1;
+ buzzer = 0;
+ lamp = 1.0;
+ syokika_flag = false;
+ }else if(times >= 6.5){
+ set_flag = false;
+ set_pos_flag = false;
+ set_pos_flag_2 = false;
+ timer.reset();
+ timer.stop();
+ }
+}
+
+//黄色投てき
+void touteki_2(float armpower ,int pos){
+ if(set_flag == true){
+ timer.start();
+ }
+ int times = timer.read();
+ if(times >= 0.5 && times < 2.5){
+ if(set_pos_flag_2 == 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_2 = true;
+ }
+ }
+ }
+ if(times >= 2.5 && times < 3.5){
+ servo_hand.move(hand_uke_degree);
+ }
+ if(times >= 3.5 && times < 4.5){
+ servo1.move(hiji_nage_degree);
+ servo1.move(hiji_nage_degree);
+ }
+ /*
+ if(times >= 4.5 && times < 6.5){
+ lamp = 0.0;
+ 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;
+ }
+ }
+ }
+ */
+ if(times >= 4.5 && times < 4.6){
+ throwing_relay = 1.0;
+ }else{
+ throwing_relay = 0.0;
+ }
+ if(times >= 4.6 && times <5.6){
+ motor_pwm = 0.0;
+ motor_dir1 = 1;
+ motor_dir2 = 1;
+ buzzer = 0;
+ lamp = 1.0;
+ syokika_flag = false;
+ }else if(times >= 5.6){
+ set_flag = false;
+ set_pos_flag = false;
+ set_pos_flag_2 = false;
+ timer.reset();
+ timer.stop();
+ }
+}
+/*
void touteki(float armpower ,int pos, bool throwing_mode){
if(set_flag == true){
timer.start();
@@ -561,7 +685,7 @@
servo1.move(hiji_nage_degree);
servo1.move(hiji_nage_degree);
}
- if(times >= 6.0 && times < 9.0){
+ if(times >= 6.0 && times < 8.5){
lamp = 0.0;
if(set_pos_flag == false){
if(Enc_Arm.read_rotate() < pos) {
@@ -606,7 +730,7 @@
timer.stop();
}
}
-
+*/
void Linecount(void){
if(directmode == RIGHT){
if(linecountcheck == false){