Diff: main.cpp

Revision:

5:c5a2a5cf600d

Parent:

4:eabb7a738ff8

Child:

6:ff7fd6556a81

--- a/main.cpp	Wed Mar 28 06:49:41 2018 +0000
+++ b/main.cpp	Sun Apr 01 04:11:13 2018 +0000
@@ -2,7 +2,6 @@
 #include "config.h"
 #include "moter.h"
 #include "PID.h"
-#include "TCS3200.h"
 #include "encoder.h"
 #include "encoder2.h"
 #include "Serialservo.h"
@@ -16,8 +15,6 @@
 PID R_pid(RP, RI, RD, 0.0);
 PID L_pid(LP, LI, LD, 0.0);
 
-//DigitalIn zpin(PA_1);
-
 Encoder Enc_Carry_Y(PA_15,PA_14,PC_1);
 Encoder Enc_Carry_X(PA_4,PB_0,PB_7);
 Encoder2 Enc_Arm(PF_1,PA_0,PA_1);
@@ -28,7 +25,6 @@
 Serialservo servo1(PC_10, PC_11);//アームの肘
 Serialservo servo2(PC_10, PC_11);//回転軸
 XQ_servo servo_hand(PB_5);
-//TCS3200 color(PB_5, PB_2, PC_4);
 DigitalOut dpin(PC_9);
 DigitalIn distance(PC_2);
 DigitalIn redIn(PC_4);
@@ -36,17 +32,10 @@
 DigitalIn micro_switch(PC_0);
 DigitalOut air(PB_4);
 
-DigitalOut motor_dir1(PA_8);              //旧ピン番号
+DigitalOut motor_dir1(PA_8); 
 DigitalOut motor_dir2(PA_9);
 PwmOut motor_pwm(PB_3);
 
-/*
-PwmOut motor_pwm(PA_9);
-DigitalOut motor_dir1(PB_3);
-DigitalOut motor_dir2(PA_8);
-*/
-//DigitalOut motor_relay(PB_10);
-
 void linecheck(char *buff ,int data[2]);
 void LineCheck(int dmode);
 void syokikanomai(void);
@@ -114,8 +103,6 @@
     timer.stop();
     servo1.init(0);//アーム
     servo2.init(1);//回転軸
-    //servo2.move(50);
-    //servo1.move(152);  
     dpin=0;
     lamp=1;
     air=1.0;
@@ -299,11 +286,9 @@
                         if(redcount >= 100 ){
                             servo_hand.move(hand_tsukami_degree);
                             servo1.move(hiji_tsukami_degree);
-                            //pre_touteki();
                         }else if(bluecount >= 100){
                             servo_hand.move(hand_tsukami_degree);
                             servo1.move(hiji_tsukami_degree);
-                            //pre_touteki();
                         }
                     }else if(distance == 0.0 && redcount >= 100){
                         servo2.move(koshi_nage_degree);
@@ -317,13 +302,6 @@
                         bluecount = 0;
                         crosscount = 0;
                     }
-                    /*
-                    if(receive == RED_RECEIVE){
-                        state = RED_RECEIVE;
-                    }else if(receive == BLUE_RECEIVE){
-                        state = BLUE_RECEIVE_1;
-                    }
-                    */
                     break;
 //------------------待機位置1から投てき位置1----------------------------------------
                     case RED_RECEIVE:
@@ -375,56 +353,39 @@
                         }else{
                             buzzer = 0.0;
                         }
-                        if(bluecount >= 50){
-                            pre_touteki();
+                        if(bluecount >= 100){
+                            servo_hand.move(hand_tsukami_degree);
+                            servo1.move(hiji_tsukami_degree);
                         }else if(yellowcount >= 500){
-                            air = 1.0;
+                            air = 0.0;
                         }
-                    }else if(distance == 0.0 && bluecount >= 50){
+                    }else if(distance == 0.0 && bluecount >= 100){
+                        servo2.move(koshi_nage_degree);
                         state = BLUE_RECEIVE_2;
                         directmode = STOP;
                         bluecount = 0;
                         crosscount = 0;
                     }
-                    /*
-                    if(kbtread == BLUE_RECEIVE){
-                        state = BLUE_RECEIVE_2;
-                    }else if(kbtread == YELLOW_RECEIVE){
-                        state = YELLOW_RECEIVE;
-                    }
-                    */
                     break;
 //------------------投てき位置2---------------------------------------------------
                     case BLUE_RECEIVE_2:
                     if(crosscount == 0){
                         directmode = LEFT;
                     }else if(crosscount == 1){
-                        if(throwing == false){
-                            directmode = STOP;
-                            prelinedata = 0;
-                            if(kbtread == THROWING){
-                                throwing = true;
-                                buzzer = 1.0;
-                                //timer.start();
-                                prelinedata = 0;
-                            }
-                        }else{
-                            /*
-                            times = timer.read();
-                            if(times > 2){
-                                buzzer = 0.0;
-                                directmode = RIGHT;
-                                timer.reset();
-                                timer.stop();
-                            }
-                            */
+                        directmode = STOP;
+                        prelinedata = 0;
+                        touteki(1.0,190,1);
+                        if(set_flag == false){
+                            directmode = RIGHT;
                         }
                     }else if(crosscount == 2){
                         directmode = STOP;
-                        state = STAND_BY_2;
-                        throwing = false;
-                        crosscount = 0;
-                    }
+                        syokikanomai2();
+                        if(set_flag == true){
+                            state = STAND_BY_2;
+                            crosscount = 0;
+                        }
+                    }                   
                     break;
 //------------------投てき位置3---------------------------------------------------
                     case YELLOW_RECEIVE:
@@ -571,7 +532,7 @@
                 motor_dir2=1;
                 servo1.move(hiji_uke_degree);
                 servo1.move(hiji_uke_degree);
-                syokika_flag = false;
+                //syokika_flag = false;
                 set_flag = true;  //投てき後，falseにする.
             }
         }
@@ -632,10 +593,18 @@
     motor_pwm=0;
     //wait(2.0);
 //----投てき-------------------------
-    if(times >= 5.0 && times < 6.0){
-        motor_dir1 = 0;
-        motor_dir2 = 1;
-        motor_pwm = armpower;
+    if(thro_mode == 0.0){
+        if(times >= 5.0 && times < 6.0){
+            motor_dir1 = 0;
+            motor_dir2 = 1;
+            motor_pwm = armpower;
+        }
+    }else if(thro_mode == 1.0){
+        if(times >= 5.0 && times < 5.5){
+            dpin = 1.0;
+        }else{
+            dpin = 0.0;
+        }
     }
     //wait(1.0);
     if(times >= 6.0 && times <7.0){