Diff: main.cpp

Revision:

11:2c61a18f10c3

Parent:

10:01aa50804d85

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){