Revision 7:f1a924244b76, committed 2018-04-06

Comitter:

kenken0721

Date:

Fri Apr 06 08:22:03 2018 +0000

Parent:

6:ff7fd6556a81

Commit message:

Changed in this revision

diff -r ff7fd6556a81 -r f1a924244b76 config.h
--- a/config.h	Sun Apr 01 11:58:23 2018 +0000
+++ b/config.h	Fri Apr 06 08:22:03 2018 +0000
@@ -10,8 +10,10 @@
 #define LP 0.024
 #define LI 0.0
 #define LD 0.00000
-#define X_STOP_DIST 1.2
-#define Y_STOP_DIST 1.2
+#define X_STOP_DIST   1.2
+#define X_STOP_DIST_2 7.5
+#define X_STOP_DIST_3 4.0
+#define Y_STOP_DIST   1.2
 #define addr1 0x24
 #define addr2 0x10
 #define addr3 0x64
@@ -19,11 +21,14 @@
 
 #define koshi_uke_degree 152
 #define koshi_nage_degree 62
+#define koshi_red _nage_degree 62
+#define koshi_blue_nage_degree 62
+
 #define hiji_uke_degree 225
 #define hiji_tsukami_degree 230
 #define hiji_nage_degree 260
-#define hand_uke_degree 0.999
-#define hand_tsukami_degree 0.001
+#define hand_uke_degree 1.0
+#define hand_tsukami_degree 0.0
 
 
 #define STRAIGHT        1

diff -r ff7fd6556a81 -r f1a924244b76 main.cpp
--- a/main.cpp	Sun Apr 01 11:58:23 2018 +0000
+++ b/main.cpp	Fri Apr 06 08:22:03 2018 +0000
@@ -32,7 +32,7 @@
 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);
 
@@ -60,16 +60,18 @@
 
 void linecheck(char *buff ,int data[2]);
 void LineCheck(int dmode);
+void Linecount(void);
 void syokikanomai(void);
 void syokikanomai2(void);
 void touteki(float armpower, int pos, bool thro_mode);
+void yellow_throwing();
 
+bool yellow_throw_flag = false;
+bool taiki_flag = false;
 int crosscount = 0;
 bool mode = true;//trueでラジコン、falseでオート
 bool stopflag = true;//trueで機体停止
 double power = POWER;
-double rpower = POWER;
-double lpower = POWER;
 int kbtread = 0;
 int count = 0;
 int counter = 0;
@@ -84,6 +86,7 @@
 float x_dist = 0.0;
 float y_dist = 0.0;
 float tar_x_dist = 0.0;
+float tar_x_dist_2 = 0.0;
 float tar_y_dist = 0.0;
 bool ontheline = false;
 int directmode = STRAIGHT;
@@ -95,6 +98,7 @@
 bool syokika_flag=false;
 bool set_flag = false;
 int encoder_count = 1000;
+int linecountcheck = false;
 
 int main() {
     timer.reset();
@@ -127,12 +131,17 @@
     buzzer = 0;
 //-----------メインループ----------------------------------------
     while (true) {
-        master.read(addr1,buff1,2);
-        linecheck(buff1,linedata_1);
-        master.read(addr2,buff2,2);
-        linecheck(buff2,linedata_2);
-        master.read(addr3,buff3,2);
-        linecheck(buff3,linedata_3);
+        if(directmode == STRAIGHT){
+            master.read(addr1,buff1,2);
+            linecheck(buff1,linedata_1);
+        }else if(directmode == RIGHT){
+            master.read(addr2,buff2,2);
+            linecheck(buff2,linedata_2);
+        }else if(directmode == LEFT){
+            master.read(addr3,buff3,2);
+            linecheck(buff3,linedata_3);
+        }
+        
         x_dist = Enc_Carry_X.read_rotate();
         y_dist = Enc_Carry_Y.read_rotate();
         kbtpc.putc(state);
@@ -188,6 +197,7 @@
                             prelinedata = 0;
                         }else if(directmode == LEFT){
                             directmode = LEFT;
+                            Linecount();
                         }
                     }else if(crosscount == 2){
                         directmode = STOP;
@@ -227,6 +237,8 @@
                         redcount = 0;
                         crosscount = 0;
                     }else if(distance == 0.0 && bluecount >= 100){
+                        servo2.move(koshi_nage_degree);
+                        servo2.move(koshi_nage_degree);
                         state = BLUE_RECEIVE_1;
                         directmode = STOP;
                         bluecount = 0;
@@ -241,14 +253,17 @@
                         directmode = STOP;
                         prelinedata = 0;
                         touteki(0.8,190,0);
+                        //syokikanomai2();
                         if(set_flag == false){
                             directmode = RIGHT;
+                            Linecount(); 
                         }
                     }else if(crosscount == 2){
                         directmode = STOP;
                         syokikanomai2();
                         if(set_flag == true){
                             state = STAND_BY_1;
+                            //taiki_flag = false;
                             crosscount = 0;
                         }
                     }
@@ -261,6 +276,7 @@
                         directmode = STRAIGHT;
                     }else if(crosscount == 2){
                         directmode = LEFT;
+                        Linecount();
                     }else if(crosscount == 3){
                         directmode = STOP;
                         prelinedata = 0;
@@ -276,7 +292,7 @@
                         if(blueIn == 1.0){
                             buzzer = 1.0;
                             bluecount++;
-                        }else if(micro_switch == 1.0){
+                        }else if(micro_switch == 0.0){
                             buzzer = 1.0;
                             yellowcount++;
                         }else{
@@ -296,6 +312,11 @@
                         directmode = STOP;
                         bluecount = 0;
                         crosscount = 0;
+                    }else if(distance == 0.0 && yellowcount >= 500){
+                        state = YELLOW_RECEIVE;
+                        directmode = STOP;
+                        yellowcount = 0;
+                        crosscount = 0;
                     }
                     break;
 //------------------投てき位置2---------------------------------------------------
@@ -306,24 +327,33 @@
                         directmode = STOP;
                         prelinedata = 0;
                         touteki(1.0,190,1);
+                        //syokikanomai2();
                         if(set_flag == false){
                             directmode = RIGHT;
+                            Linecount(); 
                         }
                     }else if(crosscount == 2){
                         directmode = STOP;
                         syokikanomai2();
                         if(set_flag == true){
                             state = STAND_BY_2;
+                            //taiki_flag = false;
                             crosscount = 0;
                         }
-                    }                   
+                    }
                     break;
 //------------------投てき位置3---------------------------------------------------
                     case YELLOW_RECEIVE:
-                    if(crosscount != 4){
+                    if(crosscount != 5){
                         directmode = LEFT;
                     }else{
                         directmode = STOP;
+                        if(yellow_throw_flag == false){
+                            yellow_throwing();
+                        }else{
+                            touteki(1.0,190,1.0);
+                            syokikanomai2();   
+                        }
                     }
                     break;
                     default:
@@ -337,37 +367,25 @@
                     case STRAIGHT:
                     output = S_pid.compute((double)linedata_1[0]);
                     if(output >= 0){
-                        rpower = power;
-                        lpower = power + output;
-                        Straight(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
+                        Straight(power, power + output, pwm_pins, dir_pins_1, dir_pins_2);
                     }else {
-                        rpower = power + (-1 * output);
-                        lpower = power;
-                        Straight(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
+                        Straight(power + (-1 * output), power, pwm_pins, dir_pins_1, dir_pins_2);
                     }
                     break;
                     case RIGHT:
                     output = R_pid.compute((double)linedata_2[0]);
                     if(output >= 0){
-                        rpower = power;
-                        lpower = power + output;
-                        Right(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
+                        Right(power, power + output, pwm_pins, dir_pins_1, dir_pins_2);
                     }else {
-                        rpower = power + (-1 * output);
-                        lpower = power;
-                        Right(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
+                        Right(power + (-1 * output), power, pwm_pins, dir_pins_1, dir_pins_2);
                     }
                     break;
                     case LEFT:
                     output = L_pid.compute((double)linedata_3[0]);
                     if(output >= 0){
-                        rpower = power;
-                        lpower = power + output;
-                        Left(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
+                        Left(power, power + output, pwm_pins, dir_pins_1, dir_pins_2);
                     }else {
-                        rpower = power + (-1 * output);
-                        lpower = power;
-                        Left(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
+                        Left(power + (-1 * output), power, pwm_pins, dir_pins_1, dir_pins_2);
                     }
                     break;
                     default:
@@ -375,29 +393,30 @@
                 }
             }
         }
-        //pc.printf("kbtread = %d \r" ,kbtread); 
-        //pc.printf("buff1[0] = %d \r" ,buff1[0]);
-        //pc.printf("buff2[0] = %d \r" ,buff2[0]);
-        //pc.printf("line1 = %d \r" ,linedata_1[0]);
-        //pc.printf("line2 = %d \r" ,linedata_3[1]);
         pc.printf("xdist = %f \r",x_dist);
-        //pc.printf("tar_xdist = %f \r",tar_x_dist);
-        //pc.printf("ontheline = %d \r",ontheline);
-        
         pc.printf("ydist = %f \r",y_dist);
-        //pc.printf("tary = %f \r",tar_y_dist);
-        //pc.printf("stopflag = %d \r",stopflag);
-        //pc.printf("cross = %d \r",crosscount);
-        //pc.printf("buff[1] = %d" ,buff1[1]);
-        //pc.printf("line    = %d" ,line);
-        //pc.printf("rpower    = %lf" ,rpower);
-        //pc.printf("lpower    = %lf" ,lpower);  
-        //pc.printf("dicmode = %d \r",directmode);
         pc.printf("output  = %lf \n" ,output);
     }
 }
 
-//---------------初期化---------------------------
+//---------------初期化----------------------------------------------------------
+
+void yellow_throwing(){
+    if(yellow_throw_flag == false){
+        timer.start();
+    }
+    int times = timer.read();
+    if(times >= 0.5 && times < 1.5){
+        servo_hand.move(hand_tsukami_degree);
+    }else if(times >=1.5 && times <2.5){
+        servo2.move(koshi_nage_degree);
+    }else if(times >= 2.5){
+        yellow_throw_flag = true;
+        timer.stop();
+        timer.reset();
+    }
+}
+
 void syokikanomai(void){
     syokika_flag = false;
     while (set_flag==false) {
@@ -410,13 +429,13 @@
         motor_dir1=0;
         motor_dir2=1;
         motor_pwm=0.15;
-        if(Enc_Arm.read_z() == 1) {
+        if(Enc_Arm.read_z() == 1){
             syokika_flag=true;
             buzzer = 1.0;
         }
         
         if (syokika_flag==true) {
-            if(encoder_count == 170) {
+            if(encoder_count == 170){
                 motor_pwm=0;
                 motor_dir1=1;
                 motor_dir2=1;
@@ -430,8 +449,12 @@
 
 void syokikanomai2(void){
     if(set_flag==false) {
-        encoder_count= Enc_Arm.read_rotate(); 
-        servo_hand.move(hand_uke_degree);
+        encoder_count= Enc_Arm.read_rotate();
+        if(yellow_throw_flag == true){
+            servo_hand.move(hand_tsukami_degree);
+        }else{
+            servo_hand.move(hand_uke_degree);
+        }
         servo1.move(hiji_nage_degree);//退避
         servo2.move(koshi_uke_degree);
         servo1.move(hiji_nage_degree);//退避
@@ -444,30 +467,36 @@
             buzzer = 1.0;
         }
         if (syokika_flag==true) {
-            if(encoder_count == 170) {
+            if(encoder_count == 170){
                 motor_pwm=0;
                 motor_dir1=1;
                 motor_dir2=1;
+                servo_hand.move(hand_uke_degree);
                 servo1.move(hiji_uke_degree);
                 servo1.move(hiji_uke_degree);
                 set_flag = true;  //投てき後，falseにする.
+                taiki_flag = true;
+                yellow_throw_flag = false;
             }
         }
     }
 }
 
 void touteki(float armpower ,int pos, bool throwing_mode){
+    led2 = 1.0;
     if(set_flag == true){
         timer.start();
     }
     int times = timer.read(); 
-    if(times >= 1.0 && times < 2.0){
+    if(times >= 2.0 && times < 3.0){
     servo_hand.move(hand_uke_degree);
+    led2 = 0.0;
     }    
-    if(times >= 2.0 && times < 3.0){
+    if(times >= 3.0 && times < 4.0){
+    servo1.move(hiji_nage_degree);
     servo1.move(hiji_nage_degree);
     }
-    if(times >= 3.0 && times < 5.0){
+    if(times >= 4.0 && times < 6.0){
         lamp = 0.0;
         while(Enc_Arm.read_rotate() < pos) {
             motor_dir1 = 0;
@@ -478,21 +507,22 @@
     motor_dir1=1;
     motor_dir2=1;
     motor_pwm=0;
+    
 //----投てき-------------------------
     if(throwing_mode == 0.0){
-        if(times >= 5.0 && times < 6.0){
+        if(times >= 6.0 && times < 7.0){
             motor_dir1 = 0;
             motor_dir2 = 1;
             motor_pwm = armpower;
         }
     }else if(throwing_mode == 1.0){
-        if(times >= 5.0 && times < 5.5){
+        if(times >= 6.0 && times < 6.5){
             throwing_relay = 1.0;
         }else{
             throwing_relay = 0.0;
         }
     }
-    if(times >= 6.0 && times <7.0){
+    if(times >= 7.0 && times <8.0){
         motor_pwm = 0.0;
         motor_dir1 = 0;
         motor_dir2 = 0;
@@ -504,6 +534,30 @@
     }
 }
 
+void Linecount(void){
+    if(directmode == RIGHT){
+        if(linecountcheck == false){
+            tar_x_dist_2 = x_dist;
+            linecountcheck = true;
+        }else{
+            if((x_dist - tar_x_dist_2) > X_STOP_DIST_3){
+                crosscount++;
+                linecountcheck = false;
+            }
+        }
+    }else if(directmode == LEFT){
+        if(linecountcheck == false){
+            tar_x_dist_2 = x_dist;
+            linecountcheck = true;
+        }else{
+            if((tar_x_dist_2 - x_dist) >= X_STOP_DIST_2){
+                crosscount++;
+                linecountcheck = false;
+            }
+        }
+    }
+}
+
 void LineCheck(int dmode){
     if(dmode == STRAIGHT){
         if(linedata_1[1] == 1){