Revision 3:aaa2fde4fafd, committed 2018-03-24

Comitter:

kenken0721

Date:

Sat Mar 24 10:50:25 2018 +0000

Parent:

2:c5996dd62e9c

Commit message:

Changed in this revision

diff -r c5996dd62e9c -r aaa2fde4fafd PID.lib
--- a/PID.lib	Fri Mar 23 12:34:57 2018 +0000
+++ b/PID.lib	Sat Mar 24 10:50:25 2018 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/kenken0721/code/PID/#09a00a9407f8
+https://os.mbed.com/users/kenken0721/code/PID/#3bc8034f569e

diff -r c5996dd62e9c -r aaa2fde4fafd config.h
--- a/config.h	Fri Mar 23 12:34:57 2018 +0000
+++ b/config.h	Sat Mar 24 10:50:25 2018 +0000
@@ -3,17 +3,18 @@
 #define scl PB_8
 #define SP 0.02
 #define SI 0.00
-#define SD 0.00
-#define RP 0.03
+#define SD 0.00005
+#define RP 0.02
 #define RI 0.0
-#define RD 0.0
-#define LP 0.03
+#define RD 0.00
+#define LP 0.024
 #define LI 0.0
-#define LD 0.0
-#define STOP_DIST 1.2
+#define LD 0.00000
+#define X_STOP_DIST 1.2
+#define Y_STOP_DIST 1.2
 #define addr1 0x24
 #define addr2 0x10
-#define addr3 0x01
+#define addr3 0x64
 
 
 #define STRAIGHT        1
@@ -30,6 +31,4 @@
 #define STAND_BY_1      12
 #define STAND_BY_2      13
 #define THROWING        14
-
-
-
+#define MANUAL          15
\ No newline at end of file

diff -r c5996dd62e9c -r aaa2fde4fafd main.cpp
--- a/main.cpp	Fri Mar 23 12:34:57 2018 +0000
+++ b/main.cpp	Sat Mar 24 10:50:25 2018 +0000
@@ -13,6 +13,9 @@
 PID L_pid(LP, LI, LD, 0.0);
 Encoder Enc_Carry_Y(PA_15,PA_14,PC_1);
 Encoder Enc_Carry_X(PA_4,PB_0,PB_7);
+DigitalOut buzzer(PA_10);
+DigitalOut lamp(PC_3);
+Timer timer;
 void linecheck(char *buff ,int data[2]);
 void LineCheck(int dmode);
 
@@ -60,25 +63,44 @@
 float tar_y_dist = 0.0;
 bool ontheline = false;
 int directmode = STRAIGHT;
-
-int main() {    
+int state = START;
+int times = 0;
+bool throwing = false;
+int main() {
+    timer.reset();
+    timer.stop();
+    //timer.start();
     S_pid.init();
+    R_pid.init();
+    L_pid.init();
     pwm_pins[0].period(0.00005);
     pwm_pins[1].period(0.00005);
     pwm_pins[2].period(0.00005);
     pwm_pins[3].period(0.00005);
+    pc.baud(115200);
     kbtpc.baud(9600);
     master.frequency(100000);
+    lamp = 1.0;
     while (true) {
-        if(directmode == STRAIGHT) master.read(addr1,buff1,2);
-        if(directmode == RIGHT) master.read(addr2,buff2,2);
-        if(directmode == LEFT) master.read(addr3,buff3,2);
-            
+        //if(directmode == STRAIGHT){
+            master.read(addr1,buff1,2);
+            linecheck(buff1,linedata_1);
+        //}
+        //if(directmode == RIGHT){
+            master.read(addr2,buff2,2);
+            linecheck(buff2,linedata_2);
+        //}
+        //if(directmode == LEFT){
+            master.read(addr3,buff3,2);
+            linecheck(buff3,linedata_3);
+        //}
         //master.read(addr1,buff1,2);
-        linecheck(buff1,linedata_1);
+        //linecheck(buff1,linedata_1);
+        //linecheck(buff1,linedata_1);
+        //linecheck(buff1,linedata_1);
         x_dist = Enc_Carry_X.read_rotate();
         y_dist = Enc_Carry_Y.read_rotate();
-        kbtpc.putc(1);
+        kbtpc.putc(state);
         if(kbtpc.readable()){
             kbtread = kbtpc.getc();
         }
@@ -96,6 +118,10 @@
             directmode = RIGHT;
         }else if(kbtread == LEFT){
             directmode = LEFT;
+        }else if(kbtread == MANUAL){
+            state = MANUAL;
+        }else if(kbtread == START){
+            state = START;
         }
 //--------------手動--------------------------------------
         if(mode == true){
@@ -114,23 +140,155 @@
 //--------------自動---------------------------------
         }else if(mode == false){
             led2 = 0.0;
-            
+            //timer.reset();
+            //timer.start();
             if(stopflag == true){
                 Stop(pwm_pins, dir_pins_1, dir_pins_2);
             }else{
                 LineCheck(directmode);
+                switch(state){
+                    case MANUAL:
+                    if(crosscount == 1){
+                        directmode = STOP;
+                        Stop(pwm_pins, dir_pins_1, dir_pins_2);
+                        crosscount = 0;
+                    }
+                    break;
+                    
+                    //------------------------------------------
+                    case START:
+                    if(crosscount == 0){
+                        directmode = STRAIGHT;
+                    }else if(crosscount == 1 && directmode == STRAIGHT){
+                        directmode = LEFT;
+                        prelinedata = 0;
+                    }else if(crosscount == 1 && directmode == LEFT){
+                        directmode = LEFT;
+                    }else if(crosscount == 2){
+                        directmode = STOP;
+                        state = STAND_BY_1;
+                        crosscount = 0;
+                        prelinedata = 0;
+                    }
+                    break;
+                    //-----------------------------------------------
+                    case STAND_BY_1:
+                    crosscount = 0;
+                    directmode = STOP;
+                    //buzzer = 1.0;
+                    if(kbtread == RED_RECEIVE){
+                        state = RED_RECEIVE;
+                    }else if(kbtread == BLUE_RECEIVE){
+                        state = BLUE_RECEIVE_1;
+                    }
+                    break;
+                    case RED_RECEIVE:
+                    if(crosscount == 0){
+                        directmode = LEFT;
+                    }else if(crosscount == 1 && throwing == false){
+                        directmode = STOP;
+                        prelinedata = 0;
+                        if(kbtread == THROWING){
+                            throwing = true;
+                            buzzer = 1.0;
+                            timer.start();
+                            //lamp = 0.0;
+                            //wait(3.0);
+                            //buzzer = 0.0;
+                            prelinedata = 0;
+                            //directmode = RIGHT;
+                        }
+                    }else if(crosscount == 1 && throwing == true){
+                        times = timer.read();
+                        if(times > 2){
+                            lamp = 0.0;
+                            buzzer = 0.0;
+                            directmode = RIGHT;
+                            timer.reset();
+                            timer.stop();
+                        }
+                    }else if(crosscount == 2){
+                        directmode = STOP;
+                        state = STAND_BY_1;
+                        throwing = false;
+                        crosscount = 0;
+                    }
+                    break;
+                    case BLUE_RECEIVE_1:
+                    if(crosscount == 0){
+                        directmode = RIGHT;
+                    }else if(crosscount == 1){
+                        directmode = STRAIGHT;
+                    }else if(crosscount == 2){
+                        directmode = LEFT;
+                    }else if(crosscount == 3){
+                        directmode = STOP;
+                        prelinedata = 0;
+                        state = STAND_BY_2;
+                        crosscount = 0;
+                    }
+                    break;
+                    case STAND_BY_2:
+                    crosscount = 0;
+                    if(kbtread == BLUE_RECEIVE){
+                        state = BLUE_RECEIVE_2;
+                    }else if(kbtread == YELLOW_RECEIVE){
+                        state = YELLOW_RECEIVE;
+                    }
+                    break;
+                    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();
+                            }
+                        }
+                    }else if(crosscount == 2){
+                        directmode = STOP;
+                        state = STAND_BY_2;
+                        throwing = false;
+                        crosscount = 0;
+                    }
+                    break;
+                    case YELLOW_RECEIVE:
+                    if(crosscount != 4){
+                        directmode = LEFT;
+                    }else{
+                        directmode = STOP;
+                    }
+                    break;
+                    default:
+                    break;
+                }
+                /*
                 if(crosscount == 1){
                     directmode = STOP;
                     Stop(pwm_pins, dir_pins_1, dir_pins_2);
                     crosscount = 0;
-                }else{
-                    switch(directmode){
+                }
+                */
+                switch(directmode){
                     case STOP:
                     Stop(pwm_pins, dir_pins_1, dir_pins_2);
                     break;
                     case STRAIGHT:
                     //master.read(addr1,buff1,2);
-                    linecheck(buff1, linedata_1);
+                    //linecheck(buff1, linedata_1);
                     output = S_pid.compute((double)linedata_1[0]);
                     if(output >= 0){
                         rpower = power;
@@ -144,7 +302,7 @@
                     break;
                     case RIGHT:
                     //master.read(addr2,buff2,2);
-                    linecheck(buff2, linedata_2);
+                    //linecheck(buff2, linedata_2);
                     output = R_pid.compute((double)linedata_2[0]);
                     if(output >= 0){
                         rpower = power;
@@ -158,12 +316,12 @@
                     break;
                     case LEFT:
                     //master.read(addr3,buff3,2);
-                    linecheck(buff3, linedata_3);
+                    //linecheck(buff3, linedata_3);
                     output = L_pid.compute((double)linedata_3[0]);
                     if(output >= 0){
                         rpower = power;
                         lpower = power + output;
-                        Left(lpower, rpower, pwm_pins, dir_pins_1, dir_pins_2);
+                        Left(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
                     }else {
                         rpower = power + (-1 * output);
                         lpower = power;
@@ -172,16 +330,18 @@
                     break;
                     default:
                     break;
-                    }
                 }
             }
         }
-        //pc.printf("kbtread = %d \r" ,kbtread);     
+        //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_1[1]);
+        //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);
@@ -189,8 +349,8 @@
         //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("lpower    = %lf" ,lpower);  
+        pc.printf("dicmode = %d \r",directmode);
         pc.printf("output  = %lf \n" ,output);
     }
 }
@@ -202,25 +362,29 @@
             tar_y_dist = y_dist;
         }
         if(ontheline == true){
-            if((y_dist - tar_y_dist) >= STOP_DIST){
+            if((y_dist - tar_y_dist) >= Y_STOP_DIST){
                 ontheline = false;
                 crosscount++;
             }
         }
     }else if(dmode == RIGHT){
-        ontheline = true;
-        tar_x_dist = x_dist;
+        if(linedata_2[1] == 1){
+            ontheline = true;
+            tar_x_dist = x_dist;
+        }
         if(ontheline == true){
-            if((x_dist - tar_x_dist) >= STOP_DIST){
+            if((x_dist - tar_x_dist) >= X_STOP_DIST){
                 ontheline = false;
                 crosscount++;
             }
         }
     }else if(dmode == LEFT){
-        ontheline = true;
-        tar_x_dist = x_dist;
+        if(linedata_3[1] == 1){
+            ontheline = true;
+            tar_x_dist = x_dist;
+        }
         if(ontheline == true){
-            if((x_dist - tar_x_dist) <= STOP_DIST){
+            if((tar_x_dist - x_dist) >= X_STOP_DIST){
                 ontheline = false;
                 crosscount++;
             }
@@ -285,5 +449,4 @@
         data[1] = 0;
     }
     prelinedata = data[0];
-
-}
\ No newline at end of file
+}